add baraff and witkin's mass-spring-based cloth

examples/testbed/tests/spring_cloth_test.h

@@ -0,0 +1,91 @@
+/*
+* 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
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class SpringCloth : public Test
+{
+public:
+	SpringCloth()
+	{
+		g_camera.m_zoom = 25.0f;
+
+		b3SpringClothDef def;
+		def.allocator = &m_clothAllocator;
+		def.mesh = m_meshes + e_clothMesh;
+		def.density = 0.2f;
+		def.ks = 100000.0f;
+		def.kd = 100.0f;
+		def.gravity.Set(2.5f, 5.0f, -10.0f);
+
+		m_cloth.Initialize(def);
+
+		m_aabb.m_lower.Set(-5.0f, -1.0f, -6.0f);
+		m_aabb.m_upper.Set(5.0f, 1.0f, -4.0f);
+
+		for (u32 i = 0; i < def.mesh->vertexCount; ++i)
+		{
+			if (m_aabb.Contains(def.mesh->vertices[i]))
+			{
+				m_cloth.SetType(i, e_staticMass);
+			}
+		}		
+	}
+
+	void Step()
+	{
+		float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+
+		if (g_settings.pause)
+		{
+			if (g_settings.singleStep)
+			{
+				g_settings.singleStep = false;
+			}
+			else
+			{
+				dt = 0.0f;
+			}
+		}
+
+		m_cloth.Step(dt);
+		m_cloth.Draw(g_debugDraw);
+
+		b3SpringClothStep step = m_cloth.GetStep();
+
+		char text[256];
+		sprintf(text, "Iterations = %u", step.iterations);
+		g_debugDraw->DrawString(text, b3Color_white);
+	}
+
+	static Test* Create()
+	{
+		return new SpringCloth();
+	}
+
+	b3StackAllocator m_clothAllocator;
+	b3SpringCloth m_cloth;
+	b3AABB3 m_aabb;
+};
+
+#endif

include/bounce/bounce.h

@@ -56,6 +56,7 @@
 
 #include <bounce/dynamics/rope/rope.h>
 #include <bounce/dynamics/cloth/cloth.h>
+#include <bounce/dynamics/cloth/spring_cloth.h>
 #include <bounce/dynamics/body.h>
 
 //#include <bounce/dynamics/tree/joints/tree_weld_joint.h>

include/bounce/dynamics/cloth/spring_cloth.h

@@ -0,0 +1,180 @@
+/*
+* 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>
+
+class b3StackAllocator;
+class b3Draw;
+
+struct b3Mesh;
+
+struct b3SpringClothDef
+{
+	b3SpringClothDef()
+	{
+		allocator = nullptr;
+		mesh = nullptr;
+		density = 0.0f;
+		ks = 0.0f;
+		kb = 0.0f;
+		kd = 0.0f;
+		gravity.SetZero();
+	}
+
+	// Stack allocator
+	b3StackAllocator* allocator;
+
+	// Cloth mesh	
+	b3Mesh* mesh;
+
+	// Cloth density in kg/m^2
+	float32 density;
+
+	// Streching stiffness
+	float32 ks;
+
+	// Bending stiffness
+	float32 kb;
+
+	// Damping stiffness
+	float32 kd;
+	
+	// Force due to gravity 
+	b3Vec3 gravity;
+};
+
+struct b3Spring
+{
+	// 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.
+// Dynamic masses have non-zero mass and can move due to internal and external forces.
+enum b3MassType
+{
+	e_staticMass,
+	e_dynamicMass
+};
+
+// Time step statistics
+struct b3SpringClothStep
+{
+	u32 iterations;
+};
+
+// This class implements 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();
+
+	//
+	void Initialize(const b3SpringClothDef& def);
+
+	//
+	void SetGravity(const b3Vec3& gravity);
+
+	//
+	const b3Vec3& GetGravity() const;
+
+	//
+	void SetType(u32 i, b3MassType type);
+
+	//
+	b3MassType GetType(u32 i) const;
+
+	// 
+	const b3SpringClothStep& GetStep() const;
+
+	//
+	void Step(float32 dt);
+
+	//
+	void Apply() const;
+
+	//
+	void Draw(b3Draw* draw) const;
+protected:
+	b3StackAllocator* m_allocator;
+
+	b3Mesh* m_mesh;
+
+	b3Vec3 m_gravity;
+
+	b3Vec3* m_x;
+	b3Vec3* m_v;
+	b3Vec3* m_f;
+	float32* m_inv_m;
+	b3MassType* m_massTypes;
+	u32 m_massCount;
+
+	b3Spring* m_springs;
+	u32 m_springCount;
+
+	b3SpringClothStep m_step;
+};
+
+inline const b3Vec3& b3SpringCloth::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline void b3SpringCloth::SetGravity(const b3Vec3& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline b3MassType b3SpringCloth::GetType(u32 i) const
+{
+	B3_ASSERT(i < m_massCount);
+	return m_massTypes[i];
+}
+
+inline void b3SpringCloth::SetType(u32 i, b3MassType type)
+{
+	B3_ASSERT(i < m_massCount);
+	m_massTypes[i] = type;
+}
+
+inline const b3SpringClothStep& b3SpringCloth::GetStep() const
+{
+	return m_step;
+}
+
+#endif

src/bounce/dynamics/cloth/spring_cloth.cpp

@@ -0,0 +1,698 @@
+/*
+* 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/collision/shapes/mesh.h>
+#include <bounce/common/memory/stack_allocator.h>
+
+// Here, we solve Ax = b using the Modified Conjugate Gradient method.
+// This work is based on the paper "Large Steps in Cloth Simulation - David Baraff, Andrew Witkin".
+
+b3SpringCloth::b3SpringCloth()
+{
+	m_allocator = nullptr;
+
+	m_mesh = nullptr;
+
+	m_gravity.SetZero();
+
+	m_x = nullptr;
+	m_v = nullptr;
+	m_f = nullptr;
+	m_inv_m = nullptr;
+	m_massTypes = nullptr;
+	m_massCount = 0;
+	
+	m_springs = nullptr;
+	m_springCount = 0;
+
+	m_step.iterations = 0;
+}
+
+b3SpringCloth::~b3SpringCloth()
+{
+	b3Free(m_x);
+	b3Free(m_v);
+	b3Free(m_f);
+	b3Free(m_inv_m);
+	b3Free(m_massTypes);
+	b3Free(m_springs);
+}
+
+void b3SpringCloth::Initialize(const b3SpringClothDef& def)
+{
+	B3_ASSERT(def.allocator);
+	B3_ASSERT(def.mesh);
+
+	m_allocator = def.allocator;
+	m_mesh = def.mesh;
+	m_gravity = def.gravity;
+
+	const b3Mesh* 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_inv_m = (float32*)b3Alloc(m_massCount * sizeof(float32));
+	m_massTypes = (b3MassType*)b3Alloc(m_massCount * sizeof(b3MassType));
+
+	for (u32 i = 0; i < m->vertexCount; ++i)
+	{
+		m_x[i] = m->vertices[i];
+		m_v[i].SetZero();
+		m_f[i].SetZero();
+		m_inv_m[i] = 0.0f;
+		m_massTypes[i] = e_staticMass;
+	}
+
+	// Initialize mass
+	for (u32 i = 0; i < m->triangleCount; ++i)
+	{
+		b3Triangle* 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);
+		float32 mass = def.density * area;
+
+		const float32 inv3 = 1.0f / 3.0f;
+
+		m_inv_m[t->v1] += inv3 * mass;
+		m_inv_m[t->v2] += inv3 * mass;
+		m_inv_m[t->v3] += inv3 * mass;
+	}
+	
+	// Invert
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		if (m_inv_m[i] > 0.0f)
+		{
+			m_inv_m[i] = 1.0f / m_inv_m[i];
+			m_massTypes[i] = e_dynamicMass;
+		}
+	}
+
+	// Initialize springs
+	m_springs = (b3Spring*)b3Alloc(3 * m->triangleCount * sizeof(b3Spring));
+	
+	// Streching
+	for (u32 i = 0; i < m->triangleCount; ++i)
+	{
+		b3Triangle* t = m->triangles + i;
+		
+		u32 is[3] = { t->v1, t->v2, t->v3 };
+		for (u32 j = 0; j < 3; ++j)
+		{
+			u32 k = j + 1 < 3 ? j + 1 : 0;
+
+			u32 v1 = is[j];
+			u32 v2 = is[k];
+
+			b3Vec3 p1 = m->vertices[v1];
+			b3Vec3 p2 = m->vertices[v2];
+
+			// Skip duplicated spring
+			bool found = false;
+			for (u32 s = 0; s < m_springCount; ++s)
+			{
+				if ((m_springs[s].i1 == v1 && m_springs[s].i2 == v2) || (m_springs[s].i1 == v2 && m_springs[s].i2 == v1))
+				{
+					found = true;
+					break;
+				}
+			}
+
+			if (found == false)
+			{
+				b3Spring* S = m_springs + m_springCount;
+				S->i1 = v1;
+				S->i2 = v2;
+				S->L0 = b3Distance(p1, p2);
+				S->ks = def.ks;
+				S->kd = def.kd;
+				++m_springCount;
+			}
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3Filter(b3Vec3* out, const b3Vec3* v, u32 size, const b3MassType* types)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		switch (types[i])
+		{
+		case e_staticMass:
+		{
+			out[i].SetZero();
+			break;
+		}
+		case e_dynamicMass:
+		{
+
+			out[i] = v[i];
+			break;
+		}
+		default:
+		{
+			B3_ASSERT(false);
+			break;
+		}
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3SetZero(b3Vec3* out, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i].SetZero();
+	}
+}
+
+static B3_FORCE_INLINE void b3Copy(b3Vec3* out, const b3Vec3* v, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i] = v[i];
+	}
+}
+
+static B3_FORCE_INLINE void b3Add(b3Vec3* out, const b3Vec3* a, const b3Vec3* b, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i] = a[i] + b[i];
+	}
+}
+
+static B3_FORCE_INLINE void b3Sub(b3Vec3* out, const b3Vec3* a, const b3Vec3* b, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i] = a[i] - b[i];
+	}
+}
+
+static B3_FORCE_INLINE float32 b3Dot(const b3Vec3* a, const b3Vec3* b, u32 size)
+{
+	float32 result = 0.0f;
+	for (u32 i = 0; i < size; ++i)
+	{
+		result += b3Dot(a[i], b[i]);
+	}
+	return result;
+}
+
+#define B3_INDEX(i, j, size) (i + j * size)
+
+static B3_FORCE_INLINE void b3SetZero(b3Mat33* out, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		for (u32 j = 0; j < size; ++j)
+		{
+			out[B3_INDEX(i, j, size)].SetZero();
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3Mul(b3Vec3* out, const b3Mat33* M, const b3Vec3* v, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i].SetZero();
+
+		for (u32 j = 0; j < size; ++j)
+		{
+			out[i] += M[ B3_INDEX(i, j, size) ] * v[j];
+		}
+	}
+}
+
+// J = dfdx or dvdx
+static B3_FORCE_INLINE void b3Mul_Jacobian(b3Vec3* out, const b3Vec3* v, u32 mass_size, 
+	const b3Mat33* J_ii, const b3Spring* springs, u32 spring_size)
+{
+	b3SetZero(out, mass_size);
+
+	for (u32 i = 0; i < spring_size; ++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];
+	}
+}
+
+static B3_FORCE_INLINE void b3SetZero_Jacobian(b3Mat33* out, u32 size)
+{
+	for (u32 i = 0; i < size; ++i)
+	{
+		out[i].SetZero();
+	}
+}
+
+// A = M - h * dfdv - h * h * dfdx
+// A * v = (M - h * dfdv - h * h * dfdx) * v = M * v + (-h * dfdv * v) + (-h * h * dfdx * v) 
+static B3_FORCE_INLINE void b3Mul_A(b3Vec3* out, const b3Vec3* v, u32 mass_size, 
+	b3StackAllocator* allocator, 
+	const float32* inv_m, float32 h, const b3Mat33* Jx, const b3Mat33* Jv, const b3Spring* springs, u32 spring_size)
+{
+	// v1 = M * v
+	b3Vec3* v1 = (b3Vec3*)allocator->Allocate(mass_size * sizeof(b3Vec3));
+	for (u32 i = 0; i < mass_size; ++i)
+	{
+		float32 m = inv_m[i] != 0.0f ? 1.0f / inv_m[i] : 0.0f;
+		v1[i] = m * v[i];
+	}
+
+	// v2 = (-h * dfdv * v)
+	b3Vec3* v2 = (b3Vec3*)allocator->Allocate(mass_size * sizeof(b3Vec3));
+	b3Mul_Jacobian(v2, v, mass_size, Jv, springs, spring_size);
+	for (u32 i = 0; i < mass_size; ++i)
+	{
+		v2[i] *= -h;
+	}
+
+	// v3 = (-h * h * dfdx * v)
+	b3Vec3* v3 = (b3Vec3*)allocator->Allocate(mass_size * sizeof(b3Vec3));
+	b3Mul_Jacobian(v3, v, mass_size, Jx, springs, spring_size);
+	for (u32 i = 0; i < mass_size; ++i)
+	{
+		v3[i] *= -h * h;
+	}
+
+	// v = v1 + v2 + v3
+	for (u32 i = 0; i < mass_size; ++i)
+	{
+		out[i] = v1[i] + v2[i] + v3[i];
+	}
+
+	allocator->Free(v3);
+	allocator->Free(v2);
+	allocator->Free(v1);
+}
+
+void b3SpringCloth::Step(float32 h)
+{
+	u32 size = m_massCount;
+	b3MassType* types = m_massTypes;
+	u32 spring_size = m_springCount;
+
+	// Add gravity
+	for (u32 i = 0; i < size; ++i)
+	{
+		m_f[i] += m_gravity;
+	}
+	
+	// Compute non-zero Jacobians Jx, Jv
+	b3Mat33* Jx = (b3Mat33*)m_allocator->Allocate(spring_size * sizeof(b3Mat33));
+	b3SetZero_Jacobian(Jx, spring_size);
+
+	b3Mat33* Jv = (b3Mat33*)m_allocator->Allocate(spring_size * sizeof(b3Mat33));
+	b3SetZero_Jacobian(Jv, spring_size);
+
+	// Compute forces and Jacobians
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* S = m_springs + i;
+
+		b3Vec3 x1 = m_x[S->i1];
+		b3Vec3 v1 = m_v[S->i1];
+
+		b3Vec3 x2 = m_x[S->i2];
+		b3Vec3 v2 = m_v[S->i2];
+
+		// Strech
+		b3Vec3 dx = x2 - x1;
+		float32 L = b3Length(dx);
+		b3Vec3 n = dx;
+		if (L > 0.0f)
+		{
+			n /= L;
+		}
+
+		float32 C = L - S->L0;
+		
+		// Compute streching forces
+		b3Vec3 sf1 = -S->ks * C * -n;
+		b3Vec3 sf2 = -sf1;
+
+		m_f[S->i1] += sf1;
+		m_f[S->i2] += sf2;
+
+		// Compute damping forces
+		b3Vec3 dv = v2 - v1;
+
+		b3Vec3 df1 = -S->kd * -dv;
+		b3Vec3 df2 = -df1;
+
+		m_f[S->i1] += df1;
+		m_f[S->i2] += df2;
+
+		b3Mat33 I = b3Mat33_identity;
+		
+		// Compute Jx11	
+		float32 inv_L = L > 0.0f ? 1.0f / L : 0.0f;
+		
+		float32 L2 = L * L;
+		float32 inv_L2 = L2 > 0.0f ? 1.0f / L2 : 0.0f;
+
+		// Hessian
+		// del^2_C / del_x
+		b3Mat33 H_11 = inv_L * I + inv_L2 * b3Outer(dx, -n);
+
+		// del_C / del_x * del_C / del_x^T
+		b3Mat33 JJ_11 = b3Outer(-n, -n);
+
+		b3Mat33 Jx11 = -S->ks * (C * H_11 + JJ_11);
+		Jx[i] = Jx11;
+
+		// Compute Jv11
+		b3Mat33 Jv11 = -S->kd * I;
+		Jv[i] = Jv11;
+	}
+	
+	// Solve Ax = b
+
+	// Compute b
+
+	// b = h * (f0 + h * dfdx * v0) 
+	b3Vec3* b = (b3Vec3*) m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	// b = dfdx * v0
+	// b3Mul(b, dfdx, m_v, size);
+	b3Mul_Jacobian(b, m_v, size, Jx, m_springs, m_springCount);
+
+	// b = h * (f0 + h * b)
+	for (u32 i = 0; i < size; ++i)
+	{
+		b[i] = h * (m_f[i] + h * b[i]);
+	}
+
+	// Solve Ax = b
+	b3Vec3* z = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* dv = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* Adv = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* r = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* c = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* q = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+	
+	b3Vec3* s = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* P = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	b3Vec3* inv_P = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+
+	// Compute z
+	for (u32 i = 0; i < size; ++i)
+	{
+		switch (types[i])
+		{
+		case e_staticMass:
+		{
+			z[i].SetZero();
+			break;
+		}
+		case e_dynamicMass:
+		{
+			z[i].SetZero();
+			break;
+		}
+		default:
+		{
+			B3_ASSERT(false);
+			break;
+		}
+		}
+	}
+
+	// dv = z
+	b3Copy(dv, z, size);
+
+	// Adv = A * dv 
+	// b3Mul(Adv, A, dv, size);
+	b3Mul_A(Adv, dv, size, m_allocator, m_inv_m, h, Jx, Jv, m_springs, m_springCount);
+
+	// Compute P, P^-1
+	// We compute A because P = diag(A)^-1
+	// Note this is not necessary, and should be optimized as soon 
+	// as possible.
+
+	// A = M - h * dfdv - h * h * dfdx
+	b3Mat33* A = (b3Mat33*)m_allocator->Allocate(size * size * sizeof(b3Mat33));
+
+	// A = 0
+	b3SetZero(A, size);
+
+	// Compute dfdx, dfdv
+	b3Mat33* dfdx = (b3Mat33*)m_allocator->Allocate(size * size * sizeof(b3Mat33));
+	b3SetZero(dfdx, size);
+
+	b3Mat33* dfdv = (b3Mat33*)m_allocator->Allocate(size * size * sizeof(b3Mat33));
+	b3SetZero(dfdv, size);
+
+	for (u32 i = 0; i < m_springCount; ++i)
+	{
+		b3Spring* S = m_springs + i;
+
+		b3Mat33 Jx11 = Jx[i];
+		b3Mat33 Jx12 = -Jx11;
+		b3Mat33 Jx21 = Jx12;
+		b3Mat33 Jx22 = Jx11;
+
+		dfdx[B3_INDEX(S->i1, S->i1, size)] += Jx11;
+		dfdx[B3_INDEX(S->i1, S->i2, size)] += Jx12;
+		dfdx[B3_INDEX(S->i2, S->i1, size)] += Jx21;
+		dfdx[B3_INDEX(S->i2, S->i2, size)] += Jx22;
+
+		b3Mat33 Jv11 = Jv[i];
+		b3Mat33 Jv12 = -Jv11;
+		b3Mat33 Jv21 = Jv12;
+		b3Mat33 Jv22 = Jv11;
+
+		dfdv[B3_INDEX(S->i1, S->i1, size)] += Jv11;
+		dfdv[B3_INDEX(S->i1, S->i2, size)] += Jv12;
+		dfdv[B3_INDEX(S->i2, S->i1, size)] += Jv21;
+		dfdv[B3_INDEX(S->i2, S->i2, size)] += Jv22;
+	}
+
+	// A += M
+	for (u32 i = 0; i < size; ++i)
+	{
+		float32 m = 1.0f / m_inv_m[i];
+
+		A[B3_INDEX(i, i, size)] += b3Diagonal(m);
+	}
+
+	// A += - h * dfdv - h * h * dfdx
+	for (u32 i = 0; i < size; ++i)
+	{
+		for (u32 j = 0; j < size; ++j)
+		{
+			A[B3_INDEX(i, j, size)] += (-h * dfdv[B3_INDEX(i, j, size)]) + (-h * h * dfdx[B3_INDEX(i, j, size)]);
+		}
+	}
+
+	for (u32 i = 0; i < size; ++i)
+	{
+		b3Mat33 D = A[B3_INDEX(i, i, size)];
+
+		B3_ASSERT(D[0][0] != 0.0f);
+		B3_ASSERT(D[1][1] != 0.0f);
+		B3_ASSERT(D[2][2] != 0.0f);
+
+		P[i] = b3Vec3(1.0f / D[0][0], 1.0f / D[1][1], 1.0f / D[2][2]);
+		inv_P[i] = b3Vec3(D[0][0], D[1][1], D[2][2]);
+	}
+
+	m_allocator->Free(dfdv);
+	m_allocator->Free(dfdx);
+	m_allocator->Free(A);
+
+	// eps0 = dot( filter(b), P * filter(b) )
+	b3Vec3* filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+	b3Filter(filter_b, b, size, types);
+
+	b3Vec3* P_filter_b = (b3Vec3*)m_allocator->Allocate(size * sizeof(b3Vec3));
+	for (u32 i = 0; i < size; ++i)
+	{
+		P_filter_b[i][0] = P[i][0] * filter_b[i][0];
+		P_filter_b[i][1] = P[i][1] * filter_b[i][1];
+		P_filter_b[i][2] = P[i][2] * filter_b[i][2];
+	}
+
+	float32 eps0 = b3Dot(filter_b, P_filter_b, size);
+
+	m_allocator->Free(P_filter_b);
+	m_allocator->Free(filter_b);
+
+	// r = filter(b - Adv)
+	b3Sub(r, b, Adv, size);
+	b3Filter(r, r, size, types);
+
+	// c = filter(P^-1 * r)
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		c[i][0] = inv_P[i][0] * r[i][0];
+		c[i][1] = inv_P[i][1] * r[i][1];
+		c[i][2] = inv_P[i][2] * r[i][2];
+	}
+	b3Filter(c, c, size, types);
+
+	// epsNew = dot(r, c)
+	float32 epsNew = b3Dot(r, c, size);
+
+	// This is in [0, 1]
+	// Making it smaller can increase accuracy, but it might increase the number 
+	// of iterations to be taken by the solver.
+	const float32 kTol = 0.75f;
+
+	// Limit number of iterations to prevent cycling.
+	const u32 kMaxIters = 100;
+	
+	// Main iteration loop.
+	u32 iter = 0;
+	while (iter < kMaxIters && epsNew > kTol * kTol * eps0)
+	{
+		// q = filter(A * c) 
+		// b3Mul(q, A, c, size);
+		b3Mul_A(q, c, size, m_allocator, m_inv_m, h, Jx, Jv, m_springs, m_springCount);
+		b3Filter(q, q, size, types);
+
+		// alpha = epsNew / dot(c, q)
+		float32 alpha = epsNew / b3Dot(c, q, size);
+
+		// x = x + alpha * c
+		for (u32 i = 0; i < m_massCount; ++i)
+		{
+			dv[i] = dv[i] + alpha * c[i];
+		}
+
+		// r = r - alpha * q 
+		for (u32 i = 0; i < m_massCount; ++i)
+		{
+			r[i] = r[i] - alpha * q[i];
+		}
+
+		// s = inv_P * r
+		for (u32 i = 0; i < m_massCount; ++i)
+		{
+			s[i][0] = inv_P[i][0] * r[i][0];
+			s[i][1] = inv_P[i][1] * r[i][1];
+			s[i][2] = inv_P[i][2] * r[i][2];
+		}
+
+		// epsOld = epsNew
+		float32 epsOld = epsNew;
+		
+		// epsNew = dot(r, s) 
+		epsNew = b3Dot(r, s, size);
+
+		// beta = epsNew / epsOld
+		float32 beta = epsNew / epsOld;
+
+		// c = filter(s + beta * c) 
+		for (u32 i = 0; i < m_massCount; ++i)
+		{
+			c[i] = s[i] + beta * c[i];
+		}
+		b3Filter(c, c, size, types);
+
+		++iter;
+	}
+
+	m_step.iterations = iter;
+
+	// Update state
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		m_v[i] += dv[i];
+		m_x[i] += h * m_v[i];
+	}
+
+	// Clear forces
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		m_f[i].SetZero();
+	}
+
+	m_allocator->Free(inv_P);
+	m_allocator->Free(P);
+	m_allocator->Free(s);
+	m_allocator->Free(q);
+	m_allocator->Free(c);
+	m_allocator->Free(r);
+	m_allocator->Free(Adv);
+	m_allocator->Free(dv);
+	m_allocator->Free(z);
+	m_allocator->Free(b);
+	m_allocator->Free(Jv);
+	m_allocator->Free(Jx);
+}
+
+void b3SpringCloth::Apply() const
+{
+	for (u32 i = 0; i < m_massCount; ++i)
+	{
+		m_mesh->vertices[i] = m_x[i];
+	}
+}
+
+void b3SpringCloth::Draw(b3Draw* draw) const
+{
+	const b3Mesh* m = m_mesh;
+
+	for (u32 i = 0; i < m->vertexCount; ++i)
+	{
+		draw->DrawPoint(m_x[i], 2.0f, b3Color_green);
+	}
+
+	for (u32 i = 0; i < m->triangleCount; ++i)
+	{
+		b3Triangle* 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();
+
+		b3Vec3 n2 = -n1;
+
+		draw->DrawSolidTriangle(n1, v1, v2, v3, b3Color_blue);
+		draw->DrawSolidTriangle(n2, v1, v3, v2, b3Color_blue);
+	}
+}