diff --git a/include/bounce/dynamics/cloth/cloth_solver.h b/include/bounce/dynamics/cloth/cloth_solver.h index 198699c..06c2810 100644 --- a/include/bounce/dynamics/cloth/cloth_solver.h +++ b/include/bounce/dynamics/cloth/cloth_solver.h @@ -68,7 +68,7 @@ struct b3AccelerationConstraint void Apply(const b3ClothSolverData* data); }; -struct b3ClothSolverContactVelocityConstraint +struct b3ClothSolverBodyContactVelocityConstraint { u32 indexA; float32 invMassA; @@ -95,7 +95,7 @@ struct b3ClothSolverContactVelocityConstraint b3Vec2 tangentImpulse; }; -struct b3ClothSolverContactPositionConstraint +struct b3ClothSolverBodyContactPositionConstraint { u32 indexA; float32 invMassA; @@ -104,15 +104,14 @@ struct b3ClothSolverContactPositionConstraint b3Vec3 localCenterA; b3Body* bodyB; - b3Vec3 localCenterB; float32 invMassB; b3Mat33 invIB; float32 radiusB; + b3Vec3 localCenterB; b3Vec3 rA; b3Vec3 rB; - b3Vec3 localNormalA; b3Vec3 localPointA; b3Vec3 localPointB; }; @@ -167,7 +166,7 @@ struct b3ClothSolverTriangleContactVelocityConstraint b3Vec3 JB; b3Vec3 JC; b3Vec3 JD; - + float32 normalMass; float32 normalImpulse; }; @@ -262,8 +261,8 @@ private: u32 m_bodyContactCapacity; u32 m_bodyContactCount; b3BodyContact** m_bodyContacts; - b3ClothSolverContactVelocityConstraint* m_bodyVelocityConstraints; - b3ClothSolverContactPositionConstraint* m_bodyPositionConstraints; + b3ClothSolverBodyContactVelocityConstraint* m_bodyVelocityConstraints; + b3ClothSolverBodyContactPositionConstraint* m_bodyPositionConstraints; u32 m_particleContactCapacity; u32 m_particleContactCount; diff --git a/include/bounce/dynamics/cloth/particle.h b/include/bounce/dynamics/cloth/particle.h index ec68dd4..5bfdbbe 100644 --- a/include/bounce/dynamics/cloth/particle.h +++ b/include/bounce/dynamics/cloth/particle.h @@ -72,7 +72,6 @@ public: b3Shape* s2; // Contact constraint - b3Vec3 localNormal1; b3Vec3 localPoint1; b3Vec3 localPoint2; float32 normalImpulse; @@ -137,7 +136,7 @@ public: b3Particle* p4; bool front; - + // Contact constraint float32 normalImpulse; diff --git a/src/bounce/dynamics/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp index 72d4e69..d768ab0 100644 --- a/src/bounce/dynamics/cloth/cloth.cpp +++ b/src/bounce/dynamics/cloth/cloth.cpp @@ -569,7 +569,6 @@ void b3Cloth::UpdateBodyContacts() c->active = true; c->p1 = p; c->s2 = shape; - c->localNormal1 = n; c->localPoint1.SetZero(); c->localPoint2 = body->GetLocalPoint(cb); c->t1 = b3Perp(n); diff --git a/src/bounce/dynamics/cloth/cloth_solver.cpp b/src/bounce/dynamics/cloth/cloth_solver.cpp index 31f2860..912e353 100644 --- a/src/bounce/dynamics/cloth/cloth_solver.cpp +++ b/src/bounce/dynamics/cloth/cloth_solver.cpp @@ -56,8 +56,8 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def) 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_bodyVelocityConstraints = (b3ClothSolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverBodyContactVelocityConstraint)); + m_bodyPositionConstraints = (b3ClothSolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3ClothSolverBodyContactPositionConstraint)); m_particleContactCapacity = def.particleContactCapacity; m_particleContactCount = 0; @@ -462,8 +462,8 @@ void b3ClothSolver::InitializeBodyContactConstraints() for (u32 i = 0; i < m_bodyContactCount; ++i) { b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; vc->indexA = c->p1->m_solverId; vc->bodyB = c->s2->GetBody(); @@ -491,7 +491,6 @@ void b3ClothSolver::InitializeBodyContactConstraints() pc->localCenterA.SetZero(); pc->localCenterB = pc->bodyB->m_sweep.localCenter; - pc->localNormalA = c->localNormal1; pc->localPointA = c->localPoint1; pc->localPointB = c->localPoint2; } @@ -499,8 +498,8 @@ void b3ClothSolver::InitializeBodyContactConstraints() for (u32 i = 0; i < m_bodyContactCount; ++i) { b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; - b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -762,7 +761,7 @@ void b3ClothSolver::WarmStart() for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -872,7 +871,7 @@ void b3ClothSolver::SolveBodyContactVelocityConstraints() for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; u32 indexA = vc->indexA; b3Body* bodyB = vc->bodyB; @@ -1083,7 +1082,7 @@ void b3ClothSolver::StoreImpulses() for (u32 i = 0; i < m_bodyContactCount; ++i) { b3BodyContact* c = m_bodyContacts[i]; - b3ClothSolverContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; + b3ClothSolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i; c->normalImpulse = vc->normalImpulse; c->tangentImpulse = vc->tangentImpulse; @@ -1109,7 +1108,7 @@ void b3ClothSolver::StoreImpulses() struct b3ClothSolverBodyContactSolverPoint { - void Initialize(const b3ClothSolverContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) + void Initialize(const b3ClothSolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB) { b3Vec3 cA = b3Mul(xfA, pc->localPointA); b3Vec3 cB = b3Mul(xfB, pc->localPointB); @@ -1147,7 +1146,7 @@ bool b3ClothSolver::SolveBodyContactPositionConstraints() for (u32 i = 0; i < m_bodyContactCount; ++i) { - b3ClothSolverContactPositionConstraint* pc = m_bodyPositionConstraints + i; + b3ClothSolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i; u32 indexA = pc->indexA; float32 mA = pc->invMassA; diff --git a/src/bounce/dynamics/cloth/particle.cpp b/src/bounce/dynamics/cloth/particle.cpp index ffe58eb..0e8159c 100644 --- a/src/bounce/dynamics/cloth/particle.cpp +++ b/src/bounce/dynamics/cloth/particle.cpp @@ -24,16 +24,24 @@ void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB) { - b3Vec3 nA = xfA.rotation * c->localNormal1; - b3Vec3 cA = xfA * c->localPoint1; - b3Vec3 cB = xfB * c->localPoint2; + 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 = b3Dot(cB - cA, nA) - rA - rB; + separation = distance - rA - rB; } void b3ParticleContactWorldPoint::Initialize(const b3ParticleContact* c)