diff --git a/src/bounce/dynamics/cloth/spring_cloth.cpp b/src/bounce/dynamics/cloth/spring_cloth.cpp index dd47da3..ef31019 100644 --- a/src/bounce/dynamics/cloth/spring_cloth.cpp +++ b/src/bounce/dynamics/cloth/spring_cloth.cpp @@ -233,6 +233,9 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def) S->i1 = e->v1; S->i2 = e->v2; S->L0 = b3Distance(p1, p2); + + B3_ASSERT(S->L0 > B3_EPSILON); + S->ks = def.ks; S->kd = def.kd; ++m_springCount; @@ -306,17 +309,18 @@ void b3SpringCloth::GetTension(b3Array& T) const b3Vec3 x2 = m_x[i2]; b3Vec3 v2 = m_v[i2]; - // Strech + // Streching b3Vec3 dx = x1 - x2; float32 L = b3Length(dx); - float32 s = 1.0f; - if (L > 0.0f) + if (L < L0) { - s -= L0 / L; + L = L0; } - b3Vec3 sf1 = -ks * s * dx; + b3Vec3 n = dx / L; + + b3Vec3 sf1 = -ks * (L - L0) * n; b3Vec3 sf2 = -sf1; T[i1] += sf1; diff --git a/src/bounce/dynamics/cloth/spring_solver.cpp b/src/bounce/dynamics/cloth/spring_solver.cpp index fb95a46..f669c20 100644 --- a/src/bounce/dynamics/cloth/spring_solver.cpp +++ b/src/bounce/dynamics/cloth/spring_solver.cpp @@ -185,6 +185,7 @@ void b3SpringSolver::ApplySpringForces() u32 i1 = S->i1; u32 i2 = S->i2; float32 L0 = S->L0; + B3_ASSERT(L0 > 0.0f); float32 ks = S->ks; float32 kd = S->kd; @@ -198,19 +199,23 @@ void b3SpringSolver::ApplySpringForces() b3Vec3 dx = x1 - x2; float32 L = b3Length(dx); - B3_ASSERT(L > 0.0f); + // Ensure force is a tension. + if (L < L0) + { + L = L0; + } - b3Vec3 sf1 = -ks * (1.0f - L0 / L) * dx; + b3Vec3 n = dx / L; + + b3Vec3 sf1 = -ks * (L - L0) * n; b3Vec3 sf2 = -sf1; m_f[i1] += sf1; m_f[i2] += sf2; - // C * n = 1 - L0 / L * dx const b3Mat33 I = b3Mat33_identity; - float32 L3 = L * L * L; - b3Mat33 Jx11 = -ks * ((1.0f - L0 / L) * I + (L0 / L3) * b3Outer(dx, dx)); + b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx))); m_Jx[i] = Jx11; @@ -557,6 +562,8 @@ void b3SpringSolver::Solve_MPCG(b3DenseVec3& dv, b3DenseVec3& e, u32& iterations inv_P[i] = b3Vec3(D[0][0], D[1][1], D[2][2]); } + B3_ASSERT(isPD == true); + m_allocator->Free(diagA); // eps0 = dot( filter(b), P * filter(b) )