add collision (no friction)

examples/testbed/framework/test_entries.cpp

@@ -58,11 +58,12 @@
 #include <testbed/tests/tumbler.h>
 #include <testbed/tests/single_pendulum.h>
 #include <testbed/tests/multiple_pendulum.h>
-#include <testbed/tests/rope_test.h>
 #include <testbed/tests/cloth_test.h>
-#include <testbed/tests/implicit_spring.h>
 #include <testbed/tests/spring_cloth_test.h>
+#include <testbed/tests/spring_cloth_collision_test.h>
+#include <testbed/tests/rope_test.h>
 //#include <testbed/tests/tree_test.h>
+#include <testbed/tests/implicit_spring.h>
 
 TestEntry g_tests[] =
 {
@@ -106,10 +107,11 @@ TestEntry g_tests[] =
 	{ "Initial Overlap", &InitialOverlap::Create },
 	{ "Single Pendulum", &SinglePendulum::Create },
 	{ "Multiple Pendulum", &MultiplePendulum::Create },
-	{ "Rope", &Rope::Create },
-	{ "Implicit Spring", &ImplicitSpring::Create },
 	{ "Cloth", &Cloth::Create },
 	{ "Spring Cloth", &SpringCloth::Create },
+	{ "Spring Cloth Collision", &SpringClothCollision::Create },
+	{ "Rope", &Rope::Create },
 	//{ "Tree", &Tree::Create },
+	{ "Implicit Spring", &ImplicitSpring::Create },
 	{ NULL, NULL }
 };

examples/testbed/tests/spring_cloth_collision_test.h

@@ -0,0 +1,82 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPRING_CLOTH_COLLISION_TESH_H
+#define SPRING_CLOTH_COLLISION_TESH_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class SpringClothCollision : public Test
+{
+public:
+	SpringClothCollision()
+	{
+		g_camera.m_zoom = 25.0f;
+
+		b3SpringClothDef def;
+		def.allocator = &m_clothAllocator;
+		def.mesh = m_meshes + e_clothMesh;
+		def.density = 0.2f;
+		def.ks = 1000.0f;
+		def.kd = 10.0f;
+		def.r = 0.2f;
+		def.gravity.Set(0.0f, -10.0f, 0.0f);
+
+		m_cloth.Initialize(def);
+
+		b3Sphere* sphere = m_cloth.CreateSphere(b3Vec3(0.0f, -5.0f, 0.0f), 4.0f);
+	}
+
+	void Step()
+	{
+		float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+
+		if (g_settings.pause)
+		{
+			if (g_settings.singleStep)
+			{
+				g_settings.singleStep = false;
+			}
+			else
+			{
+				dt = 0.0f;
+			}
+		}
+
+		m_cloth.Step(dt);
+		m_cloth.Draw(g_debugDraw);
+
+		b3SpringClothStep step = m_cloth.GetStep();
+
+		char text[256];
+		sprintf(text, "Iterations = %u", step.iterations);
+		g_debugDraw->DrawString(text, b3Color_white);
+	}
+
+	static Test* Create()
+	{
+		return new SpringClothCollision();
+	}
+
+	b3StackAllocator m_clothAllocator;
+	b3SpringCloth m_cloth;
+};
+
+#endif

include/bounce/dynamics/cloth/spring_cloth.h

@@ -20,6 +20,9 @@
 #define B3_SPRING_CLOTH_H
 
 #include <bounce/common/math/mat33.h>
+#include <bounce/collision/shapes/sphere.h>
+
+#define B3_CLOTH_SPHERE_CAPACITY 32
 
 class b3StackAllocator;
 class b3Draw;
@@ -36,6 +39,7 @@ struct b3SpringClothDef
 		ks = 0.0f;
 		kb = 0.0f;
 		kd = 0.0f;
+		r = 0.05f;
 		gravity.SetZero();
 	}
 
@@ -57,7 +61,10 @@ struct b3SpringClothDef
 	// Damping stiffness
 	float32 kd;
 	
-	// Force due to gravity 
+	// Mass radius
+	float32 r;
+	
+	// Force due to gravity
 	b3Vec3 gravity;
 };
 
@@ -87,6 +94,15 @@ enum b3MassType
 	e_dynamicMass
 };
 
+// This structure represents an acceleration constraint.
+struct b3MassCollision
+{
+	u32 j;
+	float32 s;
+	b3Vec3 n;
+	bool active;
+};
+
 // Time step statistics
 struct b3SpringClothStep
 {
@@ -119,6 +135,9 @@ public:
 	//
 	b3MassType GetType(u32 i) const;
 
+	//
+	b3Sphere* CreateSphere(const b3Vec3& center, float32 radius);
+
 	// 
 	const b3SpringClothStep& GetStep() const;
 
@@ -131,6 +150,8 @@ public:
 	//
 	void Draw(b3Draw* draw) const;
 protected:
+	void UpdateCollisions() const;
+	
 	b3StackAllocator* m_allocator;
 
 	b3Mesh* m_mesh;
@@ -141,12 +162,19 @@ protected:
 	b3Vec3* m_v;
 	b3Vec3* m_f;
 	float32* m_inv_m;
+	b3Vec3* m_y;
 	b3MassType* m_massTypes;
+	b3MassCollision* m_collisions;
 	u32 m_massCount;
 
 	b3Spring* m_springs;
 	u32 m_springCount;
 
+	float32 m_r;
+
+	b3Sphere m_spheres[B3_CLOTH_SPHERE_CAPACITY];
+	u32 m_sphereCount;
+
 	b3SpringClothStep m_step;
 };
 

src/bounce/dynamics/cloth/spring_cloth.cpp

@@ -34,12 +34,18 @@ b3SpringCloth::b3SpringCloth()
 	m_x = nullptr;
 	m_v = nullptr;
 	m_f = nullptr;
+	m_y = nullptr;
 	m_inv_m = nullptr;
 	m_massTypes = nullptr;
+	m_collisions = nullptr;
 	m_massCount = 0;
 	
 	m_springs = nullptr;
-	m_springCount = 0;
+	m_springCount = 0; 
+	
+	m_r = 0.0f;
+
+	m_sphereCount = 0;
 
 	m_step.iterations = 0;
 }
@@ -50,7 +56,9 @@ b3SpringCloth::~b3SpringCloth()
 	b3Free(m_v);
 	b3Free(m_f);
 	b3Free(m_inv_m);
+	b3Free(m_y);
 	b3Free(m_massTypes);
+	b3Free(m_collisions);
 	b3Free(m_springs);
 }
 
@@ -63,6 +71,8 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def)
 	m_mesh = def.mesh;
 	m_gravity = def.gravity;
 
+	m_r = def.r;
+	
 	const b3Mesh* m = m_mesh;
 
 	m_massCount = m->vertexCount;
@@ -70,14 +80,18 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def)
 	m_v = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
 	m_f = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
 	m_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
+	m_y = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
 	m_massTypes = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType));
+	m_collisions = (b3MassCollision*)b3Alloc(m_massCount * sizeof(b3MassCollision));
 
 	for (u32 i = 0; i < m->vertexCount; ++i)
 	{
+		m_collisions[i].active = false;
 		m_x[i] = m->vertices[i];
 		m_v[i].SetZero();
 		m_f[i].SetZero();
 		m_inv_m[i] = 0.0f;
+		m_y[i].SetZero();
 		m_massTypes[i] = e_staticMass;
 	}
 
@@ -154,7 +168,24 @@ void b3SpringCloth::Initialize(const b3SpringClothDef& def)
 	}
 }
 
-static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, const b3MassType* types)
+b3Sphere* b3SpringCloth::CreateSphere(const b3Vec3& center, float32 radius)
+{
+	B3_ASSERT(m_sphereCount < B3_CLOTH_SPHERE_CAPACITY);
+	
+	if (m_sphereCount == B3_CLOTH_SPHERE_CAPACITY)
+	{
+		return nullptr;
+	}
+
+	b3Sphere* sphere = m_spheres + m_sphereCount;
+	sphere->vertex = center;
+	sphere->radius = radius;
+	++m_sphereCount;
+	return sphere;
+}
+
+static B3_FORCE_INLINE void b3Make_z(b3Vec3* out, u32 size,
+	const b3MassType* types, const b3MassCollision* collisions)
 {
 	for (u32 i = 0; i < size; ++i)
 	{
@@ -167,6 +198,47 @@ static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, con
 		}
 		case e_dynamicMass:
 		{
+			if (collisions[i].active)
+			{
+				out[i].SetZero();
+				break;
+			}
+
+			out[i].SetZero();
+			break;
+		}
+		default:
+		{
+			B3_ASSERT(false);
+			break;
+		}
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, 
+	const b3MassType* types, const b3MassCollision* collisions)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		switch (types[i])
+		{
+		case e_staticMass:
+		{
+			out[i].SetZero();
+			break;
+		}
+		case e_dynamicMass:
+		{
+			if (collisions[i].active)
+			{
+				b3Vec3 n = collisions[i].n;
+
+				b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
+
+				out[i] = S * v[i];
+				break;
+			}
 
 			out[i] = v[i];
 			break;
@@ -319,8 +391,90 @@ static B3_FORCE_INLINE void b3Mul_A(b3Vec3* out, const b3Vec3* v, u32 mass_size,
 	allocator->Free(v1);
 }
 
+void b3SpringCloth::UpdateCollisions() const
+{
+	// Compute cloth-solid collision position alteration
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		// Clear flag
+		m_collisions[i].active = false;
+
+		b3Vec3 c1 = m_x[i];
+		float32 r1 = m_r;
+
+		// Only solve the deepest penetrations
+		float32 bestSeparation = B3_MAX_FLOAT;
+		u32 bestIndex = ~0;
+
+		for (u32 j = 0; j < m_sphereCount; ++j)
+		{
+			const b3Sphere* sphere = m_spheres + j;
+			b3Vec3 c2 = sphere->vertex;
+			float32 r2 = sphere->radius;
+
+			b3Vec3 d = c2 - c1;
+			float32 dd = b3Dot(d, d);
+			float32 totalRadius = r1 + r2;
+			if (dd > totalRadius * totalRadius)
+			{
+				continue;
+			}
+
+			float32 distance = b3Length(d);
+			float32 separation = distance - totalRadius;
+
+			if (separation < bestSeparation)
+			{
+				bestSeparation = separation;
+				bestIndex = j;
+			}
+		}
+
+		if (bestIndex != ~0)
+		{
+			const b3Sphere* sphere = m_spheres + bestIndex;
+			b3Vec3 c2 = sphere->vertex;
+			float32 r2 = sphere->radius;
+
+			float32 totalRadius = r1 + r2;
+
+			b3Vec3 d = c2 - c1;
+			float32 distance = b3Length(d);
+			float32 separation = distance - totalRadius;
+
+			b3Vec3 n(0.0f, 1.0f, 0.0f);
+			if (distance > B3_EPSILON)
+			{
+				n = d / distance;
+			}
+
+			// Avoid large corrections
+			const float32 kMaxCorrection = 0.75f;
+			separation = b3Clamp(separation, -kMaxCorrection, 0.0f);
+
+			b3Vec3 dx1 = separation * n;
+
+			// Add position alteration
+			m_y[i] += dx1;
+
+			m_collisions[i].active = true;
+			m_collisions[i].j = bestIndex;
+			m_collisions[i].s = separation;
+			m_collisions[i].n = n;
+		}
+	}
+}
+
 void b3SpringCloth::Step(float32 h)
 {
+	if (h == 0.0f)
+	{
+		return;
+	}
+
+	// Detect and store collisions
+	UpdateCollisions();
+
 	u32 size = m_massCount;
 	b3MassType* types = m_massTypes;
 	u32 spring_size = m_springCount;
@@ -328,9 +482,12 @@ void b3SpringCloth::Step(float32 h)
 	// Add gravity
 	for (u32 i = 0; i < size; ++i)
 	{
-		m_f[i] += m_gravity;
+		if (types[i] == e_dynamicMass)
+		{
+			m_f[i] += m_gravity;
+		}
 	}
-	
+
 	// Compute non-zero Jacobians Jx, Jv
 	b3Mat33* Jx = (b3Mat33*)m_allocator->Allocate(spring_size * sizeof(b3Mat33));
 	b3SetZero_Jacobian(Jx, spring_size);
@@ -403,19 +560,28 @@ void b3SpringCloth::Step(float32 h)
 
 	// Compute b
 
-	// b = h * (f0 + h * dfdx * v0) 
+	// b = h * (f0 + h * dfdx * v0 + dfdx * y) ) 
 	b3Vec3* b = (b3Vec3*) m_allocator->Allocate(size * sizeof(b3Vec3));
 
-	// b = dfdx * v0
-	// b3Mul(b, dfdx, m_v, size);
-	b3Mul_Jacobian(b, m_v, size, Jx, m_springs, m_springCount);
+	// Jx_v = dfdx * v
+	b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+	// b3Mul(Jx_v, dfdx, v, size);
+	b3Mul_Jacobian(Jx_v, m_v, size, Jx, m_springs, m_springCount);
 
-	// b = h * (f0 + h * b)
+	// Jx_v0y = dfdx * y
+	b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+	// b3Mul(Jx_y, dfdx, y, size);
+	b3Mul_Jacobian(Jx_y, m_y, size, Jx, m_springs, m_springCount);
+
+	// b = h * (f0 + h * Jx_v + Jx_y )
 	for (u32 i = 0; i < size; ++i)
 	{
-		b[i] = h * (m_f[i] + h * b[i]);
+		b[i] = h * (m_f[i] + h * Jx_v[i] + Jx_y[i]);
 	}
 
+	m_allocator->Free(Jx_y);
+	m_allocator->Free(Jx_v);
+
 	// Solve Ax = b
 	b3Vec3* z = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
 
@@ -436,27 +602,7 @@ void b3SpringCloth::Step(float32 h)
 	b3Vec3* inv_P = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
 
 	// Compute z
-	for (u32 i = 0; i < size; ++i)
-	{
-		switch (types[i])
-		{
-		case e_staticMass:
-		{
-			z[i].SetZero();
-			break;
-		}
-		case e_dynamicMass:
-		{
-			z[i].SetZero();
-			break;
-		}
-		default:
-		{
-			B3_ASSERT(false);
-			break;
-		}
-		}
-	}
+	b3Make_z(z, size, types, m_collisions);
 
 	// dv = z
 	b3Copy(dv, z, size);
@@ -543,7 +689,7 @@ void b3SpringCloth::Step(float32 h)
 
 	// eps0 = dot( filter(b), P * filter(b) )
 	b3Vec3* filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
-	b3Filter(filter_b, b, size, types);
+	b3Filter(filter_b, b, size, types, m_collisions);
 
 	b3Vec3* P_filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
 	for (u32 i = 0; i < size; ++i)
@@ -560,7 +706,7 @@ void b3SpringCloth::Step(float32 h)
 
 	// r = filter(b - Adv)
 	b3Sub(r, b, Adv, size);
-	b3Filter(r, r, size, types);
+	b3Filter(r, r, size, types, m_collisions);
 
 	// c = filter(P^-1 * r)
 	for (u32 i = 0; i < m_massCount; ++i)
@@ -569,7 +715,7 @@ void b3SpringCloth::Step(float32 h)
 		c[i][1] = inv_P[i][1] * r[i][1];
 		c[i][2] = inv_P[i][2] * r[i][2];
 	}
-	b3Filter(c, c, size, types);
+	b3Filter(c, c, size, types, m_collisions);
 
 	// epsNew = dot(r, c)
 	float32 epsNew = b3Dot(r, c, size);
@@ -577,10 +723,10 @@ void b3SpringCloth::Step(float32 h)
 	// This is in [0, 1]
 	// Making it smaller can increase accuracy, but it might increase the number 
 	// of iterations to be taken by the solver.
-	const float32 kTol = 0.75f;
+	const float32 kTol = 0.25f;
 
 	// Limit number of iterations to prevent cycling.
-	const u32 kMaxIters = 100;
+	const u32 kMaxIters = 200;
 	
 	// Main iteration loop.
 	u32 iter = 0;
@@ -589,7 +735,7 @@ void b3SpringCloth::Step(float32 h)
 		// q = filter(A * c) 
 		// b3Mul(q, A, c, size);
 		b3Mul_A(q, c, size, m_allocator, m_inv_m, h, Jx, Jv, m_springs, m_springCount);
-		b3Filter(q, q, size, types);
+		b3Filter(q, q, size, types, m_collisions);
 
 		// alpha = epsNew / dot(c, q)
 		float32 alpha = epsNew / b3Dot(c, q, size);
@@ -628,7 +774,7 @@ void b3SpringCloth::Step(float32 h)
 		{
 			c[i] = s[i] + beta * c[i];
 		}
-		b3Filter(c, c, size, types);
+		b3Filter(c, c, size, types, m_collisions);
 
 		++iter;
 	}
@@ -639,7 +785,7 @@ void b3SpringCloth::Step(float32 h)
 	for (u32 i = 0; i < m_massCount; ++i)
 	{
 		m_v[i] += dv[i];
-		m_x[i] += h * m_v[i];
+		m_x[i] += h * m_v[i] + m_y[i];
 	}
 
 	// Clear forces
@@ -648,6 +794,12 @@ void b3SpringCloth::Step(float32 h)
 		m_f[i].SetZero();
 	}
 
+	// Clear position alteration
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		m_y[i].SetZero();
+	}
+	
 	m_allocator->Free(inv_P);
 	m_allocator->Free(P);
 	m_allocator->Free(s);
@@ -672,6 +824,11 @@ void b3SpringCloth::Apply() const
 
 void b3SpringCloth::Draw(b3Draw* draw) const
 {
+	for (u32 i = 0; i < m_sphereCount; ++i)
+	{
+		draw->DrawSolidSphere(m_spheres[i].vertex, m_spheres[i].radius, b3Color_white);
+	}
+
 	const b3Mesh* m = m_mesh;
 
 	for (u32 i = 0; i < m->vertexCount; ++i)