diff --git a/include/bounce/cloth/cloth.h b/include/bounce/cloth/cloth.h index e02e6a1..3c1a5b4 100644 --- a/include/bounce/cloth/cloth.h +++ b/include/bounce/cloth/cloth.h @@ -153,9 +153,6 @@ private: // Update body contacts. void UpdateBodyContacts(); - // Update particle contacts. - void UpdateParticleContacts(); - // Update contacts void UpdateContacts(); @@ -183,17 +180,11 @@ 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; }; inline void b3Cloth::SetGravity(const b3Vec3& gravity) diff --git a/include/bounce/cloth/cloth_contact_solver.h b/include/bounce/cloth/cloth_contact_solver.h index 011d3fb..1fe3070 100644 --- a/include/bounce/cloth/cloth_contact_solver.h +++ b/include/bounce/cloth/cloth_contact_solver.h @@ -28,7 +28,6 @@ class b3Particle; class b3Body; class b3BodyContact; -class b3ParticleContact; struct b3DenseVec3; @@ -76,44 +75,11 @@ struct b3ClothSolverBodyContactPositionConstraint b3Vec3 rA; b3Vec3 rB; + b3Vec3 normalA; 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 b3ClothContactSolverDef { b3StackAllocator* allocator; @@ -123,9 +89,6 @@ struct b3ClothContactSolverDef u32 bodyContactCount; b3BodyContact** bodyContacts; - - u32 particleContactCount; - b3ParticleContact** particleContacts; }; inline float32 b3MixFriction(float32 u1, float32 u2) @@ -141,19 +104,13 @@ public: void InitializeBodyContactConstraints(); - void InitializeParticleContactConstraints(); - void WarmStart(); void SolveBodyContactVelocityConstraints(); - void SolveParticleContactVelocityConstraints(); - void StoreImpulses(); bool SolveBodyContactPositionConstraints(); - - bool SolveParticleContactPositionConstraints(); protected: b3StackAllocator* m_allocator; @@ -164,11 +121,6 @@ protected: b3BodyContact** m_bodyContacts; b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; - - u32 m_particleContactCount; - b3ParticleContact** m_particleContacts; - b3ClothSolverParticleContactVelocityConstraint* m_particleVelocityConstraints; - b3ClothSolverParticleContactPositionConstraint* m_particlePositionConstraints; }; #endif \ No newline at end of file diff --git a/include/bounce/cloth/cloth_solver.h b/include/bounce/cloth/cloth_solver.h index 6b0b896..52b6e66 100644 --- a/include/bounce/cloth/cloth_solver.h +++ b/include/bounce/cloth/cloth_solver.h @@ -41,7 +41,6 @@ struct b3ClothSolverDef u32 particleCapacity; u32 forceCapacity; u32 bodyContactCapacity; - u32 particleContactCapacity; }; struct b3ClothSolverData diff --git a/include/bounce/cloth/particle.h b/include/bounce/cloth/particle.h index 91480c2..bec7da6 100644 --- a/include/bounce/cloth/particle.h +++ b/include/bounce/cloth/particle.h @@ -103,36 +103,6 @@ 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 { diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/cloth/cloth.cpp index efbfd10..2866748 100644 --- a/src/bounce/cloth/cloth.cpp +++ b/src/bounce/cloth/cloth.cpp @@ -147,8 +147,7 @@ static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m) } b3Cloth::b3Cloth(const b3ClothDef& def) : - m_particleBlocks(sizeof(b3Particle)), - m_particleContactBlocks(sizeof(b3ParticleContact)) + m_particleBlocks(sizeof(b3Particle)) { B3_ASSERT(def.mesh); B3_ASSERT(def.density > 0.0f); @@ -652,66 +651,6 @@ void b3Cloth::UpdateBodyContacts() } } -void b3Cloth::UpdateParticleContacts() -{ - B3_PROFILE("Cloth 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("Cloth Solve"); @@ -722,7 +661,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) solverDef.particleCapacity = m_particleList.m_count; solverDef.forceCapacity = m_forceList.m_count; solverDef.bodyContactCapacity = m_particleList.m_count; - solverDef.particleContactCapacity = m_particleContactList.m_count; b3ClothSolver solver(solverDef); @@ -744,11 +682,6 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity) } } - for (b3ParticleContact* c = m_particleContactList.m_head; c; c = c->m_next) - { - solver.Add(c); - } - // Solve solver.Solve(dt, gravity); } diff --git a/src/bounce/cloth/cloth_contact_solver.cpp b/src/bounce/cloth/cloth_contact_solver.cpp index 5052198..ac97dfc 100644 --- a/src/bounce/cloth/cloth_contact_solver.cpp +++ b/src/bounce/cloth/cloth_contact_solver.cpp @@ -34,18 +34,10 @@ b3ClothContactSolver::b3ClothContactSolver(const b3ClothContactSolverDef& def) 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)); } b3ClothContactSolver::~b3ClothContactSolver() { - m_allocator->Free(m_particlePositionConstraints); - m_allocator->Free(m_particleVelocityConstraints); - m_allocator->Free(m_bodyPositionConstraints); m_allocator->Free(m_bodyVelocityConstraints); } @@ -87,6 +79,7 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() pc->localCenterA.SetZero(); pc->localCenterB = pc->bodyB->m_sweep.localCenter; + pc->normalA = c->n; pc->localPointA = c->localPoint1; pc->localPointB = c->localPoint2; } @@ -178,90 +171,6 @@ void b3ClothContactSolver::InitializeBodyContactConstraints() } } -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::WarmStart() { b3DenseVec3& v = *m_velocities; @@ -307,34 +216,6 @@ void b3ClothContactSolver::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 b3ClothContactSolver::SolveBodyContactVelocityConstraints() @@ -425,77 +306,6 @@ void b3ClothContactSolver::SolveBodyContactVelocityConstraints() } } -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::StoreImpulses() { for (u32 i = 0; i < m_bodyContactCount; ++i) @@ -506,15 +316,6 @@ void b3ClothContactSolver::StoreImpulses() 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; - } } struct b3ClothSolverBodyContactSolverPoint @@ -526,22 +327,15 @@ struct b3ClothSolverBodyContactSolverPoint 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 nA = pc->normalA; b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA; - point = 0.5f * (pA + pB); + point = cB; normal = nA; - separation = distance - rA - rB; + separation = b3Dot(cB - cA, nA) - rA - rB; } b3Vec3 normal; @@ -623,82 +417,5 @@ bool b3ClothContactSolver::SolveBodyContactPositionConstraints() 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; } \ No newline at end of file diff --git a/src/bounce/cloth/cloth_solver.cpp b/src/bounce/cloth/cloth_solver.cpp index 46a7da7..1fc7718 100644 --- a/src/bounce/cloth/cloth_solver.cpp +++ b/src/bounce/cloth/cloth_solver.cpp @@ -57,15 +57,10 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) m_bodyContactCapacity = def.bodyContactCapacity; m_bodyContactCount = 0; 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*));; } b3ClothSolver::~b3ClothSolver() { - m_allocator->Free(m_particleContacts); m_allocator->Free(m_bodyContacts); m_allocator->Free(m_constraints); @@ -329,14 +324,11 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) contactSolverDef.velocities = m_solverData.v; contactSolverDef.bodyContactCount = m_bodyContactCount; contactSolverDef.bodyContacts = m_bodyContacts; - contactSolverDef.particleContactCount = m_particleContactCount; - contactSolverDef.particleContacts = m_particleContacts; b3ClothContactSolver contactSolver(contactSolverDef); { contactSolver.InitializeBodyContactConstraints(); - contactSolver.InitializeParticleContactConstraints(); } { @@ -350,7 +342,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) for (u32 i = 0; i < kVelocityIterations; ++i) { contactSolver.SolveBodyContactVelocityConstraints(); - contactSolver.SolveParticleContactVelocityConstraints(); } } @@ -371,9 +362,8 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity) for (u32 i = 0; i < kPositionIterations; ++i) { bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints(); - bool particleContactsSolved = contactSolver.SolveParticleContactPositionConstraints(); - if (bodyContactsSolved && particleContactsSolved) + if (bodyContactsSolved) { positionSolved = true; break; diff --git a/src/bounce/cloth/particle.cpp b/src/bounce/cloth/particle.cpp index 7dde875..1b3fa6f 100644 --- a/src/bounce/cloth/particle.cpp +++ b/src/bounce/cloth/particle.cpp @@ -27,46 +27,14 @@ void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, con b3Vec3 cA = b3Mul(xfA, c->localPoint1); b3Vec3 cB = b3Mul(xfB, c->localPoint2); - 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; -} - -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 nA = c->n; b3Vec3 pA = cA + rA * nA; b3Vec3 pB = cB - rB * nA; point = 0.5f * (pA + pB); normal = nA; - separation = distance - rA - rB; + separation = b3Dot(cB - cA, nA) - rA - rB; } b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)