store tension action force inside spring, make spring output force and derivative for abstraction, cleanup
This commit is contained in:
Irlan
parent
bb9839321d
commit
8b775361a9
@ -108,6 +108,27 @@ public:
|
||||
m_cloth.Step(dt);
|
||||
m_cloth.Apply();
|
||||
|
||||
b3StackArray<b3Vec3, 256> tension;
|
||||
tension.Resize(m_cloth.GetParticleCount());
|
||||
for (u32 i = 0; i < tension.Count(); ++i)
|
||||
{
|
||||
tension[i].SetZero();
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_cloth.GetSpringCount(); ++i)
|
||||
{
|
||||
b3Spring* s = m_cloth.GetSpring(i);
|
||||
|
||||
b3Particle* p1 = s->p1;
|
||||
b3Particle* p2 = s->p2;
|
||||
|
||||
u32 i1 = m_cloth.GetParticleIndex(p1);
|
||||
u32 i2 = m_cloth.GetParticleIndex(p2);
|
||||
|
||||
tension[i1] += s->tension;
|
||||
tension[i2] -= s->tension;
|
||||
}
|
||||
|
||||
for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)
|
||||
{
|
||||
b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i;
|
||||
@ -124,13 +145,13 @@ public:
|
||||
b3Draw_draw->DrawSegment(v2, v3, b3Color_black);
|
||||
b3Draw_draw->DrawSegment(v3, v1, b3Color_black);
|
||||
|
||||
b3Vec3 f1 = p1->tension;
|
||||
b3Vec3 f1 = tension[t->v1];
|
||||
float32 L1 = b3Length(f1);
|
||||
|
||||
b3Vec3 f2 = p2->tension;
|
||||
b3Vec3 f2 = tension[t->v2];
|
||||
float32 L2 = b3Length(f2);
|
||||
|
||||
b3Vec3 f3 = p3->tension;
|
||||
b3Vec3 f3 = tension[t->v3];
|
||||
float32 L3 = b3Length(f3);
|
||||
|
||||
float32 L = (L1 + L2 + L3) / 3.0f;
|
||||
|
||||
@ -77,21 +77,18 @@ enum b3ParticleType
|
||||
// Read-only particle
|
||||
struct b3Particle
|
||||
{
|
||||
// Particle type
|
||||
// Type
|
||||
b3ParticleType type;
|
||||
|
||||
// Mass position
|
||||
// Position
|
||||
b3Vec3 position;
|
||||
|
||||
// Mass velocity
|
||||
// Velocity
|
||||
b3Vec3 velocity;
|
||||
|
||||
// Mass force
|
||||
// Applied external force
|
||||
b3Vec3 force;
|
||||
|
||||
// Mass tension force for visualization
|
||||
b3Vec3 tension;
|
||||
|
||||
// Mass
|
||||
float32 mass;
|
||||
|
||||
@ -123,6 +120,8 @@ enum b3SpringType
|
||||
e_bendSpring,
|
||||
};
|
||||
|
||||
struct b3ClothSolverData;
|
||||
|
||||
// Read-only spring
|
||||
struct b3Spring
|
||||
{
|
||||
@ -143,6 +142,17 @@ struct b3Spring
|
||||
|
||||
// Damping stiffness
|
||||
float32 kd;
|
||||
|
||||
// Solver temp
|
||||
|
||||
// Action tensile force (f_i entry)
|
||||
b3Vec3 tension;
|
||||
|
||||
// Jacobian (J_ii entry)
|
||||
b3Mat33 Jx, Jv;
|
||||
|
||||
// Initialize solver data.
|
||||
void Initialize(const b3ClothSolverData* data);
|
||||
};
|
||||
|
||||
// Read-only contact
|
||||
@ -182,6 +192,10 @@ public:
|
||||
// Return the particle at a given index in this cloth.
|
||||
b3Particle* GetParticle(u32 i) const;
|
||||
|
||||
// Convenience function.
|
||||
// Return the index of a given particle.
|
||||
u32 GetParticleIndex(const b3Particle* p) const;
|
||||
|
||||
// Set the type of a given particle.
|
||||
void SetType(b3Particle* p, b3ParticleType type);
|
||||
|
||||
@ -194,6 +208,12 @@ public:
|
||||
// Apply a force to a given particle.
|
||||
void ApplyForce(b3Particle* p, const b3Vec3& force);
|
||||
|
||||
// Return the number of springs in this cloth.
|
||||
u32 GetSpringCount() const;
|
||||
|
||||
// Return the spring at a given index in this cloth.
|
||||
b3Spring* GetSpring(u32 i) const;
|
||||
|
||||
// Return the kinetic (or dynamic) energy in this system.
|
||||
float32 GetEnergy() const;
|
||||
|
||||
@ -273,6 +293,11 @@ inline b3Particle* b3Cloth::GetParticle(u32 i) const
|
||||
return m_particles + i;
|
||||
}
|
||||
|
||||
inline u32 b3Cloth::GetParticleIndex(const b3Particle* p) const
|
||||
{
|
||||
return u32(p - m_particles);
|
||||
}
|
||||
|
||||
inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
|
||||
{
|
||||
if (p->type == type)
|
||||
@ -319,6 +344,17 @@ inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
|
||||
p->force += force;
|
||||
}
|
||||
|
||||
inline u32 b3Cloth::GetSpringCount() const
|
||||
{
|
||||
return m_springCount;
|
||||
}
|
||||
|
||||
inline b3Spring* b3Cloth::GetSpring(u32 i) const
|
||||
{
|
||||
B3_ASSERT(i < m_springCount);
|
||||
return m_springs + i;
|
||||
}
|
||||
|
||||
inline float32 b3Cloth::GetEnergy() const
|
||||
{
|
||||
float32 E = 0.0f;
|
||||
|
||||
@ -41,6 +41,15 @@ struct b3ClothSolverDef
|
||||
u32 contactCapacity;
|
||||
};
|
||||
|
||||
struct b3ClothSolverData
|
||||
{
|
||||
b3Vec3* x;
|
||||
b3Vec3* v;
|
||||
b3Vec3* f;
|
||||
float32 dt;
|
||||
float32 invdt;
|
||||
};
|
||||
|
||||
class b3ClothSolver
|
||||
{
|
||||
public:
|
||||
@ -53,9 +62,6 @@ public:
|
||||
|
||||
void Solve(float32 dt, const b3Vec3& gravity);
|
||||
private:
|
||||
// Compute forces.
|
||||
void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity);
|
||||
|
||||
// Compute A and b in Ax = b
|
||||
void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
|
||||
|
||||
@ -71,8 +77,6 @@ private:
|
||||
|
||||
b3StackAllocator* m_allocator;
|
||||
|
||||
float32 m_h;
|
||||
|
||||
u32 m_particleCapacity;
|
||||
u32 m_particleCount;
|
||||
b3Particle** m_particles;
|
||||
@ -80,12 +84,12 @@ private:
|
||||
u32 m_springCapacity;
|
||||
u32 m_springCount;
|
||||
b3Spring** m_springs;
|
||||
b3Mat33* m_Jx;
|
||||
b3Mat33* m_Jv;
|
||||
|
||||
u32 m_contactCapacity;
|
||||
u32 m_contactCount;
|
||||
b3ParticleContact** m_contacts;
|
||||
|
||||
b3ClothSolverData m_solverData;
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -31,6 +31,58 @@
|
||||
|
||||
#define B3_CLOTH_FRICTION 0
|
||||
|
||||
// b3Spring
|
||||
void b3Spring::Initialize(const b3ClothSolverData* data)
|
||||
{
|
||||
u32 i1 = p1->solverId;
|
||||
u32 i2 = p2->solverId;
|
||||
|
||||
b3Vec3 x1 = data->x[i1];
|
||||
b3Vec3 v1 = data->v[i1];
|
||||
|
||||
b3Vec3 x2 = data->x[i2];
|
||||
b3Vec3 v2 = data->v[i2];
|
||||
|
||||
const b3Mat33 I = b3Mat33_identity;
|
||||
|
||||
b3Vec3 dx = x1 - x2;
|
||||
|
||||
if (b3Dot(dx, dx) >= L0 * L0)
|
||||
{
|
||||
// Tension
|
||||
float32 L = b3Length(dx);
|
||||
b3Vec3 n = dx / L;
|
||||
|
||||
b3Vec3 sf1 = -ks * (L - L0) * n;
|
||||
b3Vec3 sf2 = -sf1;
|
||||
|
||||
tension = sf1;
|
||||
|
||||
data->f[i1] += sf1;
|
||||
data->f[i2] += sf2;
|
||||
|
||||
// Jacobian
|
||||
Jx = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
|
||||
}
|
||||
else
|
||||
{
|
||||
tension.SetZero();
|
||||
Jx.SetZero();
|
||||
}
|
||||
|
||||
// Damping
|
||||
b3Vec3 dv = v1 - v2;
|
||||
|
||||
b3Vec3 df1 = -kd * dv;
|
||||
b3Vec3 df2 = -df1;
|
||||
|
||||
data->f[i1] += df1;
|
||||
data->f[i2] += df2;
|
||||
|
||||
Jv = -kd * I;
|
||||
}
|
||||
|
||||
// b3Cloth
|
||||
b3Cloth::b3Cloth()
|
||||
{
|
||||
m_gravity.SetZero();
|
||||
@ -204,7 +256,6 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
|
||||
p->translation.SetZero();
|
||||
p->x.SetZero();
|
||||
p->tension.SetZero();
|
||||
|
||||
b3ParticleContact* c = m_contacts + i;
|
||||
c->n_active = false;
|
||||
@ -254,6 +305,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
s->L0 = b3Distance(p1->position, p2->position);
|
||||
s->ks = def.ks;
|
||||
s->kd = def.kd;
|
||||
s->tension.SetZero();
|
||||
}
|
||||
|
||||
#if B3_CLOTH_BENDING
|
||||
@ -276,6 +328,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
s->L0 = b3Distance(p1->position, p2->position);
|
||||
s->ks = def.kb;
|
||||
s->kd = def.kd;
|
||||
s->tension.SetZero();
|
||||
}
|
||||
|
||||
m_allocator.Free(sharedEdges);
|
||||
@ -298,6 +351,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
|
||||
s->L0 = 0.0f;
|
||||
s->ks = def.ks;
|
||||
s->kd = def.kd;
|
||||
s->tension.SetZero();
|
||||
}
|
||||
|
||||
B3_ASSERT(m_springCount <= springCapacity);
|
||||
@ -557,12 +611,6 @@ void b3Cloth::Solve(float32 dt)
|
||||
{
|
||||
B3_PROFILE("Solve");
|
||||
|
||||
// Clear tension
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_particles[i].tension.SetZero();
|
||||
}
|
||||
|
||||
// Solve
|
||||
b3ClothSolverDef solverDef;
|
||||
solverDef.stack = &m_allocator;
|
||||
|
||||
@ -44,8 +44,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
m_springCapacity = def.springCapacity;
|
||||
m_springCount = 0;
|
||||
m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));;
|
||||
m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
|
||||
m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
|
||||
|
||||
m_contactCapacity = def.contactCapacity;
|
||||
m_contactCount = 0;
|
||||
@ -55,8 +53,6 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
|
||||
b3ClothSolver::~b3ClothSolver()
|
||||
{
|
||||
m_allocator->Free(m_contacts);
|
||||
m_allocator->Free(m_Jv);
|
||||
m_allocator->Free(m_Jx);
|
||||
m_allocator->Free(m_springs);
|
||||
m_allocator->Free(m_particles);
|
||||
}
|
||||
@ -94,65 +90,46 @@ static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator
|
||||
allocator->Free(matrix.values);
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->position;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->velocity;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->force;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->translation;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count)
|
||||
{
|
||||
for (u32 i = 0; i < count; ++i)
|
||||
{
|
||||
v[i] = particles[i]->x;
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
{
|
||||
B3_PROFILE("Integrate");
|
||||
|
||||
m_h = dt;
|
||||
|
||||
b3DenseVec3 sx(m_particleCount);
|
||||
b3CopyPosition(sx, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sv(m_particleCount);
|
||||
b3CopyVelocity(sv, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sf(m_particleCount);
|
||||
b3CopyForce(sf, m_particles, m_particleCount);
|
||||
|
||||
b3DenseVec3 sy(m_particleCount);
|
||||
b3CopyTranslation(sy, m_particles, m_particleCount);
|
||||
b3DenseVec3 sx0(m_particleCount);
|
||||
|
||||
Compute_f(sf, sx, sv, gravity);
|
||||
m_solverData.x = sx.v;
|
||||
m_solverData.v = sv.v;
|
||||
m_solverData.f = sf.v;
|
||||
m_solverData.dt = dt;
|
||||
m_solverData.invdt = 1.0f / dt;
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
sx[i] = p->position;
|
||||
sv[i] = p->velocity;
|
||||
sf[i] = p->force;
|
||||
|
||||
// Apply weight
|
||||
if (p->type == e_dynamicParticle)
|
||||
{
|
||||
sf[i] += p->mass * gravity;
|
||||
}
|
||||
|
||||
sy[i] = p->translation;
|
||||
sx0[i] = p->x;
|
||||
}
|
||||
|
||||
// Initialize spring forces
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* s = m_springs[i];
|
||||
s->Initialize(&m_solverData);
|
||||
}
|
||||
|
||||
// Solve Ax = b, where
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
@ -174,24 +151,19 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
b3DenseVec3 z(m_particleCount);
|
||||
Compute_z(z);
|
||||
|
||||
// x0
|
||||
b3DenseVec3 x0(m_particleCount);
|
||||
b3CopyGuess(x0, m_particles, m_particleCount);
|
||||
|
||||
// x
|
||||
b3DenseVec3 x(m_particleCount);
|
||||
|
||||
// Solve Ax = b
|
||||
u32 iterations = 0;
|
||||
Solve(x, iterations, A, b, S, z, x0);
|
||||
|
||||
Solve(x, iterations, A, b, S, z, sx0);
|
||||
b3_clothSolverIterations = iterations;
|
||||
|
||||
// f = A * x - b
|
||||
b3DenseVec3 f = A * x - b;
|
||||
|
||||
// Update state
|
||||
float32 h = m_h;
|
||||
float32 h = dt;
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
@ -221,15 +193,15 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
sx[i] = x1;
|
||||
}
|
||||
|
||||
// Write x to the solution cache for improving convergence
|
||||
// Cache x to improve convergence
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
m_particles[i]->x = x[i];
|
||||
}
|
||||
|
||||
// Store the extra contact constraint forces that should have been
|
||||
// supplied in order to enforce the contact constraints exactly
|
||||
// These forces can be used in contact constraint logic
|
||||
// supplied to enforce the contact constraints exactly.
|
||||
// These forces can be used in contact constraint logic.
|
||||
for (u32 i = 0; i < m_contactCount; ++i)
|
||||
{
|
||||
b3ParticleContact* c = m_contacts[i];
|
||||
@ -257,15 +229,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
|
||||
|
||||
#define B3_INDEX(i, j, size) (i + j * size)
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Vec3* out, u32 size)
|
||||
{
|
||||
for (u32 i = 0; i < size; ++i)
|
||||
{
|
||||
out[i].SetZero();
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3SetZero(b3Mat33* out, u32 size)
|
||||
{
|
||||
@ -275,19 +238,18 @@ static void b3SetZero(b3Mat33* out, u32 size)
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
|
||||
const b3Mat33* J_ii, b3Spring** const springs, u32 springCount)
|
||||
// dfdx * v
|
||||
static void b3Mul_Jx(b3DenseVec3& out, b3Spring** springs, u32 springCount, const b3DenseVec3& v)
|
||||
{
|
||||
b3SetZero(out, massCount);
|
||||
out.SetZero();
|
||||
|
||||
for (u32 i = 0; i < springCount; ++i)
|
||||
{
|
||||
const b3Spring* S = springs[i];
|
||||
u32 i1 = S->p1->solverId;
|
||||
u32 i2 = S->p2->solverId;
|
||||
b3Spring* s = springs[i];
|
||||
u32 i1 = s->p1->solverId;
|
||||
u32 i2 = s->p2->solverId;
|
||||
|
||||
b3Mat33 J_11 = J_ii[i];
|
||||
b3Mat33 J_11 = s->Jx;
|
||||
b3Mat33 J_12 = -J_11;
|
||||
b3Mat33 J_21 = J_12;
|
||||
b3Mat33 J_22 = J_11;
|
||||
@ -297,87 +259,6 @@ static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
|
||||
}
|
||||
}
|
||||
|
||||
void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity)
|
||||
{
|
||||
// Zero force
|
||||
f.SetZero();
|
||||
|
||||
// Apply weight
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b3Particle* p = m_particles[i];
|
||||
|
||||
if (p->type == e_dynamicParticle)
|
||||
{
|
||||
f[i] += p->mass * gravity;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply tension, damping
|
||||
// Store the Jacobians
|
||||
|
||||
// Zero Jacobians
|
||||
b3SetZero(m_Jx, m_springCount);
|
||||
b3SetZero(m_Jv, m_springCount);
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
b3Spring* S = m_springs[i];
|
||||
b3SpringType type = S->type;
|
||||
b3Particle* p1 = S->p1;
|
||||
b3Particle* p2 = S->p2;
|
||||
float32 L0 = S->L0;
|
||||
float32 ks = S->ks;
|
||||
float32 kd = S->kd;
|
||||
|
||||
u32 i1 = p1->solverId;
|
||||
u32 i2 = p2->solverId;
|
||||
|
||||
b3Vec3 x1 = x[i1];
|
||||
b3Vec3 v1 = v[i1];
|
||||
|
||||
b3Vec3 x2 = x[i2];
|
||||
b3Vec3 v2 = v[i2];
|
||||
|
||||
const b3Mat33 I = b3Mat33_identity;
|
||||
|
||||
b3Vec3 dx = x1 - x2;
|
||||
|
||||
if (b3Dot(dx, dx) >= L0 * L0)
|
||||
{
|
||||
// Tension
|
||||
float32 L = b3Length(dx);
|
||||
b3Vec3 n = dx / L;
|
||||
|
||||
b3Vec3 sf1 = -ks * (L - L0) * n;
|
||||
b3Vec3 sf2 = -sf1;
|
||||
|
||||
f[i1] += sf1;
|
||||
f[i2] += sf2;
|
||||
|
||||
p1->tension += sf1;
|
||||
p2->tension += sf2;
|
||||
|
||||
b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
|
||||
|
||||
m_Jx[i] = Jx11;
|
||||
}
|
||||
|
||||
// Damping
|
||||
b3Vec3 dv = v1 - v2;
|
||||
|
||||
b3Vec3 df1 = -kd * dv;
|
||||
b3Vec3 df2 = -df1;
|
||||
|
||||
f[i1] += df1;
|
||||
f[i2] += df2;
|
||||
|
||||
b3Mat33 Jv11 = -kd * I;
|
||||
|
||||
m_Jv[i] = Jv11;
|
||||
}
|
||||
}
|
||||
|
||||
static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
|
||||
{
|
||||
bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
|
||||
@ -389,6 +270,8 @@ static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
|
||||
|
||||
void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
|
||||
{
|
||||
float32 h = m_solverData.dt;
|
||||
|
||||
// Compute dfdx, dfdv
|
||||
b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
|
||||
b3SetZero(dfdx, m_particleCount * m_particleCount);
|
||||
@ -398,11 +281,11 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
|
||||
|
||||
for (u32 i = 0; i < m_springCount; ++i)
|
||||
{
|
||||
const b3Spring* S = m_springs[i];
|
||||
u32 i1 = S->p1->solverId;
|
||||
u32 i2 = S->p2->solverId;
|
||||
b3Spring* s = m_springs[i];
|
||||
u32 i1 = s->p1->solverId;
|
||||
u32 i2 = s->p2->solverId;
|
||||
|
||||
b3Mat33 Jx11 = m_Jx[i];
|
||||
b3Mat33 Jx11 = s->Jx;
|
||||
b3Mat33 Jx12 = -Jx11;
|
||||
b3Mat33 Jx21 = Jx12;
|
||||
b3Mat33 Jx22 = Jx11;
|
||||
@ -412,7 +295,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
|
||||
dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21;
|
||||
dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22;
|
||||
|
||||
b3Mat33 Jv11 = m_Jv[i];
|
||||
b3Mat33 Jv11 = s->Jv;
|
||||
b3Mat33 Jv12 = -Jv11;
|
||||
b3Mat33 Jv21 = Jv12;
|
||||
b3Mat33 Jv22 = Jv11;
|
||||
@ -424,6 +307,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
|
||||
}
|
||||
|
||||
// Compute A
|
||||
|
||||
// A = M - h * dfdv - h * h * dfdx
|
||||
|
||||
// A = 0
|
||||
@ -441,7 +325,7 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
|
||||
{
|
||||
for (u32 j = 0; j < m_particleCount; ++j)
|
||||
{
|
||||
A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]);
|
||||
A[B3_INDEX(i, j, m_particleCount)] += (-h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-h * h * dfdx[B3_INDEX(i, j, m_particleCount)]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -482,23 +366,17 @@ void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3Dense
|
||||
m_allocator->Free(dfdx);
|
||||
|
||||
// Compute b
|
||||
// b = h * (f0 + h * Jx_v + Jx_y)
|
||||
|
||||
// Jx_v = dfdx * v
|
||||
b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount);
|
||||
b3DenseVec3 Jx_v(m_particleCount);
|
||||
b3Mul_Jx(Jx_v, m_springs, m_springCount, v);
|
||||
|
||||
// Jx_y = dfdx * y
|
||||
b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
|
||||
b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount);
|
||||
b3DenseVec3 Jx_y(m_particleCount);
|
||||
b3Mul_Jx(Jx_y, m_springs, m_springCount, y);
|
||||
|
||||
for (u32 i = 0; i < m_particleCount; ++i)
|
||||
{
|
||||
b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]);
|
||||
}
|
||||
|
||||
m_allocator->Free(Jx_y);
|
||||
m_allocator->Free(Jx_v);
|
||||
// b = h * (f0 + h * Jx_v + Jx_y)
|
||||
b = h * (f + h * Jx_v + Jx_y);
|
||||
}
|
||||
|
||||
void b3ClothSolver::Compute_z(b3DenseVec3& z)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user