use baumgarte for correcting positions

include/bounce/dynamics/body.h

@@ -293,6 +293,7 @@ private:
 	friend class b3MeshContact;
 	friend class b3ContactManager;
 	friend class b3ContactSolver;
+	friend class b3ClothSolver;
 
 	friend class b3Joint;
 	friend class b3JointManager;

include/bounce/dynamics/cloth/cloth_solver.h

@@ -64,7 +64,7 @@ struct b3AccelerationConstraint
 	void Apply(const b3ClothSolverData* data);
 };
 
-struct b3ClothContactVelocityConstraint
+struct b3ClothSolverContactVelocityConstraint
 {
 	u32 indexA;
 	float32 invMassA;
@@ -94,6 +94,28 @@ struct b3ClothContactVelocityConstraint
 	float32 motorImpulse;
 };
 
+struct b3ClothSolverContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+	float32 radiusA;
+	b3Vec3 localCenterA;
+	
+	b3Body* bodyB;
+	b3Vec3 localCenterB;
+	float32 invMassB;
+	b3Mat33 invIB;
+	float32 radiusB;
+
+	b3Vec3 rA;
+	b3Vec3 rB;
+
+	b3Vec3 localNormalA;
+	b3Vec3 localPointA;
+	b3Vec3 localPointB;
+};
+
 class b3ClothSolver
 {
 public:
@@ -116,7 +138,7 @@ private:
 	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseSymMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
 
 	// 
-	void InitializeVelocityConstraints();
+	void InitializeConstraints();
 	
 	//
 	void WarmStart();
@@ -127,6 +149,9 @@ private:
 	// 
 	void StoreImpulses();
 
+	// 
+	bool SolvePositionConstraints();
+	
 	b3StackAllocator* m_allocator;
 
 	u32 m_particleCapacity;
@@ -137,15 +162,16 @@ private:
 	u32 m_forceCount;
 	b3Force** m_forces;
 	
-	u32 m_contactCapacity;
-	u32 m_contactCount;
-	b3BodyContact** m_contacts;
-
 	u32 m_constraintCapacity;
 	u32 m_constraintCount;
 	b3AccelerationConstraint* m_constraints;
 
-	b3ClothContactVelocityConstraint* m_velocityConstraints;
+	u32 m_contactCapacity;
+	u32 m_contactCount;
+	b3BodyContact** m_contacts;
+
+	b3ClothSolverContactVelocityConstraint* m_velocityConstraints;
+	b3ClothSolverContactPositionConstraint* m_positionConstraints;
 
 	b3ClothSolverData m_solverData;
 };

include/bounce/dynamics/cloth/particle.h

@@ -82,8 +82,9 @@ public:
 	b3Shape* s2;
 
 	// Contact constraint
-	b3Vec3 p;
+	b3Vec3 localNormal1;
-	b3Vec3 n;
+	b3Vec3 localPoint1;
+	b3Vec3 localPoint2;
 	float32 normalImpulse;
 
 	// Friction constraint
@@ -97,6 +98,15 @@ public:
 	bool active;
 };
 
+struct b3BodyContactWorldPoint
+{
+	void Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB);
+
+	b3Vec3 point;
+	b3Vec3 normal;
+	float32 separation;
+};
+
 // A cloth particle.
 class b3Particle
 {

src/bounce/dynamics/cloth/cloth.cpp

@@ -556,17 +556,20 @@ void b3Cloth::UpdateContacts()
 		}
 
 		b3Shape* shape = bestShape;
+		b3Body* body = shape->GetBody();
 		float32 s = bestSeparation;
 		b3Vec3 n = bestNormal;
-		b3Vec3 cp = p->m_position - s * n;
+		b3Vec3 cp2r = p->m_position - s * n;
+		b3Vec3 cp2 = cp2r - shape->m_radius * n;
 
-		// Store the contact manifold
+		// Store the local contact manifold
-		// Here the normal points from shape 2 to the particle
+		// Ensure the the normal points from the particle 1 to shape 2
 		c->active = true;
 		c->p1 = p;
 		c->s2 = shape;
-		c->p = cp;
+		c->localNormal1 = -n;
-		c->n = n;
+		c->localPoint1.SetZero();
+		c->localPoint2 = body->GetLocalPoint(cp2);
 		c->t1 = b3Perp(n);
 		c->t2 = b3Cross(c->t1, n);
 		c->normalImpulse = 0.0f;
@@ -590,7 +593,7 @@ void b3Cloth::Solve(float32 dt, const b3Vec3& gravity)
 	b3ClothSolverDef solverDef;
 	solverDef.stack = &m_world->m_stackAllocator;
 	solverDef.particleCapacity = m_particleList.m_count;
-	solverDef.forceCapacity = m_forceList.m_count + (1 * m_particleList.m_count);
+	solverDef.forceCapacity = m_forceList.m_count;
 	solverDef.contactCapacity = m_particleList.m_count;
 
 	b3ClothSolver solver(solverDef);
@@ -661,7 +664,19 @@ void b3Cloth::Draw() const
 
 		if (c->active)
 		{
-			b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow);
+			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);
 		}
 	}
 

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -48,22 +48,24 @@ b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
 	m_forceCount = 0;
 	m_forces = (b3Force**)m_allocator->Allocate(m_forceCapacity * sizeof(b3Force*));;
 
-	m_contactCapacity = def.contactCapacity;
-	m_contactCount = 0;
-	m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
-
 	m_constraintCapacity = def.particleCapacity;
 	m_constraintCount = 0;
 	m_constraints = (b3AccelerationConstraint*)m_allocator->Allocate(m_constraintCapacity * sizeof(b3AccelerationConstraint));
 
-	m_velocityConstraints = (b3ClothContactVelocityConstraint*)m_allocator->Allocate(m_contactCount * sizeof(b3ClothContactVelocityConstraint));
+	m_contactCapacity = def.contactCapacity;
+	m_contactCount = 0;
+	m_contacts = (b3BodyContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3BodyContact*));
+
+	m_velocityConstraints = (b3ClothSolverContactVelocityConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactVelocityConstraint));
+	m_positionConstraints = (b3ClothSolverContactPositionConstraint*)m_allocator->Allocate(m_contactCapacity * sizeof(b3ClothSolverContactPositionConstraint));
 }
 
 b3ClothSolver::~b3ClothSolver()
 {
+	m_allocator->Free(m_positionConstraints);
 	m_allocator->Free(m_velocityConstraints);
-	m_allocator->Free(m_constraints);
 	m_allocator->Free(m_contacts);
+	m_allocator->Free(m_constraints);
 	m_allocator->Free(m_forces);
 	m_allocator->Free(m_particles);
 }
@@ -201,19 +203,6 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		sx0[i] = p->m_x;
 	}
 
-	// Apply internal translations
-	for (u32 i = 0; i < m_contactCount; ++i)
-	{
-		b3BodyContact* c = m_contacts[i];
-		b3Particle* p = c->p1;
-
-		if (p->m_type == e_dynamicParticle)
-		{
-			b3Vec3 dx = c->p - p->m_position;
-			sy[p->m_solverId] += dx;
-		}
-	}
-
 	// Integrate velocities
 
 	// Apply internal forces
@@ -245,9 +234,16 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	b3_clothSolverIterations = iterations;
 
 	sv = sv + x;
+	sx = sx + sy;
 
-	// Initialize velocity constraints
+	// Store delta velocities to improve convergence
-	InitializeVelocityConstraints();
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i]->m_x = x[i];
+	}
+
+	// Initialize constraints
+	InitializeConstraints();
 
 	// Warm start velocity constraints
 	WarmStart();
@@ -259,8 +255,24 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 		SolveVelocityConstraints();
 	}
 
+	// Store impulses to improve convergence
+	StoreImpulses();
+
 	// Integrate positions
-	sx = sx + h * sv + sy;
+	sx = sx + h * sv;
+
+#if 0
+	// Solve position constraints
+	const u32 kPositionIterations = 0;
+	for (u32 i = 0; i < kPositionIterations; ++i)
+	{
+		bool contactsSolved = SolvePositionConstraints();
+		if (contactsSolved)
+		{
+			break;
+		}
+	}
+#endif
 
 	// Copy state buffers back to the particles
 	for (u32 i = 0; i < m_particleCount; ++i)
@@ -269,9 +281,18 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 
 		p->m_position = sx[i];
 		p->m_velocity = sv[i];
+	}
 
-		// Cache x to improve convergence
+	// Synchronize bodies
-		p->m_x = x[i];
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3Body* body = m_contacts[i]->s2->GetBody();
+
+		body->SynchronizeTransform();
+
+		body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
+		
+		body->SynchronizeShapes();
 	}
 }
 
@@ -383,15 +404,18 @@ void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
 	iterations = iteration;
 }
 
-void b3ClothSolver::InitializeVelocityConstraints()
+void b3ClothSolver::InitializeConstraints()
 {
 	b3DenseVec3& x = *m_solverData.x;
 	b3DenseVec3& v = *m_solverData.v;
 
+	float32 inv_dt = m_solverData.invdt;
+
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
-		b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
 
 		vc->indexA = c->p1->m_solverId;
 		vc->bodyB = c->s2->GetBody();
@@ -403,12 +427,32 @@ void b3ClothSolver::InitializeVelocityConstraints()
 		vc->invIB = vc->bodyB->GetWorldInverseInertia();
 
 		vc->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->GetInverseMass();
+
+		pc->invIA.SetZero();
+		pc->invIB = vc->bodyB->GetWorldInverseInertia();
+		
+		pc->radiusA = c->p1->m_radius;
+		pc->radiusB = c->s2->m_radius;
+
+		pc->localCenterA.SetZero();
+		pc->localCenterB = pc->bodyB->m_sweep.localCenter;
+
+		pc->localNormalA = c->localNormal1;
+		pc->localPointA = c->localPoint1;
+		pc->localPointB = c->localPoint2;
 	}
 
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
-		b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + i;
 
 		u32 indexA = vc->indexA;
 		b3Body* bodyB = vc->bodyB;
@@ -420,15 +464,31 @@ void b3ClothSolver::InitializeVelocityConstraints()
 		b3Mat33 iB = vc->invIB;
 
 		b3Vec3 xA = x[indexA];
-		b3Vec3 xB = bodyB->GetPosition();
+		b3Vec3 xB = bodyB->m_sweep.worldCenter;
 
-		// Ensure the normal points from shape 1 to the shape 2
+		b3Quat qA; qA.SetIdentity();
-		vc->normal = -c->n;
+		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 = c->p;
+		vc->point = wp.point;
 
-		b3Vec3 point = c->p;
+		b3Vec3 point = vc->point;
 
 		b3Vec3 rA = point - xA;
 		b3Vec3 rB = point - xB;
@@ -449,6 +509,10 @@ void b3ClothSolver::InitializeVelocityConstraints()
 			float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
 
 			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;
 		}
 
 		{
@@ -486,7 +550,7 @@ void b3ClothSolver::WarmStart()
 
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
-		b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
 
 		u32 indexA = vc->indexA;
 		b3Body* bodyB = vc->bodyB;
@@ -534,7 +598,7 @@ void b3ClothSolver::SolveVelocityConstraints()
 
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
-		b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
 
 		u32 indexA = vc->indexA;
 		b3Body* bodyB = vc->bodyB;
@@ -551,7 +615,6 @@ void b3ClothSolver::SolveVelocityConstraints()
 		b3Mat33 iA = vc->invIA;
 		b3Mat33 iB = vc->invIB;
 
-		// Ensure the normal points from shape 1 to the shape 2
 		b3Vec3 normal = vc->normal;
 		b3Vec3 point = vc->point;
 
@@ -570,7 +633,7 @@ void b3ClothSolver::SolveVelocityConstraints()
 			b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
 			float32 Cdot = b3Dot(normal, dv);
 
-			float32 impulse = -invK * Cdot;
+			float32 impulse = invK * (-Cdot + vc->velocityBias);
 
 			float32 oldImpulse = vc->normalImpulse;
 			vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
@@ -644,10 +707,103 @@ void b3ClothSolver::StoreImpulses()
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
-		b3ClothContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ClothSolverContactVelocityConstraint* vc = m_velocityConstraints + i;
 
 		c->normalImpulse = vc->normalImpulse;
 		c->tangentImpulse = vc->tangentImpulse;
 		c->motorImpulse = vc->motorImpulse;
 	}
 }
+
+struct b3ClothSolverContactSolverPoint
+{
+	void Initialize(const b3ClothSolverContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
+	{
+		normal = b3Mul(xfA.rotation, pc->localNormalA);
+		b3Vec3 c1 = b3Mul(xfA, pc->localPointA);
+		b3Vec3 c2 = b3Mul(xfB, pc->localPointB);
+		point = c2;
+		separation = b3Dot(c2 - c1, normal) - pc->radiusA - pc->radiusB;
+	}
+
+	b3Vec3 normal;
+	b3Vec3 point;
+	float32 separation;
+};
+
+bool b3ClothSolver::SolvePositionConstraints()
+{
+	b3DenseVec3& x = *m_solverData.v;
+
+	float32 minSeparation = 0.0f;
+
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3ClothSolverContactPositionConstraint* pc = m_positionConstraints + 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);
+
+		b3ClothSolverContactSolverPoint 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;
+}

src/bounce/dynamics/cloth/particle.cpp

@@ -22,9 +22,18 @@
 #include <bounce/dynamics/cloth/dense_vec3.h>
 #include <bounce/dynamics/cloth/sparse_sym_mat33.h>
 
-void b3FrictionForce::Apply(const b3ClothSolverData* data)
+void b3BodyContactWorldPoint::Initialize(const b3BodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
 {
-	// TODO
+	b3Vec3 nA = xfA.rotation * c->localNormal1;
+	b3Vec3 cA = xfA * c->localPoint1;
+	b3Vec3 cB = xfB * c->localPoint2;
+
+	b3Vec3 pA = cA + rA * nA;
+	b3Vec3 pB = cB - rB * nA;
+
+	point = 0.5f * (pA + pB);
+	normal = nA;
+	separation = b3Dot(cB - cA, nA) - rA - rB;
 }
 
 b3Particle::b3Particle(const b3ParticleDef& def, b3Cloth* cloth)