diff --git a/include/bounce/dynamics/cloth/spring_solver.h b/include/bounce/dynamics/cloth/spring_solver.h index 5c7688f..53a0974 100644 --- a/include/bounce/dynamics/cloth/spring_solver.h +++ b/include/bounce/dynamics/cloth/spring_solver.h @@ -22,6 +22,8 @@ #include #include +class b3Shape; + class b3SpringCloth; class b3StackAllocator; @@ -56,15 +58,21 @@ private: // Compute A and b in Ax = b void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const; + // Compute the initial guess for the iterative solver. + void Compute_x0(b3DenseVec3& x0); + + // Compute the constraint projection matrix S. + void Compute_S(b3Mat33* S); + // Solve Ax = b using the Modified Conjugate Gradient (MCG). // Output x and the residual error f. - void Solve_MCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const; + void Solve_MCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const; // Solve Ax = b using MCG with Jacobi preconditioning. // Output x and the residual error f. // This method is slower than MCG because we have to compute the preconditioning // matrix P, but it can improve convergence. - void Solve_MPCG(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const; + void Solve_MPCG(b3DenseVec3& x0, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const; b3SpringCloth * m_cloth; float32 m_h; @@ -87,6 +95,8 @@ private: b3Spring* m_springs; u32 m_springCount; + + b3Shape** m_shapes; }; inline u32 b3SpringSolver::GetIterations() const diff --git a/src/bounce/dynamics/cloth/spring_cloth.cpp b/src/bounce/dynamics/cloth/spring_cloth.cpp index bd2ca14..68e9299 100644 --- a/src/bounce/dynamics/cloth/spring_cloth.cpp +++ b/src/bounce/dynamics/cloth/spring_cloth.cpp @@ -25,7 +25,7 @@ #include #include -#define B3_FORCE_THRESHOLD (0.1f) +#define B3_FORCE_THRESHOLD 0.0f #define B3_CLOTH_BENDING 0 @@ -344,7 +344,7 @@ void b3SpringCloth::GetTension(b3Array& T) const } } -static B3_FORCE_INLINE void b3MakeTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n) +static B3_FORCE_INLINE void b3CreateTangents(b3Vec3& t1, b3Vec3& t2, const b3Vec3& dv, const b3Vec3& n) { t1 = dv - b3Dot(dv, n) * n; if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON) @@ -369,9 +369,18 @@ void b3SpringCloth::UpdateContacts() continue; } + // Relative velocity + b3Vec3 dv = m_v[i]; + b3MassContact* c = m_contacts + i; - bool wasLockedN = c->lockN; + // Save the old contact + b3MassContact c0 = *c; + + // Create a new contact + c->lockN = false; + c->lockT1 = false; + c->lockT2 = false; b3Sphere s1; s1.vertex = m_x[i]; @@ -384,13 +393,13 @@ void b3SpringCloth::UpdateContacts() for (u32 j = 0; j < m_shapeCount; ++j) { - b3Shape* shape = m_shapes[j]; + b3Shape* s2 = m_shapes[j]; b3Transform xf2; xf2.SetIdentity(); b3TestSphereOutput output; - if (shape->TestSphere(&output, s1, xf2) == false) + if (s2->TestSphere(&output, s1, xf2) == false) { continue; } @@ -403,123 +412,121 @@ void b3SpringCloth::UpdateContacts() } } - if (bestIndex == ~0) + if (bestIndex != ~0) { - c->Fn = 0.0f; - c->Ft1 = 0.0f; - c->Ft2 = 0.0f; - c->lockN = false; - c->lockT1 = false; - c->lockT2 = false; - continue; + B3_ASSERT(bestSeparation <= 0.0f); + + b3Shape* shape = m_shapes[bestIndex]; + float32 s = bestSeparation; + b3Vec3 n = bestNormal; + + // Update contact manifold + // Here the normal orientation is from the shape 2 (mass) to shape 1 + c->j = bestIndex; + c->n = n; + c->lockN = true; + + // Apply position correction + m_y[i] -= s * n; } - B3_ASSERT(bestSeparation <= 0.0f); - - b3Shape* shape = m_shapes[bestIndex]; - float32 s = bestSeparation; - b3Vec3 n = bestNormal; - - // Apply position correction - m_y[i] -= s * n; - // Update contact state - if (wasLockedN) + if (c0.lockN == true && c->lockN == true) { - // Was the contact force attractive? - if (c->Fn < B3_FORCE_THRESHOLD) + // The contact persists + + // Is the contact constraint still violated? + if (c0.Fn <= -B3_FORCE_THRESHOLD) { - // Terminate the contact. - c->Fn = 0.0f; - c->Ft1 = 0.0f; - c->Ft2 = 0.0f; - c->lockN = false; - c->lockT1 = false; - c->lockT2 = false; - continue; - } + // Contact force is attractive. - // Since the contact force was repulsive - // maintain the normal acceleration constraint. - c->j = bestIndex; - c->n = n; + // Terminate the contact. + c->lockN = false; + } } - else + +#if 0 + // Notify the new contact state + if (wasLockedN == false && c->lockN == true) { - // The contact has began. - c->j = bestIndex; - c->n = n; - c->Fn = 0.0f; - c->Ft1 = 0.0f; - c->Ft2 = 0.0f; - c->lockN = true; - c->lockT1 = false; - c->lockT2 = false; + // The contact has begun + } + + if (wasLockedN == true && c->lockN == false) + { + // The contact has ended + } +#endif + if (c->lockN == false) + { + continue; } #if B3_CLOTH_FRICTION == 1 - // Apply friction impulses + // A friction force requires an associated normal force. + if (c0.lockN == false) + { + continue; + } - // Relative velocity - b3Vec3 dv = m_v[i]; + b3Shape* s = m_shapes[c->j]; + b3Vec3 n = c->n; + float32 friction = s->GetFriction(); + float32 normalForce = c0.Fn; - b3MakeTangents(c->t1, c->t2, dv, n); - - // Note without a friction force, the tangential acceleration won't be - // removed. - - // Coefficients of friction for the solid - const float32 uk = shape->GetFriction(); - const float32 us = 2.0f * uk; - - float32 dvn = b3Dot(dv, n); - float32 normalImpulse = -m_inv_m[i] * dvn; + // Tangents + b3CreateTangents(c->t1, c->t2, dv, n); b3Vec3 ts[2]; ts[0] = c->t1; ts[1] = c->t2; bool lockT[2]; + lockT[0] = c->lockT1; + lockT[1] = c->lockT2; + + bool lockT0[2]; + lockT0[0] = c0.lockT1; + lockT0[1] = c0.lockT2; + + float32 Ft0[2]; + Ft0[0] = c0.Ft1; + Ft0[1] = c0.Ft2; for (u32 k = 0; k < 2; ++k) { b3Vec3 t = ts[k]; + // Relative tangential velocity float32 dvt = b3Dot(dv, t); - float32 tangentImpulse = -m_inv_m[i] * dvt; - float32 maxStaticImpulse = us * normalImpulse; - if (tangentImpulse * tangentImpulse > maxStaticImpulse * maxStaticImpulse) - { - lockT[k] = false; - - // Dynamic friction - float32 maxDynamicImpulse = uk * normalImpulse; - if (tangentImpulse * tangentImpulse > maxDynamicImpulse * maxDynamicImpulse) - { - b3Vec3 P = tangentImpulse * t; - - m_v[i] += m_m[i] * P; - } - } - else + if (dvt * dvt <= B3_EPSILON * B3_EPSILON) { + // Lock mass on surface lockT[k] = true; + } - // Static friction - b3Vec3 P = tangentImpulse * t; - - m_v[i] += m_m[i] * P; + if (lockT0[k] == true && lockT[k] == true) + { + // The contact persists + float32 maxForce = friction * normalForce; + + if (Ft0[k] * Ft0[k] > maxForce * maxForce) + { + // Unlock mass off surface + lockT[k] = false; + } } } c->lockT1 = lockT[0]; c->lockT2 = lockT[1]; -#endif // #if B3_CLOTH_FRICTION +#endif } + } void b3SpringCloth::Step(float32 dt) @@ -532,7 +539,7 @@ void b3SpringCloth::Step(float32 dt) // Update contacts UpdateContacts(); - // Apply gravity forces + // Apply weights for (u32 i = 0; i < m_massCount; ++i) { if (m_types[i] == b3MassType::e_dynamicMass) @@ -560,14 +567,19 @@ void b3SpringCloth::Step(float32 dt) // Store constraint forces for physics logic for (u32 i = 0; i < m_massCount; ++i) { - b3Vec3 force = forces[i]; - b3MassContact* c = m_contacts + i; + if (c->lockN == false) + { + continue; + } + + b3Vec3 force = forces[i]; + // Signed normal force magnitude c->Fn = b3Dot(force, c->n); - // Signed tangent forces magnitude + // Signed tangent force magnitude c->Ft1 = b3Dot(force, c->t1); c->Ft2 = b3Dot(force, c->t2); } @@ -599,21 +611,7 @@ void b3SpringCloth::Draw() const for (u32 i = 0; i < m->vertexCount; ++i) { - if (m_contacts[i].lockN) - { - if (m_contacts[i].Fn < B3_FORCE_THRESHOLD) - { - b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_yellow); - } - else - { - b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_red); - } - } - else - { - b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green); - } + b3Draw_draw->DrawPoint(m_x[i], 6.0f, b3Color_green); } for (u32 i = 0; i < m->triangleCount; ++i) diff --git a/src/bounce/dynamics/cloth/spring_solver.cpp b/src/bounce/dynamics/cloth/spring_solver.cpp index b13bbbd..e6be939 100644 --- a/src/bounce/dynamics/cloth/spring_solver.cpp +++ b/src/bounce/dynamics/cloth/spring_solver.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include // Here, we solve Ax = b using the Modified Conjugate Gradient method. @@ -53,6 +54,8 @@ b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def) m_springs = m_cloth->m_springs; m_springCount = m_cloth->m_springCount; + + m_shapes = m_cloth->m_shapes; } b3SpringSolver::~b3SpringSolver() @@ -66,7 +69,7 @@ void b3SpringSolver::Solve(b3DenseVec3& f) m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33)); - // Apply spring forces. Also, store their unique derivatives. + // Apply internal forces. Also, store their unique derivatives. ApplySpringForces(); // Integrate @@ -86,20 +89,27 @@ void b3SpringSolver::Solve(b3DenseVec3& f) // b3DenseVec3 b(m_massCount); - - // + + // A, b Compute_A_b(A, b); // x b3DenseVec3 x(m_massCount); + // x0 + Compute_x0(x); + + // S + b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33)); + Compute_S(S); + if (b3_enablePrecontitioning) { - Solve_MPCG(x, f, m_iterations, A, b); + Solve_MPCG(x, f, m_iterations, A, b, S); } else { - Solve_MCG(x, f, m_iterations, A, b); + Solve_MCG(x, f, m_iterations, A, b, S); } // Update state @@ -110,6 +120,8 @@ void b3SpringSolver::Solve(b3DenseVec3& f) // dx = h * (v0 + dv) + y = h * v1 + y m_x[i] += m_h * m_v[i] + m_y[i]; } + + m_allocator->Free(S); } m_allocator->Free(rowPtrs); @@ -176,7 +188,7 @@ void b3SpringSolver::ApplySpringForces() b3SetZero_Jacobian(m_Jx, m_springCount); b3SetZero_Jacobian(m_Jv, m_springCount); - // Compute forces and Jacobians + // Compute spring forces and Jacobians for (u32 i = 0; i < m_springCount; ++i) { b3Spring* S = m_springs + i; @@ -185,7 +197,7 @@ void b3SpringSolver::ApplySpringForces() u32 i1 = S->i1; u32 i2 = S->i2; float32 L0 = S->L0; - B3_ASSERT(L0 > 0.0f); + B3_ASSERT(L0 > 0.0f); float32 ks = S->ks; float32 kd = S->kd; @@ -196,7 +208,7 @@ void b3SpringSolver::ApplySpringForces() b3Vec3 v2 = m_v[i2]; const b3Mat33 I = b3Mat33_identity; - + // Strech b3Vec3 dx = x1 - x2; float32 L = b3Length(dx); @@ -213,7 +225,7 @@ void b3SpringSolver::ApplySpringForces() m_f[i2] += sf2; b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); - + m_Jx[i] = Jx11; } @@ -234,9 +246,9 @@ void b3SpringSolver::ApplySpringForces() static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A) { - bool isZeroX = b3Dot(A.x, A.x) <= B3_EPSILON * B3_EPSILON; - bool isZeroY = b3Dot(A.y, A.y) <= B3_EPSILON * B3_EPSILON; - bool isZeroZ = b3Dot(A.z, A.z) <= B3_EPSILON * B3_EPSILON; + bool isZeroX = b3Dot(A.x, A.x) == 0.0f; + bool isZeroY = b3Dot(A.y, A.y) == 0.0f; + bool isZeroZ = b3Dot(A.z, A.z) == 0.0f; return isZeroX * isZeroY * isZeroZ; } @@ -276,7 +288,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const dfdv[B3_INDEX(i2, i1, m_massCount)] += Jv21; dfdv[B3_INDEX(i2, i2, m_massCount)] += Jv22; } - + // Compute A // A = M - h * dfdv - h * h * dfdx @@ -298,7 +310,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const A[B3_INDEX(i, j, m_massCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_massCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_massCount)]); } } - + // Assembly sparsity u32 nzCount = 0; @@ -310,7 +322,7 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const { ++nzCount; } -} + } #endif SA.row_ptrs[0] = 0; @@ -344,8 +356,8 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const m_allocator->Free(A); // Compute b - // b = h * (f0 + h * Jx_v + Jx_y ) - + // b = h * (f0 + h * Jx_v + Jx_y) + // Jx_v = dfdx * v b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3)); b3Mul_Jacobian(Jx_v, m_v, m_massCount, m_Jx, m_springs, m_springCount); @@ -366,20 +378,16 @@ void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const m_allocator->Free(dfdx); } -// This outputs the desired acceleration of the masses in the constrained -// directions. -static void b3Compute_z(b3DenseVec3& out, - u32 massCount, const b3MassType* types, const b3MassContact* contacts) +void b3SpringSolver::Compute_x0(b3DenseVec3& x0) { - out.SetZero(); + x0.SetZero(); } -// -static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, const b3MassContact* contacts) +void b3SpringSolver::Compute_S(b3Mat33* out) { - for (u32 i = 0; i < massCount; ++i) + for (u32 i = 0; i < m_massCount; ++i) { - switch (types[i]) + switch (m_types[i]) { case b3MassType::e_staticMass: { @@ -388,24 +396,31 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co } case b3MassType::e_dynamicMass: { - if (contacts[i].lockN == true) + if (m_contacts[i].lockN == true) { - b3Vec3 n = contacts[i].n; + b3Vec3 n = m_contacts[i].n; b3Mat33 S = b3Mat33_identity - b3Outer(n, n); - if (contacts[i].lockT1 == true) + if (m_contacts[i].lockT1 == true && m_contacts[i].lockT2 == true) { - b3Vec3 t1 = contacts[i].t1; - - S -= b3Outer(t1, t1); + S.SetZero(); } - - if (contacts[i].lockT2 == true) + else { - b3Vec3 t2 = contacts[i].t2; + if (m_contacts[i].lockT1 == true) + { + b3Vec3 t1 = m_contacts[i].t2; + + S -= b3Outer(t1, t1); + } - S -= b3Outer(t2, t2); + if (m_contacts[i].lockT2 == true) + { + b3Vec3 t2 = m_contacts[i].t2; + + S -= b3Outer(t2, t2); + } } out[i] = S; @@ -425,7 +440,7 @@ static void b3Compute_S(b3Mat33* out, u32 massCount, const b3MassType* types, co } // Maintains invariants inside the MCG solver. -static void b3Filter(b3DenseVec3& out, +static void b3Filter(b3DenseVec3& out, const b3DenseVec3& v, const b3Mat33* S, u32 massCount) { for (u32 i = 0; i < massCount; ++i) @@ -434,15 +449,8 @@ static void b3Filter(b3DenseVec3& out, } } -void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const +void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const { - // - b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33)); - b3Compute_S(S, m_massCount, m_types, m_contacts); - - // dv = z - b3Compute_z(dv, m_massCount, m_types, m_contacts); - // r = filter(b - Adv) b3DenseVec3 r = b - A * dv; b3Filter(r, r, S, m_massCount); @@ -469,7 +477,7 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, // q = filter(A * c) b3DenseVec3 q = A * c; b3Filter(q, q, S, m_massCount); - + // alpha = epsNew / dot(c, q) float32 alpha = epsNew / b3Dot(c, q); @@ -494,8 +502,6 @@ void b3SpringSolver::Solve_MCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, ++iter; } - m_allocator->Free(S); - iterations = iter; // Residual error @@ -510,7 +516,7 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n) for (u32 i = 0; i < n; ++i) { b3Mat33 a = diagA[i]; - + float32 D = b3Det(a.x, a.y, a.z); if (D <= B3_EPSILON) @@ -522,15 +528,8 @@ static bool b3IsPD(const b3Mat33* diagA, u32 n) return true; } -void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b) const +void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3Mat33* S) const { - // S - b3Mat33* S = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33)); - b3Compute_S(S, m_massCount, m_types, m_contacts); - - // dv = z - b3Compute_z(dv, m_massCount, m_types, m_contacts); - // P = diag(A)^-1 b3DenseVec3 P(m_massCount); @@ -539,7 +538,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations // diag(A) b3Mat33* diagA = (b3Mat33*)m_allocator->Allocate(m_massCount * sizeof(b3Mat33)); A.AssembleDiagonal(diagA); - + // Compute P, P^-1 bool isPD = true; @@ -563,7 +562,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations B3_ASSERT(isPD == true); m_allocator->Free(diagA); - + // eps0 = dot( filter(b), P * filter(b) ) b3DenseVec3 filtered_b(m_massCount); b3Filter(filtered_b, b, S, m_massCount); @@ -577,7 +576,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations } float32 eps0 = b3Dot(filtered_b, P_filtered_b); - + // r = filter(b - Adv) b3DenseVec3 r = b - A * dv; b3Filter(r, r, S, m_massCount); @@ -591,7 +590,7 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations c[i][2] = inv_P[i][2] * r[i][2]; } b3Filter(c, c, S, m_massCount); - + // epsNew = dot(r, c) float32 epsNew = b3Dot(r, c); @@ -610,8 +609,9 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations b3Filter(q, q, S, m_massCount); // alpha = epsNew / dot(c, q) + B3_ASSERT(b3IsValid(b3Dot(c, q))); float32 alpha = epsNew / b3Dot(c, q); - + // dv = dv + alpha * c dv = dv + alpha * c; @@ -629,9 +629,11 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations // epsOld = epsNew float32 epsOld = epsNew; + B3_ASSERT(b3IsValid(epsOld)); // epsNew = dot(r, s) epsNew = b3Dot(r, s); + B3_ASSERT(b3IsValid(epsNew)); // beta = epsNew / epsOld float32 beta = epsNew / epsOld; @@ -643,8 +645,6 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations ++iter; } - m_allocator->Free(S); - iterations = iter; // Residual error