diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 90007b9..bbf146c 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -48,6 +48,8 @@ struct b3ClothSolverData b3DenseVec3* y; b3SparseSymMat33* dfdx; b3SparseSymMat33* dfdv; + b3DiagMat33* S; + b3DenseVec3* z; float32 dt; float32 invdt; }; @@ -57,6 +59,8 @@ struct b3AccelerationConstraint u32 i1; u32 ndof; b3Vec3 p, q, z; + + void Apply(const b3ClothSolverData* data); }; class b3ClothSolver @@ -71,21 +75,15 @@ public: void Solve(float32 dt, const b3Vec3& gravity); private: - // Initialize forces. - void InitializeForces(); - // Apply forces. void ApplyForces(); - // Initialize constraints. - void InitializeConstraints(); + // Apply constraints. + void ApplyConstraints(); - // Compute A and b in Ax = b + // Compute A and b in Ax = b. void Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const; - // Compute S and z. - void Compute_S_z(b3DiagMat33& S, b3DenseVec3& z); - // Solve Ax = b. void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; diff --git a/include/bounce/dynamics/cloth/force.h b/include/bounce/dynamics/cloth/force.h index abbe9ce..78a18ba 100644 --- a/include/bounce/dynamics/cloth/force.h +++ b/include/bounce/dynamics/cloth/force.h @@ -58,11 +58,8 @@ protected: b3Force() { } virtual ~b3Force() { } - virtual void Initialize(const b3ClothSolverData* data) = 0; virtual void Apply(const b3ClothSolverData* data) = 0; - // Solver shared - // Force type b3ForceType m_type; diff --git a/include/bounce/dynamics/cloth/spring_force.h b/include/bounce/dynamics/cloth/spring_force.h index 9e636bd..c5d1bba 100644 --- a/include/bounce/dynamics/cloth/spring_force.h +++ b/include/bounce/dynamics/cloth/spring_force.h @@ -73,9 +73,7 @@ private: b3SpringForce(const b3SpringForceDef* def); ~b3SpringForce(); - - void Initialize(const b3ClothSolverData* data); - + void Apply(const b3ClothSolverData* data); // Solver shared @@ -95,16 +93,8 @@ private: // Damping stiffness float32 m_kd; - // Solver temp - - // Force (f_1 entry) + // Applied internal force (on particle 1) b3Vec3 m_f; - - // Jacobian (J_11 entry) - b3Mat33 m_Jx; - - // Jacobian (J_11 entry) - b3Mat33 m_Jv; }; inline b3Particle * b3SpringForce::GetParticle1() diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 2c4fc32..2dfbf33 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -79,14 +79,6 @@ void b3ClothSolver::Add(b3Force* f) m_forces[m_forceCount++] = f; } -void b3ClothSolver::InitializeForces() -{ - for (u32 i = 0; i < m_forceCount; ++i) - { - m_forces[i]->Initialize(&m_solverData); - } -} - void b3ClothSolver::ApplyForces() { for (u32 i = 0; i < m_forceCount; ++i) @@ -95,8 +87,50 @@ void b3ClothSolver::ApplyForces() } } -void b3ClothSolver::InitializeConstraints() +void b3AccelerationConstraint::Apply(const b3ClothSolverData* data) { + (*data->z)[i1] = z; + + b3Mat33 I; I.SetIdentity(); + + switch (ndof) + { + case 3: + { + (*data->S)[i1] = I; + break; + } + case 2: + { + (*data->S)[i1] = I - b3Outer(p, p); + break; + } + case 1: + { + (*data->S)[i1] = I - b3Outer(p, p) - b3Outer(q, q); + break; + } + case 0: + { + (*data->S)[i1].SetZero(); + break; + } + default: + { + B3_ASSERT(false); + break; + } + } +} + +void b3ClothSolver::ApplyConstraints() +{ + b3DiagMat33& S = *m_solverData.S; + b3DenseVec3& z = *m_solverData.z; + + S.SetIdentity(); + z.SetZero(); + for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; @@ -141,6 +175,11 @@ void b3ClothSolver::InitializeConstraints() } } } + + for (u32 i = 0; i < m_constraintCount; ++i) + { + m_constraints[i].Apply(&m_solverData); + } } void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) @@ -152,6 +191,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) b3DenseVec3 sx0(m_particleCount); b3SparseSymMat33 dfdx(m_particleCount); b3SparseSymMat33 dfdv(m_particleCount); + b3DiagMat33 S(m_particleCount); + b3DenseVec3 z(m_particleCount); m_solverData.x = &sx; m_solverData.v = &sv; @@ -159,6 +200,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) m_solverData.y = &sy; m_solverData.dfdx = &dfdx; m_solverData.dfdv = &dfdv; + m_solverData.S = &S; + m_solverData.z = &z; m_solverData.dt = dt; m_solverData.invdt = 1.0f / dt; @@ -188,19 +231,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sy[p->m_solverId] -= c->s * c->n; } - // Initialize internal forces - InitializeForces(); - // Apply internal forces ApplyForces(); - // Initialize constraints - InitializeConstraints(); - - // Compute S, z - b3DiagMat33 S(m_particleCount); - b3DenseVec3 z(m_particleCount); - Compute_S_z(S, z); + // Apply constraints + ApplyConstraints(); // Solve Ax = b, where // A = M - h * dfdv - h * h * dfdx @@ -267,7 +302,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const { B3_PROFILE("Compute A, b"); - + b3DenseVec3& x = *m_solverData.x; b3DenseVec3& v = *m_solverData.v; b3DenseVec3& f = *m_solverData.f; @@ -289,7 +324,7 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const } // A += - h * dfdv - h * h * dfdx - + // A += - h * dfdv for (u32 row = 0; row < dfdv.M; ++row) { @@ -300,7 +335,7 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const { u32 row_value_index = row_value_begin + row_value; u32 row_value_column = dfdv.value_columns[row_value_index]; - + A(row, row_value_column) += -h * dfdv.values[row_value_index]; } } @@ -319,50 +354,13 @@ void b3ClothSolver::Compute_A_b(b3SparseSymMat33& A, b3DenseVec3& b) const A(row, row_value_column) += -h * h * dfdx.values[row_value_index]; } } - + // Compute b // b = h * (f0 + h * dfdx * v + dfdx * y) b = h * (f + h * (dfdx * v) + dfdx * y); } -void b3ClothSolver::Compute_S_z(b3DiagMat33& S, b3DenseVec3& z) -{ - S.SetIdentity(); - z.SetZero(); - - for (u32 i = 0; i < m_constraintCount; ++i) - { - b3AccelerationConstraint* ac = m_constraints + i; - u32 ip = ac->i1; - u32 ndof = ac->ndof; - b3Vec3 p = ac->p; - b3Vec3 q = ac->q; - b3Vec3 cz = ac->z; - - z[ip] = cz; - - if (ndof == 2) - { - b3Mat33 I; I.SetIdentity(); - - S[ip] = I - b3Outer(p, p); - } - - if (ndof == 1) - { - b3Mat33 I; I.SetIdentity(); - - S[ip] = I - b3Outer(p, p) - b3Outer(q, q); - } - - if (ndof == 0) - { - S[ip].SetZero(); - } - } -} - void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const { @@ -415,7 +413,7 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, float32 delta_new = b3Dot(r, p); // Set the tolerance. - const float32 tolerance = 1.e-4f; + const float32 tolerance = 10.0f * B3_EPSILON; // Maximum number of iterations. // Stop at this iteration if diverged. diff --git a/src/bounce/dynamics/cloth/spring_force.cpp b/src/bounce/dynamics/cloth/spring_force.cpp index 400e164..20c4ec2 100644 --- a/src/bounce/dynamics/cloth/spring_force.cpp +++ b/src/bounce/dynamics/cloth/spring_force.cpp @@ -50,10 +50,13 @@ b3SpringForce::~b3SpringForce() } -void b3SpringForce::Initialize(const b3ClothSolverData* data) +void b3SpringForce::Apply(const b3ClothSolverData* data) { b3DenseVec3& x = *data->x; b3DenseVec3& v = *data->v; + b3DenseVec3& f = *data->f; + b3SparseSymMat33& dfdx = *data->dfdx; + b3SparseSymMat33& dfdv = *data->dfdv; u32 i1 = m_p1->m_solverId; u32 i2 = m_p2->m_solverId; @@ -70,6 +73,8 @@ void b3SpringForce::Initialize(const b3ClothSolverData* data) float32 L = b3Length(dx); + b3Mat33 Jx; + if (L >= m_L0) { b3Vec3 n = dx / L; @@ -78,34 +83,24 @@ void b3SpringForce::Initialize(const b3ClothSolverData* data) m_f = -m_ks * (L - m_L0) * n; // Jacobian - m_Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); + Jx = -m_ks * (b3Outer(dx, dx) + (1.0f - m_L0 / L) * (I - b3Outer(dx, dx))); } else { m_f.SetZero(); - m_Jx.SetZero(); + Jx.SetZero(); } // Damping b3Vec3 dv = v1 - v2; m_f += -m_kd * dv; - m_Jv = -m_kd * I; -} - -void b3SpringForce::Apply(const b3ClothSolverData* data) -{ - b3DenseVec3& f = *data->f; - b3SparseSymMat33& dfdx = *data->dfdx; - b3SparseSymMat33& dfdv = *data->dfdv; - - u32 i1 = m_p1->m_solverId; - u32 i2 = m_p2->m_solverId; + b3Mat33 Jv = -m_kd * I; f[i1] += m_f; f[i2] -= m_f; - b3Mat33 Jx11 = m_Jx; + b3Mat33 Jx11 = Jx; b3Mat33 Jx12 = -Jx11; //b3Mat33 Jx21 = Jx12; b3Mat33 Jx22 = Jx11; @@ -115,7 +110,7 @@ void b3SpringForce::Apply(const b3ClothSolverData* data) //dfdx(i2, i1) += Jx21; dfdx(i2, i2) += Jx22; - b3Mat33 Jv11 = m_Jv; + b3Mat33 Jv11 = Jv; b3Mat33 Jv12 = -Jv11; //b3Mat33 Jv21 = Jv12; b3Mat33 Jv22 = Jv11;