diff --git a/examples/testbed/tests/self_collision.h b/examples/testbed/tests/self_collision.h index 1ddb534..504bd1b 100644 --- a/examples/testbed/tests/self_collision.h +++ b/examples/testbed/tests/self_collision.h @@ -30,9 +30,10 @@ public: // Create 3D mesh m_rectangleClothMesh.Set(&m_rectangleGarmentMesh); - // + b3Mat33 Rx = b3Mat33RotationX(0.5f * B3_PI); for (u32 i = 0; i < m_rectangleClothMesh.vertexCount; ++i) { + m_rectangleClothMesh.vertices[i] = Rx * m_rectangleClothMesh.vertices[i]; m_rectangleClothMesh.vertices[i].y += 10.0f; } @@ -45,7 +46,8 @@ public: for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext()) { - p->SetRadius(0.25f); + p->SetRadius(0.2f); + p->SetFriction(0.2f); } { @@ -54,14 +56,13 @@ public: 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; + b3CapsuleShape capsuleShape; + capsuleShape.m_centers[0].Set(0.0f, 5.0f, -5.0f); + capsuleShape.m_centers[1].Set(0.0f, 5.0f, 5.0f); + capsuleShape.m_radius = 1.0f;; b3ShapeDef sd; - sd.shape = &boxShape; + sd.shape = &capsuleShape; sd.friction = 1.0f; b->CreateShape(sd); diff --git a/include/bounce/dynamics/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h index e6805ea..eb88630 100644 --- a/include/bounce/dynamics/cloth/cloth.h +++ b/include/bounce/dynamics/cloth/cloth.h @@ -30,6 +30,7 @@ class b3Particle; class b3Force; class b3BodyContact; class b3ParticleContact; +class b3TriangleContact; struct b3ParticleDef; struct b3ForceDef; @@ -151,6 +152,9 @@ private: // Update particle contacts. void UpdateParticleContacts(); + // Update triangle contacts. + void UpdateTriangleContacts(); + // Solve void Solve(float32 dt, const b3Vec3& gravity); @@ -169,6 +173,9 @@ private: // Pool of particle contacts b3BlockPool m_particleContactBlocks; + // Pool of triangle contacts + b3BlockPool m_triangleContactBlocks; + // List of particles b3List2 m_particleList; @@ -178,6 +185,9 @@ private: // List of particle contacts b3List2 m_particleContactList; + // List of triangle contacts + b3List2 m_triangleContactList; + // 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 d960624..198699c 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -29,6 +29,7 @@ class b3Body; class b3Force; class b3BodyContact; class b3ParticleContact; +class b3TriangleContact; struct b3DenseVec3; struct b3DiagMat33; @@ -41,6 +42,7 @@ struct b3ClothSolverDef u32 forceCapacity; u32 bodyContactCapacity; u32 particleContactCapacity; + u32 triangleContactCapacity; }; struct b3ClothSolverData @@ -149,6 +151,44 @@ struct b3ClothSolverParticleContactPositionConstraint 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: @@ -159,6 +199,7 @@ public: void Add(b3Force* f); void Add(b3BodyContact* c); void Add(b3ParticleContact* c); + void Add(b3TriangleContact* c); void Solve(float32 dt, const b3Vec3& gravity); private: @@ -177,6 +218,9 @@ private: // void InitializeParticleContactConstraints(); + // + void InitializeTriangleContactConstraints(); + // void WarmStart(); @@ -186,6 +230,9 @@ private: // void SolveParticleContactVelocityConstraints(); + // + void SolveTriangleContactVelocityConstraints(); + // void StoreImpulses(); @@ -195,6 +242,9 @@ private: // bool SolveParticleContactPositionConstraints(); + // + bool SolveTriangleContactPositionConstraints(); + b3StackAllocator* m_allocator; u32 m_particleCapacity; @@ -221,6 +271,12 @@ private: 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 52f0532..ec68dd4 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -48,6 +48,7 @@ struct b3ParticleDef velocity.SetZero(); force.SetZero(); radius = 0.0f; + friction = 0.0f; userData = nullptr; } @@ -56,21 +57,10 @@ struct b3ParticleDef b3Vec3 velocity; b3Vec3 force; float32 radius; + float32 friction; void* userData; }; -// -class b3FrictionForce : public b3Force -{ -public: - b3FrictionForce() { } - ~b3FrictionForce() { } - - void Apply(const b3ClothSolverData* data); - - b3Particle* m_p; -}; - // A contact between a particle and a solid class b3BodyContact { @@ -134,6 +124,27 @@ struct b3ParticleContactWorldPoint float32 separation; }; +// A contact between a particle and a triangle +class b3TriangleContact +{ +public: + b3TriangleContact() { } + ~b3TriangleContact() { } + + b3Particle* p1; + b3Particle* p2; + b3Particle* p3; + b3Particle* p4; + + bool front; + + // Contact constraint + float32 normalImpulse; + + b3TriangleContact* m_prev; + b3TriangleContact* m_next; +}; + // A cloth particle. class b3Particle { @@ -170,6 +181,12 @@ public: // Get the particle radius. float32 GetRadius() const; + // Set the particle coefficient of friction. + void SetFriction(float32 friction); + + // Get the particle coefficient of friction. + float32 GetFriction() const; + // Apply a force. void ApplyForce(const b3Vec3& force); @@ -214,6 +231,9 @@ private: // Radius float32 m_radius; + // Coefficient of friction + float32 m_friction; + // User data. void* m_userData; @@ -291,6 +311,16 @@ inline float32 b3Particle::GetRadius() const return m_radius; } +inline void b3Particle::SetFriction(float32 friction) +{ + m_friction = friction; +} + +inline float32 b3Particle::GetFriction() const +{ + return m_friction; +} + inline void b3Particle::ApplyForce(const b3Vec3& force) { if (m_type != e_dynamicParticle) diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 71f9ca7..45c6135 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -149,9 +149,10 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) return sharedCount; } -b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : +b3Cloth::b3Cloth(const b3ClothDef& def, b3World* world) : m_particleBlocks(sizeof(b3Particle)), - m_particleContactBlocks(sizeof(b3ParticleContact)) + m_particleContactBlocks(sizeof(b3ParticleContact)), + m_triangleContactBlocks(sizeof(b3TriangleContact)) { B3_ASSERT(def.mesh); B3_ASSERT(def.density > 0.0f); @@ -481,8 +482,7 @@ bool b3Cloth::RayCast(b3RayCastOutput* output, const b3RayCastInput* input, u32 float32 v = b3Dot(QC_x_QA, AB_x_AC); float32 w = b3Dot(QA_x_QB, AB_x_AC); - // This tolerance helps intersections lying on - // shared edges to not be missed. + // Characteristic length of triangle const float32 kTol = -B3_EPSILON; // Is the intersection on the triangle? @@ -588,7 +588,7 @@ void b3Cloth::UpdateBodyContacts() void b3Cloth::UpdateParticleContacts() { B3_PROFILE("Update Particle Contacts"); - + // Clear buffer b3ParticleContact* c = m_particleContactList.m_head; while (c) @@ -624,7 +624,7 @@ void b3Cloth::UpdateParticleContacts() { continue; } - + b3Vec3 n(0.0f, 1.0f, 0.0f); if (dd > B3_EPSILON * B3_EPSILON) { @@ -637,7 +637,7 @@ void b3Cloth::UpdateParticleContacts() c->p2 = p2; c->normalImpulse = 0.0f; c->t1 = b3Perp(n); - c->t2 = b3Cross(c->t1, n); + c->t2 = b3Cross(c->t1, n); c->tangentImpulse.SetZero(); m_particleContactList.PushFront(c); @@ -645,6 +645,241 @@ void b3Cloth::UpdateParticleContacts() } } +static B3_FORCE_INLINE void b3Barycentric(float32 out[3], + const b3Vec3& A, const b3Vec3& B, + const b3Vec3& Q) +{ + b3Vec3 AB = B - A; + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + + //float32 divisor = b3Dot(AB, AB); + + out[0] = b3Dot(QB, AB); + out[1] = -b3Dot(QA, AB); + out[2] = out[0] + out[1]; +} + +// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w) +// with respect to a triangle ABC. +// The last output value is the divisor. +static B3_FORCE_INLINE void b3Barycentric(float32 out[4], + const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, + const b3Vec3& Q) +{ + b3Vec3 AB = B - A; + b3Vec3 AC = C - A; + + b3Vec3 QA = A - Q; + b3Vec3 QB = B - Q; + b3Vec3 QC = C - Q; + + b3Vec3 QB_x_QC = b3Cross(QB, QC); + b3Vec3 QC_x_QA = b3Cross(QC, QA); + b3Vec3 QA_x_QB = b3Cross(QA, QB); + + b3Vec3 AB_x_AC = b3Cross(AB, AC); + + //float32 divisor = b3Dot(AB_x_AC, AB_x_AC); + + out[0] = b3Dot(QB_x_QC, AB_x_AC); + out[1] = b3Dot(QC_x_QA, AB_x_AC); + out[2] = b3Dot(QA_x_QB, AB_x_AC); + out[3] = out[0] + out[1] + out[2]; +} + +static B3_FORCE_INLINE void b3Solve3(float32 out[3], + const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, + const b3Vec3& Q) +{ + // Test vertex regions + float32 wAB[3], wBC[3], wCA[3]; + b3Barycentric(wAB, A, B, Q); + b3Barycentric(wBC, B, C, Q); + b3Barycentric(wCA, C, A, Q); + + // R A + if (wAB[1] <= 0.0f && wCA[0] <= 0.0f) + { + out[0] = 1.0f; + out[1] = 0.0f; + out[2] = 0.0f; + return; + } + + // R B + if (wAB[0] <= 0.0f && wBC[1] <= 0.0f) + { + out[0] = 0.0f; + out[1] = 1.0f; + out[2] = 0.0f; + return; + } + + // R C + if (wBC[0] <= 0.0f && wCA[1] <= 0.0f) + { + out[0] = 0.0f; + out[1] = 0.0f; + out[2] = 1.0f; + return; + } + + // Test edge regions + float32 wABC[4]; + b3Barycentric(wABC, A, B, C, Q); + + // This is used to help testing if the face degenerates + // into an edge. + float32 area = wABC[3]; + + // R AB + if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f) + { + float32 divisor = wAB[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = s * wAB[0]; + out[1] = s * wAB[1]; + out[2] = 0.0f; + return; + } + + // R BC + if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f) + { + float32 divisor = wBC[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = 0.0f; + out[1] = s * wBC[0]; + out[2] = s * wBC[1]; + return; + } + + // R CA + if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f) + { + float32 divisor = wCA[2]; + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = 0.0f; + out[2] = s * wCA[0]; + out[1] = s * wCA[1]; + return; + } + + // R ABC/ACB + float32 divisor = wABC[3]; + if (divisor <= 0.0f) + { + float32 s = 1.0f / 3.0f; + out[0] = s; + out[2] = s; + out[1] = s; + return; + } + + B3_ASSERT(divisor > 0.0f); + float32 s = 1.0f / divisor; + out[0] = s * wABC[0]; + out[1] = s * wABC[1]; + out[2] = s * wABC[2]; +} + +void b3Cloth::UpdateTriangleContacts() +{ + B3_PROFILE("Update Triangle Contacts"); + + // Clear buffer + b3TriangleContact* c = m_triangleContactList.m_head; + while (c) + { + b3TriangleContact* c0 = c; + c = c->m_next; + m_triangleContactList.Remove(c0); + c0->~b3TriangleContact(); + m_triangleContactBlocks.Free(c0); + } + + // Create triangle contacts + for (b3Particle* p1 = m_particleList.m_head; p1; p1 = p1->m_next) + { + for (u32 i = 0; i < m_mesh->triangleCount; ++i) + { + b3ClothMeshTriangle* triangle = m_mesh->triangles + i; + + b3Particle* p2 = m_vertexParticles[triangle->v1]; + b3Particle* p3 = m_vertexParticles[triangle->v2]; + b3Particle* p4 = m_vertexParticles[triangle->v3]; + + if (p1 == p2 || p1 == p3 || p1 == p4) + { + continue; + } + + float32 r1 = p1->m_radius; + float32 r2 = 0.0f; + + float32 totalRadius = r1 + r2; + + b3Vec3 A = p2->m_position; + b3Vec3 B = p3->m_position; + b3Vec3 C = p4->m_position; + + b3Vec3 P = p1->m_position; + + b3Vec3 n = b3Cross(B - A, C - A); + float32 n_len = n.Normalize(); + + float32 distance = b3Dot(n, P - A); + + if (distance < -totalRadius) + { + continue; + } + + if (distance > totalRadius) + { + continue; + } + + // Project particle on triangle plane + b3Vec3 Q = P - distance * n; + + // Barycentric coordinates for Q + float32 wABC[3]; + b3Solve3(wABC, A, B, C, Q); + + b3Vec3 c1 = p1->m_position; + b3Vec3 c2 = wABC[0] * A + wABC[1] * B + wABC[2] * C; + + if (b3DistanceSquared(c1, c2) > totalRadius * totalRadius) + { + continue; + } + + bool front = false; + + // Is the the other point in front of the triangle plane? + if (distance >= 0.0f) + { + front = true; + } + + b3TriangleContact* c = (b3TriangleContact*)m_triangleContactBlocks.Allocate(); + c->p1 = p1; + c->p2 = p2; + c->p3 = p3; + c->p4 = p4; + c->front = front; + c->normalImpulse = 0.0f; + + m_triangleContactList.PushFront(c); + } + } +} + void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) { B3_PROFILE("Solve"); @@ -684,6 +919,11 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) solver.Add(pc); } + for (b3TriangleContact* tc = m_triangleContactList.m_head; tc; tc = tc->m_next) + { + solver.Add(tc); + } + // Solve solver.Solve(dt, gravity); @@ -704,6 +944,9 @@ void b3Cloth::Step(float32 dt, const b3Vec3& gravity) // Update particle contacts UpdateParticleContacts(); + + // Update triangle contacts + UpdateTriangleContacts(); // 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 6cf2ba8..e0f1f25 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -63,10 +63,20 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) 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_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)); } 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); @@ -101,6 +111,11 @@ void b3ClothSolver::Add(b3ParticleContact* c) m_particleContacts[m_particleContactCount++] = c; } +void b3ClothSolver::Add(b3TriangleContact* c) +{ + m_triangleContacts[m_triangleContactCount++] = c; +} + void b3ClothSolver::ApplyForces() { for (u32 i = 0; i < m_forceCount; ++i) @@ -263,6 +278,9 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Initialize particle contact constraints InitializeParticleContactConstraints(); + // Initialize triangle contact constraints + // InitializeTriangleContactConstraints(); + // Warm start velocity constraints WarmStart(); @@ -273,6 +291,7 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) { SolveBodyContactVelocityConstraints(); SolveParticleContactVelocityConstraints(); + // SolveTriangleContactVelocityConstraints(); } // Store impulses to improve convergence @@ -283,11 +302,13 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) // Solve position constraints const u32 kPositionIterations = 2; - + bool positionSolved = false; for (u32 i = 0; i < kPositionIterations; ++i) { bool particleContactsSolved = SolveParticleContactPositionConstraints(); + // bool triangleContactsSolved = SolveTriangleContactPositionConstraints(); + //if (particleContactsSolved && triangleContactsSolved) if (particleContactsSolved) { positionSolved = true; @@ -559,6 +580,12 @@ void b3ClothSolver::InitializeBodyContactConstraints() } } +// +static B3_FORCE_INLINE float32 b3MixFriction(float32 u1, float32 u2) +{ + return b3Sqrt(u1 * u2); +} + void b3ClothSolver::InitializeParticleContactConstraints() { b3DenseVec3& x = *m_solverData.x; @@ -578,7 +605,7 @@ void b3ClothSolver::InitializeParticleContactConstraints() 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; + vc->friction = b3MixFriction(c->p1->m_friction, c->p2->m_friction); pc->indexA = c->p1->m_solverId; pc->indexB = c->p2->m_solverId; @@ -624,7 +651,7 @@ void b3ClothSolver::InitializeParticleContactConstraints() float32 K = mA + mB; vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f; - + vc->velocityBias = 0.0f; } @@ -645,6 +672,93 @@ void b3ClothSolver::InitializeParticleContactConstraints() } } +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; @@ -718,6 +832,41 @@ void b3ClothSolver::WarmStart() 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() @@ -879,6 +1028,59 @@ void b3ClothSolver::SolveParticleContactVelocityConstraints() } } +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) @@ -898,6 +1100,14 @@ void b3ClothSolver::StoreImpulses() 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 b3ClothSolverContactSolverPoint @@ -995,14 +1205,8 @@ bool b3ClothSolver::SolveBodyContactPositionConstraints() struct b3ClothSolverParticleContactSolverPoint { - void Initialize(const b3Particle* p1, const b3Particle* p2) + void Initialize(const b3Vec3& cA, float32 rA, const b3Vec3& cB, float32 rB) { - b3Vec3 cA = p1->GetPosition(); - float32 rA = p1->GetRadius(); - - b3Vec3 cB = p2->GetPosition(); - float32 rB = p2->GetRadius(); - b3Vec3 d = cB - cA; float32 distance = b3Length(d); @@ -1037,15 +1241,17 @@ bool b3ClothSolver::SolveParticleContactPositionConstraints() 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(m_particles[indexA], m_particles[indexB]); + cpcp.Initialize(xA, rA, xB, rB); b3Vec3 normal = cpcp.normal; b3Vec3 point = cpcp.point; @@ -1071,5 +1277,92 @@ bool b3ClothSolver::SolveParticleContactPositionConstraints() 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); + + // Compute effective mass. + float32 K = mA + mB + mC + mD; + + 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 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 4df5fc6..ffe58eb 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -73,6 +73,7 @@ b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth) m_mass = 0.0f; m_invMass = 0.0f; m_radius = def.radius; + m_friction = def.friction; m_userData = nullptr; m_x.SetZero(); m_vertex = ~0;