bilateral contact response for dynamic particles and rigid bodies, test update

examples/testbed/tests/pinned_cloth.h

@@ -40,21 +40,48 @@ public:
 		b3ClothDef def;
 		def.mesh = &m_rectangleClothMesh;
 		def.density = 0.2f;
-		def.structural = 10000.0f;
+		def.structural = 100000.0f;
 		def.damping = 0.0f;
 
 		m_cloth = m_world.CreateCloth(def);
 
-		b3AABB3 aabb;
-		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
-		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
+		b3AABB3 aabb1;
+		aabb1.m_lower.Set(-5.0f, -1.0f, -6.0f);
+		aabb1.m_upper.Set(5.0f, 1.0f, -4.0f);
+
+		b3AABB3 aabb2;
+		aabb2.m_lower.Set(-5.0f, -1.0f, 4.0f);
+		aabb2.m_upper.Set(5.0f, 1.0f, 6.0f);
 
 		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
 		{
-			if (aabb.Contains(p->GetPosition()))
+			if (aabb1.Contains(p->GetPosition()))
 			{
 				p->SetType(e_staticParticle);
 			}
+
+			if (aabb2.Contains(p->GetPosition()))
+			{
+				p->SetType(e_staticParticle);
+			}
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(0.0f, 5.0f, 0.0f);
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			b3SphereShape sphere;
+			sphere.m_center.SetZero();
+			sphere.m_radius = 2.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &sphere;
+			sd.density = 1.0f;
+
+			b3Shape* s = b->CreateShape(sd);
 		}
 	}
 

include/bounce/dynamics/cloth/particle.h

@@ -65,6 +65,8 @@ public:
 	b3FrictionForce() { }
 	~b3FrictionForce() { }
 	
+	void SolvePositionConstraints(const b3ClothSolverData* data);
+	
 	void Apply(const b3ClothSolverData* data);
 
 	b3Particle* m_p;
@@ -79,14 +81,21 @@ public:
 
 	b3Particle* p1;
 	b3Shape* s2;
-	float32 s;
 
+	// Contact constraint
+	bool n_active;
+	b3Vec3 p;
+	b3Vec3 n;
+	float32 Fn;
+
+	// Friction constraint
+	bool t1_active, t2_active;
+	b3Vec3 t1, t2;
+	float32 Ft1, Ft2;
+
+	// Friction force
 	bool f_active;
 	b3FrictionForce f;
-	
-	bool n_active, t1_active, t2_active;
-	b3Vec3 n, t1, t2;
-	float32 Fn, Ft1, Ft2;
 };
 
 // A cloth particle.

src/bounce/dynamics/cloth/cloth.cpp

@@ -509,9 +509,9 @@ void b3Cloth::UpdateContacts()
 	// Create contacts 
 	for (b3Particle* p = m_particleList.m_head; p; p = p->m_next)
 	{
-		// Static and kinematic particles can't participate in unilateral collisions.
-		if (p->m_type != e_dynamicParticle)
+		if (p->m_type == e_staticParticle)
 		{
+			// TODO
 			continue;
 		}
 
@@ -537,8 +537,9 @@ void b3Cloth::UpdateContacts()
 
 		for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext())
 		{
-			if (body->GetType() != e_staticBody)
+			if (p->m_type != e_dynamicParticle && body->GetType() != e_dynamicBody)
 			{
+				// At least one body should be kinematic or dynamic.
 				continue;
 			}
 
@@ -563,13 +564,14 @@ void b3Cloth::UpdateContacts()
 			b3Shape* shape = bestShape;
 			float32 s = bestSeparation;
 			b3Vec3 n = bestNormal;
+			b3Vec3 cp = p->m_position - s * n;
 
 			// Store the contact manifold
-			// Here the normal points from shape 2 to shape 1 (mass)
-			c->n_active = true;
+			// Here the normal points from shape 2 to the particle
 			c->p1 = p;
 			c->s2 = shape;
-			c->s = s;
+			c->n_active = true;
+			c->p = cp;
 			c->n = n;
 			c->t1 = b3Perp(n);
 			c->t2 = b3Cross(c->t1, n);
@@ -766,9 +768,11 @@ void b3Cloth::Draw() const
 			b3Draw_draw->DrawPoint(p->m_position, 4.0f, b3Color_green);
 		}
 
-		if (p->m_contact.n_active)
+		b3BodyContact* c = &p->m_contact;
+		
+		if (c->n_active)
 		{
-			b3Draw_draw->DrawSegment(p->m_position, p->m_position + p->m_contact.n, b3Color_yellow);
+			b3Draw_draw->DrawSegment(c->p, c->p + c->n, b3Color_yellow);
 		}
 	}
 

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -23,6 +23,8 @@
 #include <bounce/dynamics/cloth/dense_vec3.h>
 #include <bounce/dynamics/cloth/diag_mat33.h>
 #include <bounce/dynamics/cloth/sparse_sym_mat33.h>
+#include <bounce/dynamics/shapes/shape.h>
+#include <bounce/dynamics/body.h>
 #include <bounce/common/memory/stack_allocator.h>
 
 // Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
@@ -149,6 +151,11 @@ void b3ClothSolver::ApplyConstraints()
 		b3BodyContact* pc = m_contacts[i];
 		b3Particle* p = pc->p1;
 
+		if (p->m_type != e_dynamicParticle)
+		{
+			continue;
+		}
+
 		b3AccelerationConstraint* ac = m_constraints + m_constraintCount;
 		++m_constraintCount;
 		ac->i1 = p->m_solverId;
@@ -230,7 +237,10 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	{
 		b3BodyContact* c = m_contacts[i];
 		b3Particle* p = c->p1;
-		sy[p->m_solverId] -= c->s * c->n;
+
+		b3Vec3 dx = c->p - p->m_position;
+
+		sy[p->m_solverId] += dx;
 	}
 
 	// Apply internal forces
@@ -268,33 +278,47 @@ void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
 	// Copy state buffers back to the particles
 	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		m_particles[i]->m_position = sx[i];
-		m_particles[i]->m_velocity = sv[i];
+		b3Particle* p = m_particles[i];
+
+		p->m_position = sx[i];
+		p->m_velocity = sv[i];
 
 		// Cache x to improve convergence
-		m_particles[i]->m_x = x[i];
+		p->m_x = x[i];
 	}
 
-	// Store the extra contact constraint forces that should have been 
-	// supplied to enforce the contact constraints exactly.
-	// These forces can be used in contact constraint logic.
-
 	// f = A * x - b
 	b3DenseVec3 f = A * x - b;
 
 	for (u32 i = 0; i < m_contactCount; ++i)
 	{
 		b3BodyContact* c = m_contacts[i];
-		b3Particle* p = c->p1;
 
-		b3Vec3 force = f[p->m_solverId];
+		b3Particle* p1 = c->p1;
+		b3Body* b2 = c->s2->GetBody();
+
+		b3Vec3 f1 = f[p1->m_solverId];
+		b3Vec3 f2 = -f1;
+
+		// Apply constraint reaction force at the contact point on the body 
+		b2->ApplyForce(f2, c->p, true);
+
+		// Store constraint force acted on the particle
 
 		// Signed normal force magnitude
-		c->Fn = b3Dot(force, c->n);
+		c->Fn = b3Dot(f1, c->n);
 
+		if (c->t1_active)
+		{
 			// Signed tangent force magnitude
-		c->Ft1 = b3Dot(force, c->t1);
-		c->Ft2 = b3Dot(force, c->t2);
+			c->Ft1 = b3Dot(f1, c->t1);
+		}
+
+		if (c->t2_active)
+		{
+			// Signed tangent force magnitude
+			c->Ft2 = b3Dot(f1, c->t2);
+		}
 	}
 }
 

src/bounce/dynamics/cloth/particle.cpp

@@ -22,6 +22,11 @@
 #include <bounce/dynamics/cloth/dense_vec3.h>
 #include <bounce/dynamics/cloth/sparse_sym_mat33.h>
 
+void b3FrictionForce::SolvePositionConstraints(const b3ClothSolverData* data)
+{
+	B3_NOT_USED(data);
+}
+
 void b3FrictionForce::Apply(const b3ClothSolverData* data)
 {
 	// TODO