diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h index 1af0941..692201b 100644 --- a/include/bounce/dynamics/body.h +++ b/include/bounce/dynamics/body.h @@ -294,6 +294,7 @@ private: friend class b3ContactManager; friend class b3ContactSolver; friend class b3ClothSolver; + friend class b3ClothContactSolver; friend class b3Joint; friend class b3JointManager; diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index eb88630..6f2d6f5 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -154,7 +154,10 @@ private: // Update triangle contacts. void UpdateTriangleContacts(); - + + // Update contacts + void UpdateContacts(); + // Solve void Solve(float32 dt, const b3Vec3& gravity); @@ -170,6 +173,9 @@ private: // Pool of particles b3BlockPool m_particleBlocks; + // Pool of body contacts + b3BlockPool m_bodyContactBlocks; + // Pool of particle contacts b3BlockPool m_particleContactBlocks; @@ -182,6 +188,9 @@ 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/dynamics/cloth/cloth_contact_solver.h b/include/bounce/dynamics/cloth/cloth_contact_solver.h new file mode 100644 index 0000000..7fb20bb --- /dev/null +++ b/include/bounce/dynamics/cloth/cloth_contact_solver.h @@ -0,0 +1,230 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef B3_CLOTH_CONTACT_SOLVER_H +#define B3_CLOTH_CONTACT_SOLVER_H + +#include +#include + +class b3StackAllocator; + +class b3Particle; +class b3Body; + +struct b3DenseVec3; +struct b3DiagMat33; +struct b3SparseSymMat33; + +class b3BodyContact; +class b3ParticleContact; +class b3TriangleContact; + +struct b3ClothSolverBodyContactVelocityConstraint +{ + u32 indexA; + float32 invMassA; + b3Mat33 invIA; + + b3Body* bodyB; + float32 invMassB; + b3Mat33 invIB; + + float32 friction; + + b3Vec3 point; + b3Vec3 rA; + b3Vec3 rB; + + b3Vec3 normal; + float32 normalMass; + float32 normalImpulse; + float32 velocityBias; + + b3Vec3 tangent1; + b3Vec3 tangent2; + b3Mat22 tangentMass; + b3Vec2 tangentImpulse; +}; + +struct b3ClothSolverBodyContactPositionConstraint +{ + u32 indexA; + float32 invMassA; + b3Mat33 invIA; + float32 radiusA; + b3Vec3 localCenterA; + + b3Body* bodyB; + float32 invMassB; + b3Mat33 invIB; + float32 radiusB; + b3Vec3 localCenterB; + + b3Vec3 rA; + b3Vec3 rB; + + b3Vec3 localPointA; + b3Vec3 localPointB; +}; + +struct b3ClothSolverParticleContactVelocityConstraint +{ + u32 indexA; + float32 invMassA; + + u32 indexB; + float32 invMassB; + + float32 friction; + + b3Vec3 point; + + b3Vec3 normal; + float32 normalMass; + float32 normalImpulse; + float32 velocityBias; + + b3Vec3 tangent1; + b3Vec3 tangent2; + b3Mat22 tangentMass; + b3Vec2 tangentImpulse; +}; + +struct b3ClothSolverParticleContactPositionConstraint +{ + u32 indexA; + float32 invMassA; + float32 radiusA; + + u32 indexB; + float32 invMassB; + float32 radiusB; +}; + +struct b3ClothSolverTriangleContactVelocityConstraint +{ + u32 indexA; + float32 invMassA; + + u32 indexB; + float32 invMassB; + u32 indexC; + float32 invMassC; + u32 indexD; + float32 invMassD; + + b3Vec3 JA; + b3Vec3 JB; + b3Vec3 JC; + b3Vec3 JD; + + float32 normalMass; + float32 normalImpulse; +}; + +struct b3ClothSolverTriangleContactPositionConstraint +{ + u32 indexA; + float32 invMassA; + float32 radiusA; + + u32 indexB; + float32 invMassB; + u32 indexC; + float32 invMassC; + u32 indexD; + float32 invMassD; + float32 triangleRadius; + + bool front; +}; + +struct b3ClothContactSolverDef +{ + b3StackAllocator* allocator; + + b3DenseVec3* positions; + b3DenseVec3* velocities; + + u32 bodyContactCount; + b3BodyContact** bodyContacts; + + u32 particleContactCount; + b3ParticleContact** particleContacts; + + u32 triangleContactCount; + b3TriangleContact** triangleContacts; +}; + +inline float32 b3MixFriction(float32 u1, float32 u2) +{ + return b3Sqrt(u1 * u2); +} + +class b3ClothContactSolver +{ +public: + b3ClothContactSolver(const b3ClothContactSolverDef& def); + ~b3ClothContactSolver(); + + void InitializeBodyContactConstraints(); + + void InitializeParticleContactConstraints(); + + void InitializeTriangleContactConstraints(); + + void WarmStart(); + + void SolveBodyContactVelocityConstraints(); + + void SolveParticleContactVelocityConstraints(); + + void SolveTriangleContactVelocityConstraints(); + + void StoreImpulses(); + + bool SolveBodyContactPositionConstraints(); + + bool SolveParticleContactPositionConstraints(); + + bool SolveTriangleContactPositionConstraints(); + +protected: + b3StackAllocator* m_allocator; + + b3DenseVec3* m_positions; + b3DenseVec3* m_velocities; + + u32 m_bodyContactCount; + b3BodyContact** m_bodyContacts; + b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; + b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; + + u32 m_particleContactCount; + b3ParticleContact** m_particleContacts; + b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints; + b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints; + + u32 m_triangleContactCount; + b3TriangleContact** m_triangleContacts; + b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints; + b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 06c2810..ed56b82 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -27,14 +27,15 @@ class b3StackAllocator; class b3Particle; class b3Body; class b3Force; -class b3BodyContact; -class b3ParticleContact; -class b3TriangleContact; struct b3DenseVec3; struct b3DiagMat33; struct b3SparseSymMat33; +class b3BodyContact; +class b3ParticleContact; +class b3TriangleContact; + struct b3ClothSolverDef { b3StackAllocator* stack; @@ -68,126 +69,6 @@ struct b3AccelerationConstraint void Apply(const b3ClothSolverData* data); }; -struct b3ClothSolverBodyContactVelocityConstraint -{ - u32 indexA; - float32 invMassA; - b3Mat33 invIA; - - b3Body* bodyB; - float32 invMassB; - b3Mat33 invIB; - - float32 friction; - - b3Vec3 point; - b3Vec3 rA; - b3Vec3 rB; - - b3Vec3 normal; - float32 normalMass; - float32 normalImpulse; - float32 velocityBias; - - b3Vec3 tangent1; - b3Vec3 tangent2; - b3Mat22 tangentMass; - b3Vec2 tangentImpulse; -}; - -struct b3ClothSolverBodyContactPositionConstraint -{ - u32 indexA; - float32 invMassA; - b3Mat33 invIA; - float32 radiusA; - b3Vec3 localCenterA; - - b3Body* bodyB; - float32 invMassB; - b3Mat33 invIB; - float32 radiusB; - b3Vec3 localCenterB; - - b3Vec3 rA; - b3Vec3 rB; - - b3Vec3 localPointA; - b3Vec3 localPointB; -}; - -struct b3ClothSolverParticleContactVelocityConstraint -{ - u32 indexA; - float32 invMassA; - - u32 indexB; - float32 invMassB; - - float32 friction; - - b3Vec3 point; - - b3Vec3 normal; - float32 normalMass; - float32 normalImpulse; - float32 velocityBias; - - b3Vec3 tangent1; - b3Vec3 tangent2; - b3Mat22 tangentMass; - b3Vec2 tangentImpulse; -}; - -struct b3ClothSolverParticleContactPositionConstraint -{ - u32 indexA; - float32 invMassA; - float32 radiusA; - - u32 indexB; - float32 invMassB; - float32 radiusB; -}; - -struct b3ClothSolverTriangleContactVelocityConstraint -{ - u32 indexA; - float32 invMassA; - - u32 indexB; - float32 invMassB; - u32 indexC; - float32 invMassC; - u32 indexD; - float32 invMassD; - - b3Vec3 JA; - b3Vec3 JB; - b3Vec3 JC; - b3Vec3 JD; - - float32 normalMass; - float32 normalImpulse; -}; - -struct b3ClothSolverTriangleContactPositionConstraint -{ - u32 indexA; - float32 invMassA; - float32 radiusA; - - u32 indexB; - float32 invMassB; - u32 indexC; - float32 invMassC; - u32 indexD; - float32 invMassD; - float32 triangleRadius; - - bool front; -}; - class b3ClothSolver { public: @@ -211,39 +92,6 @@ private: // 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; - // - void InitializeBodyContactConstraints(); - - // - void InitializeParticleContactConstraints(); - - // - void InitializeTriangleContactConstraints(); - - // - void WarmStart(); - - // - void SolveBodyContactVelocityConstraints(); - - // - void SolveParticleContactVelocityConstraints(); - - // - void SolveTriangleContactVelocityConstraints(); - - // - void StoreImpulses(); - - // - bool SolveBodyContactPositionConstraints(); - - // - bool SolveParticleContactPositionConstraints(); - - // - bool SolveTriangleContactPositionConstraints(); - b3StackAllocator* m_allocator; u32 m_particleCapacity; @@ -257,24 +105,18 @@ private: u32 m_constraintCapacity; u32 m_constraintCount; b3AccelerationConstraint* m_constraints; - + u32 m_bodyContactCapacity; u32 m_bodyContactCount; b3BodyContact** m_bodyContacts; - b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; - b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; u32 m_particleContactCapacity; u32 m_particleContactCount; b3ParticleContact** m_particleContacts; - b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints; - b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints; - + u32 m_triangleContactCapacity; u32 m_triangleContactCount; b3TriangleContact** m_triangleContacts; - b3ClothSolverTriangleContactVelocityConstraint* m_triangleVelocityConstraints; - b3ClothSolverTriangleContactPositionConstraint* m_trianglePositionConstraints; b3ClothSolverData m_solverData; }; diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index 5bfdbbe..11b93d0 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -80,8 +80,8 @@ public: b3Vec3 t1, t2; b3Vec2 tangentImpulse; - // - bool active; + b3BodyContact* m_prev; + b3BodyContact* m_next; }; struct b3BodyContactWorldPoint @@ -198,6 +198,7 @@ private: friend class b3List2; friend class b3Cloth; friend class b3ClothSolver; + friend class b3ClothContactSolver; friend class b3Force; friend class b3SpringForce; friend class b3BendForce; @@ -239,9 +240,6 @@ private: // Cloth mesh vertex index. u32 m_vertex; - // Contact - b3BodyContact m_contact; - // Solver temp // Identifier diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index d768ab0..95632c0 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -151,6 +151,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle)), + m_bodyContactBlocks(sizeof(b3BodyContact)), m_particleContactBlocks(sizeof(b3ParticleContact)), m_triangleContactBlocks(sizeof(b3TriangleContact)) { @@ -509,25 +510,30 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 void b3Cloth::UpdateBodyContacts() { B3_PROFILE("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) { - b3BodyContact* c = &p->m_contact; - - b3BodyContact c0 = *c; - - c->active = false; - b3Sphere s1; s1.vertex = p->m_position; s1.radius = p->m_radius; // Find the deepest penetration + b3Shape* bestShape = nullptr; float32 bestSeparation = 0.0f; b3Vec3 bestPoint(0.0f, 0.0f, 0.0f); b3Vec3 bestNormal(0.0f, 0.0f, 0.0f); - b3Shape* bestShape = nullptr; for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext()) { @@ -545,10 +551,10 @@ void b3Cloth::UpdateBodyContacts() { if (output.separation < bestSeparation) { + bestShape = shape; bestSeparation = output.separation; bestPoint = output.point; bestNormal = output.normal; - bestShape = shape; } } } @@ -562,25 +568,21 @@ void b3Cloth::UpdateBodyContacts() // Ensure the the normal points from the particle 1 to shape 2 b3Shape* shape = bestShape; b3Body* body = shape->GetBody(); - float32 s = bestSeparation; - b3Vec3 cb = bestPoint; - b3Vec3 n = -bestNormal; + float32 separation = bestSeparation; + b3Vec3 point = bestPoint; + b3Vec3 normal = -bestNormal; - c->active = true; + b3BodyContact* c = (b3BodyContact*)m_bodyContactBlocks.Allocate(); c->p1 = p; c->s2 = shape; c->localPoint1.SetZero(); - c->localPoint2 = body->GetLocalPoint(cb); - c->t1 = b3Perp(n); - c->t2 = b3Cross(c->t1, n); + c->localPoint2 = body->GetLocalPoint(point); + c->t1 = b3Perp(normal); + c->t2 = b3Cross(c->t1, normal); c->normalImpulse = 0.0f; c->tangentImpulse.SetZero(); - if (c0.active == true) - { - c->normalImpulse = c->normalImpulse; - c->tangentImpulse = c->tangentImpulse; - } + m_bodyContactList.PushFront(c); } } @@ -888,7 +890,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) solverDef.stack = &m_world->m_stackAllocator; solverDef.particleCapacity = m_particleList.m_count; solverDef.forceCapacity = m_forceList.m_count; - solverDef.bodyContactCapacity = m_particleList.m_count; + solverDef.bodyContactCapacity = m_bodyContactList.m_count; solverDef.particleContactCapacity = m_particleContactList.m_count; solverDef.triangleContactCapacity = m_triangleContactList.m_count; @@ -903,25 +905,20 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) { solver.Add(f); } - - for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) + + for (b3BodyContact* c = m_bodyContactList.m_head; c; c = c->m_next) { - b3BodyContact* bc = &p->m_contact; - - if (bc->active) - { - solver.Add(bc); - } + solver.Add(c); } - - for (b3ParticleContact* pc = m_particleContactList.m_head; pc; pc = pc->m_next) + + for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next) { - solver.Add(pc); + solver.Add(c); } - - for (b3TriangleContact* tc = m_triangleContactList.m_head; tc; tc = tc->m_next) + + for (b3TriangleContact* c = m_triangleContactList.m_head; c; c = c->m_next) { - solver.Add(tc); + solver.Add(c); } // Solve @@ -935,10 +932,8 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) } } -void b3Cloth::Step(float32 dt, const b3Vec3& gravity) +void b3Cloth::UpdateContacts() { - B3_PROFILE("Step"); - // Update body contacts UpdateBodyContacts(); @@ -947,7 +942,15 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity) // Update triangle contacts UpdateTriangleContacts(); - +} + +void b3Cloth::Step(float32 dt, const b3Vec3& gravity) +{ + B3_PROFILE("Step"); + + // Update contacts + UpdateContacts(); + // Solve constraints, integrate state, clear forces and translations. if (dt > 0.0f) { @@ -973,25 +976,6 @@ void b3Cloth::Draw() const { b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green); } - - b3BodyContact* c = &p->m_contact; - - if (c->active) - { - b3Particle* pA = c->p1; - b3Body* bB = c->s2->GetBody(); - - b3Transform xfA; - xfA.rotation.SetIdentity(); - xfA.position = pA->m_position; - - b3Transform xfB = bB->GetTransform(); - - b3BodyContactWorldPoint cp; - cp.Initialize(c, pA->m_radius, xfA, c->s2->m_radius, xfB); - - b3Draw_draw->DrawSegment(cp.point, cp.point + cp.normal, b3Color_yellow); - } } for (b3Force* f = m_forceList.m_head; f; f = f->m_next) diff --git a/src/bounce/dynamics/cloth/cloth_contact_solver.cpp b/src/bounce/dynamics/cloth/cloth_contact_solver.cpp new file mode 100644 index 0000000..cf05921 --- /dev/null +++ b/src/bounce/dynamics/cloth/cloth_contact_solver.cpp @@ -0,0 +1,982 @@ +/* +* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net +* +* This software is provided 'as-is', without any express or implied +* warranty. In no event will the authors be held liable for any damages +* arising from the use of this software. +* Permission is granted to anyone to use this software for any purpose, +* including commercial applications, and to alter it and redistribute it +* freely, subject to the following restrictions: +* 1. The origin of this software must not be misrepresented; you must not +* claim that you wrote the original software. If you use this software +* in a product, an acknowledgment in the product documentation would be +* appreciated but is not required. +* 2. Altered source versions must be plainly marked as such, and must not be +* misrepresented as being the original software. +* 3. This notice may not be removed or altered from any source distribution. +*/ + +#include +#include +#include +#include +#include +#include + +b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def) +{ + m_allocator = def.allocator; + + m_positions = def.positions; + m_velocities = def.velocities; + + m_bodyContactCount = def.bodyContactCount; + m_bodyContacts = def.bodyContacts; + m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactVelocityConstraint)); + m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3ClothSolverBodyContactPositionConstraint)); + + m_particleContactCount = def.particleContactCount; + m_particleContacts = def.particleContacts; + m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactVelocityConstraint)); + m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCount * sizeof(b3ClothSolverParticleContactPositionConstraint)); + + m_triangleContactCount = def.triangleContactCount; + m_triangleContacts = def.triangleContacts; + m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactVelocityConstraint)); + m_trianglePositionConstraints = (b3ClothSolverTriangleContactPositionConstraint*)m_allocator->Allocate(m_triangleContactCount * sizeof(b3ClothSolverTriangleContactPositionConstraint)); +} + +b3ClothContactSolver::~b3ClothContactSolver() +{ + m_allocator->Free(m_trianglePositionConstraints); + m_allocator->Free(m_triangleVelocityConstraints); + + m_allocator->Free(m_particlePositionConstraints); + m_allocator->Free(m_particleVelocityConstraints); + + m_allocator->Free(m_bodyPositionConstraints); + m_allocator->Free(m_bodyVelocityConstraints); +} + +void b3ClothContactSolver::InitializeBodyContactConstraints() +{ + b3DenseVec3& x = *m_positions; + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; + + vc->indexA = c->p1->m_solverId; + vc->bodyB = c->s2->GetBody(); + + vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + vc->invMassB = vc->bodyB->GetInverseMass(); + + vc->invIA.SetZero(); + vc->invIB = vc->bodyB->GetWorldInverseInertia(); + + vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction()); + + pc->indexA = c->p1->m_solverId; + pc->bodyB = vc->bodyB; + + pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + pc->invMassB = vc->bodyB->m_invMass; + + pc->invIA.SetZero(); + pc->invIB = vc->bodyB->m_worldInvI; + + pc->radiusA = c->p1->m_radius; + pc->radiusB = c->s2->m_radius; + + pc->localCenterA.SetZero(); + pc->localCenterB = pc->bodyB->m_sweep.localCenter; + + pc->localPointA = c->localPoint1; + pc->localPointB = c->localPoint2; + } + + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; + + u32 indexA = vc->indexA; + b3Body* bodyB = vc->bodyB; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Mat33 iA = vc->invIA; + b3Mat33 iB = vc->invIB; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = bodyB->m_sweep.worldCenter; + + b3Quat qA; qA.SetIdentity(); + b3Quat qB = bodyB->m_sweep.orientation; + + b3Vec3 localCenterA = pc->localCenterA; + b3Vec3 localCenterB = pc->localCenterB; + + b3Transform xfA; + xfA.rotation = b3QuatMat33(qA); + xfA.position = xA - b3Mul(xfA.rotation, localCenterA); + + b3Transform xfB; + xfB.rotation = b3QuatMat33(qB); + xfB.position = xB - b3Mul(xfB.rotation, localCenterB); + + b3BodyContactWorldPoint wp; + wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB); + + vc->normal = wp.normal; + vc->tangent1 = c->t1; + vc->tangent2 = c->t2; + vc->point = wp.point; + + b3Vec3 point = vc->point; + + b3Vec3 rA = point - xA; + b3Vec3 rB = point - xB; + + vc->rA = rA; + vc->rB = rB; + + vc->normalImpulse = c->normalImpulse; + vc->tangentImpulse = c->tangentImpulse; + + { + b3Vec3 n = vc->normal; + + b3Vec3 rnA = b3Cross(rA, n); + b3Vec3 rnB = b3Cross(rB, n); + + float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB); + + vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; + vc->velocityBias = 0.0f; + } + + { + b3Vec3 t1 = vc->tangent1; + b3Vec3 t2 = vc->tangent2; + + b3Vec3 rn1A = b3Cross(rA, t1); + b3Vec3 rn1B = b3Cross(rB, t1); + b3Vec3 rn2A = b3Cross(rA, t2); + b3Vec3 rn2B = b3Cross(rB, t2); + + // dot(t1, t2) = 0 + // J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0 + float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B); + float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B); + float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B); + + b3Mat22 K; + K.x.Set(k11, k12); + K.y.Set(k12, k22); + + vc->tangentMass = b3Inverse(K); + } + } +} + +void b3ClothContactSolver::InitializeParticleContactConstraints() +{ + b3DenseVec3& x = *m_positions; + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ParticleContact* c = m_particleContacts[i]; + b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; + b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; + + vc->indexA = c->p1->m_solverId; + vc->indexB = c->p2->m_solverId; + + vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + + vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction); + + pc->indexA = c->p1->m_solverId; + pc->indexB = c->p2->m_solverId; + + pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + + pc->radiusA = c->p1->m_radius; + pc->radiusB = c->p2->m_radius; + } + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ParticleContact* c = m_particleContacts[i]; + b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; + b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; + + u32 indexA = vc->indexA; + u32 indexB = vc->indexB; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = x[indexB]; + + b3ParticleContactWorldPoint wp; + wp.Initialize(c); + + vc->normal = wp.normal; + vc->tangent1 = c->t1; + vc->tangent2 = c->t2; + vc->point = wp.point; + + b3Vec3 point = vc->point; + + vc->normalImpulse = c->normalImpulse; + vc->tangentImpulse = c->tangentImpulse; + + { + b3Vec3 n = vc->normal; + + float32 K = mA + mB; + + vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; + + vc->velocityBias = 0.0f; + } + + { + b3Vec3 t1 = vc->tangent1; + b3Vec3 t2 = vc->tangent2; + + float32 k11 = mA + mB; + float32 k12 = 0.0f; + float32 k22 = mA + mB; + + b3Mat22 K; + K.x.Set(k11, k12); + K.y.Set(k12, k22); + + vc->tangentMass = b3Inverse(K); + } + } +} + +void b3ClothContactSolver::InitializeTriangleContactConstraints() +{ + b3DenseVec3& x = *m_positions; + + for (u32 i = 0; i < m_triangleContactCount; ++i) + { + b3TriangleContact* c = m_triangleContacts[i]; + b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; + b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i; + + vc->indexA = c->p1->m_solverId; + vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + + vc->indexB = c->p2->m_solverId; + vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + + vc->indexC = c->p3->m_solverId; + vc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; + + vc->indexD = c->p4->m_solverId; + vc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; + + pc->indexA = c->p1->m_solverId; + pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; + pc->radiusA = c->p1->m_radius; + + pc->indexB = c->p2->m_solverId; + pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; + + pc->indexC = c->p3->m_solverId; + pc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; + + pc->indexD = c->p4->m_solverId; + pc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; + + pc->triangleRadius = 0.0f; + pc->front = c->front; + + u32 indexA = pc->indexA; + float32 mA = pc->invMassA; + + u32 indexB = pc->indexB; + float32 mB = pc->invMassB; + + u32 indexC = pc->indexC; + float32 mC = pc->invMassC; + + u32 indexD = pc->indexD; + float32 mD = pc->invMassD; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = x[indexB]; + b3Vec3 xC = x[indexC]; + b3Vec3 xD = x[indexD]; + + b3Vec3 n = b3Cross(xC - xB, xD - xB); + + if (pc->front == false) + { + n = -n; + } + + float32 n_len = n.Normalize(); + + b3Mat33 I; I.SetIdentity(); + + b3Mat33 N = I - b3Outer(n, n); + if (n_len > B3_EPSILON) + { + N = (1.0f / n_len) * N; + } + + b3Vec3 N_n = N * n; + + vc->JA = n; + vc->JC = b3Cross(xD - xB, N_n); + vc->JD = b3Cross(xC - xB, N_n); + vc->JB = b3Cross(xC - xD, N_n) - n; + + // Compute effective mass. + float32 K = mA + mB + mC + mD; + + vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; + vc->normalImpulse = c->normalImpulse; + } +} + +void b3ClothContactSolver::WarmStart() +{ + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + + u32 indexA = vc->indexA; + b3Body* bodyB = vc->bodyB; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = bodyB->GetLinearVelocity(); + + b3Vec3 wA; wA.SetZero(); + b3Vec3 wB = bodyB->GetAngularVelocity(); + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Mat33 iA = vc->invIA; + b3Mat33 iB = vc->invIB; + + b3Vec3 P = vc->normalImpulse * vc->normal; + + vA -= mA * P; + wA -= iA * b3Cross(vc->rA, P); + + vB += mB * P; + wB += iB * b3Cross(vc->rB, P); + + b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1; + b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2; + + vA -= mA * (P1 + P2); + wA -= iA * b3Cross(vc->rA, P1 + P2); + + vB += mB * (P1 + P2); + wB += iB * b3Cross(vc->rB, P1 + P2); + + v[indexA] = vA; + + bodyB->SetLinearVelocity(vB); + bodyB->SetAngularVelocity(wB); + } + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; + + u32 indexA = vc->indexA; + u32 indexB = vc->indexB; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = v[indexB]; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Vec3 P = vc->normalImpulse * vc->normal; + + vA -= mA * P; + vB += mB * P; + + b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1; + b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2; + + vA -= mA * (P1 + P2); + vB += mB * (P1 + P2); + + v[indexA] = vA; + v[indexB] = vB; + } + + for (u32 i = 0; i < m_triangleContactCount; ++i) + { + b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; + + u32 indexA = vc->indexA; + u32 indexB = vc->indexB; + u32 indexC = vc->indexC; + u32 indexD = vc->indexD; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = v[indexB]; + b3Vec3 vC = v[indexC]; + b3Vec3 vD = v[indexD]; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + float32 mC = vc->invMassC; + float32 mD = vc->invMassD; + + b3Vec3 PA = vc->normalImpulse * vc->JA; + b3Vec3 PB = vc->normalImpulse * vc->JB; + b3Vec3 PC = vc->normalImpulse * vc->JC; + b3Vec3 PD = vc->normalImpulse * vc->JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + + v[indexA] = vA; + v[indexB] = vB; + v[indexC] = vC; + v[indexD] = vD; + } +} + +void b3ClothContactSolver::SolveBodyContactVelocityConstraints() +{ + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + + u32 indexA = vc->indexA; + b3Body* bodyB = vc->bodyB; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = bodyB->GetLinearVelocity(); + + b3Vec3 wA; wA.SetZero(); + b3Vec3 wB = bodyB->GetAngularVelocity(); + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Mat33 iA = vc->invIA; + b3Mat33 iB = vc->invIB; + + b3Vec3 normal = vc->normal; + b3Vec3 point = vc->point; + + b3Vec3 rA = vc->rA; + b3Vec3 rB = vc->rB; + + // Solve normal constraint. + { + b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA); + float32 Cdot = b3Dot(normal, dv); + + float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias); + + float32 oldImpulse = vc->normalImpulse; + vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); + impulse = vc->normalImpulse - oldImpulse; + + b3Vec3 P = impulse * normal; + + vA -= mA * P; + wA -= iA * b3Cross(rA, P); + + vB += mB * P; + wB += iB * b3Cross(rB, P); + } + + // Solve tangent constraints. + { + b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA); + + b3Vec2 Cdot; + Cdot.x = b3Dot(dv, vc->tangent1); + Cdot.y = b3Dot(dv, vc->tangent2); + + b3Vec2 impulse = vc->tangentMass * -Cdot; + b3Vec2 oldImpulse = vc->tangentImpulse; + vc->tangentImpulse += impulse; + + float32 maxImpulse = vc->friction * vc->normalImpulse; + if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse) + { + vc->tangentImpulse.Normalize(); + vc->tangentImpulse *= maxImpulse; + } + + impulse = vc->tangentImpulse - oldImpulse; + + b3Vec3 P1 = impulse.x * vc->tangent1; + b3Vec3 P2 = impulse.y * vc->tangent2; + b3Vec3 P = P1 + P2; + + vA -= mA * P; + wA -= iA * b3Cross(rA, P); + + vB += mB * P; + wB += iB * b3Cross(rB, P); + } + + v[indexA] = vA; + + bodyB->SetLinearVelocity(vB); + bodyB->SetAngularVelocity(wB); + } +} + +void b3ClothContactSolver::SolveParticleContactVelocityConstraints() +{ + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; + + u32 indexA = vc->indexA; + u32 indexB = vc->indexB; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = v[indexB]; + + float32 mA = vc->invMassA; + float32 mB = vc->invMassB; + + b3Vec3 normal = vc->normal; + b3Vec3 point = vc->point; + + // Solve normal constraint. + { + b3Vec3 dv = vB - vA; + float32 Cdot = b3Dot(normal, dv); + + float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias); + + float32 oldImpulse = vc->normalImpulse; + vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); + impulse = vc->normalImpulse - oldImpulse; + + b3Vec3 P = impulse * normal; + + vA -= mA * P; + vB += mB * P; + } + + // Solve tangent constraints. + { + b3Vec3 dv = vB - vA; + + b3Vec2 Cdot; + Cdot.x = b3Dot(dv, vc->tangent1); + Cdot.y = b3Dot(dv, vc->tangent2); + + b3Vec2 impulse = vc->tangentMass * -Cdot; + b3Vec2 oldImpulse = vc->tangentImpulse; + vc->tangentImpulse += impulse; + + float32 maxImpulse = vc->friction * vc->normalImpulse; + if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse) + { + vc->tangentImpulse.Normalize(); + vc->tangentImpulse *= maxImpulse; + } + + impulse = vc->tangentImpulse - oldImpulse; + + b3Vec3 P1 = impulse.x * vc->tangent1; + b3Vec3 P2 = impulse.y * vc->tangent2; + b3Vec3 P = P1 + P2; + + vA -= mA * P; + vB += mB * P; + } + + v[indexA] = vA; + v[indexB] = vB; + } +} + +void b3ClothContactSolver::SolveTriangleContactVelocityConstraints() +{ + b3DenseVec3& v = *m_velocities; + + for (u32 i = 0; i < m_triangleContactCount; ++i) + { + b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; + + u32 indexA = vc->indexA; + float32 mA = vc->invMassA; + + u32 indexB = vc->indexB; + float32 mB = vc->invMassB; + + u32 indexC = vc->indexC; + float32 mC = vc->invMassC; + + u32 indexD = vc->indexD; + float32 mD = vc->invMassD; + + b3Vec3 vA = v[indexA]; + b3Vec3 vB = v[indexB]; + b3Vec3 vC = v[indexC]; + b3Vec3 vD = v[indexD]; + + b3Vec3 n = vc->JA; + + // Allow some slop and prevent large corrections. + float32 Cdot = b3Dot(n, vA - vB); + + float32 impulse = -vc->normalMass * Cdot; + + float32 oldImpulse = vc->normalImpulse; + vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); + impulse = vc->normalImpulse - oldImpulse; + + b3Vec3 PA = impulse * vc->JA; + b3Vec3 PB = impulse * vc->JB; + b3Vec3 PC = impulse * vc->JC; + b3Vec3 PD = impulse * vc->JD; + + vA += mA * PA; + vB += mB * PB; + vC += mC * PC; + vD += mD * PD; + + v[indexA] = vA; + v[indexB] = vB; + v[indexC] = vC; + v[indexD] = vD; + } +} + +void b3ClothContactSolver::StoreImpulses() +{ + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + + c->normalImpulse = vc->normalImpulse; + c->tangentImpulse = vc->tangentImpulse; + } + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ParticleContact* c = m_particleContacts[i]; + b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; + + c->normalImpulse = vc->normalImpulse; + c->tangentImpulse = vc->tangentImpulse; + } + + for (u32 i = 0; i < m_triangleContactCount; ++i) + { + b3TriangleContact* c = m_triangleContacts[i]; + b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; + + c->normalImpulse = vc->normalImpulse; + } +} + +struct b3ClothSolverBodyContactSolverPoint +{ + void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) + { + b3Vec3 cA = b3Mul(xfA, pc->localPointA); + b3Vec3 cB = b3Mul(xfB, pc->localPointB); + + float32 rA = pc->radiusA; + float32 rB = pc->radiusB; + + b3Vec3 d = cB - cA; + float32 distance = b3Length(d); + + b3Vec3 nA(0.0f, 1.0f, 0.0f); + if (distance > B3_EPSILON) + { + nA = d / distance; + } + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = distance - rA - rB; + } + + b3Vec3 normal; + b3Vec3 point; + float32 separation; +}; + +bool b3ClothContactSolver::SolveBodyContactPositionConstraints() +{ + b3DenseVec3& x = *m_positions; + + float32 minSeparation = 0.0f; + + for (u32 i = 0; i < m_bodyContactCount; ++i) + { + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; + + u32 indexA = pc->indexA; + float32 mA = pc->invMassA; + b3Mat33 iA = pc->invIA; + b3Vec3 localCenterA = pc->localCenterA; + + b3Body* bodyB = pc->bodyB; + float32 mB = pc->invMassB; + b3Mat33 iB = pc->invIB; + b3Vec3 localCenterB = pc->localCenterB; + + b3Vec3 cA = x[indexA]; + b3Quat qA; qA.SetIdentity(); + + b3Vec3 cB = bodyB->m_sweep.worldCenter; + b3Quat qB = bodyB->m_sweep.orientation; + + // Solve normal constraint + b3Transform xfA; + xfA.rotation = b3QuatMat33(qA); + xfA.position = cA - b3Mul(xfA.rotation, localCenterA); + + b3Transform xfB; + xfB.rotation = b3QuatMat33(qB); + xfB.position = cB - b3Mul(xfB.rotation, localCenterB); + + b3ClothSolverBodyContactSolverPoint cpcp; + cpcp.Initialize(pc, xfA, xfB); + + b3Vec3 normal = cpcp.normal; + b3Vec3 point = cpcp.point; + float32 separation = cpcp.separation; + + // Update max constraint error. + minSeparation = b3Min(minSeparation, separation); + + // Allow some slop and prevent large corrections. + float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); + + // Compute effective mass. + b3Vec3 rA = point - cA; + b3Vec3 rB = point - cB; + + b3Vec3 rnA = b3Cross(rA, normal); + b3Vec3 rnB = b3Cross(rB, normal); + float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB); + + // Compute normal impulse. + float32 impulse = K > 0.0f ? -C / K : 0.0f; + b3Vec3 P = impulse * normal; + + cA -= mA * P; + qA -= b3Derivative(qA, iA * b3Cross(rA, P)); + qA.Normalize(); + + cB += mB * P; + qB += b3Derivative(qB, iB * b3Cross(rB, P)); + qB.Normalize(); + + x[indexA] = cA; + + bodyB->m_sweep.worldCenter = cB; + bodyB->m_sweep.orientation = qB; + } + + return minSeparation >= -3.0f * B3_LINEAR_SLOP; +} + +struct b3ClothSolverParticleContactSolverPoint +{ + void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB) + { + b3Vec3 d = cB - cA; + float32 distance = b3Length(d); + + b3Vec3 nA(0.0f, 1.0f, 0.0f); + if (distance > B3_EPSILON) + { + nA = d / distance; + } + + b3Vec3 pA = cA + rA * nA; + b3Vec3 pB = cB - rB * nA; + + point = 0.5f * (pA + pB); + normal = nA; + separation = distance - rA - rB; + } + + b3Vec3 point; + b3Vec3 normal; + float32 separation; +}; + +bool b3ClothContactSolver::SolveParticleContactPositionConstraints() +{ + b3DenseVec3& x = *m_positions; + + float32 minSeparation = 0.0f; + + for (u32 i = 0; i < m_particleContactCount; ++i) + { + b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; + + u32 indexA = pc->indexA; + float32 mA = pc->invMassA; + float32 rA = pc->radiusA; + + u32 indexB = pc->indexB; + float32 mB = pc->invMassB; + float32 rB = pc->radiusB; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = x[indexB]; + + b3ClothSolverParticleContactSolverPoint cpcp; + cpcp.Initialize(xA, rA, xB, rB); + + b3Vec3 normal = cpcp.normal; + b3Vec3 point = cpcp.point; + float32 separation = cpcp.separation; + + // Update max constraint error. + minSeparation = b3Min(minSeparation, separation); + + // Allow some slop and prevent large corrections. + float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); + + // Compute effective mass. + float32 K = mA + mB; + + // Compute normal impulse. + float32 impulse = K > 0.0f ? -C / K : 0.0f; + b3Vec3 P = impulse * normal; + + xA -= mA * P; + xB += mB * P; + + x[indexA] = xA; + x[indexB] = xB; + } + + return minSeparation >= -3.0f * B3_LINEAR_SLOP; +} + +bool b3ClothContactSolver::SolveTriangleContactPositionConstraints() +{ + b3DenseVec3& x = *m_positions; + + float32 minSeparation = 0.0f; + + for (u32 i = 0; i < m_triangleContactCount; ++i) + { + b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i; + + u32 indexA = pc->indexA; + float32 mA = pc->invMassA; + float32 radiusA = pc->radiusA; + + u32 indexB = pc->indexB; + float32 mB = pc->invMassB; + + u32 indexC = pc->indexC; + float32 mC = pc->invMassC; + + u32 indexD = pc->indexD; + float32 mD = pc->invMassD; + + float32 triangleRadius = pc->triangleRadius; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = x[indexB]; + b3Vec3 xC = x[indexC]; + b3Vec3 xD = x[indexD]; + + b3Vec3 n = b3Cross(xC - xB, xD - xB); + if (pc->front == false) + { + n = -n; + } + + float32 n_len = n.Normalize(); + + float32 distance = b3Dot(n, xA - xB); + float32 separation = distance - radiusA - triangleRadius; + + // Update max constraint error. + minSeparation = b3Min(minSeparation, separation); + + // Allow some slop and prevent large corrections. + float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); + + b3Mat33 I; I.SetIdentity(); + + b3Mat33 N = I - b3Outer(n, n); + if (n_len > B3_EPSILON) + { + N = (1.0f / n_len) * N; + } + + b3Vec3 N_n = N * n; + + b3Vec3 JA = n; + b3Vec3 JC = b3Cross(xD - xB, N_n); + b3Vec3 JD = b3Cross(xC - xB, N_n); + b3Vec3 JB = b3Cross(xC - xD, N_n) - n; + + // Compute effective mass. + float32 K = mA + mB + mC + mD; + + // Compute normal impulse. + float32 impulse = K > 0.0f ? -C / K : 0.0f; + + b3Vec3 PA = impulse * JA; + b3Vec3 PB = impulse * JB; + b3Vec3 PC = impulse * JC; + b3Vec3 PD = impulse * JD; + + xA += mA * PA; + xB += mB * PB; + xC += mC * PC; + xD += mD * PD; + + x[indexA] = xA; + x[indexB] = xB; + x[indexC] = xC; + x[indexD] = xD; + } + + return minSeparation >= -3.0f * B3_LINEAR_SLOP; +} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 891b1f4..2fdedb6 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -55,35 +56,21 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_bodyContactCapacity = def.bodyContactCapacity; m_bodyContactCount = 0; - m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*)); - m_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverBodyContactVelocityConstraint)); - m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverBodyContactPositionConstraint)); + m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*));; m_particleContactCapacity = def.particleContactCapacity; m_particleContactCount = 0; - m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*)); - m_particleVelocityConstraints = (b3ClothSolverParticleContactVelocityConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactVelocityConstraint)); - m_particlePositionConstraints = (b3ClothSolverParticleContactPositionConstraint*)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ClothSolverParticleContactPositionConstraint)); + m_particleContacts = (b3ParticleContact**)m_allocator->Allocate(m_particleContactCapacity * sizeof(b3ParticleContact*));; m_triangleContactCapacity = def.triangleContactCapacity; m_triangleContactCount = 0; - m_triangleContacts = (b3TriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3TriangleContact*)); - m_triangleVelocityConstraints = (b3ClothSolverTriangleContactVelocityConstraint*)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ClothSolverTriangleContactVelocityConstraint)); - m_trianglePositionConstraints = (b3ClothSolverTriangleContactPositionConstraint*)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3ClothSolverTriangleContactPositionConstraint)); + m_triangleContacts = (b3TriangleContact**)m_allocator->Allocate(m_triangleContactCapacity * sizeof(b3TriangleContact*));; } b3ClothSolver::~b3ClothSolver() { - m_allocator->Free(m_trianglePositionConstraints); - m_allocator->Free(m_triangleVelocityConstraints); m_allocator->Free(m_triangleContacts); - - m_allocator->Free(m_particlePositionConstraints); - m_allocator->Free(m_particleVelocityConstraints); m_allocator->Free(m_particleContacts); - - m_allocator->Free(m_bodyPositionConstraints); - m_allocator->Free(m_bodyVelocityConstraints); m_allocator->Free(m_bodyContacts); m_allocator->Free(m_constraints); @@ -193,29 +180,27 @@ void b3ClothSolver::ApplyConstraints() void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { + float32 h = dt; + float32 inv_h = 1.0f / h; + b3DenseVec3 sx(m_particleCount); b3DenseVec3 sv(m_particleCount); b3DenseVec3 sf(m_particleCount); - b3DenseVec3 sy(m_particleCount); - b3DenseVec3 sx0(m_particleCount); - b3SparseSymMat33 dfdx(m_particleCount); - b3SparseSymMat33 dfdv(m_particleCount); - b3DiagMat33 S(m_particleCount); - b3DenseVec3 z(m_particleCount); + m_solverData.dt = h; + m_solverData.invdt = inv_h; m_solverData.x = &sx; m_solverData.v = &sv; m_solverData.f = &sf; - 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; + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; - float32 h = dt; - float32 inv_h = m_solverData.invdt; + sx[i] = p->m_position; + sv[i] = p->m_velocity; + sf[i] = p->m_force; + } for (u32 i = 0; i < m_particleCount; ++i) { @@ -230,91 +215,126 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { sf[i] += p->m_mass * gravity; } - - sy[i] = p->m_translation; - sx0[i] = p->m_x; } // Integrate velocities - - // Apply internal forces - ApplyForces(); - - // Apply constraints - ApplyConstraints(); - - // Solve Ax = b, where - // A = M - h * dfdv - h * h * dfdx - // b = h * (f0 + h * dfdx * v0 + dfdx * y) - - // A - b3SparseSymMat33 A(m_particleCount); - for (u32 i = 0; i < m_particleCount; ++i) { - A(i, i) = b3Diagonal(m_particles[i]->m_mass); + b3SparseSymMat33 dfdx(m_particleCount); + b3SparseSymMat33 dfdv(m_particleCount); + b3DiagMat33 S(m_particleCount); + b3DenseVec3 z(m_particleCount); + b3DenseVec3 sy(m_particleCount); + b3DenseVec3 sx0(m_particleCount); + + for (u32 i = 0; i < m_particleCount; ++i) + { + b3Particle* p = m_particles[i]; + + sy[i] = p->m_translation; + sx0[i] = p->m_x; + } + + m_solverData.y = &sy; + m_solverData.dfdx = &dfdx; + m_solverData.dfdv = &dfdv; + m_solverData.S = &S; + m_solverData.z = &z; + + // Apply internal forces + ApplyForces(); + + // Apply constraints + ApplyConstraints(); + + // Solve Ax = b, where + // A = M - h * dfdv - h * h * dfdx + // b = h * (f0 + h * dfdx * v0 + dfdx * y) + + // A + b3SparseSymMat33 A(m_particleCount); + for (u32 i = 0; i < m_particleCount; ++i) + { + A(i, i) = b3Diagonal(m_particles[i]->m_mass); + } + A += -h * dfdv - h * h * dfdx; + + // 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; + + sv = sv + x; + sx = sx + sy; + + // Store delta velocities to improve convergence + for (u32 i = 0; i < m_particleCount; ++i) + { + m_particles[i]->m_x = x[i]; + } } - A += -h * dfdv - h * h * dfdx; - // b - b3DenseVec3 b = h * (sf + h * (dfdx * sv) + dfdx * sy); + // Solve contact constraints + b3ClothContactSolverDef contactSolverDef; + contactSolverDef.allocator = m_allocator; + contactSolverDef.positions = m_solverData.x; + contactSolverDef.velocities = m_solverData.v; + contactSolverDef.bodyContactCount = m_bodyContactCount; + contactSolverDef.bodyContacts = m_bodyContacts; + contactSolverDef.particleContactCount = m_particleContactCount; + contactSolverDef.particleContacts = m_particleContacts; + contactSolverDef.triangleContactCount = m_triangleContactCount; + contactSolverDef.triangleContacts = m_triangleContacts; - // x - b3DenseVec3 x(m_particleCount); - u32 iterations = 0; + b3ClothContactSolver contactSolver(contactSolverDef); - Solve(x, iterations, A, b, S, z, sx0); - b3_clothSolverIterations = iterations; + { + contactSolver.InitializeBodyContactConstraints(); + contactSolver.InitializeParticleContactConstraints(); + contactSolver.InitializeTriangleContactConstraints(); + } + + { + contactSolver.WarmStart(); + } + + { + const u32 kVelocityIterations = 5; + + for (u32 i = 0; i < kVelocityIterations; ++i) + { + contactSolver.SolveBodyContactVelocityConstraints(); + contactSolver.SolveParticleContactVelocityConstraints(); + contactSolver.SolveTriangleContactVelocityConstraints(); + } + } - sv = sv + x; - sx = sx + sy; - - // Store delta velocities to improve convergence - for (u32 i = 0; i < m_particleCount; ++i) { - m_particles[i]->m_x = x[i]; + contactSolver.StoreImpulses(); } - // Initialize body contact constraints - InitializeBodyContactConstraints(); - - // Initialize particle contact constraints - InitializeParticleContactConstraints(); - - // Initialize triangle contact constraints - InitializeTriangleContactConstraints(); - - // Warm start velocity constraints - WarmStart(); - - // Solve velocity constraints - const u32 kVelocityIterations = 5; - - for (u32 i = 0; i < kVelocityIterations; ++i) - { - SolveBodyContactVelocityConstraints(); - SolveParticleContactVelocityConstraints(); - SolveTriangleContactVelocityConstraints(); - } - - // Store impulses to improve convergence - StoreImpulses(); - // Integrate positions sx = sx + h * sv; - // Solve position constraints - const u32 kPositionIterations = 2; - - bool positionSolved = false; - for (u32 i = 0; i < kPositionIterations; ++i) { - bool bodyContactsSolved = SolveBodyContactPositionConstraints(); - bool particleContactsSolved = SolveParticleContactPositionConstraints(); - bool triangleContactsSolved = SolveTriangleContactPositionConstraints(); - if (bodyContactsSolved && particleContactsSolved && triangleContactsSolved) + const u32 kPositionIterations = 2; + + bool positionSolved = false; + for (u32 i = 0; i < kPositionIterations; ++i) { - positionSolved = true; - break; + bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); + bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints(); + bool triangleContactsSolved = contactSolver.SolveTriangleContactPositionConstraints(); + + if (bodyContactsSolved && particleContactsSolved && triangleContactsSolved) + { + positionSolved = true; + break; + } } } @@ -447,934 +467,3 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, iterations = iteration; } - -// -static B3_FORCE_INLINE float32 b3MixFriction(float32 u1, float32 u2) -{ - return b3Sqrt(u1 * u2); -} - -void b3ClothSolver::InitializeBodyContactConstraints() -{ - b3DenseVec3& x = *m_solverData.x; - b3DenseVec3& v = *m_solverData.v; - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - - vc->indexA = c->p1->m_solverId; - vc->bodyB = c->s2->GetBody(); - - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - vc->invMassB = vc->bodyB->GetInverseMass(); - - vc->invIA.SetZero(); - vc->invIB = vc->bodyB->GetWorldInverseInertia(); - - vc->friction = b3MixFriction(c->p1->m_friction, c->s2->GetFriction()); - - pc->indexA = c->p1->m_solverId; - pc->bodyB = vc->bodyB; - - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - pc->invMassB = vc->bodyB->m_invMass; - - pc->invIA.SetZero(); - pc->invIB = vc->bodyB->m_worldInvI; - - pc->radiusA = c->p1->m_radius; - pc->radiusB = c->s2->m_radius; - - pc->localCenterA.SetZero(); - pc->localCenterB = pc->bodyB->m_sweep.localCenter; - - pc->localPointA = c->localPoint1; - pc->localPointB = c->localPoint2; - } - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - - u32 indexA = vc->indexA; - b3Body* bodyB = vc->bodyB; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Mat33 iA = vc->invIA; - b3Mat33 iB = vc->invIB; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = bodyB->m_sweep.worldCenter; - - b3Quat qA; qA.SetIdentity(); - b3Quat qB = bodyB->m_sweep.orientation; - - b3Vec3 localCenterA = pc->localCenterA; - b3Vec3 localCenterB = pc->localCenterB; - - b3Transform xfA; - xfA.rotation = b3QuatMat33(qA); - xfA.position = xA - b3Mul(xfA.rotation, localCenterA); - - b3Transform xfB; - xfB.rotation = b3QuatMat33(qB); - xfB.position = xB - b3Mul(xfB.rotation, localCenterB); - - b3BodyContactWorldPoint wp; - wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB); - - vc->normal = wp.normal; - vc->tangent1 = c->t1; - vc->tangent2 = c->t2; - vc->point = wp.point; - - b3Vec3 point = vc->point; - - b3Vec3 rA = point - xA; - b3Vec3 rB = point - xB; - - vc->rA = rA; - vc->rB = rB; - - vc->normalImpulse = c->normalImpulse; - vc->tangentImpulse = c->tangentImpulse; - - { - b3Vec3 n = vc->normal; - - b3Vec3 rnA = b3Cross(rA, n); - b3Vec3 rnB = b3Cross(rB, n); - - float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB); - - vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - vc->velocityBias = 0.0f; - } - - { - b3Vec3 t1 = vc->tangent1; - b3Vec3 t2 = vc->tangent2; - - b3Vec3 rn1A = b3Cross(rA, t1); - b3Vec3 rn1B = b3Cross(rB, t1); - b3Vec3 rn2A = b3Cross(rA, t2); - b3Vec3 rn2B = b3Cross(rB, t2); - - // dot(t1, t2) = 0 - // J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0 - float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B); - float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B); - float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B); - - b3Mat22 K; - K.x.Set(k11, k12); - K.y.Set(k12, k22); - - vc->tangentMass = b3Inverse(K); - } - } -} - -void b3ClothSolver::InitializeParticleContactConstraints() -{ - b3DenseVec3& x = *m_solverData.x; - b3DenseVec3& v = *m_solverData.v; - - float32 inv_dt = m_solverData.invdt; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - vc->indexA = c->p1->m_solverId; - vc->indexB = c->p2->m_solverId; - - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction); - - pc->indexA = c->p1->m_solverId; - pc->indexB = c->p2->m_solverId; - - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - pc->radiusA = c->p1->m_radius; - pc->radiusB = c->p2->m_radius; - } - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - - b3ParticleContactWorldPoint wp; - wp.Initialize(c); - - vc->normal = wp.normal; - vc->tangent1 = c->t1; - vc->tangent2 = c->t2; - vc->point = wp.point; - - b3Vec3 point = vc->point; - - vc->normalImpulse = c->normalImpulse; - vc->tangentImpulse = c->tangentImpulse; - - { - b3Vec3 n = vc->normal; - - float32 K = mA + mB; - - vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - - vc->velocityBias = 0.0f; - } - - { - b3Vec3 t1 = vc->tangent1; - b3Vec3 t2 = vc->tangent2; - - float32 k11 = mA + mB; - float32 k12 = 0.0f; - float32 k22 = mA + mB; - - b3Mat22 K; - K.x.Set(k11, k12); - K.y.Set(k12, k22); - - vc->tangentMass = b3Inverse(K); - } - } -} - -void b3ClothSolver::InitializeTriangleContactConstraints() -{ - b3DenseVec3& x = *m_solverData.x; - - for (u32 i = 0; i < m_triangleContactCount; ++i) - { - b3TriangleContact* c = m_triangleContacts[i]; - b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; - b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i; - - vc->indexA = c->p1->m_solverId; - vc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - - vc->indexB = c->p2->m_solverId; - vc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - vc->indexC = c->p3->m_solverId; - vc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; - - vc->indexD = c->p4->m_solverId; - vc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; - - pc->indexA = c->p1->m_solverId; - pc->invMassA = c->p1->m_type == e_staticParticle ? 0.0f : c->p1->m_invMass; - pc->radiusA = c->p1->m_radius; - - pc->indexB = c->p2->m_solverId; - pc->invMassB = c->p2->m_type == e_staticParticle ? 0.0f : c->p2->m_invMass; - - pc->indexC = c->p3->m_solverId; - pc->invMassC = c->p3->m_type == e_staticParticle ? 0.0f : c->p3->m_invMass; - - pc->indexD = c->p4->m_solverId; - pc->invMassD = c->p4->m_type == e_staticParticle ? 0.0f : c->p4->m_invMass; - - pc->triangleRadius = 0.0f; - pc->front = c->front; - - u32 indexA = pc->indexA; - float32 mA = pc->invMassA; - - u32 indexB = pc->indexB; - float32 mB = pc->invMassB; - - u32 indexC = pc->indexC; - float32 mC = pc->invMassC; - - u32 indexD = pc->indexD; - float32 mD = pc->invMassD; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; - - b3Vec3 n = b3Cross(xC - xB, xD - xB); - - if (pc->front == false) - { - n = -n; - } - - float32 n_len = n.Normalize(); - - b3Mat33 I; I.SetIdentity(); - - b3Mat33 N = I - b3Outer(n, n); - if (n_len > B3_EPSILON) - { - N = (1.0f / n_len) * N; - } - - b3Vec3 N_n = N * n; - - vc->JA = n; - vc->JC = b3Cross(xD - xB, N_n); - vc->JD = b3Cross(xC - xB, N_n); - vc->JB = b3Cross(xC - xD, N_n) - n; - - // Compute effective mass. - float32 K = mA + mB + mC + mD; - - vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - vc->normalImpulse = c->normalImpulse; - } -} - -void b3ClothSolver::WarmStart() -{ - b3DenseVec3& v = *m_solverData.v; - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - - u32 indexA = vc->indexA; - b3Body* bodyB = vc->bodyB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = bodyB->GetLinearVelocity(); - - b3Vec3 wA; wA.SetZero(); - b3Vec3 wB = bodyB->GetAngularVelocity(); - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Mat33 iA = vc->invIA; - b3Mat33 iB = vc->invIB; - - b3Vec3 P = vc->normalImpulse * vc->normal; - - vA -= mA * P; - wA -= iA * b3Cross(vc->rA, P); - - vB += mB * P; - wB += iB * b3Cross(vc->rB, P); - - b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1; - b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2; - - vA -= mA * (P1 + P2); - wA -= iA * b3Cross(vc->rA, P1 + P2); - - vB += mB * (P1 + P2); - wB += iB * b3Cross(vc->rB, P1 + P2); - - v[indexA] = vA; - - bodyB->SetLinearVelocity(vB); - bodyB->SetAngularVelocity(wB); - } - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 P = vc->normalImpulse * vc->normal; - - vA -= mA * P; - vB += mB * P; - - b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1; - b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2; - - vA -= mA * (P1 + P2); - vB += mB * (P1 + P2); - - v[indexA] = vA; - v[indexB] = vB; - } - - for (u32 i = 0; i < m_triangleContactCount; ++i) - { - b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - u32 indexC = vc->indexC; - u32 indexD = vc->indexD; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - float32 mC = vc->invMassC; - float32 mD = vc->invMassD; - - b3Vec3 PA = vc->normalImpulse * vc->JA; - b3Vec3 PB = vc->normalImpulse * vc->JB; - b3Vec3 PC = vc->normalImpulse * vc->JC; - b3Vec3 PD = vc->normalImpulse * vc->JD; - - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; - - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; - } -} - -void b3ClothSolver::SolveBodyContactVelocityConstraints() -{ - b3DenseVec3& v = *m_solverData.v; - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - - u32 indexA = vc->indexA; - b3Body* bodyB = vc->bodyB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = bodyB->GetLinearVelocity(); - - b3Vec3 wA; wA.SetZero(); - b3Vec3 wB = bodyB->GetAngularVelocity(); - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Mat33 iA = vc->invIA; - b3Mat33 iB = vc->invIB; - - b3Vec3 normal = vc->normal; - b3Vec3 point = vc->point; - - b3Vec3 rA = vc->rA; - b3Vec3 rB = vc->rB; - - // Solve normal constraint. - { - b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA); - float32 Cdot = b3Dot(normal, dv); - - float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias); - - float32 oldImpulse = vc->normalImpulse; - vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); - impulse = vc->normalImpulse - oldImpulse; - - b3Vec3 P = impulse * normal; - - vA -= mA * P; - wA -= iA * b3Cross(rA, P); - - vB += mB * P; - wB += iB * b3Cross(rB, P); - } - - // Solve tangent constraints. - { - b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA); - - b3Vec2 Cdot; - Cdot.x = b3Dot(dv, vc->tangent1); - Cdot.y = b3Dot(dv, vc->tangent2); - - b3Vec2 impulse = vc->tangentMass * -Cdot; - b3Vec2 oldImpulse = vc->tangentImpulse; - vc->tangentImpulse += impulse; - - float32 maxImpulse = vc->friction * vc->normalImpulse; - if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse) - { - vc->tangentImpulse.Normalize(); - vc->tangentImpulse *= maxImpulse; - } - - impulse = vc->tangentImpulse - oldImpulse; - - b3Vec3 P1 = impulse.x * vc->tangent1; - b3Vec3 P2 = impulse.y * vc->tangent2; - b3Vec3 P = P1 + P2; - - vA -= mA * P; - wA -= iA * b3Cross(rA, P); - - vB += mB * P; - wB += iB * b3Cross(rB, P); - } - - v[indexA] = vA; - - bodyB->SetLinearVelocity(vB); - bodyB->SetAngularVelocity(wB); - } -} - -void b3ClothSolver::SolveParticleContactVelocityConstraints() -{ - b3DenseVec3& v = *m_solverData.v; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - u32 indexA = vc->indexA; - u32 indexB = vc->indexB; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - - float32 mA = vc->invMassA; - float32 mB = vc->invMassB; - - b3Vec3 normal = vc->normal; - b3Vec3 point = vc->point; - - // Solve normal constraint. - { - b3Vec3 dv = vB - vA; - float32 Cdot = b3Dot(normal, dv); - - float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias); - - float32 oldImpulse = vc->normalImpulse; - vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); - impulse = vc->normalImpulse - oldImpulse; - - b3Vec3 P = impulse * normal; - - vA -= mA * P; - vB += mB * P; - } - - // Solve tangent constraints. - { - b3Vec3 dv = vB - vA; - - b3Vec2 Cdot; - Cdot.x = b3Dot(dv, vc->tangent1); - Cdot.y = b3Dot(dv, vc->tangent2); - - b3Vec2 impulse = vc->tangentMass * -Cdot; - b3Vec2 oldImpulse = vc->tangentImpulse; - vc->tangentImpulse += impulse; - - float32 maxImpulse = vc->friction * vc->normalImpulse; - if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse) - { - vc->tangentImpulse.Normalize(); - vc->tangentImpulse *= maxImpulse; - } - - impulse = vc->tangentImpulse - oldImpulse; - - b3Vec3 P1 = impulse.x * vc->tangent1; - b3Vec3 P2 = impulse.y * vc->tangent2; - b3Vec3 P = P1 + P2; - - vA -= mA * P; - vB += mB * P; - } - - v[indexA] = vA; - v[indexB] = vB; - } -} - -void b3ClothSolver::SolveTriangleContactVelocityConstraints() -{ - b3DenseVec3& v = *m_solverData.v; - - for (u32 i = 0; i < m_triangleContactCount; ++i) - { - b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; - - u32 indexA = vc->indexA; - float32 mA = vc->invMassA; - - u32 indexB = vc->indexB; - float32 mB = vc->invMassB; - - u32 indexC = vc->indexC; - float32 mC = vc->invMassC; - - u32 indexD = vc->indexD; - float32 mD = vc->invMassD; - - b3Vec3 vA = v[indexA]; - b3Vec3 vB = v[indexB]; - b3Vec3 vC = v[indexC]; - b3Vec3 vD = v[indexD]; - - b3Vec3 n = vc->JA; - - // Allow some slop and prevent large corrections. - float32 Cdot = b3Dot(n, vA - vB); - - float32 impulse = -vc->normalMass * Cdot; - - float32 oldImpulse = vc->normalImpulse; - vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f); - impulse = vc->normalImpulse - oldImpulse; - - b3Vec3 PA = impulse * vc->JA; - b3Vec3 PB = impulse * vc->JB; - b3Vec3 PC = impulse * vc->JC; - b3Vec3 PD = impulse * vc->JD; - - vA += mA * PA; - vB += mB * PB; - vC += mC * PC; - vD += mD * PD; - - v[indexA] = vA; - v[indexB] = vB; - v[indexC] = vC; - v[indexD] = vD; - } -} - -void b3ClothSolver::StoreImpulses() -{ - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; - } - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ParticleContact* c = m_particleContacts[i]; - b3ClothSolverParticleContactVelocityConstraint* vc = m_particleVelocityConstraints + i; - - c->normalImpulse = vc->normalImpulse; - c->tangentImpulse = vc->tangentImpulse; - } - - for (u32 i = 0; i < m_triangleContactCount; ++i) - { - b3TriangleContact* c = m_triangleContacts[i]; - b3ClothSolverTriangleContactVelocityConstraint* vc = m_triangleVelocityConstraints + i; - - c->normalImpulse = vc->normalImpulse; - } -} - -struct b3ClothSolverBodyContactSolverPoint -{ - void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) - { - b3Vec3 cA = b3Mul(xfA, pc->localPointA); - b3Vec3 cB = b3Mul(xfB, pc->localPointB); - - float32 rA = pc->radiusA; - float32 rB = pc->radiusB; - - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = distance - rA - rB; - } - - b3Vec3 normal; - b3Vec3 point; - float32 separation; -}; - -bool b3ClothSolver::SolveBodyContactPositionConstraints() -{ - b3DenseVec3& x = *m_solverData.v; - - float32 minSeparation = 0.0f; - - for (u32 i = 0; i < m_bodyContactCount; ++i) - { - b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; - - u32 indexA = pc->indexA; - float32 mA = pc->invMassA; - b3Mat33 iA = pc->invIA; - b3Vec3 localCenterA = pc->localCenterA; - - b3Body* bodyB = pc->bodyB; - float32 mB = pc->invMassB; - b3Mat33 iB = pc->invIB; - b3Vec3 localCenterB = pc->localCenterB; - - b3Vec3 cA = x[indexA]; - b3Quat qA; qA.SetIdentity(); - - b3Vec3 cB = bodyB->m_sweep.worldCenter; - b3Quat qB = bodyB->m_sweep.orientation; - - // Solve normal constraint - b3Transform xfA; - xfA.rotation = b3QuatMat33(qA); - xfA.position = cA - b3Mul(xfA.rotation, localCenterA); - - b3Transform xfB; - xfB.rotation = b3QuatMat33(qB); - xfB.position = cB - b3Mul(xfB.rotation, localCenterB); - - b3ClothSolverBodyContactSolverPoint cpcp; - cpcp.Initialize(pc, xfA, xfB); - - b3Vec3 normal = cpcp.normal; - b3Vec3 point = cpcp.point; - float32 separation = cpcp.separation; - - // Update max constraint error. - minSeparation = b3Min(minSeparation, separation); - - // Allow some slop and prevent large corrections. - float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); - - // Compute effective mass. - b3Vec3 rA = point - cA; - b3Vec3 rB = point - cB; - - b3Vec3 rnA = b3Cross(rA, normal); - b3Vec3 rnB = b3Cross(rB, normal); - float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB); - - // Compute normal impulse. - float32 impulse = K > 0.0f ? -C / K : 0.0f; - b3Vec3 P = impulse * normal; - - cA -= mA * P; - qA -= b3Derivative(qA, iA * b3Cross(rA, P)); - qA.Normalize(); - - cB += mB * P; - qB += b3Derivative(qB, iB * b3Cross(rB, P)); - qB.Normalize(); - - x[indexA] = cA; - - bodyB->m_sweep.worldCenter = cB; - bodyB->m_sweep.orientation = qB; - } - - return minSeparation >= -3.0f * B3_LINEAR_SLOP; -} - -struct b3ClothSolverParticleContactSolverPoint -{ - void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB) - { - b3Vec3 d = cB - cA; - float32 distance = b3Length(d); - - b3Vec3 nA(0.0f, 1.0f, 0.0f); - if (distance > B3_EPSILON) - { - nA = d / distance; - } - - b3Vec3 pA = cA + rA * nA; - b3Vec3 pB = cB - rB * nA; - - point = 0.5f * (pA + pB); - normal = nA; - separation = distance - rA - rB; - } - - b3Vec3 point; - b3Vec3 normal; - float32 separation; -}; - -bool b3ClothSolver::SolveParticleContactPositionConstraints() -{ - b3DenseVec3& x = *m_solverData.v; - - float32 minSeparation = 0.0f; - - for (u32 i = 0; i < m_particleContactCount; ++i) - { - b3ClothSolverParticleContactPositionConstraint* pc = m_particlePositionConstraints + i; - - u32 indexA = pc->indexA; - float32 mA = pc->invMassA; - float32 rA = pc->radiusA; - - u32 indexB = pc->indexB; - float32 mB = pc->invMassB; - float32 rB = pc->radiusB; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - - b3ClothSolverParticleContactSolverPoint cpcp; - cpcp.Initialize(xA, rA, xB, rB); - - b3Vec3 normal = cpcp.normal; - b3Vec3 point = cpcp.point; - float32 separation = cpcp.separation; - - // Update max constraint error. - minSeparation = b3Min(minSeparation, separation); - - // Allow some slop and prevent large corrections. - float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); - - // Compute effective mass. - float32 K = mA + mB; - - // Compute normal impulse. - float32 impulse = K > 0.0f ? -C / K : 0.0f; - b3Vec3 P = impulse * normal; - - xA -= mA * P; - xB += mB * P; - - x[indexA] = xA; - x[indexB] = xB; - } - - return minSeparation >= -3.0f * B3_LINEAR_SLOP; -} - -bool b3ClothSolver::SolveTriangleContactPositionConstraints() -{ - b3DenseVec3& x = *m_solverData.v; - - float32 minSeparation = 0.0f; - - for (u32 i = 0; i < m_triangleContactCount; ++i) - { - b3ClothSolverTriangleContactPositionConstraint* pc = m_trianglePositionConstraints + i; - - u32 indexA = pc->indexA; - float32 mA = pc->invMassA; - float32 radiusA = pc->radiusA; - - u32 indexB = pc->indexB; - float32 mB = pc->invMassB; - - u32 indexC = pc->indexC; - float32 mC = pc->invMassC; - - u32 indexD = pc->indexD; - float32 mD = pc->invMassD; - - float32 triangleRadius = pc->triangleRadius; - - b3Vec3 xA = x[indexA]; - b3Vec3 xB = x[indexB]; - b3Vec3 xC = x[indexC]; - b3Vec3 xD = x[indexD]; - - b3Vec3 n = b3Cross(xC - xB, xD - xB); - if (pc->front == false) - { - n = -n; - } - - float32 n_len = n.Normalize(); - - float32 distance = b3Dot(n, xA - xB); - float32 separation = distance - radiusA - triangleRadius; - - // Update max constraint error. - minSeparation = b3Min(minSeparation, separation); - - // Allow some slop and prevent large corrections. - float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f); - - b3Mat33 I; I.SetIdentity(); - - b3Mat33 N = I - b3Outer(n, n); - if (n_len > B3_EPSILON) - { - N = (1.0f / n_len) * N; - } - - b3Vec3 N_n = N * n; - - b3Vec3 JA = n; - b3Vec3 JC = b3Cross(xD - xB, N_n); - b3Vec3 JD = b3Cross(xC - xB, N_n); - b3Vec3 JB = b3Cross(xC - xD, N_n) - n; - - // Compute effective mass. - float32 K = mA + mB + mC + mD; - - // Compute normal impulse. - float32 impulse = K > 0.0f ? -C / K : 0.0f; - - b3Vec3 PA = impulse * JA; - b3Vec3 PB = impulse * JB; - b3Vec3 PC = impulse * JC; - b3Vec3 PD = impulse * JD; - - xA += mA * PA; - xB += mB * PB; - xC += mC * PC; - xD += mD * PD; - - x[indexA] = xA; - x[indexB] = xB; - x[indexC] = xC; - x[indexD] = xD; - } - - return minSeparation >= -3.0f * B3_LINEAR_SLOP; -} \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index 0e8159c..cd4c2fe 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -85,8 +85,6 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_userData = nullptr; m_x.SetZero(); m_vertex = ~0; - - m_contact.active = false; } b3Particle::~b3Particle() @@ -108,7 +106,5 @@ void b3Particle::SetType(b3ParticleType type) { m_velocity.SetZero(); m_translation.SetZero(); - - m_contact.active = false; } } \ No newline at end of file