From 46600010fedc730a87b8a48e6e44bd1e06a06bb1 Mon Sep 17 00:00:00 2001 From: Irlan Date: Sun, 2 Jun 2019 12:05:36 -0300 Subject: [PATCH] More consistency and pass velocity and position iterations to cloth constraint solver --- examples/testbed/tests/cloth_capsule.h | 2 +- examples/testbed/tests/pinned_cloth.h | 2 +- examples/testbed/tests/table_cloth.h | 2 +- examples/testbed/tests/tension_mapping.h | 2 +- include/bounce/cloth/cloth.h | 7 +- include/bounce/cloth/cloth_contact_solver.h | 6 +- include/bounce/cloth/cloth_solver.h | 14 +-- include/bounce/cloth/particle.h | 26 ++--- src/bounce/cloth/cloth.cpp | 109 ++------------------ src/bounce/cloth/cloth_contact_solver.cpp | 14 +-- src/bounce/cloth/cloth_solver.cpp | 84 ++------------- src/bounce/cloth/particle.cpp | 6 +- 12 files changed, 47 insertions(+), 227 deletions(-) diff --git a/examples/testbed/tests/cloth_capsule.h b/examples/testbed/tests/cloth_capsule.h index 0a816e3..84a4d0e 100644 --- a/examples/testbed/tests/cloth_capsule.h +++ b/examples/testbed/tests/cloth_capsule.h @@ -86,7 +86,7 @@ public: { Test::Step(); - m_cloth->Step(g_testSettings->inv_hertz); + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); m_cloth->Draw(); diff --git a/examples/testbed/tests/pinned_cloth.h b/examples/testbed/tests/pinned_cloth.h index b2a28ef..023e662 100644 --- a/examples/testbed/tests/pinned_cloth.h +++ b/examples/testbed/tests/pinned_cloth.h @@ -84,7 +84,7 @@ public: { Test::Step(); - m_cloth->Step(g_testSettings->inv_hertz); + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); m_cloth->Draw(); diff --git a/examples/testbed/tests/table_cloth.h b/examples/testbed/tests/table_cloth.h index 9b9c147..5f83489 100644 --- a/examples/testbed/tests/table_cloth.h +++ b/examples/testbed/tests/table_cloth.h @@ -88,7 +88,7 @@ public: { Test::Step(); - m_cloth->Step(g_testSettings->inv_hertz); + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); m_cloth->Draw(); diff --git a/examples/testbed/tests/tension_mapping.h b/examples/testbed/tests/tension_mapping.h index 74d93ae..1da11b2 100644 --- a/examples/testbed/tests/tension_mapping.h +++ b/examples/testbed/tests/tension_mapping.h @@ -110,7 +110,7 @@ public: { Test::Step(); - m_cloth->Step(g_testSettings->inv_hertz); + m_cloth->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations); const b3ClothMesh* mesh = m_cloth->GetMesh(); diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index d4ff2a8..a200861 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -29,8 +29,7 @@ class b3Shape; class b3Particle; class b3Force; -class b3BodyContact; -class b3ParticleContact; +struct b3ParticleBodyContact; struct b3ParticleDef; struct b3ForceDef; @@ -134,7 +133,7 @@ public: float32 GetEnergy() const; // Perform a time step. - void Step(float32 dt); + void Step(float32 dt, u32 velocityIterations, u32 positionIterations); // Debug draw the cloth using the associated cloth mesh. void Draw() const; @@ -149,7 +148,7 @@ private: void UpdateContacts(); // Solve - void Solve(float32 dt, const b3Vec3& gravity); + void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); // Stack allocator b3StackAllocator m_stackAllocator; diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h index 1fe3070..a945d03 100644 --- a/include/bounce/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/cloth_contact_solver.h @@ -27,7 +27,7 @@ class b3StackAllocator; class b3Particle; class b3Body; -class b3BodyContact; +struct b3ParticleBodyContact; struct b3DenseVec3; @@ -88,7 +88,7 @@ struct b3ClothContactSolverDef b3DenseVec3* velocities; u32 bodyContactCount; - b3BodyContact** bodyContacts; + b3ParticleBodyContact** bodyContacts; }; inline float32 b3MixFriction(float32 u1, float32 u2) @@ -118,7 +118,7 @@ protected: b3DenseVec3* m_velocities; u32 m_bodyContactCount; - b3BodyContact** m_bodyContacts; + b3ParticleBodyContact** m_bodyContacts; b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; }; diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h index 7eb4229..3a582db 100644 --- a/include/bounce/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -33,8 +33,7 @@ struct b3DiagMat33; struct b3SparseSymMat33; struct b3SparseSymMat33View; -class b3BodyContact; -class b3ParticleContact; +struct b3ParticleBodyContact; struct b3ClothSolverDef { @@ -75,10 +74,9 @@ public: void Add(b3Particle* p); void Add(b3Force* f); - void Add(b3BodyContact* c); - void Add(b3ParticleContact* c); + void Add(b3ParticleBodyContact* c); - void Solve(float32 dt, const b3Vec3& gravity); + void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations); private: // Apply forces. void ApplyForces(); @@ -107,11 +105,7 @@ private: u32 m_bodyContactCapacity; u32 m_bodyContactCount; - b3BodyContact** m_bodyContacts; - - u32 m_particleContactCapacity; - u32 m_particleContactCount; - b3ParticleContact** m_particleContacts; + b3ParticleBodyContact** m_bodyContacts; b3ClothSolverData m_solverData; }; diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index bec7da6..b496662 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -62,27 +62,13 @@ struct b3ParticleDef }; // A contact between a particle and a solid -class b3BodyContact +struct b3ParticleBodyContact { -public: - b3BodyContact() { } - ~b3BodyContact() { } - b3Particle* p1; b3Shape* s2; // Contact constraint - float32 s; - b3Vec3 p; - b3Vec3 n; - float32 fn0; - float32 fn; - float32 ft1, ft2; - bool nActive; - bool t1Active; - bool t2Active; - - // Contact constraint + b3Vec3 normal1; b3Vec3 localPoint1; b3Vec3 localPoint2; float32 normalImpulse; @@ -94,9 +80,9 @@ public: bool active; }; -struct b3BodyContactWorldPoint +struct b3ParticleBodyContactWorldPoint { - void Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); + void Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB); b3Vec3 point; b3Vec3 normal; @@ -210,8 +196,8 @@ private: // b3Cloth* m_cloth; - // - b3BodyContact m_bodyContact; + // Contact + b3ParticleBodyContact m_bodyContact; // b3Particle* m_prev; diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index 90cd13b..b86c517 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -464,23 +464,14 @@ void b3Cloth::UpdateBodyContacts() b3Vec3 point = bestPoint; b3Vec3 normal = -bestNormal; - b3BodyContact* c = &p->m_bodyContact; + b3ParticleBodyContact* c = &p->m_bodyContact; - b3BodyContact c0 = *c; + b3ParticleBodyContact 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->nActive = true; - c->t1Active = false; - c->t2Active = false; + c->normal1 = normal; c->localPoint1.SetZero(); c->localPoint2 = body->GetLocalPoint(point); c->t1 = b3Perp(normal); @@ -488,97 +479,15 @@ void b3Cloth::UpdateBodyContacts() c->normalImpulse = 0.0f; c->tangentImpulse.SetZero(); -#if 0 - // Apply position correction - p->m_translation += separation * normal; -#endif - - // Update contact state if (c0.active == true) { - if (c0.nActive == 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->nActive = false; - continue; - } -#endif - } + c->normalImpulse = c0.normalImpulse; + c->tangentImpulse = c0.tangentImpulse; } -#if 0 - if (c0.active == false) - { - continue; - } - - // A friction force requires an associated normal force. - if (c0.nActive == false) - { - continue; - } - - b3Vec3 v1; v1.SetZero(); - b3Vec3 v2 = p->m_velocity; - b3Vec3 dv = v2 - v1; - - const float32 kVelTol = 2.0f * 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 } } -void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) +void b3Cloth::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Solve"); @@ -610,7 +519,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) } // Solve - solver.Solve(dt, gravity); + solver.Solve(dt, gravity, velocityIterations, positionIterations); } void b3Cloth::UpdateContacts() @@ -619,7 +528,7 @@ void b3Cloth::UpdateContacts() UpdateBodyContacts(); } -void b3Cloth::Step(float32 dt) +void b3Cloth::Step(float32 dt, u32 velocityIterations, u32 positionIterations) { B3_PROFILE("Cloth Step"); @@ -629,7 +538,7 @@ void b3Cloth::Step(float32 dt) // Solve constraints, integrate state, clear forces and translations. if (dt > 0.0f) { - Solve(dt, m_gravity); + Solve(dt, m_gravity, velocityIterations, positionIterations); } // Clear external applied forces and translations diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp index ac97dfc..46d84ad 100644 --- a/src/bounce/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/cloth_contact_solver.cpp @@ -49,7 +49,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; @@ -79,14 +79,14 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() pc->localCenterA.SetZero(); pc->localCenterB = pc->bodyB->m_sweep.localCenter; - pc->normalA = c->n; + pc->normalA = c->normal1; pc->localPointA = c->localPoint1; pc->localPointB = c->localPoint2; } for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; @@ -116,7 +116,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() xfB.rotation = b3QuatMat33(qB); xfB.position = xB - b3Mul(xfB.rotation, localCenterB); - b3BodyContactWorldPoint wp; + b3ParticleBodyContactWorldPoint wp; wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB); vc->normal = wp.normal; @@ -310,7 +310,7 @@ void b3ClothContactSolver::StoreImpulses() { for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_bodyContacts[i]; + b3ParticleBodyContact* c = m_bodyContacts[i]; b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; c->normalImpulse = vc->normalImpulse; @@ -322,14 +322,14 @@ struct b3ClothSolverBodyContactSolverPoint { void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) { + b3Vec3 nA = pc->normalA; + b3Vec3 cA = b3Mul(xfA, pc->localPointA); b3Vec3 cB = b3Mul(xfB, pc->localPointB); float32 rA = pc->radiusA; float32 rB = pc->radiusB; - b3Vec3 nA = pc->normalA; - b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA; diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 632f04e..665b27a 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -57,7 +57,7 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_bodyContactCapacity = def.bodyContactCapacity; m_bodyContactCount = 0; - m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));; + m_bodyContacts = (b3ParticleBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ParticleBodyContact*));; } b3ClothSolver::~b3ClothSolver() @@ -80,16 +80,11 @@ void b3ClothSolver::Add(b3Force* f) m_forces[m_forceCount++] = f; } -void b3ClothSolver::Add(b3BodyContact* c) +void b3ClothSolver::Add(b3ParticleBodyContact* c) { m_bodyContacts[m_bodyContactCount++] = c; } -void b3ClothSolver::Add(b3ParticleContact* c) -{ - m_particleContacts[m_particleContactCount++] = c; -} - void b3ClothSolver::ApplyForces() { for (u32 i = 0; i < m_forceCount; ++i) @@ -149,6 +144,7 @@ void b3ClothSolver::ApplyConstraints() for (u32 i = 0; i < m_particleCount; ++i) { b3Particle* p = m_particles[i]; + if (p->m_type != e_dynamicParticle) { b3AccelerationConstraint* ac = m_constraints + m_constraintCount; @@ -158,47 +154,6 @@ void b3ClothSolver::ApplyConstraints() ac->z.SetZero(); } } -#if 0 - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3BodyContact* bc = m_bodyContacts[i]; - - if (bc->nActive == false) - { - continue; - } - - b3Particle* p1 = bc->p1; - - B3_ASSERT(p1->m_type == e_dynamicParticle); - - b3AccelerationConstraint* ac = m_constraints + m_constraintCount; - ++m_constraintCount; - ac->i1 = p1->m_solverId; - ac->ndof = 2; - 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) { @@ -206,7 +161,7 @@ void b3ClothSolver::ApplyConstraints() } } -void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) +void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations) { float32 h = dt; float32 inv_h = 1.0f / h; @@ -288,23 +243,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // x b3DenseVec3 x(m_particleCount); SolveMPCG(x, viewA, b, S, z, sx0); -#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]; - b3Particle* p1 = c->p1; - - b3Vec3 cf = f[p1->m_solverId]; - - 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; @@ -319,7 +258,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) p->m_velocity = sv[i]; } } -#if 1 + // Solve constraints b3ClothContactSolverDef contactSolverDef; contactSolverDef.allocator = m_allocator; @@ -338,11 +277,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) contactSolver.WarmStart(); } - // Solve velocity constraints { - const u32 kVelocityIterations = 10; - - for (u32 i = 0; i < kVelocityIterations; ++i) + for (u32 i = 0; i < velocityIterations; ++i) { contactSolver.SolveBodyContactVelocityConstraints(); } @@ -351,18 +287,14 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { contactSolver.StoreImpulses(); } -#endif // Integrate positions sx = sx + h * sv; -#if 1 // Solve position constraints { - const u32 kPositionIterations = 2; - bool positionSolved = false; - for (u32 i = 0; i < kPositionIterations; ++i) + for (u32 i = 0; i < positionIterations; ++i) { bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); @@ -385,7 +317,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 aa0bc67..a0e9fad 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -22,13 +22,13 @@ #include #include -void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) +void b3ParticleBodyContactWorldPoint::Initialize(const b3ParticleBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) { + b3Vec3 nA = c->normal1; + b3Vec3 cA = b3Mul(xfA, c->localPoint1); b3Vec3 cB = b3Mul(xfB, c->localPoint2); - b3Vec3 nA = c->n; - b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA;