diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index 3d5535f..e02e6a1 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -183,9 +183,6 @@ private: // Pool of particles b3BlockPool m_particleBlocks; - // Pool of body contacts - b3BlockPool m_bodyContactBlocks; - // Pool of particle contacts b3BlockPool m_particleContactBlocks; @@ -195,9 +192,6 @@ private: // List of forces b3List2 m_forceList; - // List of particle contacts - b3List2 m_bodyContactList; - // List of particle contacts b3List2 m_particleContactList; }; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index afa6318..abe1b20 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -71,6 +71,16 @@ public: b3Particle* p1; b3Shape* s2; + // Contact constraint + float32 s; + b3Vec3 p; + b3Vec3 n; + float32 fn0; + float32 fn; + float32 ft1, ft2; + bool t1Active; + bool t2Active; + // Contact constraint b3Vec3 localPoint1; b3Vec3 localPoint2; @@ -80,8 +90,7 @@ public: b3Vec3 t1, t2; b3Vec2 tangentImpulse; - b3BodyContact* m_prev; - b3BodyContact* m_next; + bool active; }; struct b3BodyContactWorldPoint @@ -198,7 +207,7 @@ private: // Applied external force b3Vec3 m_force; - // Applied external translation + // Applied translation b3Vec3 m_translation; // Mass @@ -230,6 +239,9 @@ private: // b3Cloth* m_cloth; + // + b3BodyContact m_bodyContact; + // b3Particle* m_prev; diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 7844fdf..30cfe2f 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -148,7 +148,6 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) b3Cloth::b3Cloth(const b3ClothDef& def) : m_particleBlocks(sizeof(b3Particle)), - m_bodyContactBlocks(sizeof(b3BodyContact)), m_particleContactBlocks(sizeof(b3ParticleContact)) { B3_ASSERT(def.mesh); @@ -484,17 +483,6 @@ void b3Cloth::UpdateBodyContacts() B3_PROFILE("Cloth Update Body Contacts"); - // Clear buffer - b3BodyContact* c = m_bodyContactList.m_head; - while (c) - { - b3BodyContact* c0 = c; - c = c->m_next; - m_bodyContactList.Remove(c0); - c0->~b3BodyContact(); - m_bodyContactBlocks.Free(c0); - } - // Create contacts for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { @@ -539,6 +527,7 @@ void b3Cloth::UpdateBodyContacts() if (bestShape == nullptr) { + p->m_bodyContact.active = false; continue; } @@ -549,17 +538,108 @@ void b3Cloth::UpdateBodyContacts() b3Vec3 point = bestPoint; b3Vec3 normal = -bestNormal; - b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate(); + b3BodyContact* c = &p->m_bodyContact; + + b3BodyContact c0 = *c; + + c->active = true; c->p1 = p; c->s2 = shape; + c->s = separation; + c->p = point; + c->n = normal; + c->fn0 = 0.0f; + c->fn = 0.0f; + c->ft1 = 0.0f; + c->ft2 = 0.0f; + c->t1Active = false; + c->t2Active = false; c->localPoint1.SetZero(); c->localPoint2 = body->GetLocalPoint(point); c->t1 = b3Perp(normal); c->t2 = b3Cross(c->t1, normal); c->normalImpulse = 0.0f; c->tangentImpulse.SetZero(); + +#if 0 + // Apply position correction + b3Vec3 dx = separation * normal; + p->m_translation += dx; +#endif - m_bodyContactList.PushFront(c); + // Update contact state + if (c0.active == true) + { + c->fn0 = c0.fn0; + c->fn = c0.fn; + c->ft1 = c0.ft1; + c->ft2 = c0.ft2; + + c->normalImpulse = c0.normalImpulse; + c->tangentImpulse = c0.tangentImpulse; +#if 0 + const float32 kForceTol = 0.0f; + + // Allow the contact to release when the constraint force + // switches from a repulsive force to an attractive one. + // if (c0.fn0 < kForceTol && c0.fn > kForceTol) + if (c0.fn > kForceTol) + { + // Contact force is attractive. + c->active = false; + continue; + } +#endif + } +#if 0 + // A friction force requires an associated normal force. + if (c0.active == false) + { + continue; + } + b3Vec3 v1; v1.SetZero(); + b3Vec3 v2 = p->m_velocity; + b3Vec3 dv = v2 - v1; + + const float32 kVelTol = B3_EPSILON * B3_EPSILON; + + // Lock particle on surface + float32 dvt1 = b3Dot(dv, c->t1); + if (dvt1 * dvt1 < kVelTol * kVelTol) + { + c->t1Active = true; + } + + float32 dvt2 = b3Dot(dv, c->t2); + if (dvt2 * dvt2 < kVelTol * kVelTol) + { + c->t2Active = true; + } + + // Unlock particle off surface + float32 normalForce = c->fn; + + float32 friction = shape->GetFriction(); + float32 maxFrictionForce = friction * normalForce; + + if (c0.t1Active == true) + { + float32 tangentForce1 = c0.ft1; + if (tangentForce1 * tangentForce1 > maxFrictionForce * maxFrictionForce) + { + c->t1Active = false; + } + } + + if (c0.t2Active == true) + { + float32 tangentForce2 = c0.ft2; + if (tangentForce2 * tangentForce2 > maxFrictionForce* maxFrictionForce) + { + c->t2Active = false; + } + } +#endif } } @@ -632,7 +712,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) solverDef.stack = &m_stackAllocator; solverDef.particleCapacity = m_particleList.m_count; solverDef.forceCapacity = m_forceList.m_count; - solverDef.bodyContactCapacity = m_bodyContactList.m_count; + solverDef.bodyContactCapacity = m_particleList.m_count; solverDef.particleContactCapacity = m_particleContactList.m_count; b3ClothSolver solver(solverDef); @@ -646,26 +726,22 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) { solver.Add(f); } - - for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next) + + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - solver.Add(c); + if (p->m_bodyContact.active) + { + solver.Add(&p->m_bodyContact); + } } - + for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next) { solver.Add(c); } - + // Solve solver.Solve(dt, gravity); - - // Clear external applied forces and translations - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) - { - p->m_force.SetZero(); - p->m_translation.SetZero(); - } } void b3Cloth::UpdateContacts() @@ -691,6 +767,13 @@ void b3Cloth::Step(float32 dt) { Solve(dt, m_gravity); } + + // Clear external applied forces and translations + for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + { + p->m_force.SetZero(); + p->m_translation.SetZero(); + } } void b3Cloth::Draw() const diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 737ffd6..cc46370 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -149,7 +149,7 @@ void b3ClothSolver::ApplyConstraints() S.SetIdentity(); z.SetZero(); - + for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; @@ -162,33 +162,42 @@ void b3ClothSolver::ApplyConstraints() ac->z.SetZero(); } } - +#if 0 for (u32 i = 0; i < m_bodyContactCount; ++i) { b3BodyContact* bc = m_bodyContacts[i]; - + b3Particle* p1 = bc->p1; - + B3_ASSERT(p1->m_type == e_dynamicParticle); - b3Transform xf1; - xf1.position = x[p1->m_solverId]; - xf1.rotation.SetIdentity(); - - b3Shape* s2 = bc->s2; - b3Body* b2 = s2->GetBody(); - b3Transform xf2 = b2->GetTransform(); - - b3BodyContactWorldPoint bcwp; - bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2); - b3AccelerationConstraint* ac = m_constraints + m_constraintCount; ++m_constraintCount; ac->i1 = p1->m_solverId; ac->ndof = 2; - ac->p = bcwp.normal; + ac->p = bc->n; ac->z.SetZero(); + + if (bc->t1Active && bc->t2Active) + { + ac->ndof = 0; + } + else + { + if (bc->t1Active) + { + ac->ndof = 1; + ac->q = bc->t1; + } + + if (bc->t2Active) + { + ac->ndof = 1; + ac->q = bc->t2; + } + } } +#endif for (u32 i = 0; i < m_constraintCount; ++i) { @@ -210,7 +219,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) m_solverData.x = &sx; m_solverData.v = &sv; m_solverData.f = &sf; - + for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; @@ -249,25 +258,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) sy[i] = p->m_translation; sx0[i] = p->m_x; } - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3BodyContact* bc = m_bodyContacts[i]; - - b3Particle* p1 = bc->p1; - b3Transform xf1; - xf1.position = sx[p1->m_solverId]; - xf1.rotation.SetIdentity(); - - b3Shape* s2 = bc->s2; - b3Body* b2 = s2->GetBody(); - b3Transform xf2 = b2->GetTransform(); - - b3BodyContactWorldPoint bcwp; - bcwp.Initialize(bc, p1->m_radius, xf1, s2->m_radius, xf2); - - sy[p1->m_solverId] += bcwp.separation * bcwp.normal; - } // Apply internal forces ApplyForces(); @@ -290,16 +280,33 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // b b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); - + // x b3DenseVec3 x(m_particleCount); u32 iterations = 0; Solve(x, iterations, A, b, S, z, sx0); b3_clothSolverIterations = iterations; +#if 0 + // Copy constraint forces to the contacts + b3DenseVec3 f = A * x - b; + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* c = m_bodyContacts[i]; + + b3Vec3 cf = f[i]; + + c->fn0 = c->fn; + c->fn = b3Dot(cf, c->n); + c->ft1 = b3Dot(cf, c->t1); + c->ft2 = b3Dot(cf, c->t2); + } +#endif + // sv = sv + x; sx = sx + sy; + // Copy state buffers back to the particle for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; @@ -309,7 +316,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) p->m_velocity = sv[i]; } } - +#if 1 // Solve constraints b3ClothContactSolverDef contactSolverDef; contactSolverDef.allocator = m_allocator; @@ -341,10 +348,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) contactSolver.SolveParticleContactVelocityConstraints(); } } - + { contactSolver.StoreImpulses(); } +#endif // Integrate positions sx = sx + h * sv; @@ -358,7 +366,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints(); - + if (bodyContactsSolved && particleContactsSolved) { positionSolved = true; @@ -366,7 +374,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) } } } - +#if 0 // Synchronize bodies for (u32 i = 0; i < m_bodyContactCount; ++i) { @@ -378,7 +386,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) body->SynchronizeShapes(); } - +#endif // Copy state buffers back to the particles for (u32 i = 0; i < m_particleCount; ++i) { diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp index 169e5bc..7dde875 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -73,7 +73,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) { m_cloth = cloth; m_type = def.type; - m_position = def.position; m_velocity = def.velocity; m_force = def.force; @@ -85,6 +84,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_userData = nullptr; m_x.SetZero(); m_vertex = ~0; + m_bodyContact.active = false; } b3Particle::~b3Particle()