diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp index b54d95e..d7d5475 100644 --- a/examples/testbed/framework/test_entries.cpp +++ b/examples/testbed/framework/test_entries.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,7 @@ TestEntry g_tests[] = { "Shirt", &Shirt::Create }, { "Particle Types", &ParticleTypes::Create }, { "Tension Mapping", &TensionMapping::Create }, + { "Self-Collision", &SelfCollision::Create }, { "Mass-Spring System", &MassSpring::Create }, { "Single Pendulum", &SinglePendulum::Create }, { "Rope", &Rope::Create }, diff --git a/examples/testbed/tests/self_collision.h b/examples/testbed/tests/self_collision.h new file mode 100644 index 0000000..1ddb534 --- /dev/null +++ b/examples/testbed/tests/self_collision.h @@ -0,0 +1,83 @@ +/* +* 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 SELF_COLLISION_H +#define SELF_COLLISION_H + +class SelfCollision : public ClothTest +{ +public: + SelfCollision() : m_rectangleGarment(5.0f, 5.0f) + { + // Generate 2D mesh + m_rectangleGarmentMesh.Set(&m_rectangleGarment, 1.0f); + + // Create 3D mesh + m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); + + // + for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) + { + m_rectangleClothMesh.vertices[i].y += 10.0f; + } + + b3ClothDef def; + def.mesh = &m_rectangleClothMesh; + def.density = 1.0f; + def.structural = 100000.0f; + + m_cloth = m_world.CreateCloth(def); + + for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) + { + p->SetRadius(0.25f); + } + + { + b3BodyDef bd; + bd.type = e_staticBody; + + b3Body* b = m_world.CreateBody(bd); + + m_boxHull.Set(50.0f, 1.0f, 50.0f); + + b3HullShape boxShape; + boxShape.m_hull = &m_boxHull; + boxShape.m_radius = 0.2f; + + b3ShapeDef sd; + sd.shape = &boxShape; + sd.friction = 1.0f; + + b->CreateShape(sd); + } + } + + static Test* Create() + { + return new SelfCollision(); + } + + b3RectangleGarment m_rectangleGarment; + b3GarmentMesh m_rectangleGarmentMesh; + b3GarmentClothMesh m_rectangleClothMesh; + + b3BoxHull m_boxHull; +}; + +#endif \ No newline at end of file diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index 8271654..e6805ea 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -28,6 +28,8 @@ class b3Shape; class b3Particle; class b3Force; +class b3BodyContact; +class b3ParticleContact; struct b3ParticleDef; struct b3ForceDef; @@ -143,10 +145,12 @@ private: // Compute mass of each particle. void ComputeMass(); - // Update contacts. - // This is where some contacts might be initiated or terminated. - void UpdateContacts(); + // Update body contacts. + void UpdateBodyContacts(); + // Update particle contacts. + void UpdateParticleContacts(); + // Solve void Solve(float32 dt, const b3Vec3& gravity); @@ -162,12 +166,18 @@ private: // Pool of particles b3BlockPool m_particleBlocks; + // Pool of particle contacts + b3BlockPool m_particleContactBlocks; + // List of particles b3List2 m_particleList; // List of forces b3List2 m_forceList; + // List of particle contacts + b3List2 m_particleContactList; + // The parent world of this cloth. b3World* m_world; diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 7afa9a0..d960624 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -28,6 +28,7 @@ class b3Particle; class b3Body; class b3Force; class b3BodyContact; +class b3ParticleContact; struct b3DenseVec3; struct b3DiagMat33; @@ -38,7 +39,8 @@ struct b3ClothSolverDef b3StackAllocator* stack; u32 particleCapacity; u32 forceCapacity; - u32 contactCapacity; + u32 bodyContactCapacity; + u32 particleContactCapacity; }; struct b3ClothSolverData @@ -113,6 +115,40 @@ struct b3ClothSolverContactPositionConstraint 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; +}; + class b3ClothSolver { public: @@ -122,6 +158,7 @@ public: void Add(b3Particle* p); void Add(b3Force* f); void Add(b3BodyContact* c); + void Add(b3ParticleContact* c); void Solve(float32 dt, const b3Vec3& gravity); private: @@ -135,19 +172,28 @@ private: void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const; // - void InitializeConstraints(); + void InitializeBodyContactConstraints(); + + // + void InitializeParticleContactConstraints(); // void WarmStart(); // - void SolveVelocityConstraints(); + void SolveBodyContactVelocityConstraints(); + // + void SolveParticleContactVelocityConstraints(); + // void StoreImpulses(); // - bool SolvePositionConstraints(); + bool SolveBodyContactPositionConstraints(); + + // + bool SolveParticleContactPositionConstraints(); b3StackAllocator* m_allocator; @@ -163,12 +209,17 @@ private: u32 m_constraintCount; b3AccelerationConstraint* m_constraints; - u32 m_contactCapacity; - u32 m_contactCount; - b3BodyContact** m_contacts; + u32 m_bodyContactCapacity; + u32 m_bodyContactCount; + b3BodyContact** m_bodyContacts; + b3ClothSolverContactVelocityConstraint* m_bodyVelocityConstraints; + b3ClothSolverContactPositionConstraint* m_bodyPositionConstraints; - b3ClothSolverContactVelocityConstraint* m_velocityConstraints; - b3ClothSolverContactPositionConstraint* m_positionConstraints; + u32 m_particleContactCapacity; + u32 m_particleContactCount; + b3ParticleContact** m_particleContacts; + b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints; + b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints; b3ClothSolverData m_solverData; }; diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index ea1c1c9..52f0532 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -104,6 +104,36 @@ struct b3BodyContactWorldPoint float32 separation; }; +// A contact between two particles +class b3ParticleContact +{ +public: + b3ParticleContact() { } + ~b3ParticleContact() { } + + b3Particle* p1; + b3Particle* p2; + + // Contact constraint + float32 normalImpulse; + + // Friction constraint + b3Vec3 t1, t2; + b3Vec2 tangentImpulse; + + b3ParticleContact* m_prev; + b3ParticleContact* m_next; +}; + +struct b3ParticleContactWorldPoint +{ + void Initialize(const b3ParticleContact* c); + + b3Vec3 point; + b3Vec3 normal; + float32 separation; +}; + // A cloth particle. class b3Particle { @@ -134,7 +164,10 @@ public: // Get the particle mass. float32 GetMass() const; - // Get the particle radius; + // Set the particle radius. + void SetRadius(float32 radius); + + // Get the particle radius. float32 GetRadius() const; // Apply a force. @@ -248,6 +281,11 @@ inline float32 b3Particle::GetMass() const return m_mass; } +inline void b3Particle::SetRadius(float32 radius) +{ + m_radius = radius; +} + inline float32 b3Particle::GetRadius() const { return m_radius; diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h index 36e09bb..53aa35e 100644 --- a/include/bounce/dynamics/shapes/shape.h +++ b/include/bounce/dynamics/shapes/shape.h @@ -40,6 +40,7 @@ enum b3ShapeType struct b3TestSphereOutput { + b3Vec3 point; float32 separation; b3Vec3 normal; }; diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index db8e4d4..71f9ca7 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -149,7 +149,9 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) return sharedCount; } -b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle)) +b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : + m_particleBlocks(sizeof(b3Particle)), + m_particleContactBlocks(sizeof(b3ParticleContact)) { B3_ASSERT(def.mesh); B3_ASSERT(def.density > 0.0f); @@ -504,9 +506,9 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 return false; } -void b3Cloth::UpdateContacts() +void b3Cloth::UpdateBodyContacts() { - B3_PROFILE("Update Contacts"); + B3_PROFILE("Update Body Contacts"); // Create contacts for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) @@ -523,6 +525,7 @@ void b3Cloth::UpdateContacts() // Find the deepest penetration float32 bestSeparation = 0.0f; + b3Vec3 bestPoint(0.0f, 0.0f, 0.0f); b3Vec3 bestNormal(0.0f, 0.0f, 0.0f); b3Shape* bestShape = nullptr; @@ -543,6 +546,7 @@ void b3Cloth::UpdateContacts() if (output.separation < bestSeparation) { bestSeparation = output.separation; + bestPoint = output.point; bestNormal = output.normal; bestShape = shape; } @@ -555,21 +559,19 @@ void b3Cloth::UpdateContacts() continue; } + // Ensure the the normal points from the particle 1 to shape 2 b3Shape* shape = bestShape; b3Body* body = shape->GetBody(); float32 s = bestSeparation; - b3Vec3 n = bestNormal; - b3Vec3 cp2r = p->m_position - s * n; - b3Vec3 cp2 = cp2r - shape->m_radius * n; + b3Vec3 cb = bestPoint; + b3Vec3 n = -bestNormal; - // Store the local contact manifold - // Ensure the the normal points from the particle 1 to shape 2 c->active = true; c->p1 = p; c->s2 = shape; - c->localNormal1 = -n; + c->localNormal1 = n; c->localPoint1.SetZero(); - c->localPoint2 = body->GetLocalPoint(cp2); + c->localPoint2 = body->GetLocalPoint(cb); c->t1 = b3Perp(n); c->t2 = b3Cross(c->t1, n); c->normalImpulse = 0.0f; @@ -583,6 +585,66 @@ void b3Cloth::UpdateContacts() } } +void b3Cloth::UpdateParticleContacts() +{ + B3_PROFILE("Update Particle Contacts"); + + // Clear buffer + b3ParticleContact* c = m_particleContactList.m_head; + while (c) + { + b3ParticleContact* c0 = c; + c = c->m_next; + m_particleContactList.Remove(c0); + c0->~b3ParticleContact(); + m_particleContactBlocks.Free(c0); + } + + // Create particle contacts + for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next) + { + for (b3Particle* p2 = p1->m_next; p2; p2 = p2->m_next) + { + if (p1->m_type != e_dynamicParticle && p2->m_type != e_dynamicBody) + { + // At least one particle should be kinematic or dynamic. + continue; + } + + b3Vec3 c1 = p1->m_position; + float32 r1 = p1->m_radius; + + b3Vec3 c2 = p2->m_position; + float32 r2 = p2->m_radius; + + b3Vec3 d = c2 - c1; + float32 dd = b3Dot(d, d); + float32 totalRadius = r1 + r2; + if (dd > totalRadius * totalRadius) + { + continue; + } + + b3Vec3 n(0.0f, 1.0f, 0.0f); + if (dd > B3_EPSILON * B3_EPSILON) + { + float32 distance = b3Sqrt(dd); + n = d / distance; + } + + b3ParticleContact* c = (b3ParticleContact*)m_particleContactBlocks.Allocate(); + c->p1 = p1; + c->p2 = p2; + c->normalImpulse = 0.0f; + c->t1 = b3Perp(n); + c->t2 = b3Cross(c->t1, n); + c->tangentImpulse.SetZero(); + + m_particleContactList.PushFront(c); + } + } +} + void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Solve"); @@ -592,7 +654,8 @@ 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.contactCapacity = m_particleList.m_count; + solverDef.bodyContactCapacity = m_particleList.m_count; + solverDef.particleContactCapacity = m_particleContactList.m_count; b3ClothSolver solver(solverDef); @@ -608,12 +671,19 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) for (b3Particle* p = m_particleList.m_head; p; p = p->m_next) { - if (p->m_contact.active) + b3BodyContact* bc = &p->m_contact; + + if (bc->active) { - solver.Add(&p->m_contact); + solver.Add(bc); } } + for (b3ParticleContact* pc = m_particleContactList.m_head; pc; pc = pc->m_next) + { + solver.Add(pc); + } + // Solve solver.Solve(dt, gravity); @@ -629,9 +699,12 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Step"); - // Update contacts - UpdateContacts(); + // Update body contacts + UpdateBodyContacts(); + // Update particle contacts + UpdateParticleContacts(); + // Solve constraints, integrate state, clear forces and translations. if (dt > 0.0f) { diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 5099292..6cf2ba8 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -52,19 +52,29 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_constraintCount = 0; m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint)); - m_contactCapacity = def.contactCapacity; - m_contactCount = 0; - m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*)); + m_bodyContactCapacity = def.bodyContactCapacity; + m_bodyContactCount = 0; + m_bodyContacts = (b3BodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3BodyContact*)); + m_bodyVelocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverContactVelocityConstraint)); + m_bodyPositionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverContactPositionConstraint)); - m_velocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactVelocityConstraint)); - m_positionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactPositionConstraint)); + 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)); } b3ClothSolver::~b3ClothSolver() { - m_allocator->Free(m_positionConstraints); - m_allocator->Free(m_velocityConstraints); - m_allocator->Free(m_contacts); + 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); m_allocator->Free(m_forces); m_allocator->Free(m_particles); @@ -76,16 +86,21 @@ void b3ClothSolver::Add(b3Particle* p) m_particles[m_particleCount++] = p; } -void b3ClothSolver::Add(b3BodyContact* c) -{ - m_contacts[m_contactCount++] = c; -} - void b3ClothSolver::Add(b3Force* f) { m_forces[m_forceCount++] = f; } +void b3ClothSolver::Add(b3BodyContact* 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) @@ -242,17 +257,22 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) m_particles[i]->m_x = x[i]; } - // Initialize constraints - InitializeConstraints(); + // Initialize body contact constraints + InitializeBodyContactConstraints(); + + // Initialize particle contact constraints + InitializeParticleContactConstraints(); // Warm start velocity constraints WarmStart(); // Solve velocity constraints - const u32 kVelocityIterations = 10; + const u32 kVelocityIterations = 8; + for (u32 i = 0; i < kVelocityIterations; ++i) { - SolveVelocityConstraints(); + SolveBodyContactVelocityConstraints(); + SolveParticleContactVelocityConstraints(); } // Store impulses to improve convergence @@ -261,22 +281,24 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Integrate positions sx = sx + h * sv; -#if 0 // Solve position constraints - const u32 kPositionIterations = 0; + const u32 kPositionIterations = 2; + + bool positionSolved = false; for (u32 i = 0; i < kPositionIterations; ++i) { - bool contactsSolved = SolvePositionConstraints(); - if (contactsSolved) + bool particleContactsSolved = SolveParticleContactPositionConstraints(); + if (particleContactsSolved) { + positionSolved = true; break; } } // Synchronize bodies - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3Body* body = m_contacts[i]->s2->GetBody(); + b3Body* body = m_bodyContacts[i]->s2->GetBody(); body->SynchronizeTransform(); @@ -284,7 +306,6 @@ 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) @@ -404,18 +425,18 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations, iterations = iteration; } -void b3ClothSolver::InitializeConstraints() +void b3ClothSolver::InitializeBodyContactConstraints() { b3DenseVec3& x = *m_solverData.x; b3DenseVec3& v = *m_solverData.v; float32 inv_dt = m_solverData.invdt; - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_contacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i; - b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i; + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; vc->indexA = c->p1->m_solverId; vc->bodyB = c->s2->GetBody(); @@ -436,7 +457,7 @@ void b3ClothSolver::InitializeConstraints() pc->invIA.SetZero(); pc->invIB = vc->bodyB->GetWorldInverseInertia(); - + pc->radiusA = c->p1->m_radius; pc->radiusB = c->s2->m_radius; @@ -448,11 +469,11 @@ void b3ClothSolver::InitializeConstraints() pc->localPointB = c->localPoint2; } - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_contacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i; - b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i; + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -468,7 +489,7 @@ void b3ClothSolver::InitializeConstraints() b3Quat qA; qA.SetIdentity(); b3Quat qB = bodyB->m_sweep.orientation; - + b3Vec3 localCenterA = pc->localCenterA; b3Vec3 localCenterB = pc->localCenterB; @@ -510,7 +531,7 @@ void b3ClothSolver::InitializeConstraints() vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; float32 C = b3Min(0.0f, wp.separation + B3_LINEAR_SLOP); - + vc->velocityBias = -inv_dt * B3_BAUMGARTE * C; } @@ -538,13 +559,99 @@ void b3ClothSolver::InitializeConstraints() } } +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 = 1.0f; + + 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::WarmStart() { b3DenseVec3& v = *m_solverData.v; - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i; + b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -583,15 +690,43 @@ void b3ClothSolver::WarmStart() 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; + } } -void b3ClothSolver::SolveVelocityConstraints() +void b3ClothSolver::SolveBodyContactVelocityConstraints() { b3DenseVec3& v = *m_solverData.v; - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i; + b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -673,12 +808,92 @@ void b3ClothSolver::SolveVelocityConstraints() } } +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::StoreImpulses() { - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3BodyContact* c = m_contacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i; + b3BodyContact* c = m_bodyContacts[i]; + b3ClothSolverContactVelocityConstraint* 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; @@ -701,15 +916,15 @@ struct b3ClothSolverContactSolverPoint float32 separation; }; -bool b3ClothSolver::SolvePositionConstraints() +bool b3ClothSolver::SolveBodyContactPositionConstraints() { b3DenseVec3& x = *m_solverData.v; float32 minSeparation = 0.0f; - for (u32 i = 0; i < m_contactCount; ++i) + for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i; + b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; u32 indexA = pc->indexA; float32 mA = pc->invMassA; @@ -775,5 +990,86 @@ bool b3ClothSolver::SolvePositionConstraints() bodyB->m_sweep.orientation = qB; } + return minSeparation >= -3.0f * B3_LINEAR_SLOP; +} + +struct b3ClothSolverParticleContactSolverPoint +{ + void Initialize(const b3Particle* p1, const b3Particle* p2) + { + b3Vec3 cA = p1->GetPosition(); + float32 rA = p1->GetRadius(); + + b3Vec3 cB = p2->GetPosition(); + float32 rB = p2->GetRadius(); + + 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; + + u32 indexB = pc->indexB; + float32 mB = pc->invMassB; + + b3Vec3 xA = x[indexA]; + b3Vec3 xB = x[indexB]; + + b3ClothSolverParticleContactSolverPoint cpcp; + cpcp.Initialize(m_particles[indexA], m_particles[indexB]); + + 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; } \ No newline at end of file diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index 14e628b..4df5fc6 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -36,6 +36,31 @@ void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, con separation = b3Dot(cB - cA, nA) - rA - rB; } +void b3ParticleContactWorldPoint::Initialize(const b3ParticleContact* c) +{ + b3Vec3 cA = c->p1->GetPosition(); + float32 rA = c->p1->GetRadius(); + + b3Vec3 cB = c->p2->GetPosition(); + float32 rB = c->p2->GetRadius(); + + 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; +} + b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) { m_cloth = cloth; diff --git a/src/bounce/dynamics/shapes/capsule_shape.cpp b/src/bounce/dynamics/shapes/capsule_shape.cpp index 077eb18..76af86d 100644 --- a/src/bounce/dynamics/shapes/capsule_shape.cpp +++ b/src/bounce/dynamics/shapes/capsule_shape.cpp @@ -215,6 +215,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe n = d / len; } + output->point = A; output->separation = len - radius; output->normal = n; @@ -239,6 +240,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe n = d / len; } + output->point = B; output->separation = len - radius; output->normal = n; @@ -274,6 +276,7 @@ bool b3CapsuleShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphe } n.Normalize(); + output->point = P; output->separation = b3Sqrt(dd) - radius; output->normal = -n; return true; diff --git a/src/bounce/dynamics/shapes/hull_shape.cpp b/src/bounce/dynamics/shapes/hull_shape.cpp index 0521a05..912e145 100644 --- a/src/bounce/dynamics/shapes/hull_shape.cpp +++ b/src/bounce/dynamics/shapes/hull_shape.cpp @@ -374,8 +374,10 @@ bool b3HullShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& sphere, if (maxIndex != ~0) { + b3Plane plane = b3Mul(xf, m_hull->GetPlane(maxIndex)); + output->point = b3ClosestPointOnPlane(sphere.vertex, plane); output->separation = maxSeparation - radius; - output->normal = b3Mul(xf.rotation, m_hull->GetPlane(maxIndex).normal); + output->normal = plane.normal; return true; } diff --git a/src/bounce/dynamics/shapes/sphere_shape.cpp b/src/bounce/dynamics/shapes/sphere_shape.cpp index 6fc8d24..8d3819c 100644 --- a/src/bounce/dynamics/shapes/sphere_shape.cpp +++ b/src/bounce/dynamics/shapes/sphere_shape.cpp @@ -80,6 +80,7 @@ bool b3SphereShape::TestSphere(b3TestSphereOutput* output, const b3Sphere& spher { float32 d_len = b3Sqrt(dd); + output->point = center; output->separation = d_len - radius; output->normal.Set(0.0f, 1.0, 0.0f); if (d_len > B3_EPSILON)