refactor cloth

examples/testbed/framework/test_entries.cpp

@@ -60,11 +60,10 @@
 #include <testbed/tests/tumbler.h>
 #include <testbed/tests/multiple_pendulum.h>
 #include <testbed/tests/cloth_test.h>
-#include <testbed/tests/spring_cloth_test.h>
 #include <testbed/tests/table_cloth.h>
 #include <testbed/tests/pinned_cloth.h>
 #include <testbed/tests/shirt.h>
-#include <testbed/tests/mass_types.h>
+#include <testbed/tests/particle_types.h>
 #include <testbed/tests/tension_mapping.h>
 #include <testbed/tests/single_pendulum.h>
 #include <testbed/tests/rope_test.h>
@@ -113,11 +112,10 @@ TestEntry g_tests[] =
 	{ "Tumbler", &Tumbler::Create },
 	{ "Initial Overlap", &InitialOverlap::Create },
 	{ "Multiple Pendulum", &MultiplePendulum::Create },
-	{ "Cloth", &Cloth::Create },
 	{ "Table Cloth", &TableCloth::Create },
 	{ "Pinned Cloth", &PinnedCloth::Create },
 	{ "Shirt", &Shirt::Create },
-	{ "Mass Types", &MassTypes::Create },
+	{ "Particle Types", &ParticleTypes::Create },
 	{ "Tension Mapping", &TensionMapping::Create },
 	{ "Mass-Spring System", &MassSpring::Create },
 	{ "Single Pendulum", &SinglePendulum::Create },

examples/testbed/tests/cloth_test.h

@@ -16,52 +16,344 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef CLOTH_H
+#ifndef CLOTH_TESH_H
-#define CLOTH_H
+#define CLOTH_TESH_H
 
-class Cloth : public Test
+class ClothDragger
 {
 public:
-	Cloth()
+	ClothDragger(Ray3* ray, b3Cloth* cloth)
 	{
-		b3ClothDef def;
+		m_ray = ray;
-		def.mesh = &m_clothMesh;
+		m_cloth = cloth;
-		def.density = 0.2f;
+		m_isSelected = false;
-		def.gravity.Set(0.0f, -10.0f, 0.0f);
+	}
-		def.k1 = 0.2f;
-		def.k2 = 0.1f;
-		def.kd = 0.005f;
-		def.r = 1.0f;
 
-		m_cloth.Initialize(def);
+	~ClothDragger()
-
-		b3AABB3 aabb;
-		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
-		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
-
-		b3Particle* vs = m_cloth.GetVertices();
-		for (u32 i = 0; i < m_cloth.GetVertexCount(); ++i)
 	{
-			if (aabb.Contains(vs[i].p))
+
+	}
+
+	bool IsSelected() const
 	{
-				vs[i].im = 0.0f;
+		return m_isSelected;
+	}
+
+	b3Vec3 GetPointA() const
+	{
+		B3_ASSERT(m_isSelected);
+
+		b3ClothMesh* m = m_cloth->GetMesh();
+		b3ClothMeshTriangle* t = m->triangles + m_selection;
+
+		b3Vec3 A = m->vertices[t->v1];
+		b3Vec3 B = m->vertices[t->v2];
+		b3Vec3 C = m->vertices[t->v3];
+
+		return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
+	}
+
+	b3Vec3 GetPointB() const
+	{
+		B3_ASSERT(m_isSelected);
+		return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
+	}
+
+	bool StartDragging()
+	{
+		B3_ASSERT(m_isSelected == false);
+
+		if (Select(m_selection, m_x) == false)
+		{
+			return false;
+		}
+
+		m_isSelected = true;
+
+		b3ClothMesh* m = m_cloth->GetMesh();
+		b3ClothMeshTriangle* t = m->triangles + m_selection;
+
+		b3Particle* p1 = m_cloth->GetParticle(t->v1);
+		m_t1 = p1->type;
+		m_cloth->SetType(p1, e_staticParticle);
+
+		b3Particle* p2 = m_cloth->GetParticle(t->v2);
+		m_t2 = p2->type;
+		m_cloth->SetType(p2, e_staticParticle);
+		
+		b3Particle* p3 = m_cloth->GetParticle(t->v3);
+		m_t3 = p3->type;
+		m_cloth->SetType(p3, e_staticParticle);
+
+		b3Vec3 v1 = p1->position;
+		b3Vec3 v2 = p2->position;
+		b3Vec3 v3 = p3->position;
+
+		b3Vec3 B = GetPointB();
+
+		float32 wABC[4];
+		b3BarycentricCoordinates(wABC, v1, v2, v3, B);
+
+		if (wABC[3] > B3_EPSILON)
+		{
+			m_u = wABC[0] / wABC[3];
+			m_v = wABC[1] / wABC[3];
+		}
+		else
+		{
+			m_u = m_v = 0.0f;
+		}
+
+		return true;
+	}
+
+	void Drag()
+	{
+		B3_ASSERT(m_isSelected);
+
+		b3ClothMesh* m = m_cloth->GetMesh();
+		b3ClothMeshTriangle* t = m->triangles + m_selection;
+
+		b3Vec3 A = GetPointA();
+		b3Vec3 B = GetPointB();
+
+		b3Vec3 dx = B - A;
+
+		b3Particle* p1 = m_cloth->GetParticle(t->v1);
+		m_cloth->Translate(p1, dx);
+		
+		b3Particle* p2 = m_cloth->GetParticle(t->v2);
+		m_cloth->Translate(p2, dx);
+		
+		b3Particle* p3 = m_cloth->GetParticle(t->v3);
+		m_cloth->Translate(p3, dx);
+	}
+
+	void StopDragging()
+	{
+		B3_ASSERT(m_isSelected);
+
+		m_isSelected = false;
+
+		b3ClothMesh* m = m_cloth->GetMesh();
+		b3ClothMeshTriangle* t = m->triangles + m_selection;
+
+		b3Particle* p1 = m_cloth->GetParticle(t->v1);
+		m_cloth->SetType(p1, m_t1);
+		
+		b3Particle* p2 = m_cloth->GetParticle(t->v2);
+		m_cloth->SetType(p2, m_t2);
+		
+		b3Particle* p3 = m_cloth->GetParticle(t->v3);
+		m_cloth->SetType(p3, m_t3);
+	}
+
+private:
+	bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const
+	{
+		b3ClothMesh* mesh = m_cloth->GetMesh();		
+		B3_ASSERT(triangleIndex < mesh->triangleCount);
+		b3ClothMeshTriangle* triangle = mesh->triangles + triangleIndex;
+		
+		b3Vec3 v1 = mesh->vertices[triangle->v1];
+		b3Vec3 v2 = mesh->vertices[triangle->v2];
+		b3Vec3 v3 = mesh->vertices[triangle->v3];
+
+		b3Vec3 p1 = m_ray->A();
+		b3Vec3 p2 = m_ray->B();
+		b3Vec3 d = p2 - p1;
+		float32 maxFraction = b3Length(d);
+
+		b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
+		n.Normalize();
+
+		float32 numerator = b3Dot(n, v1 - p1);
+		float32 denominator = b3Dot(n, d);
+
+		if (denominator == 0.0f)
+		{
+			return false;
+		}
+
+		float32 t = numerator / denominator;
+
+		// Is the intersection not on the segment?
+		if (t < 0.0f || maxFraction < t)
+		{
+			return false;
+		}
+
+		b3Vec3 q = p1 + t * d;
+
+		// Barycentric coordinates for q
+		b3Vec3 Q = q;
+		b3Vec3 A = v1;
+		b3Vec3 B = v2;
+		b3Vec3 C = v3;
+
+		b3Vec3 AB = B - A;
+		b3Vec3 AC = C - A;
+
+		b3Vec3 QA = A - Q;
+		b3Vec3 QB = B - Q;
+		b3Vec3 QC = C - Q;
+
+		b3Vec3 QB_x_QC = b3Cross(QB, QC);
+		b3Vec3 QC_x_QA = b3Cross(QC, QA);
+		b3Vec3 QA_x_QB = b3Cross(QA, QB);
+
+		b3Vec3 AB_x_AC = b3Cross(AB, AC);
+
+		float32 u = b3Dot(QB_x_QC, AB_x_AC);
+		float32 v = b3Dot(QC_x_QA, AB_x_AC);
+		float32 w = b3Dot(QA_x_QB, AB_x_AC);
+
+		// This tolerance helps intersections lying on  
+		// shared edges to not be missed.
+		const float32 kTol = -0.005f;
+
+		// Is the intersection on the triangle?
+		if (u > kTol && v > kTol && w > kTol)
+		{
+			output->fraction = t;
+
+			// Does the ray start from below or above the triangle?
+			if (numerator > 0.0f)
+			{
+				output->normal = -n;
+			}
+			else
+			{
+				output->normal = n;
+			}
+
+			return true;
+		}
+
+		return false;
+	}
+
+	bool Select(u32& selection, float32& fraction) const
+	{
+		b3ClothMesh* m = m_cloth->GetMesh();
+
+		b3RayCastInput input;
+		input.p1 = m_ray->A();
+		input.p2 = m_ray->B();
+		input.maxFraction = m_ray->fraction;
+
+		float32 minFraction = B3_MAX_FLOAT;
+		b3Vec3 minNormal(0.0f, 0.0f, 0.0f);
+		u32 minIndex = ~0;
+
+		for (u32 i = 0; i < m->triangleCount; ++i)
+		{
+			b3RayCastOutput subOutput;
+			if (RayCast(&subOutput, i) == true)
+			{
+				if (subOutput.fraction < minFraction)
+				{
+					minFraction = subOutput.fraction;
+					minNormal = subOutput.normal;
+					minIndex = i;
 				}
 			}
 		}
 
+		if (minIndex != ~0)
+		{
+			selection = minIndex;
+			fraction = minFraction;
+			return true;
+		}
+
+		return false;
+	}
+
+	bool m_isSelected;
+
+	Ray3* m_ray;
+	float32 m_x;
+
+	b3Cloth * m_cloth;
+	u32 m_selection;
+	float32 m_u, m_v;
+	b3ParticleType m_t1, m_t2, m_t3;
+};
+
+class ClothTest : public Test
+{
+public:
+	ClothTest() : m_clothDragger(&m_clothRay, &m_cloth)
+	{
+		m_cloth.SetGravity(b3Vec3(0.0f, -10.0f, 0.0f));
+
+		m_clothRay.origin.SetZero();
+		m_clothRay.direction.Set(0.0f, 0.0f, -1.0f);
+		m_clothRay.fraction = g_camera->m_zFar;
+	}
+
 	void Step()
 	{
-		m_cloth.Step(g_testSettings->inv_hertz, g_testSettings->positionIterations);
+		float32 dt = g_testSettings->inv_hertz;
-		m_cloth.Draw();
-	}
 
-	static Test* Create()
+		m_cloth.Step(dt);
+		m_cloth.Apply();
+
+		b3Shape** shapes = m_cloth.GetShapeList();
+		for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i)
 		{
-		return new Cloth();
+			b3Shape* s = shapes[i];
+
+			b3Transform xf;
+			xf.SetIdentity();
+
+			g_draw->DrawSolidShape(s, b3Color_white, xf);
 		}
 
-	b3GridMesh<10, 10> m_clothMesh;
+		m_cloth.Draw();
+
+		extern u32 b3_clothSolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations);
+		
+		float32 E = m_cloth.GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+
+		if (m_clothDragger.IsSelected() == true)
+		{
+			g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white);
+		}
+	}
+
+	void MouseMove(const Ray3& pw)
+	{
+		m_clothRay = pw;
+
+		if (m_clothDragger.IsSelected() == true)
+		{
+			m_clothDragger.Drag();
+		}
+	}
+
+	void MouseLeftDown(const Ray3& pw)
+	{
+		if (m_clothDragger.IsSelected() == false)
+		{
+			m_clothDragger.StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const Ray3& pw)
+	{
+		if (m_clothDragger.IsSelected() == true)
+		{
+			m_clothDragger.StopDragging();
+		}
+	}
+
+	Ray3 m_clothRay;
 	b3Cloth m_cloth;
+	ClothDragger m_clothDragger;
 };
 
 #endif

examples/testbed/tests/mass_types.h

@@ -1,175 +0,0 @@
-/*
-* 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 MASS_TYPES_H
-#define MASS_TYPES_H
-
-class MassTypes : public PinnedCloth
-{
-public:
-	MassTypes()
-	{
-	}
-
-	void Step()
-	{
-		PinnedCloth::Step();
-
-		g_draw->DrawString(b3Color_white, "S - Static");
-		g_draw->DrawString(b3Color_white, "D - Dynamic");
-		g_draw->DrawString(b3Color_white, "K - Kinematic");
-		g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
-	}
-
-	void SetClothType(b3MassType type)
-	{
-		for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
-		{
-			m_cloth.SetType(i, type);
-		}
-	}
-
-	void KeyDown(int button)
-	{
-		if (button == GLFW_KEY_S)
-		{
-			SetClothType(b3MassType::e_staticMass);
-		}
-
-		if (button == GLFW_KEY_K)
-		{
-			SetClothType(b3MassType::e_kinematicMass);
-		}
-
-		if (button == GLFW_KEY_D)
-		{
-			SetClothType(b3MassType::e_dynamicMass);
-		}
-
-		for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
-		{
-			if (m_cloth.GetType(i) == b3MassType::e_staticMass)
-			{
-				if (button == GLFW_KEY_LEFT)
-				{
-					b3Vec3 p = m_cloth.GetPosition(i);
-
-					p.x -= 1.0f;
-
-					m_cloth.SetPosition(i, p);
-				}
-
-				if (button == GLFW_KEY_RIGHT)
-				{
-					b3Vec3 p = m_cloth.GetPosition(i);
-
-					p.x += 1.0f;
-
-					m_cloth.SetPosition(i, p);
-				}
-
-				if (button == GLFW_KEY_UP)
-				{
-					b3Vec3 p = m_cloth.GetPosition(i);
-
-					p.z += 1.0f;
-
-					m_cloth.SetPosition(i, p);
-				}
-
-				if (button == GLFW_KEY_DOWN)
-				{
-					b3Vec3 p = m_cloth.GetPosition(i);
-
-					p.z -= 1.0f;
-
-					m_cloth.SetPosition(i, p);
-				}
-			}
-
-			if (m_cloth.GetType(i) == b3MassType::e_kinematicMass)
-			{
-				if (button == GLFW_KEY_LEFT)
-				{
-					b3Vec3 v = m_cloth.GetVelocity(i);
-
-					v.x -= 5.0f;
-
-					m_cloth.SetVelocity(i, v);
-				}
-
-				if (button == GLFW_KEY_RIGHT)
-				{
-					b3Vec3 v = m_cloth.GetVelocity(i);
-
-					v.x += 5.0f;
-
-					m_cloth.SetVelocity(i, v);
-				}
-
-				if (button == GLFW_KEY_UP)
-				{
-					b3Vec3 v = m_cloth.GetVelocity(i);
-
-					v.z -= 5.0f;
-
-					m_cloth.SetVelocity(i, v);
-				}
-
-				if (button == GLFW_KEY_DOWN)
-				{
-					b3Vec3 v = m_cloth.GetVelocity(i);
-
-					v.z += 5.0f;
-
-					m_cloth.SetVelocity(i, v);
-				}
-			}
-
-			if (m_cloth.GetType(i) == b3MassType::e_dynamicMass)
-			{
-				if (button == GLFW_KEY_LEFT)
-				{
-					m_cloth.ApplyForce(i, b3Vec3(-100.0f, 0.0f, 0.0f));
-				}
-
-				if (button == GLFW_KEY_RIGHT)
-				{
-					m_cloth.ApplyForce(i, b3Vec3(100.0f, 0.0f, 0.0f));
-				}
-
-				if (button == GLFW_KEY_UP)
-				{
-					m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, -100.0f));
-				}
-
-				if (button == GLFW_KEY_DOWN)
-				{
-					m_cloth.ApplyForce(i, b3Vec3(0.0f, 0.0f, 100.0f));
-				}
-			}
-		}
-	}
-
-	static Test* Create()
-	{
-		return new MassTypes();
-	}
-};
-
-#endif

examples/testbed/tests/particle_types.h

@@ -0,0 +1,127 @@
+/*
+* 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 PARTICLE_TYPES_H
+#define PARTICLE_TYPES_H
+
+class ParticleTypes : public PinnedCloth
+{
+public:
+	ParticleTypes()
+	{
+	}
+
+	void Step()
+	{
+		PinnedCloth::Step();
+
+		g_draw->DrawString(b3Color_white, "S - Static");
+		g_draw->DrawString(b3Color_white, "D - Dynamic");
+		g_draw->DrawString(b3Color_white, "K - Kinematic");
+		g_draw->DrawString(b3Color_white, "Arrows - Apply Force/Velocity/Position");
+	}
+
+	void SetClothType(b3ParticleType type)
+	{
+		for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
+		{
+			b3Particle* p = m_cloth.GetParticle(i);
+			m_cloth.SetType(p, type);
+		}
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_S)
+		{
+			SetClothType(e_staticParticle);
+		}
+
+		if (button == GLFW_KEY_K)
+		{
+			SetClothType(e_kinematicParticle);
+		}
+
+		if (button == GLFW_KEY_D)
+		{
+			SetClothType(e_dynamicParticle);
+		}
+
+		for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
+		{
+			b3Particle* p = m_cloth.GetParticle(i);
+
+			b3Vec3 d;
+			d.SetZero();
+
+			if (button == GLFW_KEY_LEFT)
+			{
+				d.x = -1.0f;
+			}
+
+			if (button == GLFW_KEY_RIGHT)
+			{
+				d.x = 1.0f;
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				d.y = 1.0f;
+			}
+
+			if (button == GLFW_KEY_DOWN)
+			{
+				d.y = -1.0f;
+			}
+
+			if (button == GLFW_KEY_LEFT ||
+				button == GLFW_KEY_RIGHT ||
+				button == GLFW_KEY_UP ||
+				button == GLFW_KEY_DOWN)
+			{
+				if (p->type == e_staticParticle)
+				{
+					m_cloth.Translate(p, d);
+				}
+
+				if (p->type == e_kinematicParticle)
+				{
+					b3Vec3 v = p->velocity;
+
+					v += 5.0f * d;
+
+					m_cloth.SetVelocity(p, d);
+				}
+
+				if (p->type == e_dynamicParticle)
+				{
+					b3Vec3 f = 100.0f * d;
+
+					m_cloth.ApplyForce(p, f);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ParticleTypes();
+	}
+};
+
+#endif

examples/testbed/tests/pinned_cloth.h

@@ -19,7 +19,7 @@
 #ifndef PINNED_CLOTH_H
 #define PINNED_CLOTH_H
 
-class PinnedCloth : public SpringClothTest
+class PinnedCloth : public ClothTest
 {
 public:
 	PinnedCloth()
@@ -42,14 +42,12 @@ public:
 		m_gridClothMesh.sewingLineCount = 0;
 		m_gridClothMesh.sewingLines = nullptr;
 
-		b3SpringClothDef def;
+		b3ClothDef def;
-		def.allocator = &m_clothAllocator;
 		def.mesh = &m_gridClothMesh;
 		def.density = 0.2f;
 		def.ks = 10000.0f;
 		def.kd = 0.0f;
 		def.r = 0.05f;
-		def.gravity.Set(0.0f, -10.0f, 0.0f);
 
 		m_cloth.Initialize(def);
 
@@ -57,11 +55,12 @@ public:
 		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
 		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
 
-		for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
+		for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
 		{
-			if (aabb.Contains(m_cloth.GetPosition(i)))
+			b3Particle* p = m_cloth.GetParticle(i);
+			if (aabb.Contains(p->position))
 			{
-				m_cloth.SetType(i, b3MassType::e_staticMass);
+				m_cloth.SetType(p, e_staticParticle);
 			}
 		}
 	}

examples/testbed/tests/shirt.h

@@ -23,7 +23,7 @@
 #include <bounce/garment/sewing_pattern.h>
 #include <bounce/garment/garment_mesh.h>
 
-class Shirt : public SpringClothTest
+class Shirt : public ClothTest
 {
 public:
 	Shirt()
@@ -56,13 +56,11 @@ public:
 		}
 
 		// Create cloth
-		b3SpringClothDef def;
+		b3ClothDef def;
-		def.allocator = &m_clothAllocator;
 		def.mesh = &m_shirtClothMesh;
 		def.density = 0.2f;
-		def.ks = 10000.0f;
 		def.r = 0.2f;
-		def.gravity.Set(0.0f, -10.0f, 0.0f);
+		def.ks = 10000.0f;
 
 		m_cloth.Initialize(def);
 	}

examples/testbed/tests/spring_cloth_test.h

@@ -1,356 +0,0 @@
-/*
-* 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_TESH_H
-#define SPRING_CLOTH_TESH_H
-
-class ClothDragger
-{
-public:
-	ClothDragger(Ray3* ray, b3SpringCloth* cloth)
-	{
-		m_ray = ray;
-		m_cloth = cloth;
-		m_isSelected = false;
-	}
-
-	~ClothDragger()
-	{
-
-	}
-
-	bool IsSelected() const
-	{
-		return m_isSelected;
-	}
-
-	b3Vec3 GetPointA() const
-	{
-		B3_ASSERT(m_isSelected);
-
-		b3ClothMesh* m = m_cloth->GetMesh();
-		b3ClothMeshTriangle* t = m->triangles + m_selection;
-
-		b3Vec3 A = m->vertices[t->v1];
-		b3Vec3 B = m->vertices[t->v2];
-		b3Vec3 C = m->vertices[t->v3];
-
-		return m_u * A + m_v * B + (1.0f - m_u - m_v) * C;
-	}
-
-	b3Vec3 GetPointB() const
-	{
-		B3_ASSERT(m_isSelected);
-		return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
-	}
-
-	bool StartDragging()
-	{
-		B3_ASSERT(m_isSelected == false);
-
-		if (Select(m_selection, m_x) == false)
-		{
-			return false;
-		}
-
-		m_isSelected = true;
-
-		b3ClothMesh* m = m_cloth->GetMesh();
-		b3ClothMeshTriangle* t = m->triangles + m_selection;
-
-		m_t1 = m_cloth->GetType(t->v1);
-		m_cloth->SetType(t->v1, b3MassType::e_staticMass);
-
-		m_t2 = m_cloth->GetType(t->v2);
-		m_cloth->SetType(t->v2, b3MassType::e_staticMass);
-		
-		m_t3 = m_cloth->GetType(t->v3);
-		m_cloth->SetType(t->v3, b3MassType::e_staticMass);
-
-		b3Vec3 v1 = m_cloth->GetPosition(t->v1);
-		b3Vec3 v2 = m_cloth->GetPosition(t->v2);
-		b3Vec3 v3 = m_cloth->GetPosition(t->v3);
-
-		b3Vec3 B = GetPointB();
-
-		float32 wABC[4];
-		b3BarycentricCoordinates(wABC, v1, v2, v3, B);
-
-		if (wABC[3] > B3_EPSILON)
-		{
-			m_u = wABC[0] / wABC[3];
-			m_v = wABC[1] / wABC[3];
-		}
-		else
-		{
-			m_u = m_v = 0.0f;
-		}
-
-		return true;
-	}
-
-	void Drag()
-	{
-		B3_ASSERT(m_isSelected);
-
-		b3ClothMesh* m = m_cloth->GetMesh();
-		b3ClothMeshTriangle* t = m->triangles + m_selection;
-
-		b3Vec3 A = GetPointA();
-		b3Vec3 B = GetPointB();
-
-		b3Vec3 dx = B - A;
-
-		b3Vec3 v1 = m_cloth->GetPosition(t->v1);
-		v1 += dx;
-		m_cloth->SetPosition(t->v1, v1);
-
-		b3Vec3 v2 = m_cloth->GetPosition(t->v2);
-		v2 += dx;
-		m_cloth->SetPosition(t->v2, v2);
-
-		b3Vec3 v3 = m_cloth->GetPosition(t->v3);
-		v3 += dx;
-		m_cloth->SetPosition(t->v3, v3);
-	}
-
-	void StopDragging()
-	{
-		B3_ASSERT(m_isSelected);
-
-		m_isSelected = false;
-
-		b3ClothMesh* m = m_cloth->GetMesh();
-		b3ClothMeshTriangle* t = m->triangles + m_selection;
-
-		m_cloth->SetType(t->v1, m_t1);
-		m_cloth->SetType(t->v2, m_t2);
-		m_cloth->SetType(t->v3, m_t3);
-	}
-
-private:
-	bool RayCast(b3RayCastOutput* output, u32 triangleIndex) const
-	{
-		b3ClothMesh* mesh = m_cloth->GetMesh();		
-		B3_ASSERT(triangleIndex < mesh->triangleCount);
-		b3ClothMeshTriangle* triangle = mesh->triangles + triangleIndex;
-		
-		b3Vec3 v1 = mesh->vertices[triangle->v1];
-		b3Vec3 v2 = mesh->vertices[triangle->v2];
-		b3Vec3 v3 = mesh->vertices[triangle->v3];
-
-		b3Vec3 p1 = m_ray->A();
-		b3Vec3 p2 = m_ray->B();
-		b3Vec3 d = p2 - p1;
-		float32 maxFraction = b3Length(d);
-
-		b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
-		n.Normalize();
-
-		float32 numerator = b3Dot(n, v1 - p1);
-		float32 denominator = b3Dot(n, d);
-
-		if (denominator == 0.0f)
-		{
-			return false;
-		}
-
-		float32 t = numerator / denominator;
-
-		// Is the intersection not on the segment?
-		if (t < 0.0f || maxFraction < t)
-		{
-			return false;
-		}
-
-		b3Vec3 q = p1 + t * d;
-
-		// Barycentric coordinates for q
-		b3Vec3 Q = q;
-		b3Vec3 A = v1;
-		b3Vec3 B = v2;
-		b3Vec3 C = v3;
-
-		b3Vec3 AB = B - A;
-		b3Vec3 AC = C - A;
-
-		b3Vec3 QA = A - Q;
-		b3Vec3 QB = B - Q;
-		b3Vec3 QC = C - Q;
-
-		b3Vec3 QB_x_QC = b3Cross(QB, QC);
-		b3Vec3 QC_x_QA = b3Cross(QC, QA);
-		b3Vec3 QA_x_QB = b3Cross(QA, QB);
-
-		b3Vec3 AB_x_AC = b3Cross(AB, AC);
-
-		float32 u = b3Dot(QB_x_QC, AB_x_AC);
-		float32 v = b3Dot(QC_x_QA, AB_x_AC);
-		float32 w = b3Dot(QA_x_QB, AB_x_AC);
-
-		// This tolerance helps intersections lying on  
-		// shared edges to not be missed.
-		const float32 kTol = -0.005f;
-
-		// Is the intersection on the triangle?
-		if (u > kTol && v > kTol && w > kTol)
-		{
-			output->fraction = t;
-
-			// Does the ray start from below or above the triangle?
-			if (numerator > 0.0f)
-			{
-				output->normal = -n;
-			}
-			else
-			{
-				output->normal = n;
-			}
-
-			return true;
-		}
-
-		return false;
-	}
-
-	bool Select(u32& selection, float32& fraction) const
-	{
-		b3ClothMesh* m = m_cloth->GetMesh();
-
-		b3RayCastInput input;
-		input.p1 = m_ray->A();
-		input.p2 = m_ray->B();
-		input.maxFraction = m_ray->fraction;
-
-		float32 minFraction = B3_MAX_FLOAT;
-		b3Vec3 minNormal(0.0f, 0.0f, 0.0f);
-		u32 minIndex = ~0;
-
-		for (u32 i = 0; i < m->triangleCount; ++i)
-		{
-			b3RayCastOutput subOutput;
-			if (RayCast(&subOutput, i) == true)
-			{
-				if (subOutput.fraction < minFraction)
-				{
-					minFraction = subOutput.fraction;
-					minNormal = subOutput.normal;
-					minIndex = i;
-				}
-			}
-		}
-
-		if (minIndex != ~0)
-		{
-			selection = minIndex;
-			fraction = minFraction;
-			return true;
-		}
-
-		return false;
-	}
-
-	bool m_isSelected;
-
-	Ray3* m_ray;
-	float32 m_x;
-
-	b3SpringCloth * m_cloth;
-	u32 m_selection;
-	float32 m_u, m_v;
-	b3MassType m_t1, m_t2, m_t3;
-};
-
-class SpringClothTest : public Test
-{
-public:
-	SpringClothTest() : m_clothDragger(&m_clothRay, &m_cloth)
-	{
-		m_clothRay.origin.SetZero();
-		m_clothRay.direction.Set(0.0f, 0.0f, -1.0f);
-		m_clothRay.fraction = g_camera->m_zFar;
-	}
-
-	void Step()
-	{
-		float32 dt = g_testSettings->inv_hertz;
-
-		m_cloth.Step(dt);
-		m_cloth.Apply();
-
-		b3Shape** shapes = m_cloth.GetShapes();
-		for (u32 i = 0; i < m_cloth.GetShapeCount(); ++i)
-		{
-			b3Shape* s = shapes[i];
-
-			b3Transform xf;
-			xf.SetIdentity();
-
-			g_draw->DrawSolidShape(s, b3Color_white, xf);
-		}
-
-		m_cloth.Draw();
-
-		b3SpringClothStep step = m_cloth.GetStep();
-
-		g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations);
-		
-		float32 E = m_cloth.GetEnergy();
-		g_draw->DrawString(b3Color_white, "E = %f", E);
-
-		if (m_clothDragger.IsSelected() == true)
-		{
-			g_draw->DrawSegment(m_clothDragger.GetPointA(), m_clothDragger.GetPointB(), b3Color_white);
-		}
-	}
-
-	void MouseMove(const Ray3& pw)
-	{
-		m_clothRay = pw;
-
-		if (m_clothDragger.IsSelected() == true)
-		{
-			m_clothDragger.Drag();
-		}
-	}
-
-	void MouseLeftDown(const Ray3& pw)
-	{
-		if (m_clothDragger.IsSelected() == false)
-		{
-			m_clothDragger.StartDragging();
-		}
-	}
-
-	void MouseLeftUp(const Ray3& pw)
-	{
-		if (m_clothDragger.IsSelected() == true)
-		{
-			m_clothDragger.StopDragging();
-		}
-	}
-
-	Ray3 m_clothRay;
-
-	b3StackAllocator m_clothAllocator;
-	b3SpringCloth m_cloth;
-	
-	ClothDragger m_clothDragger;
-};
-
-#endif

examples/testbed/tests/table_cloth.h

@@ -19,7 +19,7 @@
 #ifndef TABLE_CLOTH_H
 #define TABLE_CLOTH_H
 
-class TableCloth : public SpringClothTest
+class TableCloth : public ClothTest
 {
 public:
 	TableCloth()
@@ -48,14 +48,12 @@ public:
 		m_gridClothMesh.sewingLineCount = 0;
 		m_gridClothMesh.sewingLines = nullptr;
 
-		b3SpringClothDef def;
+		b3ClothDef def;
-		def.allocator = &m_clothAllocator;
 		def.mesh = &m_gridClothMesh;
 		def.density = 0.2f;
 		def.ks = 10000.0f;
 		def.kd = 0.0f;
 		def.r = 0.05f;
-		def.gravity.Set(0.0f, -10.0f, 0.0f);
 
 		m_cloth.Initialize(def);
 

examples/testbed/tests/tension_mapping.h

@@ -55,7 +55,7 @@ static inline b3Color Color(float32 x, float32 a, float32 b)
 	return c;
 }
 
-class TensionMapping : public SpringClothTest
+class TensionMapping : public ClothTest
 {
 public:
 	TensionMapping()
@@ -78,14 +78,12 @@ public:
 		m_gridClothMesh.sewingLineCount = 0;
 		m_gridClothMesh.sewingLines = nullptr;
 		
-		b3SpringClothDef def;
+		b3ClothDef def;
-		def.allocator = &m_clothAllocator;
 		def.mesh = &m_gridClothMesh;
 		def.density = 0.2f;
 		def.ks = 10000.0f;
 		def.kd = 0.0f;
 		def.r = 0.2f;
-		def.gravity.Set(0.0f, -10.0f, 0.0f);
 
 		m_cloth.Initialize(def);
 
@@ -93,11 +91,12 @@ public:
 		aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
 		aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
 
-		for (u32 i = 0; i < m_cloth.GetMassCount(); ++i)
+		for (u32 i = 0; i < m_cloth.GetParticleCount(); ++i)
 		{
-			if (aabb.Contains(m_cloth.GetPosition(i)))
+			b3Particle* p = m_cloth.GetParticle(i);
+			if (aabb.Contains(p->position))
 			{
-				m_cloth.SetType(i, b3MassType::e_staticMass);
+				m_cloth.SetType(p, e_staticParticle);
 			}
 		}
 	}
@@ -109,28 +108,29 @@ public:
 		m_cloth.Step(dt);
 		m_cloth.Apply();
 
-		b3StackArray<b3Vec3, 100> T;
-		m_cloth.GetTension(T);
-
 		for (u32 i = 0; i < m_gridClothMesh.triangleCount; ++i)
 		{
 			b3ClothMeshTriangle* t = m_gridClothMesh.triangles + i;
 
-			b3Vec3 v1 = m_gridClothMesh.vertices[t->v1];
+			b3Particle* p1 = m_cloth.GetParticle(t->v1);
-			b3Vec3 v2 = m_gridClothMesh.vertices[t->v2];
+			b3Particle* p2 = m_cloth.GetParticle(t->v2);
-			b3Vec3 v3 = m_gridClothMesh.vertices[t->v3];
+			b3Particle* p3 = m_cloth.GetParticle(t->v3);
+
+			b3Vec3 v1 = p1->position;
+			b3Vec3 v2 = p2->position;
+			b3Vec3 v3 = p3->position;
 
 			b3Draw_draw->DrawSegment(v1, v2, b3Color_black);
 			b3Draw_draw->DrawSegment(v2, v3, b3Color_black);
 			b3Draw_draw->DrawSegment(v3, v1, b3Color_black);
 
-			b3Vec3 f1 = T[t->v1];
+			b3Vec3 f1 = p1->tension;
 			float32 L1 = b3Length(f1);
 
-			b3Vec3 f2 = T[t->v2];
+			b3Vec3 f2 = p2->tension;
 			float32 L2 = b3Length(f2);
 
-			b3Vec3 f3 = T[t->v3];
+			b3Vec3 f3 = p3->tension;
 			float32 L3 = b3Length(f3);
 
 			float32 L = (L1 + L2 + L3) / 3.0f;
@@ -140,16 +140,14 @@ public:
 			
 			b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
 			n1.Normalize();
+			g_draw->DrawSolidTriangle(n1, v1, v2, v3, color);
 
 			b3Vec3 n2 = -n1;
-
-			g_draw->DrawSolidTriangle(n1, v1, v2, v3, color);
 			g_draw->DrawSolidTriangle(n2, v1, v3, v2, color);
 		}
 
-		b3SpringClothStep step = m_cloth.GetStep();
+		extern u32 b3_clothSolverIterations;
-
+		g_draw->DrawString(b3Color_white, "Iterations = %u", b3_clothSolverIterations);
-		g_draw->DrawString(b3Color_white, "Iterations = %u", step.iterations);
 
 		if (m_clothDragger.IsSelected() == true)
 		{

include/bounce/bounce.h

@@ -60,7 +60,6 @@
 
 #include <bounce/dynamics/cloth/cloth_mesh.h>
 #include <bounce/dynamics/cloth/cloth.h>
-#include <bounce/dynamics/cloth/spring_cloth.h>
 
 #include <bounce/dynamics/body.h>
 

include/bounce/dynamics/cloth/cloth.h

@@ -19,117 +19,324 @@
 #ifndef B3_CLOTH_H
 #define B3_CLOTH_H
 
-#include <bounce/common/math/vec3.h>
+#include <bounce/common/math/mat33.h>
-#include <bounce/collision/collision.h>
+#include <bounce/common/template/array.h>
+#include <bounce/common/memory/stack_allocator.h>
 
-struct b3Mesh;
+// Maximum number of shapes per cloth.
+#define B3_CLOTH_SHAPE_CAPACITY 32
 
+class b3Shape;
+
+struct b3ClothMesh;
+
+// Cloth mesh definition
 struct b3ClothDef
 {
 	b3ClothDef()
 	{
-		mesh = NULL;
+		mesh = nullptr;
 		density = 0.0f;
-		gravity.SetZero();
+		r = 0.05f;
-		k1 = 0.9f;
+		ks = 0.0f;
-		k2 = 0.2f;
+		kb = 0.0f;
-		kd = 0.1f;
+		kd = 0.0f;
-		r = 0.0f;
 	}
 
-	// Cloth mesh
+	// Cloth proxy mesh 
-	// Each edge must be shared by at most two triangles (manifold)
+	b3ClothMesh* mesh;
-	const b3Mesh* mesh;
 
-	// Cloth density in kilograms per meter squared
+	// Radius
+	// This should be a small value. It can be used for correcting visual artifacts when 
+	// the masses are colliding against a solid.
+	float32 r;
+
+	// Cloth density in kg/m^3
 	float32 density;
 
-	// Gravity force
-	b3Vec3 gravity;
-
 	// Streching stiffness
-	float32 k1;
+	float32 ks;
 
 	// Bending stiffness
-	float32 k2;
+	float32 kb;
 
-	// Damping
+	// Damping stiffness
 	float32 kd;
-
-	// Cloth thickness
-	float32 r;
 };
 
+// Static particles have zero mass and velocity, and therefore they can't move.
+// Kinematic particles are't moved by external and internal forces but can be moved by contact forces.
+// Dynamic particles have non-zero mass and can move due to internal and external forces.
+enum b3ParticleType
+{
+	e_staticParticle,
+	e_kinematicParticle,
+	e_dynamicParticle
+};
+
+// Read-only particle 
 struct b3Particle
 {
-	float32 im;
+	// Particle type
-	b3Vec3 p0;
+	b3ParticleType type;
-	b3Vec3 p;
+
-	b3Vec3 v;
+	// Mass position
+	b3Vec3 position;
+
+	// Mass velocity
+	b3Vec3 velocity;
+	
+	// Mass force
+	b3Vec3 force;
+	
+	// Mass tension force for visualization
+	b3Vec3 tension;
+
+	// Mass
+	float32 mass;
+	
+	// Inverse mass
+	float32 invMass;
+	
+	// Radius
+	float32 radius;
+
+	// User data
+	void* userData;
+
+	// Translation used for direct position manipulation
+	b3Vec3 translation;
+
+	// Solver temp
+
+	// Identifier
+	u32 solverId;
+	
+	// Solution
+	b3Vec3 x;
 };
 
-struct b3C1
+// Spring types
+enum b3SpringType
 {
-	float32 L;
+	e_strechSpring,
-	u32 i1;
+	e_bendSpring,
-	u32 i2;
 };
 
-struct b3C2
+// Read-only spring
+struct b3Spring
 {
-	float32 angle;
+	// Spring type
-	u32 i1;
+	b3SpringType type;
-	u32 i2;
+
-	u32 i3;
+	// Particle 1
-	u32 i4;
+	b3Particle* p1;
+
+	// Particle 2
+	b3Particle* p2;
+
+	// Rest length
+	float32 L0;
+
+	// Structural stiffness
+	float32 ks;
+	
+	// Damping stiffness
+	float32 kd;
 };
 
+// Read-only contact
+struct b3ParticleContact
+{
+	b3Particle* p1;
+	b3Shape* s2;
+	b3Vec3 n, t1, t2;
+	float32 Fn, Ft1, Ft2;
+	bool n_active, t1_active, t2_active;
+};
+
+// A cloth represents a deformable surface/mesh.
+// b3Cloth simulates this surface motion using particles and springs.
 class b3Cloth
 {
 public:
 	b3Cloth();
 	~b3Cloth();
 
+	// Initialize this cloth from a definition.
 	void Initialize(const b3ClothDef& def);
 
-	void Step(float32 dt, u32 iterations);
+	// Return the cloth mesh used to initialize this cloth.
+	b3ClothMesh* GetMesh() const;
 
-	u32 GetVertexCount() const
+	// Set the gravitational acceleration applied to this cloth.
-	{
+	// Units are m/s^2.
-		return m_pCount;
+	void SetGravity(const b3Vec3& gravity);
-	}
 
-	const b3Particle* GetVertices() const
+	// Return the gravitational acceleration applied to this cloth.
-	{
+	const b3Vec3& GetGravity() const;
-		return m_ps;
-	}
 
-	b3Particle* GetVertices()
+	// Return the number of particles in this cloth.
-	{
+	u32 GetParticleCount() const;
-		return m_ps;
-	}
 
+	// Return the particle at a given index in this cloth.
+	b3Particle* GetParticle(u32 i) const;
+	
+	// Set the type of a given particle.
+	void SetType(b3Particle* p, b3ParticleType type);
+
+	// Translate a particle in the next time step.
+	void Translate(b3Particle* p, const b3Vec3& translation);
+
+	// Set the velocity of a given particle.
+	void SetVelocity(b3Particle* p, const b3Vec3& velocity);
+
+	// Apply a force to a given particle.
+	void ApplyForce(b3Particle* p, const b3Vec3& force);
+
+	// Return the kinetic (or dynamic) energy in this system.
+	float32 GetEnergy() const;
+
+	// Add a collision shape to the list of shapes in this cloth. 
+	// The cloth will be able to respond to collisions with each shape in the list of shapes.
+	// Current the shape will be treated as a static shape.
+	void AddShape(b3Shape* shape);
+
+	// Return the number of collision shapes in this cloth.
+	u32 GetShapeCount() const;
+
+	// Return the list of collision shapes added to this cloth.
+	b3Shape** GetShapeList();
+
+	// Perform a time step.
+	void Step(float32 dt);
+
+	// Set the positions of the mesh vertices to the positions of their associated particles.
+	void Apply() const;
+	
+	// Debug draw the cloth using the associated cloth mesh.
 	void Draw() const;
-private:
+protected:
-	void SolveC1();
+	// Compute mass of each particle.
-	void SolveC2();
+	void ResetMass();
 
-	b3Particle* m_ps;
+	// Update contacts. 
-	u32 m_pCount;
+	// This is where some contacts might be initiated or terminated.
+	void UpdateContacts();
 
-	b3C1* m_c1s;
+	// Solve
-	u32 m_c1Count;
+	void Solve(float32 dt);
 
-	b3C2* m_c2s;
+	b3StackAllocator m_allocator;
-	u32 m_c2Count;
 
-	float32 m_k1;
-	float32 m_k2;
-	float32 m_kd;
-	float32 m_r;
 	b3Vec3 m_gravity;
 
-	const b3Mesh* m_mesh;
+	u32 m_particleCount;
+	b3Particle* m_particles;
+
+	b3Spring* m_springs;
+	u32 m_springCount;
+
+	b3ParticleContact* m_contacts;
+	//u32 m_contactCount;
+
+	b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY];
+	u32 m_shapeCount;
+
+	b3ClothMesh* m_mesh;
+	float32 m_density;
 };
 
+inline b3ClothMesh* b3Cloth::GetMesh() const
+{
+	return m_mesh;
+}
+
+inline const b3Vec3& b3Cloth::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline void b3Cloth::SetGravity(const b3Vec3& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline u32 b3Cloth::GetParticleCount() const
+{
+	return m_particleCount;
+}
+
+inline b3Particle* b3Cloth::GetParticle(u32 i) const
+{
+	B3_ASSERT(i < m_particleCount);
+	return m_particles + i;
+}
+
+inline void b3Cloth::SetType(b3Particle* p, b3ParticleType type)
+{
+	if (p->type == type)
+	{
+		return;
+	}
+
+	p->type = type;
+	p->force.SetZero();
+	
+	if (type == e_staticParticle)
+	{
+		p->velocity.SetZero();
+		p->translation.SetZero();
+
+		u32 ip = u32(p - m_particles);
+
+		m_contacts[ip].n_active = false;
+		m_contacts[ip].t1_active = false;
+		m_contacts[ip].t2_active = false;
+	}
+}
+
+inline void b3Cloth::Translate(b3Particle* p, const b3Vec3& translation)
+{
+	p->translation += translation;
+}
+
+inline void b3Cloth::SetVelocity(b3Particle* p, const b3Vec3& velocity)
+{
+	if (p->type == e_staticParticle)
+	{
+		return;
+	}
+	p->velocity = velocity;
+}
+
+inline void b3Cloth::ApplyForce(b3Particle* p, const b3Vec3& force)
+{
+	if (p->type != e_dynamicParticle)
+	{
+		return;
+	}
+	p->force += force;
+}
+
+inline float32 b3Cloth::GetEnergy() const
+{
+	float32 E = 0.0f;
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		E += m_particles[i].mass * b3Dot(m_particles[i].velocity, m_particles[i].velocity);
+	}
+	return 0.5f * E;
+}
+
+inline u32 b3Cloth::GetShapeCount() const
+{
+	return m_shapeCount;
+}
+
+inline b3Shape** b3Cloth::GetShapeList() 
+{
+	return m_shapes;
+}
+
 #endif

include/bounce/dynamics/cloth/cloth_solver.h

@@ -16,48 +16,48 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef B3_SPRING_SOLVER_H
+#ifndef B3_CLOTH_SOLVER_H
-#define B3_SPRING_SOLVER_H
+#define B3_CLOTH_SOLVER_H
 
 #include <bounce/common/math/vec3.h>
 #include <bounce/common/math/mat33.h>
 
-class b3Shape;
-
-class b3SpringCloth;
-class b3StackAllocator;
-
 struct b3DenseVec3;
 struct b3DiagMat33;
 struct b3SparseMat33;
 
-struct b3MassContact;
+struct b3Particle;
 struct b3Spring;
+struct b3ParticleContact;
 
-enum class b3MassType : u32;
+class b3Shape;
+class b3StackAllocator;
 
-struct b3SpringSolverDef
+struct b3ClothSolverDef
 {
-	b3SpringCloth* cloth;
+	b3StackAllocator* stack;
-	float32 dt;
+	u32 particleCapacity;
+	u32 springCapacity;
+	u32 contactCapacity;
 };
 
-class b3SpringSolver
+class b3ClothSolver
 {
 public:
-	b3SpringSolver(const b3SpringSolverDef& def);
+	b3ClothSolver(const b3ClothSolverDef& def);
+	~b3ClothSolver();
 	
-	~b3SpringSolver();
+	void Add(b3Particle* p);
+	void Add(b3Spring* s);
+	void Add(b3ParticleContact* c);
 
-	void Solve(b3DenseVec3& extraForces);
+	void Solve(float32 dt, const b3Vec3& gravity);
-
-	u32 GetIterations() const;
 private:
-	// Apply internal forces and store their unique derivatives.
+	// Compute forces.
-	void ApplySpringForces();
+	void Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity);
 	
 	// Compute A and b in Ax = b
-	void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b) const;
+	void Compute_A_b(b3SparseMat33& A, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const;
 
 	// Compute S.
 	void Compute_S(b3DiagMat33& S);
@@ -67,38 +67,25 @@ private:
 
 	// Solve Ax = b.
 	// Output x and the residual error f = Ax - b ~ 0.
-	void Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
+	void Solve(b3DenseVec3& x, u32& iterations, const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const;
-
-	b3SpringCloth * m_cloth;
-	float32 m_h;
-	b3Mat33* m_Jx;
-	b3Mat33* m_Jv;
-	u32 m_iterations;
 
 	b3StackAllocator* m_allocator;
 
-	b3Vec3* m_x;
+	float32 m_h;
-	b3Vec3* m_v;
-	b3Vec3* m_f;
-	float32* m_m;
-	float32* m_inv_m;
-	b3Vec3* m_y;
-	b3Vec3* m_z;
-	b3Vec3* m_x0;
-	b3MassType* m_types;
-	u32 m_massCount;
 	
-	b3MassContact* m_contacts;
+	u32 m_particleCapacity;
+	u32 m_particleCount;
+	b3Particle** m_particles;
 
-	b3Spring* m_springs;
+	u32 m_springCapacity;
 	u32 m_springCount;
+	b3Spring** m_springs;
+	b3Mat33* m_Jx;
+	b3Mat33* m_Jv;
 
-	b3Shape** m_shapes;
+	u32 m_contactCapacity;
+	u32 m_contactCount;
+	b3ParticleContact** m_contacts;
 };
 
-inline u32 b3SpringSolver::GetIterations() const
-{
-	return m_iterations;
-}
-
 #endif

include/bounce/dynamics/cloth/spring_cloth.h

@@ -1,356 +0,0 @@
-/*
-* 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_SPRING_CLOTH_H
-#define B3_SPRING_CLOTH_H
-
-#include <bounce/common/math/mat33.h>
-#include <bounce/common/template/array.h>
-
-// Maximum number of shapes per cloth.
-#define B3_CLOTH_SHAPE_CAPACITY 32
-
-class b3StackAllocator;
-
-class b3Shape;
-
-struct b3ClothMesh;
-
-struct b3SpringClothDef
-{
-	b3SpringClothDef()
-	{
-		allocator = nullptr;
-		mesh = nullptr;
-		density = 0.0f;
-		ks = 0.0f;
-		kb = 0.0f;
-		kd = 0.0f;
-		r = 0.05f;
-		gravity.SetZero();
-	}
-
-	// Stack allocator
-	b3StackAllocator* allocator;
-
-	// Cloth mesh	
-	b3ClothMesh* mesh;
-
-	// Cloth density in kg/m^3
-	float32 density;
-
-	// Streching stiffness
-	float32 ks;
-
-	// Bending stiffness
-	float32 kb;
-
-	// Damping stiffness
-	float32 kd;
-	
-	// Mass radius
-	// This should be a small value. It can be used for correcting visual artifacts when 
-	// the masses are colliding against a solid.
-	float32 r;
-	
-	// Acceleration due to gravity (m/s^2)
-	b3Vec3 gravity;
-};
-
-enum b3SpringType
-{
-	e_strechSpring,
-	e_bendSpring,
-	// e_sewingSpring
-};
-
-struct b3Spring
-{
-	// Spring type
-	b3SpringType type;
-
-	// Mass 1
-	u32 i1;
-
-	// Mass 2
-	u32 i2;
-
-	// Rest length
-	float32 L0;
-
-	// Structural stiffness
-	float32 ks;
-	
-	// Damping stiffness
-	float32 kd;
-};
-
-// Static masses have zero mass and velocity, and therefore they can't move.
-// Kinematic masses are't moved by external and internal forces but can be moved by contact forces.
-// Dynamic masses have non-zero mass and can move due to internal and external forces.
-enum class b3MassType : u32
-{
-	e_staticMass,
-	e_kinematicMass,
-	e_dynamicMass
-};
-
-// 
-struct b3MassContact
-{
-	u32 j;
-	b3Vec3 n, t1, t2;
-	float32 Fn, Ft1, Ft2;
-	bool lockN, lockT1, lockT2;
-};
-
-// Time step statistics
-struct b3SpringClothStep
-{
-	u32 iterations;
-};
-
-// A cloth it treats cloth as a collection of masses connected by springs. 
-// Large time steps can be taken.
-// If accuracy and stability are required, not performance, 
-// you can use this class instead of using b3Cloth.
-class b3SpringCloth
-{
-public:
-	b3SpringCloth();
-	~b3SpringCloth();
-
-	// Initialize this cloth from a definition.
-	void Initialize(const b3SpringClothDef& def);
-
-	// Return the cloth mesh used to initialize this cloth.
-	b3ClothMesh* GetMesh() const;
-
-	// Set the gravitational acceleration applied to this cloth.
-	// Units are m/s^2.
-	void SetGravity(const b3Vec3& gravity);
-
-	// Return the number of masses in this cloth.
-	u32 GetMassCount() const;
-
-	// Return the gravitational acceleration applied to this cloth.
-	const b3Vec3& GetGravity() const;
-
-	// Set the type of a given point mass.
-	void SetType(u32 i, b3MassType type);
-
-	// Return the type of a given point mass.
-	b3MassType GetType(u32 i) const;
-
-	// Set the position of a given point mass.
-	// This function will have effect on the position of the point mass 
-	// after performing a time step.
-	void SetPosition(u32 i, const b3Vec3& position);
-
-	// Return the position of a given point mass.
-	const b3Vec3& GetPosition(u32 i) const;
-
-	// Set the velocity of a given point mass.
-	void SetVelocity(u32 i, const b3Vec3& velocity);
-
-	// Return the velocity of a given point mass.
-	const b3Vec3& GetVelocity(u32 i) const;
-	
-	// Apply a force to a given point mass.
-	void ApplyForce(u32 i, const b3Vec3& force);
-
-	// Return the kinetic (or dynamic) energy in this system.
-	float32 GetEnergy() const;
-	
-	// Return the tension forces (due to springs) acting at each point mass.
-	// Units are kg * m / s^2
-	void GetTension(b3Array<b3Vec3>& tensions) const;
-
-	// Add a shape to the list of shapes in this cloth. 
-	// The cloth will be able to respond to collisions with each shape in the list of shapes.
-	void AddShape(b3Shape* shape);
-
-	// Return the number of shapes added to this cloth.
-	u32 GetShapeCount() const;
-
-	// Return the list of shapes added to this cloth.
-	b3Shape** GetShapes();
-
-	// Return the statistics of the last time step.
-	const b3SpringClothStep& GetStep() const;
-
-	// Perform a time step (marches time forward).
-	void Step(float32 dt);
-
-	// Set the positions of the mesh vertices to the positions of their associated point masses.
-	void Apply() const;
-
-	// Debug draw the cloth mesh.
-	void Draw() const;
-protected:
-	friend class b3SpringSolver;
-	
-	// Update contacts. 
-	// This is where some contacts might be initiated or terminated.
-	void UpdateContacts();
-
-	b3StackAllocator* m_allocator;
-
-	b3ClothMesh* m_mesh;
-	float32 m_r;
-
-	b3Vec3 m_gravity;
-
-	b3Vec3* m_x;
-	b3Vec3* m_v;
-	b3Vec3* m_f;
-	float32* m_m;
-	float32* m_inv_m;
-	b3Vec3* m_y;
-	b3Vec3* m_z;
-	b3Vec3* m_x0;
-	b3MassType* m_types;
-	u32 m_massCount;
-
-	b3MassContact* m_contacts;
-	
-	b3Spring* m_springs;
-	u32 m_springCount;
-
-	b3Shape* m_shapes[B3_CLOTH_SHAPE_CAPACITY];
-	u32 m_shapeCount;
-
-	b3SpringClothStep m_step;
-};
-
-inline b3ClothMesh* b3SpringCloth::GetMesh() const
-{
-	return m_mesh;
-}
-
-inline const b3Vec3& b3SpringCloth::GetGravity() const
-{
-	return m_gravity;
-}
-
-inline void b3SpringCloth::SetGravity(const b3Vec3& gravity)
-{
-	m_gravity = gravity;
-}
-
-inline u32 b3SpringCloth::GetMassCount() const
-{
-	return m_massCount;
-}
-
-inline b3MassType b3SpringCloth::GetType(u32 i) const
-{
-	B3_ASSERT(i < m_massCount);
-	return m_types[i];
-}
-
-inline void b3SpringCloth::SetType(u32 i, b3MassType type)
-{
-	B3_ASSERT(i < m_massCount);
-	
-	if (m_types[i] == type)
-	{
-		return;
-	}
-
-	m_types[i] = type;
-	
-	m_f[i].SetZero();
-	
-	if (type == b3MassType::e_staticMass)
-	{
-		m_v[i].SetZero();
-		m_y[i].SetZero();
-		
-		m_contacts[i].lockN = false;
-	}
-}
-
-inline void b3SpringCloth::SetPosition(u32 i, const b3Vec3& position)
-{
-	B3_ASSERT(i < m_massCount);
-	m_y[i] += position - m_x[i];
-}
-
-inline const b3Vec3& b3SpringCloth::GetPosition(u32 i) const
-{
-	B3_ASSERT(i < m_massCount);
-	return m_x[i];
-}
-
-inline void b3SpringCloth::SetVelocity(u32 i, const b3Vec3& velocity)
-{
-	B3_ASSERT(i < m_massCount);
-	
-	if (m_types[i] == b3MassType::e_staticMass)
-	{
-		return;
-	}
-
-	m_v[i] = velocity;
-}
-
-inline const b3Vec3& b3SpringCloth::GetVelocity(u32 i) const
-{
-	B3_ASSERT(i < m_massCount);
-	return m_v[i];
-}
-
-inline void b3SpringCloth::ApplyForce(u32 i, const b3Vec3& force)
-{
-	B3_ASSERT(i < m_massCount);
-	
-	if (m_types[i] != b3MassType::e_dynamicMass)
-	{
-		return;
-	}
-
-	m_f[i] += force;
-}
-
-inline float32 b3SpringCloth::GetEnergy() const
-{
-	float32 E = 0.0f;
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		E += m_m[i] * b3Dot(m_v[i], m_v[i]);
-	}
-	return 0.5f * E;
-}
-
-inline u32 b3SpringCloth::GetShapeCount() const
-{
-	return m_shapeCount;
-}
-
-inline b3Shape** b3SpringCloth::GetShapes() 
-{
-	return m_shapes;
-}
-
-inline const b3SpringClothStep& b3SpringCloth::GetStep() const
-{
-	return m_step;
-}
-
-#endif

src/bounce/dynamics/cloth/cloth.cpp

@@ -17,101 +17,116 @@
 */
 
 #include <bounce/dynamics/cloth/cloth.h>
-#include <bounce/collision/shapes/mesh.h>
+#include <bounce/dynamics/cloth/cloth_solver.h>
-#include <bounce/common/template/array.h>
+#include <bounce/dynamics/cloth/dense_vec3.h>
+#include <bounce/dynamics/cloth/sparse_mat33.h>
+#include <bounce/dynamics/cloth/cloth_mesh.h>
+#include <bounce/dynamics/shapes/shape.h>
+#include <bounce/common/memory/stack_allocator.h>
 #include <bounce/common/draw.h>
 
+#define B3_FORCE_THRESHOLD 0.005f
+
+#define B3_CLOTH_BENDING 0
+
+#define B3_CLOTH_FRICTION 0
+
 b3Cloth::b3Cloth()
 {
-	m_pCount = 0;
+	m_gravity.SetZero();
-	m_ps = NULL;
+
-	m_c1Count = 0;
+	m_particleCount = 0;
-	m_c1s = NULL;
+	m_particles = nullptr;
-	m_c2Count = 0;
+
-	m_c2s = NULL;
+	m_springs = nullptr;
+	m_springCount = 0;
+
+	m_contacts = nullptr;
+	//m_contactCount = 0;
+
+	m_shapeCount = 0;
+
+	m_mesh = nullptr;
 
-	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_particles);
-	b3Free(m_c1s);
+	b3Free(m_springs);
-	b3Free(m_c2s);
+	b3Free(m_contacts);
 }
 
-void b3Cloth::Initialize(const b3ClothDef& def)
+static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
 {
-	B3_ASSERT(def.mesh);
+	return i + 1 < 3 ? i + 1 : 0;
-	m_mesh = def.mesh;
+}
 
-	const b3Mesh* m = m_mesh;
+struct b3UniqueEdge
+{
+	u32 v1, v2;
+};
 
-	m_pCount = m->vertexCount;
+static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
-	m_ps = (b3Particle*)b3Alloc(m_pCount * sizeof(b3Particle));
+{
-	
+	u32 uniqueCount = 0;
-	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;
+		b3ClothMeshTriangle* t1 = m->triangles + i;
+		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
 
-		u32 is[3] = { t->v1, t->v2, t->v3 };
+		for (u32 j1 = 0; j1 < 3; ++j1)
-		for (u32 j = 0; j < 3; ++j)
 		{
-			u32 k = j + 1 < 3 ? j + 1 : 0;
+			u32 t1v1 = i1s[j1];
+			u32 t1v2 = i1s[b3NextIndex(j1)];
 
-			u32 v1 = is[j];
+			bool unique = true;
-			u32 v2 = is[k];
 
-			b3Vec3 p1 = m->vertices[v1];
+			for (u32 j = 0; j < uniqueCount; ++j)
-			b3Vec3 p2 = m->vertices[v2];
-
-			b3C1* C = m_c1s + m_c1Count;
-			C->i1 = v1;
-			C->i2 = v2;
-			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;
+				b3UniqueEdge* ue = uniqueEdges + j;
+
+				if (ue->v1 == t1v1 && ue->v2 == t1v2)
+				{
+					unique = false;
+					break;
 				}
 				
-	u32 c2Capacity = 0;
+				if (ue->v2 == t1v1 && ue->v1 == t1v2)
+				{
+					unique = false;
+					break;
+				}
+			}
+
+			if (unique)
+			{
+				b3UniqueEdge ue;
+				ue.v1 = t1v1;
+				ue.v2 = t1v2;
+				uniqueEdges[uniqueCount++] = ue;
+			}
+		}
+	}
+
+	return uniqueCount;
+}
+
+struct b3SharedEdge
+{
+	u32 v1, v2;
+	u32 nsv1, nsv2;
+};
+
+static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
+{
+	u32 sharedCount = 0;
 
 	for (u32 i = 0; i < m->triangleCount; ++i)
 	{
-		b3Triangle* t1 = m->triangles + i;
+		b3ClothMeshTriangle* t1 = m->triangles + i;
 		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
 
 		for (u32 j1 = 0; j1 < 3; ++j1)
@@ -123,43 +138,7 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 
 			for (u32 j = i + 1; j < m->triangleCount; ++j)
 			{
-				b3Triangle* t2 = m->triangles + j;
+				b3ClothMeshTriangle* t2 = m->triangles + j;
-				u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
-
-				for (u32 j2 = 0; j2 < 3; ++j2)
-				{
-					u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0;
-
-					u32 t2v1 = i2s[j2];
-					u32 t2v2 = i2s[k2];
-
-					if (t1v1 == t2v2 && t1v2 == t2v1)
-					{
-						++c2Capacity;
-					}
-				}
-			}
-		}
-	}
-
-	m_c2Count = 0;
-	m_c2s = (b3C2*)b3Alloc(c2Capacity * sizeof(b3C2));
-
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3Triangle* t1 = m->triangles + i;
-		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
-
-		for (u32 j1 = 0; j1 < 3; ++j1)
-		{
-			u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0;
-
-			u32 t1v1 = i1s[j1];
-			u32 t1v2 = i1s[k1];
-
-			for (u32 j = i + 1; j < m->triangleCount; ++j)
-			{
-				b3Triangle* t2 = m->triangles + j;
 				u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
 
 				for (u32 j2 = 0; j2 < 3; ++j2)
@@ -171,36 +150,21 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 
 					if (t1v1 == t2v2 && t1v2 == t2v1)
 					{
+						// The triangles are adjacent.
 						u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0;
 						u32 t1v3 = i1s[k3];
 
 						u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0;
 						u32 t2v3 = i2s[k4];
 
-						b3Vec3 p1 = m->vertices[t1v1];
+						// Add shared edge and non-shared vertices.
-						b3Vec3 p2 = m->vertices[t1v2];
+						b3SharedEdge se;
-						b3Vec3 p3 = m->vertices[t1v3];
+						se.v1 = t1v1;
-						b3Vec3 p4 = m->vertices[t2v3];
+						se.v2 = t1v2;
+						se.nsv1 = t1v3;
+						se.nsv2 = t2v3;
 
-						b3Vec3 n1 = b3Cross(p2 - p1, p3 - p1);
+						sharedEdges[sharedCount++] = se;
-						n1.Normalize();
-						
-						b3Vec3 n2 = b3Cross(p2 - p1, p4 - p1);
-						n2.Normalize();
-						
-						float32 x = b3Dot(n1, n2);
-
-						b3Vec3 n3 = b3Cross(n1, n2);
-						float32 y = b3Length(n3);
-
-						b3C2* c = m_c2s + m_c2Count;
-						c->i1 = t1v1;
-						c->i2 = t1v2;
-						c->i3 = t1v3;
-						c->i4 = t2v3;
-						c->angle = atan2(y, x);
-						
-						++m_c2Count;
 
 						break;
 					}
@@ -209,187 +173,524 @@ void b3Cloth::Initialize(const b3ClothDef& def)
 		}
 	}
 
-	m_k1 = def.k1;
+	return sharedCount;
-	m_k2 = def.k2;
-	m_kd = def.kd;
-	m_r = def.r;
-	m_gravity = def.gravity;
 }
 
-void b3Cloth::Step(float32 h, u32 iterations)
+void b3Cloth::Initialize(const b3ClothDef& def)
 {
-	if (h == 0.0f)
+	B3_ASSERT(def.mesh);
+	B3_ASSERT(def.density > 0.0f);
+
+	m_mesh = def.mesh;
+	m_density = def.density;
+
+	const b3ClothMesh* m = m_mesh;
+
+	m_particleCount = m->vertexCount;
+	m_particles = (b3Particle*)b3Alloc(m_particleCount * sizeof(b3Particle));
+	m_contacts = (b3ParticleContact*)b3Alloc(m_particleCount * sizeof(b3ParticleContact));
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles + i;	
+		p->type = e_dynamicParticle;
+		p->position = m->vertices[i];
+		p->velocity.SetZero();
+		p->force.SetZero();
+		p->mass = 0.0f;
+		p->invMass = 0.0f;
+		p->radius = def.r;
+		p->userData = nullptr;
+
+		p->translation.SetZero();
+		p->x.SetZero();
+		p->tension.SetZero();
+
+		b3ParticleContact* c = m_contacts + i;
+		c->n_active = false;
+		c->t1_active = false;
+		c->t2_active = false;
+	}
+
+	// Compute mass
+	ResetMass();
+
+	// Initialize springs
+	u32 edgeCount = 3 * m->triangleCount;
+
+	b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator.Allocate(edgeCount * sizeof(b3UniqueEdge));
+	u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m);
+
+	u32 springCapacity = uniqueCount;
+	
+#if B3_CLOTH_BENDING
+	
+	b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator.Allocate(edgeCount * sizeof(b3SharedEdge));
+	u32 sharedCount = b3FindSharedEdges(sharedEdges, m);
+
+	springCapacity += sharedCount;
+
+#endif
+	
+	springCapacity += m->sewingLineCount;
+
+	m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring));
+
+	// Tension
+	for (u32 i = 0; i < uniqueCount; ++i)
+	{
+		b3UniqueEdge* e = uniqueEdges + i;
+
+		b3Vec3 v1 = m->vertices[e->v1];
+		b3Vec3 v2 = m->vertices[e->v2];
+
+		b3Particle* p1 = m_particles + e->v1;
+		b3Particle* p2 = m_particles + e->v2;
+
+		b3Spring* s = m_springs + m_springCount++;
+		s->type = e_strechSpring;
+		s->p1 = p1;
+		s->p2 = p2;
+		s->L0 = b3Distance(p1->position, p2->position);
+		s->ks = def.ks;
+		s->kd = def.kd;
+	}
+
+#if B3_CLOTH_BENDING
+
+	// Bending
+	for (u32 i = 0; i < sharedCount; ++i)
+	{
+		b3SharedEdge* e = sharedEdges + i;
+
+		b3Vec3 v1 = m->vertices[e->nsv1];
+		b3Vec3 v2 = m->vertices[e->nsv2];
+
+		b3Particle* p1 = m_particles + e->nsv1;
+		b3Particle* p2 = m_particles + e->nsv2;
+
+		b3Spring* s = m_springs + m_springCount++;
+		s->type = e_bendSpring;
+		s->p1 = p1;
+		s->p2 = p2;
+		s->L0 = b3Distance(p1->position, p2->position);
+		s->ks = def.kb;
+		s->kd = def.kd;
+	}
+
+	m_allocator.Free(sharedEdges);
+#endif
+
+	m_allocator.Free(uniqueEdges);
+
+	// Sewing
+	for (u32 i = 0; i < m->sewingLineCount; ++i)
+	{
+		b3ClothMeshSewingLine* line = m->sewingLines + i;
+		
+		b3Particle* p1 = m_particles + line->v1;
+		b3Particle* p2 = m_particles + line->v2;
+
+		b3Spring* s = m_springs + m_springCount++;
+		s->type = e_strechSpring;
+		s->p1 = p1;
+		s->p2 = p2;
+		s->L0 = 0.0f;
+		s->ks = def.ks;
+		s->kd = def.kd;
+	}
+
+	B3_ASSERT(m_springCount <= springCapacity);
+}
+
+void b3Cloth::ResetMass()
+{
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i].mass = 0.0f;
+		m_particles[i].invMass = 0.0f;
+	}
+
+	const float32 inv3 = 1.0f / 3.0f;
+	const float32 rho = m_density;
+
+	// Accumulate the mass about the body origin of all triangles.
+	for (u32 i = 0; i < m_mesh->triangleCount; ++i)
+	{
+		b3ClothMeshTriangle* triangle = m_mesh->triangles + i;
+		u32 v1 = triangle->v1;
+		u32 v2 = triangle->v2;
+		u32 v3 = triangle->v3;
+
+		b3Vec3 p1 = m_mesh->vertices[v1];
+		b3Vec3 p2 = m_mesh->vertices[v2];
+		b3Vec3 p3 = m_mesh->vertices[v3];
+
+		float32 area = b3Area(p1, p2, p3);
+		B3_ASSERT(area > B3_EPSILON);
+
+		float32 mass = rho * area;
+
+		m_particles[v1].mass += inv3 * mass;
+		m_particles[v2].mass += inv3 * mass;
+		m_particles[v3].mass += inv3 * mass;
+	}
+
+	// Invert
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		B3_ASSERT(m_particles[i].mass > 0.0f);
+		m_particles[i].invMass = 1.0f / m_particles[i].mass;
+	}
+}
+
+void b3Cloth::AddShape(b3Shape* shape)
+{
+	B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY);
+
+	if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY)
 	{
 		return;
 	}
 
-	float32 d = exp(-h * m_kd);
+	m_shapes[m_shapeCount++] = shape;
-
-	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)
-	{
-		SolveC2();
-		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()
+void b3Cloth::UpdateContacts()
 {
-	for (u32 i = 0; i < m_c1Count; ++i)
+	B3_PROFILE("Update Contacts");
+	
+	// Clear active flags
+	for (u32 i = 0; i < m_particleCount; ++i)
 	{
-		b3C1* c = m_c1s + i;
+		m_contacts[i].n_active = false;
+		m_contacts[i].t1_active = false;
+		m_contacts[i].t2_active = false;
+	}
 
-		b3Particle* p1 = m_ps + c->i1;
+	// Create contacts 
-		b3Particle* p2 = m_ps + c->i2;
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles + i;
 
-		float32 m1 = p1->im;
+		// Static particles can't participate in collisions.
-		float32 m2 = p2->im;
+		if (p->type == e_staticParticle)
-
-		float32 mass = m1 + m2;
-		if (mass == 0.0f)
 		{
 			continue;
 		}
 
-		mass = 1.0f / mass;
+		b3ParticleContact* c = m_contacts + i;
 
-		b3Vec3 J2 = p2->p - p1->p;
+		// Save the old contact
-		float32 L = b3Length(J2);
+		b3ParticleContact c0 = *c;
-		if (L > B3_EPSILON)
+
+		b3Sphere s1;
+		s1.vertex = p->position;
+		s1.radius = p->radius;
+
+		// Find the deepest penetration
+		float32 bestSeparation = 0.0f;
+		b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
+		u32 bestIndex = ~0;
+
+		for (u32 j = 0; j < m_shapeCount; ++j)
 		{
-			J2 /= L;
+			b3Shape* s2 = m_shapes[j];
-		}
 
-		b3Vec3 J1 = -J2;
+			b3Transform xf2;
+			xf2.SetIdentity();
 
-		float32 C = L - c->L;
+			b3TestSphereOutput output;
-		float32 impulse = -m_k1 * mass * C;
+			if (s2->TestSphere(&output, s1, xf2) == false)
-
-		p1->p += (m1 * impulse) * J1;
-		p2->p += (m2 * impulse) * J2;
-	}
-}
-
-void b3Cloth::SolveC2()
-{
-	for (u32 i = 0; i < m_c2Count; ++i)
-	{
-		b3C2* c = m_c2s + i;
-
-		b3Particle* p1 = m_ps + c->i1;
-		b3Particle* p2 = m_ps + c->i2;
-		b3Particle* p3 = m_ps + c->i3;
-		b3Particle* p4 = m_ps + c->i4;
-
-		float32 m1 = p1->im;
-		float32 m2 = p2->im;
-		float32 m3 = p3->im;
-		float32 m4 = p4->im;
-
-		b3Vec3 v2 = p2->p - p1->p;
-		b3Vec3 v3 = p3->p - p1->p;
-		b3Vec3 v4 = p4->p - p1->p;
-		
-		b3Vec3 n1 = b3Cross(v2, v3);
-		n1.Normalize();
-
-		b3Vec3 n2 = b3Cross(v2, v4);
-		n2.Normalize();
-
-		float32 x = b3Dot(n1, n2);
-
-		b3Vec3 J3 = b3Cross(v2, n2) + x * b3Cross(n1, v2);
-		float32 L3 = b3Length(b3Cross(v2, v3));
-		if (L3 > B3_EPSILON)
-		{
-			J3 /= L3;
-		}
-		
-		b3Vec3 J4 = b3Cross(v2, n1) + x * b3Cross(n2, v2);
-		float32 L4 = b3Length(b3Cross(v2, v4));
-		if (L4 > B3_EPSILON)
-		{
-			J4 /= L4;
-		}
-
-		b3Vec3 J2_1 = b3Cross(v3, n2) + x * b3Cross(n1, v3);
-		if (L3 > B3_EPSILON)
-		{
-			J2_1 /= L3;
-		}
-
-		b3Vec3 J2_2 = b3Cross(v4, n1) + x * b3Cross(n2, v4);
-		if (L4 > B3_EPSILON)
-		{
-			J2_2 /= L4;
-		}
-		
-		b3Vec3 J2 = -J2_1 - J2_2;
-		
-		b3Vec3 J1 = -J2 - J3 - J4;
-
-		float32 mass = m1 * b3Dot(J1, J1) + m2 * b3Dot(J2, J2) + m3 * b3Dot(J3, J3) + m4 * b3Dot(J4, J4);
-		if (mass == 0.0f)
 			{
 				continue;
 			}
 
-		mass = 1.0f / mass;
+			if (output.separation < bestSeparation)
+			{
+				bestSeparation = output.separation;
+				bestNormal = output.normal;
+				bestIndex = j;
+			}
+		}
 
-		b3Vec3 n3 = b3Cross(n1, n2);
+		if (bestIndex != ~0)
-		float32 y = b3Length(n3);
+		{
+			B3_ASSERT(bestSeparation <= 0.0f);
 
-		float32 angle = atan2(y, x);
+			b3Shape* shape = m_shapes[bestIndex];
-		float32 C = angle - c->angle;
+			float32 s = bestSeparation;
+			b3Vec3 n = bestNormal;
 
-		float32 impulse = -m_k2 * mass * y * C;
+			// Update contact manifold
+			// Remember the normal orientation is from shape 2 to shape 1 (mass)
+			c->n_active = true;
+			c->p1 = p;
+			c->s2 = shape;
+			c->n = n;
+			c->t1 = b3Perp(n);
+			c->t2 = b3Cross(c->t1, n);
 
-		p1->p += (m1 * impulse) * J1;
+			// Apply position correction
-		p2->p += (m2 * impulse) * J2;
+			p->translation -= s * n;
-		p3->p += (m3 * impulse) * J3;
+		}
-		p4->p += (m4 * impulse) * J4;
+
+		// Update contact state
+		if (c0.n_active == true && c->n_active == true)
+		{
+			// The contact persists
+			
+			// Has the contact constraint been satisfied?
+			if (c0.Fn <= -B3_FORCE_THRESHOLD)
+			{
+				// Contact force is attractive.
+
+				// Terminate the contact.
+				c->n_active = false;
+			}
+		}
+
+#if 0
+		// Notify the new contact state
+		if (c0.n_active == false && c->n_active == true)
+		{
+			// The contact has begun
+		}
+
+		if (c0.n_active == true && c->active_n == false)
+		{
+			// The contact has ended
+		}
+#endif
+		if (c->n_active == false)
+		{
+			continue;
+		}
+
+#if B3_CLOTH_FRICTION == 1
+
+		// A friction force requires an associated normal force.
+		if (c0.n_active == false)
+		{
+			continue;
+		}
+
+		b3Shape* s = c->s;
+		b3Vec3 n = c->n;
+		float32 u = s->GetFriction();
+		float32 normalForce = c0.Fn;
+
+		// Relative velocity
+		b3Vec3 dv = p->velocity;
+
+		b3Vec3 t1 = dv - b3Dot(dv, n) * n;
+		if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
+		{
+			// Create a dynamic basis
+			t1.Normalize();
+			
+			b3Vec3 t2 = b3Cross(t1, n);
+			t2.Normalize();
+
+			c->t1 = t1;
+			c->t2 = t2;
+		}
+		else
+		{
+			c->t1_active = true;
+			c->t2_active = true;
+			continue;
+		}
+
+		b3Vec3 ts[2];
+		ts[0] = c->t1;
+		ts[1] = c->t2;
+
+		bool t_active[2];
+		t_active[0] = c->t1_active;
+		t_active[1] = c->t2_active;
+
+		bool t_active0[2];
+		t_active0[0] = c0.t1_active;
+		t_active0[1] = c0.t2_active;
+
+		float32 Ft0[2];
+		Ft0[0] = c0.Ft1;
+		Ft0[1] = c0.Ft2;
+
+		for (u32 k = 0; k < 2; ++k)
+		{
+			b3Vec3 t = ts[k];
+
+			// Relative tangential velocity
+			float32 dvt = b3Dot(dv, t);
+
+			if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
+			{
+				// Lock particle on surface
+				t_active[k] = true;
+			}
+
+			if (t_active0[k] == true && t_active[k] == true)
+			{
+				// The contact persists
+				float32 maxForce = u * normalForce;
+				
+				if (Ft0[k] * Ft0[k] > maxForce * maxForce)
+				{
+					// Unlock particle off surface
+					t_active[k] = false;
+				}
+			}
+		}
+
+		c->t1_active = t_active[0];
+		c->t2_active = t_active[1];
+
+#endif
+
+	}
+
+}
+
+void b3Cloth::Solve(float32 dt)
+{
+	B3_PROFILE("Solve");
+
+	// Clear tension
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i].tension.SetZero();
+	}
+
+	// Solve
+	b3ClothSolverDef solverDef;
+	solverDef.stack = &m_allocator;
+	solverDef.particleCapacity = m_particleCount;
+	solverDef.springCapacity = m_springCount;
+	solverDef.contactCapacity = m_particleCount;
+
+	b3ClothSolver solver(solverDef);
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		solver.Add(m_particles + i);
+	}
+
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		solver.Add(m_springs + i);
+	}
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		if (m_contacts[i].n_active)
+		{
+			solver.Add(m_contacts + i);
+		}
+	}
+
+	// Solve
+	solver.Solve(dt, m_gravity);
+
+	// Clear external applied forces
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i].force.SetZero();
+	}
+
+	// Clear translations
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i].translation.SetZero();
+	}
+}
+
+void b3Cloth::Step(float32 dt)
+{
+	B3_PROFILE("Step");
+
+	// Update contacts
+	UpdateContacts();
+
+	// Solve constraints, integrate state, clear forces and translations. 
+	if (dt > 0.0f)
+	{
+		Solve(dt);
+	}
+}
+
+void b3Cloth::Apply() const
+{
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_mesh->vertices[i] = m_particles[i].position;
 	}
 }
 
 void b3Cloth::Draw() const
 {
-	const b3Mesh* m = m_mesh;
+	const b3ClothMesh* m = m_mesh;
 
 	for (u32 i = 0; i < m->vertexCount; ++i)
 	{
-		b3Draw_draw->DrawPoint(m_ps[i].p, 6.0f, b3Color_green);
+		b3Particle* p = m_particles + i;
+
+		if (p->type == e_staticParticle)
+		{
+			b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_white);
+		}
+		
+		if (p->type == e_kinematicParticle)
+		{
+			b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_blue);
+		}
+		
+		if (p->type == e_dynamicParticle)
+		{
+			b3Draw_draw->DrawPoint(p->position, 4.0f, b3Color_green);
+		}
+
+		b3ParticleContact* c = m_contacts + i;
+		
+		if (c->n_active)
+		{
+			b3Draw_draw->DrawSegment(p->position, p->position + c->n, b3Color_yellow);
+		}
+	}
+
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* s = m_springs + i;
+		b3Particle* p1 = s->p1;
+		b3Particle* p2 = s->p2;
+
+		if (s->type == e_strechSpring)
+		{
+			b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_black);
+		}
+	}
+	
+	for (u32 i = 0; i < m->sewingLineCount; ++i)
+	{
+		b3ClothMeshSewingLine* s = m->sewingLines + i;
+		b3Particle* p1 = m_particles + s->v1;
+		b3Particle* p2 = m_particles + s->v2;
+
+		b3Draw_draw->DrawSegment(p1->position, p2->position, b3Color_white);
 	}
 
 	for (u32 i = 0; i < m->triangleCount; ++i)
 	{
-		b3Triangle* t = m->triangles + i;
+		b3ClothMeshTriangle* t = m->triangles + i;
 
-		b3Particle* p1 = m_ps + t->v1;
+		b3Particle* p1 = m_particles + t->v1;
-		b3Particle* p2 = m_ps + t->v2;
+		b3Particle* p2 = m_particles + t->v2;
-		b3Particle* p3 = m_ps + t->v3;
+		b3Particle* p3 = m_particles + t->v3;
 
-		b3Vec3 v1 = p1->p;
+		b3Vec3 v1 = p1->position;
-		b3Vec3 v2 = p2->p;
+		b3Vec3 v2 = p2->position;
-		b3Vec3 v3 = p3->p;
+		b3Vec3 v3 = p3->position;
-
-		b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black);
 
 		b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
 		n1.Normalize();

src/bounce/dynamics/cloth/cloth_solver.cpp

@@ -0,0 +1,656 @@
+/*
+* 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/dynamics/cloth/cloth_solver.h>
+#include <bounce/dynamics/cloth/cloth.h>
+#include <bounce/dynamics/shapes/shape.h>
+#include <bounce/common/memory/stack_allocator.h>
+#include <bounce/dynamics/cloth/dense_vec3.h>
+#include <bounce/dynamics/cloth/diag_mat33.h>
+#include <bounce/dynamics/cloth/sparse_mat33.h>
+
+// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
+// described in the paper:
+// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
+
+// Some improvements for the original MPCG algorithm are described in the paper:
+// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
+
+u32 b3_clothSolverIterations = 0;
+
+b3ClothSolver::b3ClothSolver(const b3ClothSolverDef& def)
+{
+	m_allocator = def.stack;
+
+	m_particleCapacity = def.particleCapacity;
+	m_particleCount = 0;
+	m_particles = (b3Particle**)m_allocator->Allocate(m_particleCapacity * sizeof(b3Particle*));
+
+	m_springCapacity = def.springCapacity;
+	m_springCount = 0;
+	m_springs = (b3Spring**)m_allocator->Allocate(m_springCapacity * sizeof(b3Spring*));;
+	m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
+	m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCapacity * sizeof(b3Mat33));
+
+	m_contactCapacity = def.contactCapacity;
+	m_contactCount = 0;
+	m_contacts = (b3ParticleContact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3ParticleContact*));;
+}
+
+b3ClothSolver::~b3ClothSolver()
+{
+	m_allocator->Free(m_contacts);
+	m_allocator->Free(m_Jv);
+	m_allocator->Free(m_Jx);
+	m_allocator->Free(m_springs);
+	m_allocator->Free(m_particles);
+}
+
+void b3ClothSolver::Add(b3Particle* p)
+{
+	p->solverId = m_particleCount;
+	m_particles[m_particleCount++] = p;
+}
+
+void b3ClothSolver::Add(b3ParticleContact* c)
+{
+	m_contacts[m_contactCount++] = c;
+}
+
+void b3ClothSolver::Add(b3Spring* s)
+{
+	m_springs[m_springCount++] = s;
+}
+
+static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
+{
+	u32 size = M * N;
+	b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33));
+	u32* cols = (u32*)allocator->Allocate(size * sizeof(u32));
+	u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
+
+	return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
+}
+
+static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
+{
+	allocator->Free(matrix.row_ptrs);
+	allocator->Free(matrix.cols);
+	allocator->Free(matrix.values);
+}
+
+static B3_FORCE_INLINE void b3CopyPosition(b3DenseVec3& v, b3Particle** const particles, u32 count)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		v[i] = particles[i]->position;
+	}
+}
+
+static B3_FORCE_INLINE void b3CopyVelocity(b3DenseVec3& v, b3Particle** const particles, u32 count)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		v[i] = particles[i]->velocity;
+	}
+}
+
+static B3_FORCE_INLINE void b3CopyForce(b3DenseVec3& v, b3Particle** const particles, u32 count)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		v[i] = particles[i]->force;
+	}
+}
+
+static B3_FORCE_INLINE void b3CopyTranslation(b3DenseVec3& v, b3Particle** const particles, u32 count)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		v[i] = particles[i]->translation;
+	}
+}
+
+static B3_FORCE_INLINE void b3CopyGuess(b3DenseVec3& v, b3Particle** const particles, u32 count)
+{
+	for (u32 i = 0; i < count; ++i)
+	{
+		v[i] = particles[i]->x;
+	}
+}
+
+void b3ClothSolver::Solve(float32 dt, const b3Vec3& gravity)
+{
+	B3_PROFILE("Integrate");
+
+	m_h = dt;
+
+	b3DenseVec3 sx(m_particleCount);
+	b3CopyPosition(sx, m_particles, m_particleCount);
+
+	b3DenseVec3 sv(m_particleCount);
+	b3CopyVelocity(sv, m_particles, m_particleCount);
+
+	b3DenseVec3 sf(m_particleCount);
+	b3CopyForce(sf, m_particles, m_particleCount);
+
+	b3DenseVec3 sy(m_particleCount);
+	b3CopyTranslation(sy, m_particles, m_particleCount);
+
+	Compute_f(sf, sx, sv, gravity);
+
+	// Solve Ax = b, where
+	// A = M - h * dfdv - h * h * dfdx
+	// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
+
+	// A
+	b3SparseMat33 A = b3AllocSparse(m_allocator, m_particleCount, m_particleCount);
+
+	// b
+	b3DenseVec3 b(m_particleCount);
+
+	Compute_A_b(A, b, sf, sx, sv, sy);
+
+	// S
+	b3DiagMat33 S(m_particleCount);
+	Compute_S(S);
+
+	// z
+	b3DenseVec3 z(m_particleCount);
+	Compute_z(z);
+
+	// x0
+	b3DenseVec3 x0(m_particleCount);
+	b3CopyGuess(x0, m_particles, m_particleCount);
+
+	// x
+	b3DenseVec3 x(m_particleCount);
+
+	// Solve Ax = b
+	u32 iterations = 0;
+	Solve(x, iterations, A, b, S, z, x0);
+
+	b3_clothSolverIterations = iterations;
+
+	// f = A * x - b
+	b3DenseVec3 f = A * x - b;
+
+	// Update state
+	float32 h = m_h;
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+		b3ParticleType type = p->type;
+
+		b3Vec3 ix0 = sx[i];
+		b3Vec3 iv0 = sv[i];
+		b3Vec3 iy = sy[i];
+
+		b3Vec3 dv = x[i];
+
+		// v1 = v0 + dv
+		b3Vec3 v1 = iv0;
+		if (type == e_dynamicParticle)
+		{
+			v1 += dv;
+		}
+
+		// dx = h * (v0 + dv) + y = h * v1 + y
+		b3Vec3 dx = h * v1 + iy;
+
+		// x1 = x0 + dx
+		b3Vec3 x1 = ix0 + dx;
+
+		sv[i] = v1;
+		sx[i] = x1;
+	}
+
+	// Write x to the solution cache for improving convergence
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i]->x = x[i];
+	}
+
+	// Store the extra contact constraint forces that should have been 
+	// supplied in order to enforce the contact constraints exactly
+	// These forces can be used in contact constraint logic
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3ParticleContact* c = m_contacts[i];
+		b3Particle* p = c->p1;
+
+		b3Vec3 force = f[p->solverId];
+
+		// Signed normal force magnitude
+		c->Fn = b3Dot(force, c->n);
+
+		// Signed tangent force magnitude
+		c->Ft1 = b3Dot(force, c->t1);
+		c->Ft2 = b3Dot(force, c->t2);
+	}
+
+	// Copy state buffers back to the particles
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		m_particles[i]->position = sx[i];
+		m_particles[i]->velocity = sv[i];
+	}
+
+	b3FreeSparse(A, m_allocator);
+}
+
+#define B3_INDEX(i, j, size) (i + j * size)
+
+//
+static void b3SetZero(b3Vec3* out, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i].SetZero();
+	}
+}
+
+//
+static void b3SetZero(b3Mat33* out, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i].SetZero();
+	}
+}
+
+// 
+static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
+	const b3Mat33* J_ii, b3Spring** const springs, u32 springCount)
+{
+	b3SetZero(out, massCount);
+
+	for (u32 i = 0; i < springCount; ++i)
+	{
+		const b3Spring* S = springs[i];
+		u32 i1 = S->p1->solverId;
+		u32 i2 = S->p2->solverId;
+
+		b3Mat33 J_11 = J_ii[i];
+		b3Mat33 J_12 = -J_11;
+		b3Mat33 J_21 = J_12;
+		b3Mat33 J_22 = J_11;
+
+		out[i1] += J_11 * v[i1] + J_12 * v[i2];
+		out[i2] += J_21 * v[i1] + J_22 * v[i2];
+	}
+}
+
+void b3ClothSolver::Compute_f(b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3Vec3& gravity)
+{
+	// Zero force
+	f.SetZero();
+
+	// Apply weight
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+
+		if (p->type == e_dynamicParticle)
+		{
+			f[i] += p->mass * gravity;
+		}
+	}
+
+	// Apply tension, damping
+	// Store the Jacobians
+
+	// Zero Jacobians
+	b3SetZero(m_Jx, m_springCount);
+	b3SetZero(m_Jv, m_springCount);
+
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* S = m_springs[i];
+		b3SpringType type = S->type;
+		b3Particle* p1 = S->p1;
+		b3Particle* p2 = S->p2;
+		float32 L0 = S->L0;
+		float32 ks = S->ks;
+		float32 kd = S->kd;
+
+		u32 i1 = p1->solverId;
+		u32 i2 = p2->solverId;
+
+		b3Vec3 x1 = x[i1];
+		b3Vec3 v1 = v[i1];
+
+		b3Vec3 x2 = x[i2];
+		b3Vec3 v2 = v[i2];
+
+		const b3Mat33 I = b3Mat33_identity;
+
+		b3Vec3 dx = x1 - x2;
+
+		if (b3Dot(dx, dx) >= L0 * L0)
+		{
+			// Tension
+			float32 L = b3Length(dx);
+			b3Vec3 n = dx / L;
+
+			b3Vec3 sf1 = -ks * (L - L0) * n;
+			b3Vec3 sf2 = -sf1;
+
+			f[i1] += sf1;
+			f[i2] += sf2;
+
+			p1->tension += sf1;
+			p2->tension += sf2;
+
+			b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
+
+			m_Jx[i] = Jx11;
+		}
+
+		// Damping
+		b3Vec3 dv = v1 - v2;
+
+		b3Vec3 df1 = -kd * dv;
+		b3Vec3 df2 = -df1;
+
+		f[i1] += df1;
+		f[i2] += df2;
+
+		b3Mat33 Jv11 = -kd * I;
+
+		m_Jv[i] = Jv11;
+	}
+}
+
+static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
+{
+	bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
+	bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
+	bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
+
+	return isZeroX * isZeroY * isZeroZ;
+}
+
+void b3ClothSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b, const b3DenseVec3& f, const b3DenseVec3& x, const b3DenseVec3& v, const b3DenseVec3& y) const
+{
+	// Compute dfdx, dfdv
+	b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
+	b3SetZero(dfdx, m_particleCount * m_particleCount);
+
+	b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
+	b3SetZero(dfdv, m_particleCount * m_particleCount);
+
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		const b3Spring* S = m_springs[i];
+		u32 i1 = S->p1->solverId;
+		u32 i2 = S->p2->solverId;
+
+		b3Mat33 Jx11 = m_Jx[i];
+		b3Mat33 Jx12 = -Jx11;
+		b3Mat33 Jx21 = Jx12;
+		b3Mat33 Jx22 = Jx11;
+
+		dfdx[B3_INDEX(i1, i1, m_particleCount)] += Jx11;
+		dfdx[B3_INDEX(i1, i2, m_particleCount)] += Jx12;
+		dfdx[B3_INDEX(i2, i1, m_particleCount)] += Jx21;
+		dfdx[B3_INDEX(i2, i2, m_particleCount)] += Jx22;
+
+		b3Mat33 Jv11 = m_Jv[i];
+		b3Mat33 Jv12 = -Jv11;
+		b3Mat33 Jv21 = Jv12;
+		b3Mat33 Jv22 = Jv11;
+
+		dfdv[B3_INDEX(i1, i1, m_particleCount)] += Jv11;
+		dfdv[B3_INDEX(i1, i2, m_particleCount)] += Jv12;
+		dfdv[B3_INDEX(i2, i1, m_particleCount)] += Jv21;
+		dfdv[B3_INDEX(i2, i2, m_particleCount)] += Jv22;
+	}
+
+	// Compute A
+	// A = M - h * dfdv - h * h * dfdx
+
+	// A = 0
+	b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_particleCount * m_particleCount * sizeof(b3Mat33));
+	b3SetZero(A, m_particleCount * m_particleCount);
+
+	// A += M
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		A[B3_INDEX(i, i, m_particleCount)] += b3Diagonal(m_particles[i]->mass);
+	}
+
+	// A += - h * dfdv - h * h * dfdx
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		for (u32 j = 0; j < m_particleCount; ++j)
+		{
+			A[B3_INDEX(i, j, m_particleCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_particleCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_particleCount)]);
+		}
+	}
+
+	// Assembly sparsity
+	u32 nzCount = 0;
+
+	SA.row_ptrs[0] = 0;
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		u32 rowNzCount = 0;
+
+		for (u32 j = 0; j < m_particleCount; ++j)
+		{
+			b3Mat33 a = A[B3_INDEX(i, j, m_particleCount)];
+
+			if (b3IsZero(a) == false)
+			{
+				B3_ASSERT(nzCount <= SA.valueCount);
+
+				SA.values[nzCount] = a;
+				SA.cols[nzCount] = j;
+
+				++nzCount;
+				++rowNzCount;
+			}
+		}
+
+		SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
+	}
+
+	B3_ASSERT(nzCount <= SA.valueCount);
+	SA.valueCount = nzCount;
+
+	m_allocator->Free(A);
+
+	m_allocator->Free(dfdv);
+	m_allocator->Free(dfdx);
+
+	// Compute b
+	// b = h * (f0 + h * Jx_v + Jx_y)
+
+	// Jx_v = dfdx * v
+	b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
+	b3Mul_Jacobian(Jx_v, v.v, m_particleCount, m_Jx, m_springs, m_springCount);
+
+	// Jx_y = dfdx * y
+	b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_particleCount * sizeof(b3Vec3));
+	b3Mul_Jacobian(Jx_y, y.v, m_particleCount, m_Jx, m_springs, m_springCount);
+
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b[i] = m_h * (f[i] + m_h * Jx_v[i] + Jx_y[i]);
+	}
+
+	m_allocator->Free(Jx_y);
+	m_allocator->Free(Jx_v);
+}
+
+void b3ClothSolver::Compute_z(b3DenseVec3& z)
+{
+	// TODO
+	z.SetZero();
+}
+
+void b3ClothSolver::Compute_S(b3DiagMat33& out)
+{
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Particle* p = m_particles[i];
+
+		if (p->type == e_staticParticle)
+		{
+			out[i].SetZero();
+			continue;
+		}
+
+		out[i].SetIdentity();
+	}
+
+	for (u32 i = 0; i < m_contactCount; ++i)
+	{
+		b3ParticleContact* c = m_contacts[i];
+		
+		B3_ASSERT(c->n_active);
+
+		b3Vec3 n = c->n;
+
+		b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
+
+		if (c->t1_active == true && c->t2_active == true)
+		{
+			S.SetZero();
+		}
+		else
+		{
+			if (c->t1_active == true)
+			{
+				b3Vec3 t1 = c->t1;
+
+				S -= b3Outer(t1, t1);
+			}
+
+			if (c->t2_active == true)
+			{
+				b3Vec3 t2 = c->t2;
+
+				S -= b3Outer(t2, t2);
+			}
+		}
+
+		b3Particle* p = c->p1;
+		out[p->solverId] = S;
+	}
+}
+
+void b3ClothSolver::Solve(b3DenseVec3& x, u32& iterations,
+	const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
+{
+	B3_PROFILE("Solve A * x = b");
+
+	// P = diag(A)
+	b3DiagMat33 inv_P(m_particleCount);
+	A.AssembleDiagonal(inv_P);
+
+	// Invert
+	for (u32 i = 0; i < m_particleCount; ++i)
+	{
+		b3Mat33& D = inv_P[i];
+
+		// Sylvester Criterion to ensure PD-ness
+		B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON);
+
+		B3_ASSERT(D.x.x != 0.0f);
+		float32 xx = 1.0f / D.x.x;
+
+		B3_ASSERT(D.y.y != 0.0f);
+		float32 yy = 1.0f / D.y.y;
+
+		B3_ASSERT(D.z.z != 0.0f);
+		float32 zz = 1.0f / D.z.z;
+
+		D = b3Diagonal(xx, yy, zz);
+	}
+
+	// I
+	b3DiagMat33 I(m_particleCount);
+	I.SetIdentity();
+
+	// x = S * y + (I - S) * z 
+	x = (S * y) + (I - S) * z;
+
+	// b^ = S * (b - A * (I - S) * z)
+	b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
+
+	// b_delta = dot(b^, P^-1 * b_^)
+	float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
+
+	// r = S * (b - A * x)
+	b3DenseVec3 r = S * (b - A * x);
+
+	// p = S * (P^-1 * r)
+	b3DenseVec3 p = S * (inv_P * r);
+
+	// deltaNew = dot(r, p)
+	float32 deltaNew = b3Dot(r, p);
+
+	// Tolerance.
+	// This is the main stopping criteria.
+	// [0, 1]
+	const float32 tolerance = 10.0f * B3_EPSILON;
+
+	// Maximum number of iterations.
+	const u32 maxIters = 1000;
+
+	// Main iteration loop.
+	u32 iter = 0;
+	while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
+	{
+		// s = S * (A * p)
+		b3DenseVec3 s = S * (A * p);
+
+		// alpha = deltaNew / dot(c, q)
+		float32 alpha = deltaNew / b3Dot(p, s);
+
+		// x = x + alpha * p
+		x = x + alpha * p;
+
+		// r = r - alpha * s
+		r = r - alpha * s;
+
+		// h = inv_P * r
+		b3DenseVec3 h = inv_P * r;
+
+		// deltaOld = deltaNew
+		float32 deltaOld = deltaNew;
+
+		// deltaNew = dot(r, h)
+		deltaNew = b3Dot(r, h);
+		//B3_ASSERT(b3IsValid(deltaNew));
+
+		// beta = deltaNew / deltaOld
+		float32 beta = deltaNew / deltaOld;
+
+		// p = S * (h + beta * p)
+		p = S * (h + beta * p);
+
+		++iter;
+	}
+
+	iterations = iter;
+}

src/bounce/dynamics/cloth/spring_cloth.cpp

@@ -1,765 +0,0 @@
-/*
-* 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/dynamics/cloth/spring_cloth.h>
-#include <bounce/dynamics/cloth/spring_solver.h>
-#include <bounce/dynamics/cloth/dense_vec3.h>
-#include <bounce/dynamics/cloth/sparse_mat33.h>
-#include <bounce/dynamics/cloth/cloth_mesh.h>
-#include <bounce/dynamics/shapes/shape.h>
-#include <bounce/common/memory/stack_allocator.h>
-#include <bounce/common/draw.h>
-
-#define B3_FORCE_THRESHOLD 0.005f
-
-#define B3_CLOTH_BENDING 0
-
-#define B3_CLOTH_FRICTION 1
-
-b3SpringCloth::b3SpringCloth()
-{
-	m_allocator = nullptr;
-
-	m_mesh = nullptr;
-
-	m_gravity.SetZero();
-
-	m_x = nullptr;
-	m_v = nullptr;
-	m_f = nullptr;
-	m_y = nullptr;
-	m_z = nullptr;
-	m_x0 = nullptr;
-	m_m = nullptr;
-	m_inv_m = nullptr;
-	m_types = nullptr;
-	m_contacts = nullptr;
-	m_massCount = 0;
-
-	m_springs = nullptr;
-	m_springCount = 0;
-
-	m_r = 0.0f;
-
-	m_shapeCount = 0;
-
-	m_step.iterations = 0;
-}
-
-b3SpringCloth::~b3SpringCloth()
-{
-	b3Free(m_x);
-	b3Free(m_v);
-	b3Free(m_f);
-	b3Free(m_inv_m);
-	b3Free(m_m);
-	b3Free(m_y);
-	b3Free(m_z);
-	b3Free(m_x0);
-	b3Free(m_types);
-	b3Free(m_contacts);
-	b3Free(m_springs);
-}
-
-static B3_FORCE_INLINE u32 b3NextIndex(u32 i)
-{
-	return i + 1 < 3 ? i + 1 : 0;
-}
-
-struct b3UniqueEdge
-{
-	u32 v1, v2;
-};
-
-static u32 b3FindUniqueEdges(b3UniqueEdge* uniqueEdges, const b3ClothMesh* m)
-{
-	u32 uniqueCount = 0;
-
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3ClothMeshTriangle* t1 = m->triangles + i;
-		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
-
-		for (u32 j1 = 0; j1 < 3; ++j1)
-		{
-			u32 t1v1 = i1s[j1];
-			u32 t1v2 = i1s[b3NextIndex(j1)];
-
-			bool unique = true;
-
-			for (u32 j = 0; j < uniqueCount; ++j)
-			{
-				b3UniqueEdge* ue = uniqueEdges + j;
-
-				if (ue->v1 == t1v1 && ue->v2 == t1v2)
-				{
-					unique = false;
-					break;
-				}
-				
-				if (ue->v2 == t1v1 && ue->v1 == t1v2)
-				{
-					unique = false;
-					break;
-				}
-			}
-
-			if (unique)
-			{
-				b3UniqueEdge ue;
-				ue.v1 = t1v1;
-				ue.v2 = t1v2;
-				uniqueEdges[uniqueCount++] = ue;
-			}
-		}
-	}
-
-	return uniqueCount;
-}
-
-struct b3SharedEdge
-{
-	u32 v1, v2;
-	u32 nsv1, nsv2;
-};
-
-static u32 b3FindSharedEdges(b3SharedEdge* sharedEdges, const b3ClothMesh* m)
-{
-	u32 sharedCount = 0;
-
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3ClothMeshTriangle* t1 = m->triangles + i;
-		u32 i1s[3] = { t1->v1, t1->v2, t1->v3 };
-
-		for (u32 j1 = 0; j1 < 3; ++j1)
-		{
-			u32 k1 = j1 + 1 < 3 ? j1 + 1 : 0;
-
-			u32 t1v1 = i1s[j1];
-			u32 t1v2 = i1s[k1];
-
-			for (u32 j = i + 1; j < m->triangleCount; ++j)
-			{
-				b3ClothMeshTriangle* t2 = m->triangles + j;
-				u32 i2s[3] = { t2->v1, t2->v2, t2->v3 };
-
-				for (u32 j2 = 0; j2 < 3; ++j2)
-				{
-					u32 k2 = j2 + 1 < 3 ? j2 + 1 : 0;
-
-					u32 t2v1 = i2s[j2];
-					u32 t2v2 = i2s[k2];
-
-					if (t1v1 == t2v2 && t1v2 == t2v1)
-					{
-						// The triangles are adjacent.
-						u32 k3 = k1 + 1 < 3 ? k1 + 1 : 0;
-						u32 t1v3 = i1s[k3];
-
-						u32 k4 = k2 + 1 < 3 ? k2 + 1 : 0;
-						u32 t2v3 = i2s[k4];
-
-						// Add shared edge and non-shared vertices.
-						b3SharedEdge se;
-						se.v1 = t1v1;
-						se.v2 = t1v2;
-						se.nsv1 = t1v3;
-						se.nsv2 = t2v3;
-
-						sharedEdges[sharedCount++] = se;
-
-						break;
-					}
-				}
-			}
-		}
-	}
-
-	return sharedCount;
-}
-
-void b3SpringCloth::Initialize(const b3SpringClothDef& def)
-{
-	B3_ASSERT(def.allocator);
-	B3_ASSERT(def.mesh);
-	B3_ASSERT(def.density > 0.0f);
-
-	m_allocator = def.allocator;
-
-	m_mesh = def.mesh;
-	m_r = def.r;
-
-	m_gravity = def.gravity;
-
-	const b3ClothMesh* m = m_mesh;
-
-	m_massCount = m->vertexCount;
-	m_x = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_v = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_f = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
-	m_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
-	m_y = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_z = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_x0 = (b3Vec3*)b3Alloc(m_massCount * sizeof(b3Vec3));
-	m_types = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType));
-	m_contacts = (b3MassContact*)b3Alloc(m_massCount * sizeof(b3MassContact));
-
-	for (u32 i = 0; i < m->vertexCount; ++i)
-	{
-		m_contacts[i].Fn = 0.0f;
-		m_contacts[i].Ft1 = 0.0f;
-		m_contacts[i].Ft2 = 0.0f;
-		m_contacts[i].lockN = false;
-		m_contacts[i].lockT1 = false;
-		m_contacts[i].lockT2 = false;
-
-		m_x[i] = m->vertices[i];
-		m_v[i].SetZero();
-		m_f[i].SetZero();
-		m_m[i] = 0.0f;
-		m_inv_m[i] = 0.0f;
-		m_y[i].SetZero();
-		m_z[i].SetZero();
-		m_x0[i].SetZero();
-		m_types[i] = b3MassType::e_staticMass;
-	}
-
-	// Initialize mass
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3ClothMeshTriangle* t = m->triangles + i;
-
-		b3Vec3 p1 = m->vertices[t->v1];
-		b3Vec3 p2 = m->vertices[t->v2];
-		b3Vec3 p3 = m->vertices[t->v3];
-
-		float32 area = b3Area(p1, p2, p3);
-
-		B3_ASSERT(area > B3_EPSILON);
-
-		float32 mass = def.density * area;
-
-		const float32 inv3 = 1.0f / 3.0f;
-
-		m_m[t->v1] += inv3 * mass;
-		m_m[t->v2] += inv3 * mass;
-		m_m[t->v3] += inv3 * mass;
-	}
-
-	// Invert
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		B3_ASSERT(m_m[i] > 0.0f);
-		m_inv_m[i] = 1.0f / m_m[i];
-		m_types[i] = b3MassType::e_dynamicMass;
-	}
-
-	// Initialize springs
-	u32 edgeCount = 3 * m->triangleCount;
-
-	b3UniqueEdge* uniqueEdges = (b3UniqueEdge*)m_allocator->Allocate(edgeCount * sizeof(b3UniqueEdge));
-	u32 uniqueCount = b3FindUniqueEdges(uniqueEdges, m);
-
-	u32 springCapacity = uniqueCount;
-	
-#if B3_CLOTH_BENDING
-	
-	b3SharedEdge* sharedEdges = (b3SharedEdge*)m_allocator->Allocate(edgeCount * sizeof(b3SharedEdge));
-	u32 sharedCount = b3FindSharedEdges(sharedEdges, m);
-
-	springCapacity += sharedCount;
-
-#endif
-	
-	springCapacity += m->sewingLineCount;
-
-	m_springs = (b3Spring*)b3Alloc(springCapacity * sizeof(b3Spring));
-
-	// Tension
-	for (u32 i = 0; i < uniqueCount; ++i)
-	{
-		b3UniqueEdge* e = uniqueEdges + i;
-
-		b3Vec3 p1 = m->vertices[e->v1];
-		b3Vec3 p2 = m->vertices[e->v2];
-
-		b3Spring* S = m_springs + m_springCount;
-		S->type = e_strechSpring;
-		S->i1 = e->v1;
-		S->i2 = e->v2;
-		S->L0 = b3Distance(p1, p2);
-		
-		B3_ASSERT(S->L0 > B3_EPSILON);
-
-		S->ks = def.ks;
-		S->kd = def.kd;
-		++m_springCount;
-	}
-
-#if B3_CLOTH_BENDING
-
-	// Bending
-	for (u32 i = 0; i < sharedCount; ++i)
-	{
-		b3SharedEdge* e = sharedEdges + i;
-
-		b3Vec3 p1 = m->vertices[e->nsv1];
-		b3Vec3 p2 = m->vertices[e->nsv2];
-
-		b3Spring* S = m_springs + m_springCount;
-		S->type = e_bendSpring;
-		S->i1 = e->nsv1;
-		S->i2 = e->nsv2;
-		S->L0 = b3Distance(p1, p2);
-		S->ks = def.kb;
-		S->kd = def.kd;
-		++m_springCount;
-	}
-
-	m_allocator->Free(sharedEdges);
-#endif
-
-	m_allocator->Free(uniqueEdges);
-
-	// Sewing
-	for (u32 i = 0; i < m->sewingLineCount; ++i)
-	{
-		b3ClothMeshSewingLine* line = m->sewingLines + i;
-
-		b3Spring* S = m_springs + m_springCount;
-		S->type = e_strechSpring;
-		S->i1 = line->v1;
-		S->i2 = line->v2;
-		S->L0 = 0.0f;
-		S->ks = def.ks;
-		S->kd = def.kd;
-		++m_springCount;
-	}
-
-	B3_ASSERT(m_springCount <= springCapacity);
-}
-
-void b3SpringCloth::AddShape(b3Shape* shape)
-{
-	B3_ASSERT(m_shapeCount < B3_CLOTH_SHAPE_CAPACITY);
-
-	if (m_shapeCount == B3_CLOTH_SHAPE_CAPACITY)
-	{
-		return;
-	}
-
-	m_shapes[m_shapeCount++] = shape;
-}
-
-void b3SpringCloth::GetTension(b3Array<b3Vec3>& T) const
-{
-	B3_ASSERT(T.Count() == 0);
-
-	T.Resize(m_massCount);
-
-	for (u32 i = 0; i < T.Count(); ++i)
-	{
-		T[i].SetZero();
-	}
-
-	// T = F - mg
-	for (u32 i = 0; i < m_springCount; ++i)
-	{
-		b3Spring* S = m_springs + i;
-
-		b3SpringType type = S->type;
-		u32 i1 = S->i1;
-		u32 i2 = S->i2;
-		float32 L0 = S->L0;
-		float32 ks = S->ks;
-		float32 kd = S->kd;
-
-		b3Vec3 x1 = m_x[i1];
-		b3Vec3 v1 = m_v[i1];
-
-		b3Vec3 x2 = m_x[i2];
-		b3Vec3 v2 = m_v[i2];
-
-		// Streching
-		b3Vec3 dx = x1 - x2;
-		float32 L = b3Length(dx);
-
-		if (L >= L0)
-		{
-			// Force is tension.
-			b3Vec3 n = dx / L;
-
-			b3Vec3 sf1 = -ks * (L - L0) * n;
-			b3Vec3 sf2 = -sf1;
-
-			T[i1] += sf1;
-			T[i2] += sf2;
-		}
-
-		// Damping
-		b3Vec3 dv = v1 - v2;
-
-		b3Vec3 df1 = -kd * dv;
-		b3Vec3 df2 = -df1;
-
-		T[i1] += df1;
-		T[i2] += df2;
-	}
-
-	for (u32 i = 0; i < T.Count(); ++i)
-	{
-		if (m_types[i] != b3MassType::e_dynamicMass)
-		{
-			T[i].SetZero();
-		}
-	}
-}
-
-void b3SpringCloth::UpdateContacts()
-{
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		// Static masses can't participate in collisions.
-		if (m_types[i] == b3MassType::e_staticMass)
-		{
-			continue;
-		}
-
-		b3MassContact* c = m_contacts + i;
-
-		// Save the old contact
-		b3MassContact c0 = *c;
-
-		// Create a new contact
-		c->lockN = false;
-		c->lockT1 = false;
-		c->lockT2 = false;
-
-		b3Sphere s1;
-		s1.vertex = m_x[i];
-		s1.radius = m_r;
-
-		// Find the deepest penetration
-		float32 bestSeparation = 0.0f;
-		b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
-		u32 bestIndex = ~0;
-
-		for (u32 j = 0; j < m_shapeCount; ++j)
-		{
-			b3Shape* s2 = m_shapes[j];
-
-			b3Transform xf2;
-			xf2.SetIdentity();
-
-			b3TestSphereOutput output;
-			if (s2->TestSphere(&output, s1, xf2) == false)
-			{
-				continue;
-			}
-
-			if (output.separation < bestSeparation)
-			{
-				bestSeparation = output.separation;
-				bestNormal = output.normal;
-				bestIndex = j;
-			}
-		}
-
-		if (bestIndex != ~0)
-		{
-			B3_ASSERT(bestSeparation <= 0.0f);
-
-			b3Shape* shape = m_shapes[bestIndex];
-			float32 s = bestSeparation;
-			b3Vec3 n = bestNormal;
-
-			// Update contact manifold
-			// Remember the normal orientation is from shape 2 to shape 1 (mass)
-			c->j = bestIndex;
-			c->n = n;
-			c->t1 = b3Perp(n);
-			c->t2 = b3Cross(c->t1, n);
-			c->lockN = true;
-
-			// Apply position correction
-			m_y[i] -= s * n;
-		}
-
-		// Update contact state
-		if (c0.lockN == true && c->lockN == true)
-		{
-			// The contact persists
-			
-			// Has the contact constraint been satisfied?
-			if (c0.Fn <= -B3_FORCE_THRESHOLD)
-			{
-				// Contact force is attractive.
-
-				// Terminate the contact.
-				c->lockN = false;
-			}
-		}
-
-#if 0
-		// Notify the new contact state
-		if (wasLockedN == false && c->lockN == true)
-		{
-			// The contact has begun
-		}
-
-		if (wasLockedN == true && c->lockN == false)
-		{
-			// The contact has ended
-		}
-#endif
-		if (c->lockN == false)
-		{
-			continue;
-		}
-
-#if B3_CLOTH_FRICTION == 1
-
-		// A friction force requires an associated normal force.
-		if (c0.lockN == false)
-		{
-			continue;
-		}
-
-		b3Shape* s = m_shapes[c->j];
-		b3Vec3 n = c->n;
-		float32 u = s->GetFriction();
-		float32 normalForce = c0.Fn;
-
-		// Relative velocity
-		b3Vec3 dv = m_v[i];
-
-		b3Vec3 t1 = dv - b3Dot(dv, n) * n;
-		if (b3Dot(t1, t1) > B3_EPSILON * B3_EPSILON)
-		{
-			// Create a dynamic basis
-			t1.Normalize();
-			
-			b3Vec3 t2 = b3Cross(t1, n);
-			t2.Normalize();
-
-			c->t1 = t1;
-			c->t2 = t2;
-		}
-		else
-		{
-			c->lockT1 = true;
-			c->lockT2 = true;
-			continue;
-		}
-
-		b3Vec3 ts[2];
-		ts[0] = c->t1;
-		ts[1] = c->t2;
-
-		bool lockT[2];
-		lockT[0] = c->lockT1;
-		lockT[1] = c->lockT2;
-
-		bool lockT0[2];
-		lockT0[0] = c0.lockT1;
-		lockT0[1] = c0.lockT2;
-
-		float32 Ft0[2];
-		Ft0[0] = c0.Ft1;
-		Ft0[1] = c0.Ft2;
-
-		for (u32 k = 0; k < 2; ++k)
-		{
-			b3Vec3 t = ts[k];
-
-			// Relative tangential velocity
-			float32 dvt = b3Dot(dv, t);
-
-			if (dvt * dvt <= B3_EPSILON * B3_EPSILON)
-			{
-				// Lock mass on surface
-				lockT[k] = true;
-			}
-
-			if (lockT0[k] == true && lockT[k] == true)
-			{
-				// The contact persists
-				float32 maxForce = u * normalForce;
-				
-				if (Ft0[k] * Ft0[k] > maxForce * maxForce)
-				{
-					// Unlock mass off surface
-					lockT[k] = false;
-				}
-			}
-		}
-
-		c->lockT1 = lockT[0];
-		c->lockT2 = lockT[1];
-
-#endif
-
-	}
-
-}
-
-void b3SpringCloth::Step(float32 dt)
-{
-	if (dt == 0.0f)
-	{
-		return;
-	}
-
-	// Update contacts
-	UpdateContacts();
-
-	// Apply weights
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		if (m_types[i] == b3MassType::e_dynamicMass)
-		{
-			m_f[i] += m_m[i] * m_gravity;
-		}
-	}
-
-	// Integrate
-	b3SpringSolverDef solverDef;
-	solverDef.cloth = this;
-	solverDef.dt = dt;
-
-	b3SpringSolver solver(solverDef);
-
-	// Extra constraint forces that should have been applied to satisfy the constraints
-	// todo Find the applied constraint forces.
-	b3DenseVec3 forces(m_massCount);
-
-	solver.Solve(forces);
-
-	m_step.iterations = solver.GetIterations();
-
-	// Store constraint forces for physics logic
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		b3MassContact* c = m_contacts + i;
-
-		if (c->lockN == false)
-		{
-			continue;
-		}
-
-		b3Vec3 force = forces[i];
-
-		// Signed normal force magnitude
-		c->Fn = b3Dot(force, c->n);
-
-		// Signed tangent force magnitude
-		c->Ft1 = b3Dot(force, c->t1);
-		c->Ft2 = b3Dot(force, c->t2);
-	}
-
-	// Clear position alteration
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		m_y[i].SetZero();
-	}
-
-	// Clear forces
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		m_f[i].SetZero();
-	}
-}
-
-void b3SpringCloth::Apply() const
-{
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		m_mesh->vertices[i] = m_x[i];
-	}
-}
-
-void b3SpringCloth::Draw() const
-{
-	const b3ClothMesh* m = m_mesh;
-
-	for (u32 i = 0; i < m->vertexCount; ++i)
-	{
-		if (m_types[i] == b3MassType::e_staticMass)
-		{
-			b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_white);
-		}
-		
-		if (m_types[i] == b3MassType::e_kinematicMass)
-		{
-			b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_blue);
-		}
-		
-		if (m_types[i] == b3MassType::e_dynamicMass)
-		{
-			b3Draw_draw->DrawPoint(m_x[i], 4.0f, b3Color_green);
-		}
-
-		b3MassContact* c = m_contacts + i;
-		
-		if (c->lockN)
-		{
-			b3Draw_draw->DrawSegment(m_x[i], m_x[i] + c->n, b3Color_yellow);
-		}
-	}
-
-	for (u32 i = 0; i < m_springCount; ++i)
-	{
-		b3Spring* s = m_springs + i;
-
-		b3Vec3 x1 = m_x[s->i1];
-		b3Vec3 x2 = m_x[s->i2];
-		
-		if (s->type == e_strechSpring)
-		{
-			b3Draw_draw->DrawSegment(x1, x2, b3Color_black);
-		}
-	}
-	
-	for (u32 i = 0; i < m->sewingLineCount; ++i)
-	{
-		b3ClothMeshSewingLine* s = m->sewingLines + i;
-
-		b3Vec3 x1 = m_x[s->v1];
-		b3Vec3 x2 = m_x[s->v2];
-
-		b3Draw_draw->DrawSegment(x1, x2, b3Color_white);
-	}
-
-	for (u32 i = 0; i < m->triangleCount; ++i)
-	{
-		b3ClothMeshTriangle* t = m->triangles + i;
-
-		b3Vec3 v1 = m_x[t->v1];
-		b3Vec3 v2 = m_x[t->v2];
-		b3Vec3 v3 = m_x[t->v3];
-
-		b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
-		n1.Normalize();
-		b3Draw_draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue);
-
-		b3Vec3 n2 = -n1;
-		b3Draw_draw->DrawSolidTriangle(n2, v1, v3, v2, b3Color_blue);
-	}
-}

src/bounce/dynamics/cloth/spring_solver.cpp

@@ -1,550 +0,0 @@
-/*
-* 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/dynamics/cloth/spring_solver.h>
-#include <bounce/dynamics/cloth/spring_cloth.h>
-#include <bounce/dynamics/cloth/dense_vec3.h>
-#include <bounce/dynamics/cloth/diag_mat33.h>
-#include <bounce/dynamics/cloth/sparse_mat33.h>
-#include <bounce/dynamics/shapes/shape.h>
-#include <bounce/common/memory/stack_allocator.h>
-
-// Here, we solve Ax = b using the Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
-// described in the paper:
-// "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
-
-// Some improvements for the original MPCG algorithm are described in the paper:
-// "On the modified conjugate gradient method in cloth simulation - Uri M. Ascher, Eddy Boxerman".
-
-b3SpringSolver::b3SpringSolver(const b3SpringSolverDef& def)
-{
-	m_cloth = def.cloth;
-	m_h = def.dt;
-	m_iterations = 0;
-	m_Jx = nullptr;
-	m_Jv = nullptr;
-
-	m_allocator = m_cloth->m_allocator;
-
-	m_x = m_cloth->m_x;
-	m_v = m_cloth->m_v;
-	m_f = m_cloth->m_f;
-	m_m = m_cloth->m_m;
-	m_inv_m = m_cloth->m_inv_m;
-	m_y = m_cloth->m_y;
-	m_z = m_cloth->m_y;
-	m_x0 = m_cloth->m_x0;
-	m_types = m_cloth->m_types;
-	m_massCount = m_cloth->m_massCount;
-
-	m_contacts = m_cloth->m_contacts;
-
-	m_springs = m_cloth->m_springs;
-	m_springCount = m_cloth->m_springCount;
-
-	m_shapes = m_cloth->m_shapes;
-}
-
-b3SpringSolver::~b3SpringSolver()
-{
-
-}
-
-static B3_FORCE_INLINE b3SparseMat33 b3AllocSparse(b3StackAllocator* allocator, u32 M, u32 N)
-{
-	u32 size = M * N;
-	b3Mat33* elements = (b3Mat33*)allocator->Allocate(size * sizeof(b3Mat33));
-	u32* cols = (u32*)allocator->Allocate(size * sizeof(u32));
-	u32* row_ptrs = (u32*)allocator->Allocate((M + 1) * sizeof(u32));
-
-	return b3SparseMat33(M, N, size, elements, row_ptrs, cols);
-}
-
-static B3_FORCE_INLINE void b3FreeSparse(b3SparseMat33& matrix, b3StackAllocator* allocator)
-{
-	allocator->Free(matrix.row_ptrs);
-	allocator->Free(matrix.cols);
-	allocator->Free(matrix.values);
-}
-
-void b3SpringSolver::Solve(b3DenseVec3& f)
-{
-	B3_PROFILE("Solve");
-
-	//
-	m_Jx = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
-	m_Jv = (b3Mat33*)m_allocator->Allocate(m_springCount * sizeof(b3Mat33));
-
-	// Apply internal forces. Also, store their unique derivatives.
-	ApplySpringForces();
-
-	// Integrate
-
-	// Solve Ax = b, where
-	// A = M - h * dfdv - h * h * dfdx
-	// b = h * (f0 + h * dfdx * v0 + dfdx * y) 
-
-	{
-		// A
-		b3SparseMat33 A = b3AllocSparse(m_allocator, m_massCount, m_massCount);
-
-		// b
-		b3DenseVec3 b(m_massCount);
-
-		Compute_A_b(A, b);
-
-		// S
-		b3DiagMat33 S(m_massCount);
-		Compute_S(S);
-
-		// z
-		b3DenseVec3 z(m_massCount);
-		Compute_z(z);
-
-		// x0
-		b3DenseVec3 x0(m_massCount);
-		for (u32 i = 0; i < m_massCount; ++i)
-		{
-			x0[i] = m_x0[i];
-		}
-
-		// x
-		b3DenseVec3 x(m_massCount);
-
-		// Solve Ax = b
-		Solve(x, f, m_iterations, A, b, S, z, x0);
-
-		// Store x0
-		for (u32 i = 0; i < m_massCount; ++i)
-		{
-			m_x0[i] = x[i];
-		}
-		
-		// Update state
-		for (u32 i = 0; i < m_massCount; ++i)
-		{
-			if (m_types[i] == b3MassType::e_dynamicMass)
-			{
-				m_v[i] += x[i];
-			}
-
-			// dx = h * (v0 + dv) + y = h * v1 + y
-			m_x[i] += m_h * m_v[i] + m_y[i];
-		}
-
-		b3FreeSparse(A, m_allocator);
-	}
-
-	m_allocator->Free(m_Jv);
-	m_allocator->Free(m_Jx);
-	m_Jv = nullptr;
-	m_Jx = nullptr;
-}
-
-#define B3_INDEX(i, j, size) (i + j * size)
-
-//
-static void b3SetZero(b3Vec3* out, u32 size)
-{
-	for (u32 i = 0; i < size; ++i)
-	{
-		out[i].SetZero();
-	}
-}
-
-//
-static void b3SetZero(b3Mat33* out, u32 size)
-{
-	for (u32 i = 0; i < size; ++i)
-	{
-		out[i].SetZero();
-	}
-}
-
-// 
-static void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 massCount,
-	const b3Mat33* J_ii, const b3Spring* springs, u32 springCount)
-{
-	b3SetZero(out, massCount);
-
-	for (u32 i = 0; i < springCount; ++i)
-	{
-		const b3Spring* S = springs + i;
-		u32 i1 = S->i1;
-		u32 i2 = S->i2;
-
-		b3Mat33 J_11 = J_ii[i];
-		b3Mat33 J_12 = -J_11;
-		b3Mat33 J_21 = J_12;
-		b3Mat33 J_22 = J_11;
-
-		out[i1] += J_11 * v[i1] + J_12 * v[i2];
-		out[i2] += J_21 * v[i1] + J_22 * v[i2];
-	}
-}
-
-void b3SpringSolver::ApplySpringForces()
-{
-	// Zero Jacobians
-	b3SetZero(m_Jx, m_springCount);
-	b3SetZero(m_Jv, m_springCount);
-
-	// Compute spring forces and Jacobians
-	for (u32 i = 0; i < m_springCount; ++i)
-	{
-		b3Spring* S = m_springs + i;
-
-		b3SpringType type = S->type;
-		u32 i1 = S->i1;
-		u32 i2 = S->i2;
-		float32 L0 = S->L0;
-		B3_ASSERT(L0 > 0.0f);
-		float32 ks = S->ks;
-		float32 kd = S->kd;
-
-		b3Vec3 x1 = m_x[i1];
-		b3Vec3 v1 = m_v[i1];
-
-		b3Vec3 x2 = m_x[i2];
-		b3Vec3 v2 = m_v[i2];
-
-		const b3Mat33 I = b3Mat33_identity;
-
-		// Strech
-		b3Vec3 dx = x1 - x2;
-		float32 L = b3Length(dx);
-
-		if (L >= L0)
-		{
-			// Force is tension.
-			b3Vec3 n = dx / L;
-
-			b3Vec3 sf1 = -ks * (L - L0) * n;
-			b3Vec3 sf2 = -sf1;
-
-			m_f[i1] += sf1;
-			m_f[i2] += sf2;
-
-			b3Mat33 Jx11 = -ks * (b3Outer(dx, dx) + (1.0f - L0 / L) * (I - b3Outer(dx, dx)));
-
-			m_Jx[i] = Jx11;
-		}
-
-		// Damping
-		b3Vec3 dv = v1 - v2;
-
-		b3Vec3 df1 = -kd * dv;
-		b3Vec3 df2 = -df1;
-
-		m_f[i1] += df1;
-		m_f[i2] += df2;
-
-		b3Mat33 Jv11 = -kd * I;
-
-		m_Jv[i] = Jv11;
-	}
-}
-
-static B3_FORCE_INLINE bool b3IsZero(const b3Mat33& A)
-{
-	bool isZeroX = b3Dot(A.x, A.x) == 0.0f;
-	bool isZeroY = b3Dot(A.y, A.y) == 0.0f;
-	bool isZeroZ = b3Dot(A.z, A.z) == 0.0f;
-
-	return isZeroX * isZeroY * isZeroZ;
-}
-
-void b3SpringSolver::Compute_A_b(b3SparseMat33& SA, b3DenseVec3& b) const
-{
-	// Compute dfdx, dfdv
-	b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
-	b3SetZero(dfdx, m_massCount * m_massCount);
-	
-	b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
-	b3SetZero(dfdv, m_massCount * m_massCount);
-
-	for (u32 i = 0; i < m_springCount; ++i)
-	{
-		const b3Spring* S = m_springs + i;
-		u32 i1 = S->i1;
-		u32 i2 = S->i2;
-
-		b3Mat33 Jx11 = m_Jx[i];
-		b3Mat33 Jx12 = -Jx11;
-		b3Mat33 Jx21 = Jx12;
-		b3Mat33 Jx22 = Jx11;
-
-		dfdx[B3_INDEX(i1, i1, m_massCount)] += Jx11;
-		dfdx[B3_INDEX(i1, i2, m_massCount)] += Jx12;
-		dfdx[B3_INDEX(i2, i1, m_massCount)] += Jx21;
-		dfdx[B3_INDEX(i2, i2, m_massCount)] += Jx22;
-
-		b3Mat33 Jv11 = m_Jv[i];
-		b3Mat33 Jv12 = -Jv11;
-		b3Mat33 Jv21 = Jv12;
-		b3Mat33 Jv22 = Jv11;
-
-		dfdv[B3_INDEX(i1, i1, m_massCount)] += Jv11;
-		dfdv[B3_INDEX(i1, i2, m_massCount)] += Jv12;
-		dfdv[B3_INDEX(i2, i1, m_massCount)] += Jv21;
-		dfdv[B3_INDEX(i2, i2, m_massCount)] += Jv22;
-	}
-
-	// Compute A
-	// A = M - h * dfdv - h * h * dfdx
-
-	// A = 0
-	b3Mat33* A = (b3Mat33*)m_allocator->Allocate(m_massCount * m_massCount * sizeof(b3Mat33));
-	b3SetZero(A, m_massCount * m_massCount);
-
-	// A += M
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		A[B3_INDEX(i, i, m_massCount)] += b3Diagonal(m_m[i]);
-	}
-	
-	// A += - h * dfdv - h * h * dfdx
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		for (u32 j = 0; j < m_massCount; ++j)
-		{
-			A[B3_INDEX(i, j, m_massCount)] += (-m_h * dfdv[B3_INDEX(i, j, m_massCount)]) + (-m_h * m_h * dfdx[B3_INDEX(i, j, m_massCount)]);
-		}
-	}
-
-	// Assembly sparsity
-	u32 nzCount = 0;
-
-	SA.row_ptrs[0] = 0;
-
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		u32 rowNzCount = 0;
-
-		for (u32 j = 0; j < m_massCount; ++j)
-		{
-			b3Mat33 a = A[B3_INDEX(i, j, m_massCount)];
-
-			if (b3IsZero(a) == false)
-			{
-				B3_ASSERT(nzCount <= SA.valueCount);
-
-				SA.values[nzCount] = a;
-				SA.cols[nzCount] = j;
-
-				++nzCount;
-				++rowNzCount;
-			}
-		}
-
-		SA.row_ptrs[i + 1] = SA.row_ptrs[(i + 1) - 1] + rowNzCount;
-	}
-
-	B3_ASSERT(nzCount <= SA.valueCount);
-	SA.valueCount = nzCount;
-
-	m_allocator->Free(A);
-
-	// Compute b
-	// b = h * (f0 + h * Jx_v + Jx_y)
-
-	// Jx_v = dfdx * v
-	b3Vec3* Jx_v = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
-	b3Mul_Jacobian(Jx_v, m_v, m_massCount, m_Jx, m_springs, m_springCount);
-
-	// Jx_y = dfdx * y
-	b3Vec3* Jx_y = (b3Vec3*)m_allocator->Allocate(m_massCount * sizeof(b3Vec3));
-	b3Mul_Jacobian(Jx_y, m_y, m_massCount, m_Jx, m_springs, m_springCount);
-
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		b[i] = m_h * (m_f[i] + m_h * Jx_v[i] + Jx_y[i]);
-	}
-
-	m_allocator->Free(Jx_y);
-	m_allocator->Free(Jx_v);
-
-	m_allocator->Free(dfdv);
-	m_allocator->Free(dfdx);
-}
-
-void b3SpringSolver::Compute_z(b3DenseVec3& z)
-{
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		z[i] = m_z[i];
-	}
-}
-
-void b3SpringSolver::Compute_S(b3DiagMat33& out)
-{
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		out[i].SetIdentity();
-
-		if (m_types[i] == b3MassType::e_staticMass)
-		{
-			out[i].SetZero();
-			continue;
-		}
-
-		b3MassContact* c = m_contacts + i;
-
-		if (c->lockN == true)
-		{
-			b3Vec3 n = c->n;
-
-			b3Mat33 S = b3Mat33_identity - b3Outer(n, n);
-
-			if (c->lockT1 == true && c->lockT2 == true)
-			{
-				S.SetZero();
-			}
-			else
-			{
-				if (c->lockT1 == true)
-				{
-					b3Vec3 t1 = c->t1;
-
-					S -= b3Outer(t1, t1);
-				}
-
-				if (c->lockT2 == true)
-				{
-					b3Vec3 t2 = c->t2;
-
-					S -= b3Outer(t2, t2);
-				}
-			}
-
-			out[i] = S;
-		}
-	}
-}
-
-void b3SpringSolver::Solve(b3DenseVec3& x, b3DenseVec3& f, u32& iterations, 
-	const b3SparseMat33& A, const b3DenseVec3& b, const b3DiagMat33& S, const b3DenseVec3& z, const b3DenseVec3& y) const
-{
-	// P = diag(A)
-	b3DiagMat33 inv_P(m_massCount);
-	A.AssembleDiagonal(inv_P);
-
-	// Invert
-	for (u32 i = 0; i < m_massCount; ++i)
-	{
-		b3Mat33& D = inv_P[i];
-		
-		// Sylvester Criterion to ensure PD-ness
-		B3_ASSERT(b3Det(D.x, D.y, D.z) > B3_EPSILON);
-
-		B3_ASSERT(D.x.x != 0.0f);
-		float32 xx = 1.0f / D.x.x;
-
-		B3_ASSERT(D.y.y != 0.0f);
-		float32 yy = 1.0f / D.y.y;
-		
-		B3_ASSERT(D.z.z != 0.0f);
-		float32 zz = 1.0f / D.z.z;
-
-		D = b3Diagonal(xx, yy, zz);
-	}
-
-	// I
-	b3DiagMat33 I(m_massCount);
-	I.SetIdentity();
-
-	// x = S * y + (I - S) * z 
-	x = (S * y) + (I - S) * z;
-
-	// b^ = S * (b - A * (I - S) * z)
-	b3DenseVec3 b_hat = S * (b - A * ((I - S) * z));
-
-	// b_delta = dot(b^, P^-1 * b_^)
-	float32 b_delta = b3Dot(b_hat, inv_P * b_hat);
-
-	// r = S * (b - A * x)
-	b3DenseVec3 r = S * (b - A * x);
-
-	// p = S * (P^-1 * r)
-	b3DenseVec3 p = S * (inv_P * r);
-
-	// deltaNew = dot(r, p)
-	float32 deltaNew = b3Dot(r, p);
-
-	// Tolerance.
-	// This is the main stopping criteria.
-	// [0, 1]
-	const float32 tolerance = 10.0f * B3_EPSILON;
-
-	// Maximum number of iterations.
-	const u32 maxIters = 1000;
-
-	// Main iteration loop.
-	u32 iter = 0;
-	while (deltaNew > tolerance * tolerance * b_delta && iter < maxIters)
-	{
-		// s = S * (A * p)
-		b3DenseVec3 s = S * (A * p);
-
-		// alpha = deltaNew / dot(c, q)
-		float32 alpha = deltaNew / b3Dot(p, s);
-
-		// x = x + alpha * p
-		x = x + alpha * p;
-
-		// r = r - alpha * s
-		r = r - alpha * s;
-
-		// h = inv_P * r
-		b3DenseVec3 h = inv_P * r;
-
-		// deltaOld = deltaNew
-		float32 deltaOld = deltaNew;
-
-		// deltaNew = dot(r, h)
-		deltaNew = b3Dot(r, h);
-		//B3_ASSERT(b3IsValid(deltaNew));
-
-		// beta = deltaNew / deltaOld
-		float32 beta = deltaNew / deltaOld;
-
-		// p = S * (h + beta * p)
-		p = S * (h + beta * p);
-
-		++iter;
-	}
-
-	iterations = iter;
-
-	float32 x2 = b3Dot(x, x);
-	if (b3IsValid(x2) == false)
-	{
-		// Probably the condition number of A is large.
-		// Solve unconstrained (S = I)?
-
-		// Reject the solution.
-		x.SetZero();
-		f.SetZero();
-	}
-	else
-	{
-		// Accept the solution.
-		// Residual
-		f = A * x - b;
-	}
-}