add simple cloth

include/bounce/bounce.h

@@ -57,4 +57,6 @@
 #include <bounce/dynamics/world.h>
 #include <bounce/dynamics/world_listeners.h>
 
+#include <bounce/cloth/cloth.h>
+
 #endif

include/bounce/cloth/cloth.h

@@ -0,0 +1,135 @@
+/*
+* 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 B3_CLOTH_H
+#define B3_CLOTH_H
+
+#include <bounce/common/math/vec3.h>
+#include <bounce/collision/distance.h>
+
+struct b3Mesh;
+class b3Draw;
+
+struct b3ClothDef
+{
+	b3ClothDef()
+	{
+		mesh = NULL;
+		density = 0.0f;
+		gravity.SetZero();
+		k1 = 0.9f;
+		k2 = 0.2f;
+		kd = 0.1f;
+		r = 0.0f;
+	}
+
+	// Cloth mesh
+	// Each edge must be shared by at most two triangles (manifold)
+	const b3Mesh* mesh;
+
+	// Cloth density in kilograms per meter squared
+	float32 density;
+
+	// Gravity force
+	b3Vec3 gravity;
+
+	// Streching stiffness
+	float32 k1;
+
+	// Bending stiffness
+	float32 k2;
+	
+	// Damping
+	float32 kd;
+
+	// Cloth thickness
+	float32 r;
+};
+
+struct b3Particle
+{
+	float32 im;
+	b3Vec3 p0;
+	b3Vec3 p;
+	b3Vec3 v;
+};
+
+struct b3C1
+{
+	float32 L;
+	u32 i1;
+	u32 i2;
+};
+
+struct b3C2
+{
+	float32 angle;
+	u32 i1;
+	u32 i2;
+	u32 i3;
+	u32 i4;
+};
+
+class b3Cloth
+{
+public:
+	b3Cloth();
+	~b3Cloth();
+
+	void Initialize(const b3ClothDef& def);
+
+	void Step(float32 dt, u32 iterations);
+
+	u32 GetVertexCount() const
+	{
+		return m_pCount;
+	}
+
+	const b3Particle* GetVertices() const
+	{
+		return m_ps;
+	}
+
+	b3Particle* GetVertices()
+	{
+		return m_ps;
+	}
+
+	void Draw(b3Draw* draw) const;
+private:
+	void SolveC1();
+
+	b3Particle* m_ps;
+	u32 m_pCount;
+	
+	b3C1* m_c1s;
+	u32 m_c1Count;
+	
+	b3C2* m_c2s;
+	u32 m_c2Count;
+
+	float32 m_k1;
+	float32 m_k2;
+	float32 m_kd;
+	float32 m_r;
+	b3Vec3 m_gravity;
+
+	const b3Mesh* m_mesh;
+};
+
+#endif

include/testbed/tests/cloth_test.h

@@ -0,0 +1,79 @@
+/*
+* 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 CLOTH_H
+#define CLOTH_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class Cloth : public Test
+{
+public:
+	Cloth()
+	{
+		g_camera.m_zoom = 25.0f;
+
+		b3ClothDef def;
+		def.mesh = m_meshes + e_clothMesh;
+		def.density = 0.4f;
+		def.gravity.Set(-10.0f, 1.0f, 0.0f);
+		def.k1 = 0.9f;
+		def.k2 = 0.2f;
+		def.kd = 0.1f;
+		def.r = 1.0f;
+
+		m_cloth.Initialize(def);
+
+		b3Particle* vs = m_cloth.GetVertices();
+		for (u32 i = 0; i < 5; ++i)
+		{
+			vs[i].im = 0.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, g_settings.positionIterations);
+		m_cloth.Draw(g_debugDraw);
+	}
+
+	static Test* Create()
+	{
+		return new Cloth();
+	}
+
+	b3Cloth m_cloth;
+};
+
+#endif

include/testbed/tests/test.h

@@ -116,6 +116,7 @@ public:
 	{
 		e_gridMesh,
 		e_terrainMesh,
+		e_clothMesh,
 		e_maxMeshes,
 	};
 

src/bounce/cloth/cloth.cpp

@@ -0,0 +1,209 @@
+/*
+* 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.
+*/
+
+#include <bounce/cloth/cloth.h>
+#include <bounce/collision/shapes/mesh.h>
+#include <bounce/common/template/array.h>
+#include <bounce/common/draw.h>
+
+b3Cloth::b3Cloth()
+{
+	m_pCount = 0;
+	m_ps = NULL;
+	m_c1Count = 0;
+	m_c1s = NULL;
+	m_c2Count = 0;
+	m_c2s = NULL;
+
+	m_k1 = 0.0f;
+	m_k2 = 0.0f;
+	m_kd = 0.0f;
+	m_r = 0.0f;
+	m_gravity.SetZero();
+}
+
+b3Cloth::~b3Cloth()
+{
+	b3Free(m_ps);
+	b3Free(m_c1s);
+	b3Free(m_c2s);
+}
+
+void b3Cloth::Initialize(const b3ClothDef& def)
+{
+	B3_ASSERT(def.mesh);
+	m_mesh = def.mesh;
+	
+	const b3Mesh* m = m_mesh;
+
+	m_pCount = m->vertexCount;
+	m_ps = (b3Particle*)b3Alloc(m_pCount * sizeof(b3Particle));
+	
+	for (u32 i = 0; i < m->vertexCount; ++i)
+	{
+		b3Particle* p = m_ps + i;
+		p->im = 0.0f;
+		p->p = m->vertices[i];
+		p->p0 = p->p;
+		p->v.SetZero();
+	}
+
+	m_c1s = (b3C1*)b3Alloc(3 * m->triangleCount * sizeof(b3C1));
+	m_c1Count = 0;
+
+	for (u32 i = 0; i < m->triangleCount; ++i)
+	{
+		b3Triangle* t = m->triangles + i;
+
+		u32 is[3] = { t->v1, t->v2, t->v3 };
+		for (u32 j = 0; j < 3; ++j)
+		{
+			u32 k = j + 1 < 3 ? j + 1 : 0;
+
+			u32 i1 = is[j];
+			u32 i2 = is[k];
+
+			b3Vec3 p1 = m->vertices[i1];
+			b3Vec3 p2 = m->vertices[i2];
+
+			b3C1* C = m_c1s + m_c1Count;
+			C->i1 = i1;
+			C->i2 = i2;
+			C->L = b3Distance(p1, p2);
+			++m_c1Count;
+		}
+
+		b3Vec3 p1 = m->vertices[t->v1];
+		b3Vec3 p2 = m->vertices[t->v2];
+		b3Vec3 p3 = m->vertices[t->v3];
+
+		float32 area = b3Area(p1, p2, p3);
+		float32 mass = def.density * area;
+
+		const float32 inv3 = 1.0f / 3.0f;
+
+		m_ps[t->v1].im += inv3 * mass;
+		m_ps[t->v2].im += inv3 * mass;
+		m_ps[t->v3].im += inv3 * mass;
+	}
+
+	for (u32 i = 0; i < m_pCount; ++i)
+	{
+		m_ps[i].im = m_ps[i].im > 0.0f ? 1.0f / m_ps[i].im : 0.0f;
+	}
+
+	m_k1 = def.k1;
+	m_k2 = def.k2;
+	m_kd = def.kd;
+	m_r = def.r;
+	m_gravity = def.gravity;
+}
+
+void b3Cloth::Step(float32 h, u32 iterations)
+{
+	if (h == 0.0f)
+	{
+		return;
+	}
+
+	float32 d = exp(-h * m_kd);
+
+	for (u32 i = 0; i < m_pCount; ++i)
+	{
+		b3Particle* p = m_ps + i;
+
+		p->v += h * p->im * m_gravity;
+		p->v *= d;
+
+		p->p0 = p->p;
+		p->p += h * p->v;
+	}
+
+	for (u32 i = 0; i < iterations; ++i)
+	{
+		SolveC1();
+	}
+
+	float32 inv_h = 1.0f / h;
+	for (u32 i = 0; i < m_pCount; ++i)
+	{
+		b3Particle* p = m_ps + i;
+		p->v = inv_h * (p->p - p->p0);
+	}
+}
+
+void b3Cloth::SolveC1()
+{
+	float32 k = m_k1;
+
+	for (u32 i = 0; i < m_c1Count; ++i)
+	{
+		b3C1* c = m_c1s + i;
+		
+		b3Particle* p1 = m_ps + c->i1;
+		b3Particle* p2 = m_ps + c->i2;
+
+		b3Vec3 d = p2->p - p1->p;
+		float32 L = b3Length(d);
+		if (L > B3_EPSILON)
+		{
+			d /= L;
+		}
+
+		float32 C = L - c->L;
+		
+		float32 im1 = p1->im;
+		float32 im2 = p2->im;
+
+		if (im1 + im2 == 0.0f)
+		{
+			continue;
+		}
+
+		float32 s1 = im1 / (im1 + im2);
+		float32 s2 = im2 / (im1 + im2);
+
+		p1->p -= k * s1 * -C * d;
+		p2->p += k * s2 * -C * d;
+	}
+}
+
+void b3Cloth::Draw(b3Draw* draw) const
+{
+	const b3Color color1(1.0f, 0.0f, 0.0f);
+	const b3Color color2(0.0f, 1.0f, 0.0f);
+	const b3Color color3(0.0f, 0.0f, 1.0f);
+	const b3Color color4(0.0f, 0.0f, 0.0f);
+
+	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+	{
+		b3Triangle* t = m_mesh->triangles + i;
+
+		b3Particle* p1 = m_ps + t->v1;
+		b3Particle* p2 = m_ps + t->v2;
+		b3Particle* p3 = m_ps + t->v3;
+
+		b3Vec3 ps[3];
+		ps[0] = p1->p;
+		ps[1] = p2->p;
+		ps[2] = p3->p;
+
+		draw->DrawPolygon(ps, 3, color4);
+		draw->DrawSolidPolygon(ps, 3, color3);
+	}
+}

src/bounce/dynamics/island.cpp

@@ -87,6 +87,15 @@ void b3Island::Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32
 {
 	float32 h = dt;
 
+	// Measure coefficient of damping.
+	// Box2D.
+	// ODE: dv/dt + c * v = 0
+	// Solution: v(t) = v0 * exp(-c * t)
+	// Step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+	// v2 = exp(-c * dt) * v1
+	const float32 k_d = 0.1f;
+	float32 d = exp(-h * k_d);
+
 	// 1. Integrate velocities
 	for (u32 i = 0; i < m_bodyCount; ++i) 
 	{
@@ -116,16 +125,9 @@ void b3Island::Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32
 			// Clear torques
 			b->m_torque.SetZero();
 			
-			// Apply some damping
+			// Apply damping
-			// Box2D.
+			v *= d;
-			// ODE: dv/dt + c * v = 0
+			w *= d;
-			// Solution: v(t) = v0 * exp(-c * t)
-			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
-			// v2 = exp(-c * dt) * v1
-			// Pade approximation:
-			// v2 = v1 * 1 / (1 + c * dt)
-			v *= 1.0f / (1.0f + h * 0.1f);
-			w *= 1.0f / (1.0f + h * 0.1f);
 		}
 
 		m_velocities[i].v = v;

src/testbed/framework/test.cpp

@@ -110,7 +110,7 @@ Test::Test()
 		t.y = 0.0f;
 		t.z = -0.5f * float32(h);
 
-		b3Mesh* mesh = m_meshes + 0;
+		b3Mesh* mesh = m_meshes + e_gridMesh;
 
 		mesh->vertexCount = w * h;
 		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
@@ -168,7 +168,90 @@ Test::Test()
 
 		mesh->BuildTree();
 	}
-	
+
+	{
+		const u32 w = 5;
+		const u32 h = 5;
+
+		b3Vec3 t;
+		t.x = -0.5f * float32(w);
+		t.y = 0.0f;
+		t.z = -0.5f * float32(h);
+
+		b3Mesh* mesh = m_meshes + e_clothMesh;
+
+		mesh->vertexCount = w * h;
+		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
+
+		for (u32 i = 0; i < w; ++i)
+		{
+			for (u32 j = 0; j < h; ++j)
+			{
+				u32 v1 = i * w + j;
+
+				b3Vec3 v;
+				v.x = float32(i);
+				v.y = 0.0f;
+				v.z = float32(j);
+
+				v += t;
+
+				mesh->vertices[v1] = v;
+			}
+		}
+
+		mesh->triangleCount = 2 * 2 * (w - 1) * (h - 1);
+		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));
+
+		u32 triangleCount = 0;
+		for (u32 i = 0; i < w - 1; ++i)
+		{
+			for (u32 j = 0; j < h - 1; ++j)
+			{
+				u32 v1 = i * w + j;
+				u32 v2 = (i + 1) * w + j;
+				u32 v3 = (i + 1) * w + (j + 1);
+				u32 v4 = i * w + (j + 1);
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t1 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t1->v1 = v3;
+				t1->v2 = v2;
+				t1->v3 = v1;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t2 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t2->v1 = v1;
+				t2->v2 = v4;
+				t2->v3 = v3;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t3 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t3->v1 = v1;
+				t3->v2 = v2;
+				t3->v3 = v3;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t4 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t4->v1 = v3;
+				t4->v2 = v4;
+				t4->v3 = v1;
+			}
+		}
+
+		B3_ASSERT(triangleCount == mesh->triangleCount);
+
+		mesh->BuildTree();
+	}
+
 	{
 		const u32 w = 100;
 		const u32 h = 100;
@@ -178,7 +261,7 @@ Test::Test()
 		t.y = 0.0f;
 		t.z = -0.5f * float32(h);
 
-		b3Mesh* mesh = m_meshes + 1;
+		b3Mesh* mesh = m_meshes + e_terrainMesh;
 		
 		mesh->vertexCount = w * h;
 		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));

src/testbed/framework/test_entries.cpp

@@ -46,6 +46,7 @@
 #include <testbed/tests/body_types.h>
 #include <testbed/tests/varying_friction.h>
 #include <testbed/tests/varying_restitution.h>
+#include <testbed/tests/cloth_test.h>
 
 TestEntry g_tests[] =
 {
@@ -77,5 +78,6 @@ TestEntry g_tests[] =
 	{ "Body Types", &BodyTypes::Create },
 	{ "Varying Friction", &VaryingFriction::Create },
 	{ "Varying Restitution", &VaryingRestitution::Create },
+	{ "Cloth", &Cloth::Create },
 	{ NULL, NULL }
 };