diff --git a/examples/testbed/framework/softbody_dragger.cpp b/examples/testbed/framework/softbody_dragger.cpp
new file mode 100644
index 0000000..1667c2e
--- /dev/null
+++ b/examples/testbed/framework/softbody_dragger.cpp
@@ -0,0 +1,128 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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 <testbed/framework/softbody_dragger.h>
+
+b3SoftBodyDragger::b3SoftBodyDragger(b3Ray3* ray, b3SoftBody* body)
+{
+	m_ray = ray;
+	m_body = body;
+	m_tetrahedron = nullptr;
+}
+
+b3SoftBodyDragger::~b3SoftBodyDragger()
+{
+
+}
+
+bool b3SoftBodyDragger::StartDragging()
+{
+	B3_ASSERT(IsDragging() == false);
+
+	b3SoftBodyRayCastSingleOutput rayOut;
+	if (m_body->RayCastSingle(&rayOut, m_ray->A(), m_ray->B()) == false)
+	{
+		return false;
+	}
+
+	m_mesh = m_body->GetMesh();
+	m_tetrahedron = m_mesh->tetrahedrons + rayOut.tetrahedron;
+	m_v1 = m_tetrahedron->v1;
+	m_v2 = m_tetrahedron->v2;
+	m_v3 = m_tetrahedron->v3;
+	m_v4 = m_tetrahedron->v4;
+	m_x = rayOut.fraction;
+
+	b3SoftBodyNode* n1 = m_body->GetVertexNode(m_v1);
+	b3SoftBodyNode* n2 = m_body->GetVertexNode(m_v2);
+	b3SoftBodyNode* n3 = m_body->GetVertexNode(m_v3);
+	b3SoftBodyNode* n4 = m_body->GetVertexNode(m_v4);
+
+	b3Vec3 v1 = n1->GetPosition();
+	b3Vec3 v2 = n2->GetPosition();
+	b3Vec3 v3 = n3->GetPosition();
+	b3Vec3 v4 = n4->GetPosition();
+
+	b3Vec3 B = GetPointB();
+
+	float32 wABCD[5];
+	b3BarycentricCoordinates(wABCD, v1, v2, v3, v4, B);
+
+	if (wABCD[4] > B3_EPSILON)
+	{
+		m_tu = wABCD[0] / wABCD[4];
+		m_tv = wABCD[1] / wABCD[4];
+		m_tw = wABCD[2] / wABCD[4];
+		m_tx = wABCD[3] / wABCD[4];
+	}
+	else
+	{
+		m_tu = m_tv = m_tw = m_tx = 0.0f;
+	}
+
+	return true;
+}
+
+void b3SoftBodyDragger::Drag()
+{
+	B3_ASSERT(IsDragging() == true);
+
+	b3Vec3 A = GetPointA();
+	b3Vec3 B = GetPointB();
+
+	b3Vec3 dx = B - A;
+
+	const float32 k = 100.0f;
+
+	b3Vec3 f = k * dx;
+
+	b3Vec3 f1 = m_tu * f;
+	b3Vec3 f2 = m_tv * f;
+	b3Vec3 f3 = m_tw * f;
+	b3Vec3 f4 = m_tx * f;
+
+	m_body->GetVertexNode(m_v1)->ApplyForce(f1);
+	m_body->GetVertexNode(m_v2)->ApplyForce(f2);
+	m_body->GetVertexNode(m_v3)->ApplyForce(f3);
+	m_body->GetVertexNode(m_v4)->ApplyForce(f4);
+}
+
+void b3SoftBodyDragger::StopDragging()
+{
+	B3_ASSERT(IsDragging() == true);
+	
+	m_tetrahedron = nullptr;
+}
+
+b3Vec3 b3SoftBodyDragger::GetPointA() const
+{
+	B3_ASSERT(IsDragging() == true);
+
+	b3Vec3 A = m_body->GetVertexNode(m_v1)->GetPosition();
+	b3Vec3 B = m_body->GetVertexNode(m_v2)->GetPosition();
+	b3Vec3 C = m_body->GetVertexNode(m_v3)->GetPosition();
+	b3Vec3 D = m_body->GetVertexNode(m_v4)->GetPosition();
+
+	return m_tu * A + m_tv * B + m_tw * C + m_tx * D;
+}
+
+b3Vec3 b3SoftBodyDragger::GetPointB() const
+{
+	B3_ASSERT(IsDragging() == true);
+	return (1.0f - m_x) * m_ray->A() + m_x * m_ray->B();
+}
\ No newline at end of file
diff --git a/examples/testbed/framework/softbody_dragger.h b/examples/testbed/framework/softbody_dragger.h
new file mode 100644
index 0000000..e8a6a92
--- /dev/null
+++ b/examples/testbed/framework/softbody_dragger.h
@@ -0,0 +1,61 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFTBODY_DRAGGER_H
+#define B3_SOFTBODY_DRAGGER_H
+
+#include <bounce/common/geometry.h>
+#include <bounce/softbody/softbody.h>
+#include <bounce/softbody/softbody_mesh.h>
+#include <bounce/softbody/softbody_node.h>
+
+// A soft body triangle dragger.
+class b3SoftBodyDragger
+{
+public:
+	b3SoftBodyDragger(b3Ray3* ray, b3SoftBody* body);
+	~b3SoftBodyDragger();
+
+	bool IsDragging() const;
+
+	bool StartDragging();
+
+	void Drag();
+
+	void StopDragging();
+
+	b3Vec3 GetPointA() const;
+
+	b3Vec3 GetPointB() const;
+private:
+	b3Ray3* m_ray;
+	float32 m_x;
+
+	b3SoftBody* m_body;
+	const b3SoftBodyMesh* m_mesh;
+	const b3SoftBodyMeshTetrahedron* m_tetrahedron;
+	u32 m_v1, m_v2, m_v3, m_v4;
+	float32 m_tu, m_tv, m_tw, m_tx;
+};
+
+inline bool b3SoftBodyDragger::IsDragging() const
+{
+	return m_tetrahedron != nullptr;
+}
+
+#endif
\ No newline at end of file
diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp
index d0aaca9..5f4d116 100644
--- a/examples/testbed/framework/test_entries.cpp
+++ b/examples/testbed/framework/test_entries.cpp
@@ -65,8 +65,10 @@
 #include <testbed/tests/particle_types.h>
 #include <testbed/tests/tension_mapping.h>
 #include <testbed/tests/self_collision.h>
-#include <testbed/tests/soft_body.h>
 #include <testbed/tests/rope_test.h>
+#include <testbed/tests/softbody.h>
+#include <testbed/tests/pinned_softbody.h>
+#include <testbed/tests/smash_softbody.h>
 
 TestEntry g_tests[] =
 {
@@ -118,6 +120,8 @@ TestEntry g_tests[] =
 	{ "Tension Mapping", &TensionMapping::Create },
 	{ "Self-Collision", &SelfCollision::Create },
 	{ "Soft Body", &SoftBody::Create },
+	{ "Pinned Soft Body", &PinnedSoftBody::Create },
+	{ "Smash Soft Body", &SmashSoftBody::Create },
 	{ "Rope", &Rope::Create },
 	{ NULL, NULL }
 };
diff --git a/examples/testbed/tests/pinned_softbody.h b/examples/testbed/tests/pinned_softbody.h
new file mode 100644
index 0000000..023b490
--- /dev/null
+++ b/examples/testbed/tests/pinned_softbody.h
@@ -0,0 +1,135 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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 PINNED_SOFTBODY_H
+#define PINNED_SOFTBODY_H
+
+#include <testbed/framework/softbody_dragger.h>
+
+class PinnedSoftBody : public Test
+{
+public:
+	PinnedSoftBody()
+	{
+		m_mesh.SetAsSphere(5.0f, 0);
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 100.0f;
+		def.nu = 0.33f;
+
+		m_body = new b3SoftBody(def);
+
+		u32 pinIndex = ~0;
+		float32 pinDot = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			float32 dot = b3Dot(m_mesh.vertices[i], b3Vec3_y);
+			if (dot > pinDot)
+			{
+				pinDot = dot;
+				pinIndex = i;
+			}
+		}
+
+		b3SoftBodyNode* pinNode = m_body->GetVertexNode(pinIndex);
+		pinNode->SetType(e_staticSoftBodyNode);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~PinnedSoftBody()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		float32 E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new PinnedSoftBody();
+	}
+
+	b3QSoftBodyMesh m_mesh;
+
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/testbed/tests/smash_softbody.h b/examples/testbed/tests/smash_softbody.h
new file mode 100644
index 0000000..1b54d7c
--- /dev/null
+++ b/examples/testbed/tests/smash_softbody.h
@@ -0,0 +1,172 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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 SMASH_SOFTBODY_H
+#define SMASH_SOFTBODY_H
+
+#include <testbed/framework/softbody_dragger.h>
+
+class SmashSoftBody : public Test
+{
+public:
+	SmashSoftBody()
+	{
+		m_mesh.SetAsSphere(2.0f, 1);
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			m_mesh.vertices[i].y += 3.0f;
+		}
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 100.0f;
+		def.nu = 0.33f;
+
+		m_body = new b3SoftBody(def);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+		m_body->SetWorld(&m_world);
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			b3SoftBodyNode* n = m_body->GetVertexNode(i);
+
+			n->SetRadius(0.05f);
+			n->SetFriction(0.2f);
+		}
+
+		// Create ground
+		{
+			b3BodyDef bd;
+			bd.type = e_staticBody;
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			b3HullShape groundShape;
+			groundShape.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &groundShape;
+			sd.friction = 0.3f;
+
+			b->CreateShape(sd);
+		}
+
+		// Create body
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.y = 10.0f;
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			static b3BoxHull boxHull(5.0f, 1.0f, 5.0f);
+
+			b3HullShape boxShape;
+			boxShape.m_hull = &boxHull;
+
+			b3ShapeDef sd;
+			sd.shape = &boxShape;
+			sd.density = 0.1f;
+			sd.friction = 0.3f;
+
+			b->CreateShape(sd);
+		}
+		
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~SmashSoftBody()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		float32 E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new SmashSoftBody();
+	}
+
+	b3QSoftBodyMesh m_mesh;
+
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/testbed/tests/soft_body.h b/examples/testbed/tests/soft_body.h
deleted file mode 100644
index aae10ce..0000000
--- a/examples/testbed/tests/soft_body.h
+++ /dev/null
@@ -1,293 +0,0 @@
-/*
-* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
-*
-* 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 SOFT_BODY_H
-#define SOFT_BODY_H
-
-#include <bounce/meshgen/sphere_mesh.h>
-
-struct b3QClothMesh : public b3ClothMesh
-{
-	b3StackArray<b3Vec3, 256> clothVertices;
-	b3StackArray<b3ClothMeshTriangle, 256> clothTriangles;
-	b3ClothMeshMesh clothMesh;
-
-	b3QClothMesh()
-	{
-		vertices = nullptr;
-		vertexCount = 0;
-		triangles = nullptr;
-		triangleCount = 0;
-		meshCount = 0;
-		meshes = nullptr;
-		sewingLineCount = 0;
-		sewingLines = nullptr;
-	}
-
-	void SetAsSphere(float32 radius = 1.0f)
-	{
-		smMesh mesh;
-		smCreateMesh(mesh, 2);
-
-		clothVertices.Resize(mesh.vertexCount);
-		for (u32 i = 0; i < mesh.vertexCount; ++i)
-		{
-			clothVertices[i] = radius * mesh.vertices[i];
-		}
-
-		clothTriangles.Resize(mesh.indexCount / 3);
-		for (u32 i = 0; i < mesh.indexCount / 3; ++i)
-		{
-			clothTriangles[i].v1 = mesh.indices[3 * i + 0];
-			clothTriangles[i].v2 = mesh.indices[3 * i + 1];
-			clothTriangles[i].v3 = mesh.indices[3 * i + 2];
-		}
-
-		clothMesh.startTriangle = 0;
-		clothMesh.triangleCount = clothTriangles.Count();
-		clothMesh.startVertex = 0;
-		clothMesh.vertexCount = clothVertices.Count();
-
-		vertices = clothVertices.Begin();
-		vertexCount = clothVertices.Count();
-		triangles = clothTriangles.Begin();
-		triangleCount = clothTriangles.Count();
-		meshCount = 1;
-		meshes = &clothMesh;
-	}
-};
-
-class SoftBody : public Test
-{
-public:
-	SoftBody()
-	{
-		m_mesh.SetAsSphere(2.0f);
-
-		// Translate the cloth mesh upwards
-		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
-		{
-			m_mesh.vertices[i].y += 10.0f;
-		}
-
-		// Create cloth
-		b3ClothDef def;
-		def.mesh = &m_mesh;
-		def.density = 0.2f;
-		def.structural = 10000.0f;
-		def.bending = 0.0f;
-		def.damping = 0.0f;
-
-		m_cloth = new b3Cloth(def);
-
-		for (b3Particle* p = m_cloth->GetParticleList().m_head; p; p = p->GetNext())
-		{
-			p->SetRadius(0.05f);
-			p->SetFriction(0.5f);
-		}
-
-		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
-		
-		m_cloth->SetGravity(gravity);
-
-		// Attach a world to the cloth
-		m_cloth->SetWorld(&m_world);
-
-		// Create static shapes
-		{
-			b3BodyDef bd;
-			bd.type = e_staticBody;
-
-			b3Body* b = m_world.CreateBody(bd);
-
-			static b3BoxHull boxHull(10.0f, 1.0f, 10.0f);
-
-			b3HullShape boxShape;
-			boxShape.m_hull = &boxHull;
-
-			b3ShapeDef sd;
-			sd.shape = &boxShape;
-			sd.friction = 0.5f;
-
-			b->CreateShape(sd);
-		}
-
-		m_clothDragger = new b3ClothDragger(&m_ray, m_cloth);
-	}
-
-	~SoftBody()
-	{
-		delete m_clothDragger;
-		delete m_cloth;
-	}
-
-	// Compute the volume of soft body
-	float32 ComputeVolume() const
-	{
-		const b3ClothMesh* mesh = m_cloth->GetMesh();
-		
-		b3StackArray<b3Vec3, 256> positions;
-		positions.Resize(mesh->vertexCount);
-		
-		for (u32 i = 0; i < mesh->vertexCount; ++i)
-		{
-			b3Particle* p = m_cloth->GetVertexParticle(i);
-			positions[i] = p->GetPosition();
-		}
-
-		b3AABB3 aabb;
-		aabb.Compute(positions.Begin(), positions.Count());
-
-		return aabb.Volume();
-	}
-
-	// Apply pressure forces
-	// Explanation available in the paper 
-	// "Pressure Model of Soft Body Simulation"
-	void ApplyPressureForces()
-	{
-		const b3ClothMesh* mesh = m_cloth->GetMesh();
-
-		// Volume in m^3
-		float32 V = ComputeVolume();
-		
-		// Inverse volume
-		float32 invV = V > 0.0f ? 1.0f / V : 0.0f;
-
-		// Apply pressure forces on particles
-		for (u32 i = 0; i < m_mesh.triangleCount; ++i)
-		{
-			u32 i1 = m_mesh.triangles[i].v1;
-			u32 i2 = m_mesh.triangles[i].v2;
-			u32 i3 = m_mesh.triangles[i].v3;
-
-			b3Particle* p1 = m_cloth->GetVertexParticle(i1);
-			b3Particle* p2 = m_cloth->GetVertexParticle(i2);
-			b3Particle* p3 = m_cloth->GetVertexParticle(i3);
-
-			b3Vec3 v1 = p1->GetPosition();
-			b3Vec3 v2 = p2->GetPosition();
-			b3Vec3 v3 = p3->GetPosition();
-
-			b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
-			
-			// Triangle area
-			float32 A = n.Normalize();
-			A *= 0.5f;
-
-			// Ideal Gas Approximation
-
-			// Number of gas moles
-			const float32 k_n = 1.0f;
-			
-			// Ideal Gas Constant [J / K mol]
-			const float32 k_R = 8.31f;
-			
-			// Gas temperature in Kelvin
-			const float32 k_T = 100.0f;
-
-			// Pressure in Pascals
-			float32 P = invV * k_n * k_R * k_T;
-
-			// Pressure vector
-			b3Vec3 Pn = P * n;
-
-			// Pressure force
-			b3Vec3 FP = Pn * A;
-
-			const float32 inv3 = 1.0f / 3.0f;
-
-			b3Vec3 f = inv3 * FP;
-
-			// Distribute
-			p1->ApplyForce(f);
-			p2->ApplyForce(f);
-			p3->ApplyForce(f);
-		}
-	}
-
-	void Step()
-	{
-		Test::Step();
-
-		ApplyPressureForces();
-
-		m_cloth->Step(g_testSettings->inv_hertz);
-
-		m_cloth->Draw();
-
-		if (m_clothDragger->IsDragging())
-		{
-			b3Vec3 pA = m_clothDragger->GetPointA();
-			b3Vec3 pB = m_clothDragger->GetPointB();
-
-			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
-
-			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
-
-			g_draw->DrawSegment(pA, pB, b3Color_white);
-		}
-
-		extern u32 b3_clothSolverIterations;
-		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_clothSolverIterations);
-
-		float32 E = m_cloth->GetEnergy();
-		g_draw->DrawString(b3Color_white, "E = %f", E);
-	}
-
-	void MouseMove(const b3Ray3& pw)
-	{
-		Test::MouseMove(pw);
-
-		if (m_clothDragger->IsDragging() == true)
-		{
-			m_clothDragger->Drag();
-		}
-	}
-
-	void MouseLeftDown(const b3Ray3& pw)
-	{
-		Test::MouseLeftDown(pw);
-
-		if (m_clothDragger->IsDragging() == false)
-		{
-			m_clothDragger->StartDragging();
-		}
-	}
-
-	void MouseLeftUp(const b3Ray3& pw)
-	{
-		Test::MouseLeftUp(pw);
-
-		if (m_clothDragger->IsDragging() == true)
-		{
-			m_clothDragger->StopDragging();
-		}
-	}
-
-	static Test* Create()
-	{
-		return new SoftBody();
-	}
-
-	b3QClothMesh m_mesh;
-	b3Cloth* m_cloth;
-	b3ClothDragger* m_clothDragger;
-};
-
-#endif
\ No newline at end of file
diff --git a/examples/testbed/tests/softbody.h b/examples/testbed/tests/softbody.h
new file mode 100644
index 0000000..a7af14a
--- /dev/null
+++ b/examples/testbed/tests/softbody.h
@@ -0,0 +1,155 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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 SOFTBODY_H
+#define SOFTBODY_H
+
+#include <testbed/framework/softbody_dragger.h>
+
+class SoftBody : public Test
+{
+public:
+	SoftBody()
+	{
+		m_mesh.SetAsSphere(5.0f, 1);
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			m_mesh.vertices[i].y += 10.0f;
+		}
+
+		// Create soft body
+		b3SoftBodyDef def;
+		def.mesh = &m_mesh;
+		def.density = 0.2f;
+		def.E = 100.0f;
+		def.nu = 0.33f;
+
+		m_body = new b3SoftBody(def);
+
+		b3Vec3 gravity(0.0f, -9.8f, 0.0f);
+		m_body->SetGravity(gravity);
+		m_body->SetWorld(&m_world);
+
+		for (u32 i = 0; i < m_mesh.vertexCount; ++i)
+		{
+			b3SoftBodyNode* n = m_body->GetVertexNode(i);
+
+			n->SetRadius(0.05f);
+			n->SetFriction(0.2f);
+		}
+
+		// Create ground
+		{
+			b3BodyDef bd;
+			bd.type = e_staticBody;
+
+			b3Body* b = m_world.CreateBody(bd);
+
+			m_groundHull.SetAsCylinder(20.0f, 2.0f);
+
+			b3HullShape groundShape;
+			groundShape.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &groundShape;
+			sd.friction = 0.3f;
+
+			b->CreateShape(sd);
+		}
+
+		m_bodyDragger = new b3SoftBodyDragger(&m_ray, m_body);
+	}
+
+	~SoftBody()
+	{
+		delete m_bodyDragger;
+		delete m_body;
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			m_bodyDragger->Drag();
+		}
+
+		m_body->Step(g_testSettings->inv_hertz, g_testSettings->velocityIterations, g_testSettings->positionIterations);
+
+		m_body->Draw();
+
+		if (m_bodyDragger->IsDragging())
+		{
+			b3Vec3 pA = m_bodyDragger->GetPointA();
+			b3Vec3 pB = m_bodyDragger->GetPointB();
+
+			g_draw->DrawPoint(pA, 2.0f, b3Color_green);
+
+			g_draw->DrawPoint(pB, 2.0f, b3Color_green);
+
+			g_draw->DrawSegment(pA, pB, b3Color_white);
+		}
+
+		extern u32 b3_softBodySolverIterations;
+		g_draw->DrawString(b3Color_white, "Iterations = %d", b3_softBodySolverIterations);
+
+		float32 E = m_body->GetEnergy();
+		g_draw->DrawString(b3Color_white, "E = %f", E);
+	}
+
+	void MouseMove(const b3Ray3& pw)
+	{
+		Test::MouseMove(pw);
+	}
+
+	void MouseLeftDown(const b3Ray3& pw)
+	{
+		Test::MouseLeftDown(pw);
+
+		if (m_bodyDragger->IsDragging() == false)
+		{
+			m_bodyDragger->StartDragging();
+		}
+	}
+
+	void MouseLeftUp(const b3Ray3& pw)
+	{
+		Test::MouseLeftUp(pw);
+
+		if (m_bodyDragger->IsDragging() == true)
+		{
+			m_bodyDragger->StopDragging();
+		}
+	}
+
+	static Test* Create()
+	{
+		return new SoftBody();
+	}
+
+	b3QSoftBodyMesh m_mesh;
+	
+	b3SoftBody* m_body;
+	b3SoftBodyDragger* m_bodyDragger;
+	
+	b3QHull m_groundHull;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h
index b44bacc..393e011 100644
--- a/include/bounce/bounce.h
+++ b/include/bounce/bounce.h
@@ -72,4 +72,8 @@
 #include <bounce/cloth/garment/garment.h>
 #include <bounce/cloth/garment/garment_mesh.h>
 
+#include <bounce/softbody/softbody_mesh.h>
+#include <bounce/softbody/softbody.h>
+#include <bounce/softbody/softbody_node.h>
+
 #endif
\ No newline at end of file
diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h
index 212a17f..50adc14 100644
--- a/include/bounce/common/geometry.h
+++ b/include/bounce/common/geometry.h
@@ -144,6 +144,32 @@ inline void b3BarycentricCoordinates(float32 out[4],
 	out[3] = out[0] + out[1] + out[2];
 }
 
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w, x) 
+// with respect to a tetrahedron ABCD.
+// The last output value is the (positive) divisor.
+inline void b3BarycentricCoordinates(float32 out[5],
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D,
+	const b3Vec3& Q)
+{
+	b3Vec3 AB = B - A;
+	b3Vec3 AC = C - A;
+	b3Vec3 AD = D - A;
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+	b3Vec3 QD = D - Q;
+
+	float32 divisor = b3Det(AB, AC, AD);
+	float32 sign = b3Sign(divisor);
+
+	out[0] = sign * b3Det(QB, QC, QD);
+	out[1] = sign * b3Det(QA, QD, QC);
+	out[2] = sign * b3Det(QA, QB, QD);
+	out[3] = sign * b3Det(QA, QC, QB);
+	out[4] = sign * divisor;
+}
+
 // Project a point onto a segment AB.
 inline b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P, const b3Vec3& A, const b3Vec3& B)
 {
diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h
index 24ba1c8..b945ee8 100644
--- a/include/bounce/dynamics/body.h
+++ b/include/bounce/dynamics/body.h
@@ -293,9 +293,7 @@ private:
 	friend class b3MeshContact;
 	friend class b3ContactManager;
 	friend class b3ContactSolver;
-	friend class b3ClothSolver;
-	friend class b3ClothContactSolver;
-
+	
 	friend class b3Joint;
 	friend class b3JointManager;
 	friend class b3JointSolver;
@@ -308,6 +306,13 @@ private:
 
 	friend class b3List2<b3Body>;
 
+	friend class b3ClothSolver;
+	friend class b3ClothContactSolver;
+
+	friend class b3SoftBody;
+	friend class b3SoftBodySolver;
+	friend class b3SoftBodyContactSolver;
+
 	enum b3BodyFlags 
 	{
 		e_awakeFlag = 0x0001,
diff --git a/include/bounce/softbody/softbody.h b/include/bounce/softbody/softbody.h
new file mode 100644
index 0000000..9ea65e9
--- /dev/null
+++ b/include/bounce/softbody/softbody.h
@@ -0,0 +1,193 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_H
+#define B3_SOFT_BODY_H
+
+#include <bounce/common/math/transform.h>
+#include <bounce/common/memory/stack_allocator.h>
+
+class b3World;
+
+struct b3SoftBodyMesh;
+
+class b3SoftBodyNode;
+
+struct b3RayCastInput;
+struct b3RayCastOutput;
+
+struct b3SoftBodyRayCastSingleOutput
+{
+	u32 tetrahedron;
+	u32 v1, v2, v3;
+	float32 fraction;
+	b3Vec3 normal;
+};
+
+// Soft body tetrahedron element
+struct b3SoftBodyElement
+{
+	float32 invP[16];
+	b3Mat33 K[16];
+	b3Quat q;
+};
+
+// Soft body tetrahedron triangle
+struct b3SoftBodyTriangle
+{
+	u32 v1, v2, v3;
+	u32 tetrahedron;
+};
+
+// Soft body definition
+// This requires defining a soft body mesh which is typically bound to a render mesh
+struct b3SoftBodyDef
+{
+	b3SoftBodyDef()
+	{
+		mesh = nullptr;
+		density = 0.1f;
+		E = 100.0f;
+		nu = 0.3f;
+	}
+
+	// Soft body mesh
+	const b3SoftBodyMesh* mesh;
+
+	// Density in kg/m^3
+	float32 density;
+
+	// Material Young's modulus in [0, inf]
+	// Units are 1e3N/m^2
+	float32 E;
+
+	// Material Poisson ratio in [0, 0.5]
+	// This is a dimensionless value
+	float32 nu;
+};
+
+// A soft body represents a deformable volume as a collection of nodes.
+class b3SoftBody
+{
+public:
+	b3SoftBody(const b3SoftBodyDef& def);
+	~b3SoftBody();
+
+	// Perform a ray cast with the soft body.
+	bool RayCastSingle(b3SoftBodyRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const;
+
+	// Set the acceleration of gravity.
+	void SetGravity(const b3Vec3& gravity);
+
+	// Get the acceleration of gravity.
+	b3Vec3 GetGravity() const;
+
+	// Attach a world to this cloth. 
+	// The cloth will be able to respond to collisions with the bodies in the attached world.
+	void SetWorld(b3World* world);
+
+	// Get the world attached to this cloth.
+	const b3World* GetWorld() const;
+	b3World* GetWorld();
+
+	// Return the soft body mesh proxy.
+	const b3SoftBodyMesh* GetMesh() const;
+
+	// Return the node associated with the given vertex.
+	b3SoftBodyNode* GetVertexNode(u32 i);
+
+	// Return the kinetic (or dynamic) energy in this system.
+	float32 GetEnergy() const;
+
+	// Perform a time step. 
+	void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
+
+	// Debug draw the body using the associated mesh.
+	void Draw() const;
+private:
+	// Compute mass of each node.
+	void ComputeMass();
+
+	// Update contacts.
+	void UpdateContacts();
+
+	// Solve
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
+
+	// Stack allocator
+	b3StackAllocator m_stackAllocator;
+
+	// Gravity acceleration
+	b3Vec3 m_gravity;
+
+	// Proxy mesh
+	const b3SoftBodyMesh* m_mesh;
+
+	// Soft body density
+	float32 m_density;
+
+	// Material Young's modulus
+	float32 m_E;
+
+	// Material poisson ratio
+	float32 m_nu;
+
+	// Soft body nodes
+	b3SoftBodyNode* m_nodes;
+
+	// Soft body elements
+	b3SoftBodyElement* m_elements;
+
+	// Soft body triangles
+	b3SoftBodyTriangle* m_triangles;
+
+	// Attached world
+	b3World* m_world;
+};
+
+inline void b3SoftBody::SetGravity(const b3Vec3& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline b3Vec3 b3SoftBody::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline void b3SoftBody::SetWorld(b3World* world)
+{
+	m_world = world;
+}
+
+inline const b3World* b3SoftBody::GetWorld() const
+{
+	return m_world;
+}
+
+inline b3World* b3SoftBody::GetWorld()
+{
+	return m_world;
+}
+
+inline const b3SoftBodyMesh* b3SoftBody::GetMesh() const
+{
+	return m_mesh;
+}
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/softbody/softbody_contact_solver.h b/include/bounce/softbody/softbody_contact_solver.h
new file mode 100644
index 0000000..ff8350b
--- /dev/null
+++ b/include/bounce/softbody/softbody_contact_solver.h
@@ -0,0 +1,129 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_CONTACT_SOLVER_H
+#define B3_SOFT_BODY_CONTACT_SOLVER_H
+
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
+
+class b3StackAllocator;
+
+class b3SoftBodyNode;
+class b3Body;
+
+class b3NodeBodyContact;
+
+struct b3DenseVec3;
+
+struct b3SoftBodySolverBodyContactVelocityConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+
+	b3Body* bodyB;
+	float32 invMassB;
+	b3Mat33 invIB;
+
+	float32 friction;
+
+	b3Vec3 point;
+	b3Vec3 rA;
+	b3Vec3 rB;
+
+	b3Vec3 normal;
+	float32 normalMass;
+	float32 normalImpulse;
+	float32 velocityBias;
+
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;
+};
+
+struct b3SoftBodySolverBodyContactPositionConstraint
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+	float32 radiusA;
+	b3Vec3 localCenterA;
+
+	b3Body* bodyB;
+	float32 invMassB;
+	b3Mat33 invIB;
+	float32 radiusB;
+	b3Vec3 localCenterB;
+
+	b3Vec3 rA;
+	b3Vec3 rB;
+
+	b3Vec3 normalA;
+	b3Vec3 localPointA;
+	b3Vec3 localPointB;
+};
+
+struct b3SoftBodyContactSolverDef
+{
+	b3StackAllocator* allocator;
+
+	b3DenseVec3* positions;
+	b3DenseVec3* velocities;
+
+	u32 bodyContactCapacity;
+};
+
+inline float32 b3MixFriction(float32 u1, float32 u2)
+{
+	return b3Sqrt(u1 * u2);
+}
+
+class b3SoftBodyContactSolver
+{
+public:
+	b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def);
+	~b3SoftBodyContactSolver();
+
+	void Add(b3NodeBodyContact* c);
+
+	void InitializeBodyContactConstraints();
+
+	void WarmStart();
+
+	void SolveBodyContactVelocityConstraints();
+
+	void StoreImpulses();
+
+	bool SolveBodyContactPositionConstraints();
+protected:
+	b3StackAllocator* m_allocator;
+
+	b3DenseVec3* m_positions;
+	b3DenseVec3* m_velocities;
+
+	u32 m_bodyContactCapacity;
+	u32 m_bodyContactCount;
+	b3NodeBodyContact** m_bodyContacts;
+	
+	b3SoftBodySolverBodyContactVelocityConstraint* m_bodyVelocityConstraints;
+	b3SoftBodySolverBodyContactPositionConstraint* m_bodyPositionConstraints;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/softbody/softbody_mesh.h b/include/bounce/softbody/softbody_mesh.h
new file mode 100644
index 0000000..e7f392a
--- /dev/null
+++ b/include/bounce/softbody/softbody_mesh.h
@@ -0,0 +1,45 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_MESH_H
+#define B3_SOFT_BODY_MESH_H
+
+#include <bounce/common/math/vec3.h>
+
+struct b3SoftBodyMeshTetrahedron
+{
+	u32 v1, v2, v3, v4;
+};
+
+struct b3SoftBodyMesh
+{
+	u32 vertexCount;
+	b3Vec3* vertices;
+	u32 tetrahedronCount;
+	b3SoftBodyMeshTetrahedron* tetrahedrons;
+};
+
+struct b3QSoftBodyMesh : public b3SoftBodyMesh
+{
+	b3QSoftBodyMesh();
+	~b3QSoftBodyMesh();
+
+	void SetAsSphere(float32 radius, u32 subdivisions);
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/softbody/softbody_node.h b/include/bounce/softbody/softbody_node.h
new file mode 100644
index 0000000..5ddfba1
--- /dev/null
+++ b/include/bounce/softbody/softbody_node.h
@@ -0,0 +1,259 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_NODE_H
+#define B3_SOFT_BODY_NODE_H
+
+#include <bounce/common/math/vec2.h>
+#include <bounce/common/math/vec3.h>
+#include <bounce/common/math/transform.h>
+
+class b3Shape;
+class b3SoftBody;
+class b3SoftBodyNode;
+
+// A contact between a node and a body
+class b3NodeBodyContact
+{
+public:
+	b3NodeBodyContact() { }
+	~b3NodeBodyContact() { }
+
+	b3SoftBodyNode* n1;
+	b3Shape* s2;
+
+	// Contact constraint
+	b3Vec3 normal1;
+	b3Vec3 localPoint1;
+	b3Vec3 localPoint2;
+	float32 normalImpulse;
+
+	// Friction constraint
+	b3Vec3 t1, t2;
+	b3Vec2 tangentImpulse;
+
+	bool active;
+};
+
+struct b3NodeBodyContactWorldPoint
+{
+	void Initialize(const b3NodeBodyContact* c, float32 rA, const b3Transform& xfA, float32 rB, const b3Transform& xfB)
+	{
+		b3Vec3 nA = c->normal1;
+		
+		b3Vec3 cA = xfA * c->localPoint1;
+		b3Vec3 cB = xfB * c->localPoint2;
+
+		b3Vec3 pA = cA + rA * nA;
+		b3Vec3 pB = cB - rB * nA;
+
+		point = 0.5f * (pA + pB);
+		normal = nA;
+		separation = b3Dot(cB - cA, nA) - rA - rB;
+	}
+
+	b3Vec3 point;
+	b3Vec3 normal;
+	float32 separation;
+};
+
+// Static node: Can be moved manually.
+// Dynamic node: Non-zero velocity determined by force, can be moved by the solver.
+enum b3SoftBodyNodeType
+{
+	e_staticSoftBodyNode,
+	e_dynamicSoftBodyNode
+};
+
+// A soft body node.
+class b3SoftBodyNode 
+{
+public:
+	// Set the node type.
+	void SetType(b3SoftBodyNodeType type);
+
+	// Get the node type.
+	b3SoftBodyNodeType GetType() const;
+
+	// Get the vertex index.
+	u32 GetVertex() const;
+
+	// Set the particle position. 
+	// If the particle is dynamic changing the position directly might lead 
+	// to physically incorrect simulation behaviour.
+	void SetPosition(const b3Vec3& position);
+
+	// Get the particle position.
+	const b3Vec3& GetPosition() const;
+
+	// Set the particle velocity.
+	void SetVelocity(const b3Vec3& velocity);
+
+	// Get the particle velocity.
+	const b3Vec3& GetVelocity() const;
+
+	// Get the particle mass.
+	float32 GetMass() const;
+
+	// Set the particle radius.
+	void SetRadius(float32 radius);
+	
+	// Get the particle radius.
+	float32 GetRadius() const;
+
+	// Set the particle coefficient of friction.
+	void SetFriction(float32 friction);
+	
+	// Get the particle coefficient of friction.
+	float32 GetFriction() const;
+	
+	// Apply a force.
+	void ApplyForce(const b3Vec3& force);
+private:
+	friend class b3SoftBody;
+	friend class b3SoftBodySolver;
+	friend class b3SoftBodyContactSolver;
+
+	b3SoftBodyNode() { }
+
+	~b3SoftBodyNode() { }
+
+	// Type
+	b3SoftBodyNodeType m_type;
+
+	// Position
+	b3Vec3 m_position;
+	
+	// Velocity
+	b3Vec3 m_velocity;
+
+	// Applied external force
+	b3Vec3 m_force;
+
+	// Mass
+	float32 m_mass;
+
+	// Inverse mass
+	float32 m_invMass;
+
+	// Radius
+	float32 m_radius;
+
+	// Coefficient of friction
+	float32 m_friction;
+	
+	// User data. 
+	void* m_userData;
+
+	// Soft body mesh vertex index.
+	u32 m_vertex;
+
+	// Node and body contact
+	b3NodeBodyContact m_bodyContact;
+
+	// Soft body
+	b3SoftBody* m_body;
+};
+
+inline void b3SoftBodyNode::SetType(b3SoftBodyNodeType type)
+{
+	if (m_type == type)
+	{
+		return;
+	}
+
+	m_type = type;
+	m_force.SetZero();
+
+	if (type == e_staticSoftBodyNode)
+	{
+		m_velocity.SetZero();
+	}
+
+	m_bodyContact.active = false;
+}
+
+inline b3SoftBodyNodeType b3SoftBodyNode::GetType() const
+{
+	return m_type;
+}
+
+inline u32 b3SoftBodyNode::GetVertex() const
+{
+	return m_vertex;
+}
+
+inline void b3SoftBodyNode::SetPosition(const b3Vec3& position)
+{
+	m_position = position;
+}
+
+inline const b3Vec3& b3SoftBodyNode::GetPosition() const
+{
+	return m_position;
+}
+
+inline void b3SoftBodyNode::SetVelocity(const b3Vec3& velocity)
+{
+	if (m_type == e_staticSoftBodyNode)
+	{
+		return;
+	}
+	m_velocity = velocity;
+}
+
+inline const b3Vec3& b3SoftBodyNode::GetVelocity() const
+{
+	return m_velocity;
+}
+
+inline float32 b3SoftBodyNode::GetMass() const
+{
+	return m_mass;
+}
+
+inline void b3SoftBodyNode::SetRadius(float32 radius)
+{
+	m_radius = radius;
+}
+
+inline float32 b3SoftBodyNode::GetRadius() const
+{
+	return m_radius;
+}
+
+inline void b3SoftBodyNode::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b3SoftBodyNode::GetFriction() const
+{
+	return m_friction;
+}
+
+inline void b3SoftBodyNode::ApplyForce(const b3Vec3& force)
+{
+	if (m_type != e_dynamicSoftBodyNode)
+	{
+		return;
+	}
+	m_force += force;
+}
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/softbody/softbody_solver.h b/include/bounce/softbody/softbody_solver.h
new file mode 100644
index 0000000..30661da
--- /dev/null
+++ b/include/bounce/softbody/softbody_solver.h
@@ -0,0 +1,56 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SOFT_BODY_SOLVER_H
+#define B3_SOFT_BODY_SOLVER_H
+
+#include <bounce/common/math/mat22.h>
+#include <bounce/common/math/mat33.h>
+
+class b3StackAllocator;
+
+class b3SoftBodyMesh;
+
+class b3SoftBodyNode;
+struct b3SoftBodyElement;
+
+class b3NodeBodyContact;
+
+struct b3SoftBodySolverDef
+{
+	b3StackAllocator* stack;
+	const b3SoftBodyMesh* mesh;
+	b3SoftBodyNode* nodes;
+	b3SoftBodyElement* elements;
+};
+
+class b3SoftBodySolver
+{
+public:
+	b3SoftBodySolver(const b3SoftBodySolverDef& def);
+	~b3SoftBodySolver();
+
+	void Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations);
+private:
+	b3StackAllocator* m_allocator;
+	const b3SoftBodyMesh* m_mesh;
+	b3SoftBodyNode* m_nodes;
+	b3SoftBodyElement* m_elements;
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/softbody/sparse_mat33.h b/include/bounce/softbody/sparse_mat33.h
new file mode 100644
index 0000000..a746849
--- /dev/null
+++ b/include/bounce/softbody/sparse_mat33.h
@@ -0,0 +1,322 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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_SPARSE_MAT_33_H
+#define B3_SPARSE_MAT_33_H
+
+#include <bounce/common/math/mat33.h>
+#include <bounce/cloth/diag_mat33.h>
+#include <bounce/cloth/dense_vec3.h>
+#include <bounce/cloth/sparse_sym_mat33.h>
+
+// A sparse matrix.
+// Each row is a list of non-zero elements in the row.
+struct b3SparseMat33
+{
+	//
+	b3SparseMat33();
+
+	// 
+	b3SparseMat33(u32 m);
+
+	//
+	b3SparseMat33(const b3SparseMat33& _m);
+
+	//
+	~b3SparseMat33();
+
+	//
+	b3SparseMat33& operator=(const b3SparseMat33& _m);
+
+	// 
+	void Copy(const b3SparseMat33& _m);
+
+	//
+	void Destroy();
+
+	// 
+	b3Mat33& operator()(u32 i, u32 j);
+
+	// 
+	const b3Mat33& operator()(u32 i, u32 j) const;
+
+	// 
+	void operator+=(const b3SparseMat33& m);
+
+	// 
+	void operator-=(const b3SparseMat33& m);
+
+	u32 rowCount;
+	b3RowValueList* rows;
+};
+
+inline b3SparseMat33::b3SparseMat33()
+{
+	rowCount = 0;
+	rows = nullptr;
+}
+
+inline b3SparseMat33::b3SparseMat33(u32 m)
+{
+	rowCount = m;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+}
+
+inline b3SparseMat33::b3SparseMat33(const b3SparseMat33& m)
+{
+	rowCount = m.rowCount;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+
+	Copy(m);
+}
+
+inline b3SparseMat33::~b3SparseMat33()
+{
+	Destroy();
+}
+
+inline void b3SparseMat33::Destroy()
+{
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		b3RowValueList* vs = rows + i;
+
+		b3RowValue* v = vs->head;
+		while (v)
+		{
+			b3RowValue* v0 = v->next;
+			b3Free(v);
+			v = v0;
+		}
+
+		vs->~b3RowValueList();
+	}
+
+	b3Free(rows);
+}
+
+inline b3SparseMat33& b3SparseMat33::operator=(const b3SparseMat33& _m)
+{
+	if (_m.rows == rows)
+	{
+		return *this;
+	}
+
+	Destroy();
+
+	rowCount = _m.rowCount;
+	rows = (b3RowValueList*)b3Alloc(rowCount * sizeof(b3RowValueList));
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		new (rows + i)b3RowValueList();
+	}
+
+	Copy(_m);
+
+	return *this;
+}
+
+inline void b3SparseMat33::Copy(const b3SparseMat33& _m)
+{
+	B3_ASSERT(rowCount == _m.rowCount);
+
+	for (u32 i = 0; i < rowCount; ++i)
+	{
+		b3RowValueList* vs1 = _m.rows + i;
+		b3RowValueList* vs2 = rows + i;
+
+		B3_ASSERT(vs2->count == 0);
+
+		for (b3RowValue* v1 = vs1->head; v1; v1 = v1->next)
+		{
+			b3RowValue* v2 = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
+
+			v2->column = v1->column;
+			v2->value = v1->value;
+
+			vs2->PushFront(v2);
+		}
+	}
+}
+
+inline const b3Mat33& b3SparseMat33::operator()(u32 i, u32 j) const
+{
+	B3_ASSERT(i < rowCount);
+	B3_ASSERT(j < rowCount);
+
+	b3RowValueList* vs = rows + i;
+
+	for (b3RowValue* v = vs->head; v; v = v->next)
+	{
+		if (v->column == j)
+		{
+			return v->value;
+		}
+	}
+
+	return b3Mat33_zero;
+}
+
+inline b3Mat33& b3SparseMat33::operator()(u32 i, u32 j)
+{
+	B3_ASSERT(i < rowCount);
+	B3_ASSERT(j < rowCount);
+
+	b3RowValueList* vs = rows + i;
+
+	for (b3RowValue* v = vs->head; v; v = v->next)
+	{
+		if (v->column == j)
+		{
+			return v->value;
+		}
+	}
+
+	b3RowValue* v = (b3RowValue*)b3Alloc(sizeof(b3RowValue));
+	v->column = j;
+	v->value.SetZero();
+
+	vs->PushFront(v);
+
+	return v->value;
+}
+
+inline void b3SparseMat33::operator+=(const b3SparseMat33& m)
+{
+	B3_ASSERT(rowCount == m.rowCount);
+
+	for (u32 i = 0; i < m.rowCount; ++i)
+	{
+		b3RowValueList* mvs = m.rows + i;
+
+		for (b3RowValue* v = mvs->head; v; v = v->next)
+		{
+			u32 j = v->column;
+
+			(*this)(i, j) += v->value;
+		}
+	}
+}
+
+inline void b3SparseMat33::operator-=(const b3SparseMat33& m)
+{
+	B3_ASSERT(rowCount == m.rowCount);
+
+	for (u32 i = 0; i < m.rowCount; ++i)
+	{
+		b3RowValueList* mvs = m.rows + i;
+
+		for (b3RowValue* v = mvs->head; v; v = v->next)
+		{
+			u32 j = v->column;
+
+			(*this)(i, j) -= v->value;
+		}
+	}
+}
+
+inline void b3Add(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
+{
+	out = a;
+	out += b;
+}
+
+inline void b3Sub(b3SparseMat33& out, const b3SparseMat33& a, const b3SparseMat33& b)
+{
+	out = a;
+	out -= b;
+}
+
+inline void b3Mul(b3DenseVec3& out, const b3SparseMat33& A, const b3DenseVec3& v)
+{
+	B3_ASSERT(A.rowCount == out.n);
+
+	out.SetZero();
+
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		b3RowValueList* vs = A.rows + i;
+
+		for (b3RowValue* vA = vs->head; vA; vA = vA->next)
+		{
+			u32 j = vA->column;
+			b3Mat33 a = vA->value;
+
+			out[i] += a * v[j];
+		}
+	}
+}
+
+inline void b3Mul(b3SparseMat33& out, float32 s, const b3SparseMat33& B)
+{
+	B3_ASSERT(out.rowCount == B.rowCount);
+
+	if (s == 0.0f)
+	{
+		return;
+	}
+
+	out = B;
+
+	for (u32 i = 0; i < out.rowCount; ++i)
+	{
+		b3RowValueList* vs = out.rows + i;
+		for (b3RowValue* v = vs->head; v; v = v->next)
+		{
+			v->value = s * v->value;
+		}
+	}
+}
+
+inline b3SparseMat33 operator+(const b3SparseMat33& A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(A.rowCount);
+	b3Add(result, A, B);
+	return result;
+}
+
+inline b3SparseMat33 operator-(const b3SparseMat33& A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(A.rowCount);
+	b3Sub(result, A, B);
+	return result;
+}
+
+inline b3SparseMat33 operator*(float32 A, const b3SparseMat33& B)
+{
+	b3SparseMat33 result(B.rowCount);
+	b3Mul(result, A, B);
+	return result;
+}
+
+inline b3DenseVec3 operator*(const b3SparseMat33& A, const b3DenseVec3& v)
+{
+	b3DenseVec3 result(v.n);
+	b3Mul(result, A, v);
+	return result;
+}
+
+#endif
diff --git a/premake5.lua b/premake5.lua
index f0ef7e9..cc2c029 100644
--- a/premake5.lua
+++ b/premake5.lua
@@ -275,9 +275,11 @@ workspace(solution_name)
 			
 			examples_src_dir .. "/testbed/framework/body_dragger.h", 
 			examples_src_dir .. "/testbed/framework/cloth_dragger.h", 
+			examples_src_dir .. "/testbed/framework/softbody_dragger.h", 
 			
 			examples_src_dir .. "/testbed/framework/body_dragger.cpp", 
 			examples_src_dir .. "/testbed/framework/cloth_dragger.cpp", 
+			examples_src_dir .. "/testbed/framework/softbody_dragger.cpp", 
 			
 			examples_inc_dir .. "/testbed/tests/**.h", 
 			
@@ -370,7 +372,7 @@ end
 -- clean
 newaction
 {
-    trigger = "clean",
+	trigger = "clean",
     description = "Clean solution",
     execute = function ()
           os.rmdir( "doc" )
diff --git a/src/bounce/softbody/softbody.cpp b/src/bounce/softbody/softbody.cpp
new file mode 100644
index 0000000..17b02d2
--- /dev/null
+++ b/src/bounce/softbody/softbody.cpp
@@ -0,0 +1,992 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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/softbody/softbody.h>
+#include <bounce/softbody/softbody_mesh.h>
+#include <bounce/softbody/softbody_node.h>
+
+#include <bounce/softbody/softbody_solver.h>
+
+#include <bounce/collision/collision.h>
+
+#include <bounce/dynamics/world.h>
+#include <bounce/dynamics/body.h>
+#include <bounce/dynamics/shapes/shape.h>
+
+#include <bounce/common/draw.h>
+
+static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN)
+{
+	B3_ASSERT(AN == BM);
+
+	for (u32 i = 0; i < AM; ++i)
+	{
+		for (u32 j = 0; j < BN; ++j)
+		{
+			C[i + AM * j] = 0.0f;
+
+			for (u32 k = 0; k < AN; ++k)
+			{
+				C[i + AM * j] += A[i + AM * k] * B[k + BM * j];
+			}
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3Transpose(float32* B, float32* A, u32 AM, u32 AN)
+{
+	for (u32 i = 0; i < AM; ++i)
+	{
+		for (u32 j = 0; j < AN; ++j)
+		{
+			B[j + AN * i] = A[i + AM * j];
+		}
+	}
+}
+
+static B3_FORCE_INLINE void b3Inverse4(float32 out[16], float32 m[16])
+{
+	float32 inv[16];
+
+	inv[0] = m[5] * m[10] * m[15] -
+		m[5] * m[11] * m[14] -
+		m[9] * m[6] * m[15] +
+		m[9] * m[7] * m[14] +
+		m[13] * m[6] * m[11] -
+		m[13] * m[7] * m[10];
+
+	inv[4] = -m[4] * m[10] * m[15] +
+		m[4] * m[11] * m[14] +
+		m[8] * m[6] * m[15] -
+		m[8] * m[7] * m[14] -
+		m[12] * m[6] * m[11] +
+		m[12] * m[7] * m[10];
+
+	inv[8] = m[4] * m[9] * m[15] -
+		m[4] * m[11] * m[13] -
+		m[8] * m[5] * m[15] +
+		m[8] * m[7] * m[13] +
+		m[12] * m[5] * m[11] -
+		m[12] * m[7] * m[9];
+
+	inv[12] = -m[4] * m[9] * m[14] +
+		m[4] * m[10] * m[13] +
+		m[8] * m[5] * m[14] -
+		m[8] * m[6] * m[13] -
+		m[12] * m[5] * m[10] +
+		m[12] * m[6] * m[9];
+
+	inv[1] = -m[1] * m[10] * m[15] +
+		m[1] * m[11] * m[14] +
+		m[9] * m[2] * m[15] -
+		m[9] * m[3] * m[14] -
+		m[13] * m[2] * m[11] +
+		m[13] * m[3] * m[10];
+
+	inv[5] = m[0] * m[10] * m[15] -
+		m[0] * m[11] * m[14] -
+		m[8] * m[2] * m[15] +
+		m[8] * m[3] * m[14] +
+		m[12] * m[2] * m[11] -
+		m[12] * m[3] * m[10];
+
+	inv[9] = -m[0] * m[9] * m[15] +
+		m[0] * m[11] * m[13] +
+		m[8] * m[1] * m[15] -
+		m[8] * m[3] * m[13] -
+		m[12] * m[1] * m[11] +
+		m[12] * m[3] * m[9];
+
+	inv[13] = m[0] * m[9] * m[14] -
+		m[0] * m[10] * m[13] -
+		m[8] * m[1] * m[14] +
+		m[8] * m[2] * m[13] +
+		m[12] * m[1] * m[10] -
+		m[12] * m[2] * m[9];
+
+	inv[2] = m[1] * m[6] * m[15] -
+		m[1] * m[7] * m[14] -
+		m[5] * m[2] * m[15] +
+		m[5] * m[3] * m[14] +
+		m[13] * m[2] * m[7] -
+		m[13] * m[3] * m[6];
+
+	inv[6] = -m[0] * m[6] * m[15] +
+		m[0] * m[7] * m[14] +
+		m[4] * m[2] * m[15] -
+		m[4] * m[3] * m[14] -
+		m[12] * m[2] * m[7] +
+		m[12] * m[3] * m[6];
+
+	inv[10] = m[0] * m[5] * m[15] -
+		m[0] * m[7] * m[13] -
+		m[4] * m[1] * m[15] +
+		m[4] * m[3] * m[13] +
+		m[12] * m[1] * m[7] -
+		m[12] * m[3] * m[5];
+
+	inv[14] = -m[0] * m[5] * m[14] +
+		m[0] * m[6] * m[13] +
+		m[4] * m[1] * m[14] -
+		m[4] * m[2] * m[13] -
+		m[12] * m[1] * m[6] +
+		m[12] * m[2] * m[5];
+
+	inv[3] = -m[1] * m[6] * m[11] +
+		m[1] * m[7] * m[10] +
+		m[5] * m[2] * m[11] -
+		m[5] * m[3] * m[10] -
+		m[9] * m[2] * m[7] +
+		m[9] * m[3] * m[6];
+
+	inv[7] = m[0] * m[6] * m[11] -
+		m[0] * m[7] * m[10] -
+		m[4] * m[2] * m[11] +
+		m[4] * m[3] * m[10] +
+		m[8] * m[2] * m[7] -
+		m[8] * m[3] * m[6];
+
+	inv[11] = -m[0] * m[5] * m[11] +
+		m[0] * m[7] * m[9] +
+		m[4] * m[1] * m[11] -
+		m[4] * m[3] * m[9] -
+		m[8] * m[1] * m[7] +
+		m[8] * m[3] * m[5];
+
+	inv[15] = m[0] * m[5] * m[10] -
+		m[0] * m[6] * m[9] -
+		m[4] * m[1] * m[10] +
+		m[4] * m[2] * m[9] +
+		m[8] * m[1] * m[6] -
+		m[8] * m[2] * m[5];
+
+	float32 det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
+
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	for (u32 i = 0; i < 16; ++i)
+	{
+		out[i] = det * inv[i];
+	}
+}
+
+static B3_FORCE_INLINE void b3Lame(float32& lambda, float32& mu, float32 E, float32 nu)
+{
+	lambda = (nu * E) / ((1 + nu) * (1 - 2 * nu));
+	mu = E / (2 * (1 + nu));
+}
+
+static B3_FORCE_INLINE void b3CreateE(float32 out[36], float32 lambda, float32 mu)
+{
+	float32 E[36] =
+	{
+		lambda + 2 * mu, lambda,          lambda,          0,  0,  0,
+		lambda,          lambda + 2 * mu, lambda,          0,  0,  0,
+		lambda,          lambda,          lambda + 2 * mu, 0,  0,  0,
+		0, 				 0, 			  0, 			   mu, 0,  0,
+		0, 				 0, 			  0, 			   0,  mu, 0,
+		0, 			 	 0, 			  0, 			   0,  0,  mu
+	};
+
+	for (u32 i = 0; i < 36; ++i)
+	{
+		out[i] = E[i];
+	}
+}
+
+static B3_FORCE_INLINE void b3CreateB(float32 out[72], float32 invP[16])
+{
+	float32 a11 = invP[0];
+	float32 a21 = invP[1];
+	float32 a31 = invP[2];
+	float32 a41 = invP[3];
+
+	float32 a12 = invP[4];
+	float32 a22 = invP[5];
+	float32 a32 = invP[6];
+	float32 a42 = invP[7];
+
+	float32 a13 = invP[8];
+	float32 a23 = invP[9];
+	float32 a33 = invP[10];
+	float32 a43 = invP[11];
+
+	//float32 a14 = invP[12];
+	//float32 a24 = invP[13];
+	//float32 a34 = invP[14];
+	//float32 a44 = invP[15];
+
+	// 6 x 12
+	// a11 0   0   a21 0   0   a31 0   0   a41 0   0
+	// 0   a12 0   0   a22 0   0   a32 0   0   a42 0
+	// 0   0   a13 0   0   a23 0   0   a33 0   0   a43
+	// a12 a11 0   a22 a21 0   a32 a31 0   a42 a41 0
+	// 0   a13 a12 0   a23 a22 0   a33 a32 0   a43 a42
+	// a13 0   a11 a23 0   a21 a33 0   a31 a43 0   a41
+	float32 B[72] =
+	{
+		a11, 0, 0, a12, 0, a13,
+		0, a12, 0, a11, a13, 0,
+		0, 0, a13, 0, a12, a11,
+		a21, 0, 0, a22, 0, a23,
+		0, a22, 0, a21, a23, 0,
+		0, 0, a23, 0, a22, a21,
+		a31, 0, 0, a32, 0, a33,
+		0, a32, 0, a31, a33, 0,
+		0, 0, a33, 0, a32, a31,
+		a41, 0, 0, a42, 0, a43,
+		0, a42, 0, a41, a43, 0,
+		0, 0, a43, 0, a42, a41
+	};
+
+	for (u32 i = 0; i < 72; ++i)
+	{
+		out[i] = B[i];
+	}
+}
+
+static B3_FORCE_INLINE void b3SetK(b3Mat33 K[16], float32 Ke[144])
+{
+	b3Mat33& k11 = K[0 + 4 * 0];
+	b3Mat33& k12 = K[0 + 4 * 1];
+	b3Mat33& k13 = K[0 + 4 * 2];
+	b3Mat33& k14 = K[0 + 4 * 3];
+
+	b3Mat33& k21 = K[1 + 4 * 0];
+	b3Mat33& k22 = K[1 + 4 * 1];
+	b3Mat33& k23 = K[1 + 4 * 2];
+	b3Mat33& k24 = K[1 + 4 * 3];
+
+	b3Mat33& k31 = K[2 + 4 * 0];
+	b3Mat33& k32 = K[2 + 4 * 1];
+	b3Mat33& k33 = K[2 + 4 * 2];
+	b3Mat33& k34 = K[2 + 4 * 3];
+
+	b3Mat33& k41 = K[3 + 4 * 0];
+	b3Mat33& k42 = K[3 + 4 * 1];
+	b3Mat33& k43 = K[3 + 4 * 2];
+	b3Mat33& k44 = K[3 + 4 * 3];
+
+	// k11                  
+	// a11 a12 a13 
+	// a21 a22 a23 
+	// a31 a32 a33
+	k11.x.x = Ke[0 + 12 * 0];
+	k11.x.y = Ke[1 + 12 * 0];
+	k11.x.z = Ke[2 + 12 * 0];
+
+	k11.y.x = Ke[0 + 12 * 1];
+	k11.y.y = Ke[1 + 12 * 1];
+	k11.y.z = Ke[2 + 12 * 1];
+
+	k11.z.x = Ke[0 + 12 * 2];
+	k11.z.y = Ke[1 + 12 * 2];
+	k11.z.z = Ke[2 + 12 * 2];
+
+	// k12
+	// a14 a15 a16
+	// a24 a25 a26
+	// a34 a35 a36
+	k12.x.x = Ke[0 + 12 * 3];
+	k12.x.y = Ke[1 + 12 * 3];
+	k12.x.z = Ke[2 + 12 * 3];
+
+	k12.y.x = Ke[0 + 12 * 4];
+	k12.y.y = Ke[1 + 12 * 4];
+	k12.y.z = Ke[2 + 12 * 4];
+
+	k12.z.x = Ke[0 + 12 * 5];
+	k12.z.y = Ke[1 + 12 * 5];
+	k12.z.z = Ke[2 + 12 * 5];
+
+	// k13
+	// a17 a18 a19
+	// a27 a28 a29
+	// a37 a38 a39
+	k13.x.x = Ke[0 + 12 * 6];
+	k13.x.y = Ke[1 + 12 * 6];
+	k13.x.z = Ke[2 + 12 * 6];
+
+	k13.y.x = Ke[0 + 12 * 7];
+	k13.y.y = Ke[1 + 12 * 7];
+	k13.y.z = Ke[2 + 12 * 7];
+
+	k13.z.x = Ke[0 + 12 * 8];
+	k13.z.y = Ke[1 + 12 * 8];
+	k13.z.z = Ke[2 + 12 * 8];
+
+	// k14
+	// a1_10 a1_11 a1_12
+	// a2_10 a2_11 a2_12
+	// a3_10 a3_11 a3_12
+	k14.x.x = Ke[0 + 12 * 9];
+	k14.x.y = Ke[1 + 12 * 9];
+	k14.x.z = Ke[2 + 12 * 9];
+
+	k14.y.x = Ke[0 + 12 * 10];
+	k14.y.y = Ke[1 + 12 * 10];
+	k14.y.z = Ke[2 + 12 * 10];
+
+	k14.z.x = Ke[0 + 12 * 11];
+	k14.z.y = Ke[1 + 12 * 11];
+	k14.z.z = Ke[2 + 12 * 11];
+
+	// k21	           
+	// a41 a42 a43	
+	// a51 a52 a53		
+	// a61 a62 a63	
+	k21.x.x = Ke[3 + 12 * 0];
+	k21.x.y = Ke[4 + 12 * 0];
+	k21.x.z = Ke[5 + 12 * 0];
+
+	k21.y.x = Ke[3 + 12 * 1];
+	k21.y.y = Ke[4 + 12 * 1];
+	k21.y.z = Ke[5 + 12 * 1];
+
+	k21.z.x = Ke[3 + 12 * 2];
+	k21.z.y = Ke[4 + 12 * 2];
+	k21.z.z = Ke[5 + 12 * 2];
+
+	// k22	            
+	// a44 a45 a46	
+	// a54 a55 a56	
+	// a64 a65 a66	
+	k22.x.x = Ke[3 + 12 * 3];
+	k22.x.y = Ke[4 + 12 * 3];
+	k22.x.z = Ke[5 + 12 * 3];
+
+	k22.y.x = Ke[3 + 12 * 4];
+	k22.y.y = Ke[4 + 12 * 4];
+	k22.y.z = Ke[5 + 12 * 4];
+
+	k22.z.x = Ke[3 + 12 * 5];
+	k22.z.y = Ke[4 + 12 * 5];
+	k22.z.z = Ke[5 + 12 * 5];
+
+	// k23			            
+	// a47 a48 a49	
+	// a57 a58 a59	
+	// a67 a68 a69	
+	k23.x.x = Ke[3 + 12 * 6];
+	k23.x.y = Ke[4 + 12 * 6];
+	k23.x.z = Ke[5 + 12 * 6];
+
+	k23.y.x = Ke[3 + 12 * 7];
+	k23.y.y = Ke[4 + 12 * 7];
+	k23.y.z = Ke[5 + 12 * 7];
+
+	k23.z.x = Ke[3 + 12 * 8];
+	k23.z.y = Ke[4 + 12 * 8];
+	k23.z.z = Ke[5 + 12 * 8];
+
+	// k24            
+	// a4_10 a4_11 a4_12
+	// a5_10 a5_11 a5_12
+	// a6_10 a6_11 a6_12
+	k24.x.x = Ke[3 + 12 * 9];
+	k24.x.y = Ke[4 + 12 * 9];
+	k24.x.z = Ke[5 + 12 * 9];
+
+	k24.y.x = Ke[3 + 12 * 10];
+	k24.y.y = Ke[4 + 12 * 10];
+	k24.y.z = Ke[5 + 12 * 10];
+
+	k24.z.x = Ke[3 + 12 * 11];
+	k24.z.y = Ke[4 + 12 * 11];
+	k24.z.z = Ke[5 + 12 * 11];
+
+	// k31	
+	// a71 a72 a73	
+	// a81 a82 a83	
+	// a91 a92 a93	
+	k31.x.x = Ke[6 + 12 * 0];
+	k31.x.y = Ke[7 + 12 * 0];
+	k31.x.z = Ke[8 + 12 * 0];
+
+	k31.y.x = Ke[6 + 12 * 1];
+	k31.y.y = Ke[7 + 12 * 1];
+	k31.y.z = Ke[8 + 12 * 1];
+
+	k31.z.x = Ke[6 + 12 * 2];
+	k31.z.y = Ke[7 + 12 * 2];
+	k31.z.z = Ke[8 + 12 * 2];
+
+	// k32	
+	// a74 a75 a76	
+	// a84 a85 a86	
+	// a94 a95 a96	
+	k32.x.x = Ke[6 + 12 * 3];
+	k32.x.y = Ke[7 + 12 * 3];
+	k32.x.z = Ke[8 + 12 * 3];
+
+	k32.y.x = Ke[6 + 12 * 4];
+	k32.y.y = Ke[7 + 12 * 4];
+	k32.y.z = Ke[8 + 12 * 4];
+
+	k32.z.x = Ke[6 + 12 * 5];
+	k32.z.y = Ke[7 + 12 * 5];
+	k32.z.z = Ke[8 + 12 * 5];
+
+	// k33 
+	// a77 a78 a79 
+	// a87 a88 a89 
+	// a97 a98 a99 
+	k33.x.x = Ke[6 + 12 * 6];
+	k33.x.y = Ke[7 + 12 * 6];
+	k33.x.z = Ke[8 + 12 * 6];
+
+	k33.y.x = Ke[6 + 12 * 7];
+	k33.y.y = Ke[7 + 12 * 7];
+	k33.y.z = Ke[8 + 12 * 7];
+
+	k33.z.x = Ke[6 + 12 * 8];
+	k33.z.y = Ke[7 + 12 * 8];
+	k33.z.z = Ke[8 + 12 * 8];
+
+	// k34
+	// a7_10 a7_11 a7_12
+	// a8_10 a8_11 a8_12
+	// a9_10 a9_11 a9_12
+	k34.x.x = Ke[6 + 12 * 9];
+	k34.x.y = Ke[7 + 12 * 9];
+	k34.x.z = Ke[8 + 12 * 9];
+
+	k34.y.x = Ke[6 + 12 * 10];
+	k34.y.y = Ke[7 + 12 * 10];
+	k34.y.z = Ke[8 + 12 * 10];
+
+	k34.z.x = Ke[6 + 12 * 11];
+	k34.z.y = Ke[7 + 12 * 11];
+	k34.z.z = Ke[8 + 12 * 11];
+
+	// k41	
+	// a10_1 a10_2 a10_3	
+	// a11_1 a11_2 a11_3	
+	// a12_1 a12_2 a12_3	
+	k41.x.x = Ke[9 + 12 * 0];
+	k41.x.y = Ke[10 + 12 * 0];
+	k41.x.z = Ke[11 + 12 * 0];
+
+	k41.y.x = Ke[9 + 12 * 1];
+	k41.y.y = Ke[10 + 12 * 1];
+	k41.y.z = Ke[11 + 12 * 1];
+
+	k41.z.x = Ke[9 + 12 * 2];
+	k41.z.y = Ke[10 + 12 * 2];
+	k41.z.z = Ke[11 + 12 * 2];
+
+	// k42 
+	// a10_4 a10_5 a10_6 
+	// a11_4 a11_5 a11_6 
+	// a12_4 a12_5 a12_6 
+	k42.x.x = Ke[9 + 12 * 3];
+	k42.x.y = Ke[10 + 12 * 3];
+	k42.x.z = Ke[11 + 12 * 3];
+
+	k42.y.x = Ke[9 + 12 * 4];
+	k42.y.y = Ke[10 + 12 * 4];
+	k42.y.z = Ke[11 + 12 * 4];
+
+	k42.z.x = Ke[9 + 12 * 5];
+	k42.z.y = Ke[10 + 12 * 5];
+	k42.z.z = Ke[11 + 12 * 5];
+
+	// k43 
+	// a10_7 a10_8 a10_9 
+	// a11_7 a11_8 a11_9 
+	// a12_7 a12_8 a12_9 
+	k43.x.x = Ke[9 + 12 * 6];
+	k43.x.y = Ke[10 + 12 * 6];
+	k43.x.z = Ke[11 + 12 * 6];
+
+	k43.y.x = Ke[9 + 12 * 7];
+	k43.y.y = Ke[10 + 12 * 7];
+	k43.y.z = Ke[11 + 12 * 7];
+
+	k43.z.x = Ke[9 + 12 * 8];
+	k43.z.y = Ke[10 + 12 * 8];
+	k43.z.z = Ke[11 + 12 * 8];
+
+	// k44
+	// a10_10 a10_11 a10_12
+	// a11_10 a11_11 a11_12
+	// a12_10 a12_11 a12_12
+	k44.x.x = Ke[9 + 12 * 9];
+	k44.x.y = Ke[10 + 12 * 9];
+	k44.x.z = Ke[11 + 12 * 9];
+
+	k44.y.x = Ke[9 + 12 * 10];
+	k44.y.y = Ke[10 + 12 * 10];
+	k44.y.z = Ke[11 + 12 * 10];
+
+	k44.z.x = Ke[9 + 12 * 11];
+	k44.z.y = Ke[10 + 12 * 11];
+	k44.z.z = Ke[11 + 12 * 11];
+}
+
+b3SoftBody::b3SoftBody(const b3SoftBodyDef& def)
+{
+	B3_ASSERT(def.mesh);
+	B3_ASSERT(def.density > 0.0f);
+
+	m_mesh = def.mesh;
+	m_density = def.density;
+	m_E = def.E;
+	m_nu = def.nu;
+	m_gravity.SetZero();
+	m_world = nullptr;
+
+	const b3SoftBodyMesh* m = m_mesh;
+
+	m_nodes = (b3SoftBodyNode*)b3Alloc(m->vertexCount * sizeof(b3SoftBodyNode));
+
+	// Initialize nodes
+	for (u32 i = 0; i < m->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		n->m_body = this;
+		n->m_type = e_dynamicSoftBodyNode;
+		n->m_position = m->vertices[i];
+		n->m_velocity.SetZero();
+		n->m_force.SetZero();
+		n->m_mass = 0.0f;
+		n->m_invMass = 0.0f;
+		n->m_radius = 0.0f;
+		n->m_friction = 0.0f;
+		n->m_userData = nullptr;
+		n->m_vertex = i;
+		n->m_bodyContact.active = false;
+	}
+
+	// Compute mass
+	ComputeMass();
+
+	// Initialize elements
+	m_elements = (b3SoftBodyElement*)b3Alloc(m->tetrahedronCount * sizeof(b3SoftBodyElement));
+	for (u32 ei = 0; ei < m->tetrahedronCount; ++ei)
+	{
+		b3SoftBodyMeshTetrahedron* mt = m->tetrahedrons + ei;
+		b3SoftBodyElement* e = m_elements + ei;
+
+		u32 v1 = mt->v1;
+		u32 v2 = mt->v2;
+		u32 v3 = mt->v3;
+		u32 v4 = mt->v4;
+
+		b3Vec3 p1 = m->vertices[v1];
+		b3Vec3 p2 = m->vertices[v2];
+		b3Vec3 p3 = m->vertices[v3];
+		b3Vec3 p4 = m->vertices[v4];
+
+		float32 lambda, mu;
+		b3Lame(lambda, mu, m_E, m_nu);
+
+		// 6 x 6
+		float32 E[36];
+		b3CreateE(E, lambda, mu);
+
+		// 4 x 4
+		float32 P[16] =
+		{
+			p1.x, p1.y, p1.z, 1.0f,
+			p2.x, p2.y, p2.z, 1.0f,
+			p3.x, p3.y, p3.z, 1.0f,
+			p4.x, p4.y, p4.z, 1.0f
+		};
+
+		b3Inverse4(e->invP, P);
+
+		// 6 x 12
+		float32 B[72];
+		b3CreateB(B, e->invP);
+
+		// 6 x 12
+		float32 EB[72];
+		b3Mul(EB, E, 6, 6, B, 6, 12);
+
+		// 12 x 6
+		float32 BT[72];
+		b3Transpose(BT, B, 6, 12);
+
+		float32 V = b3Volume(p1, p2, p3, p4);
+
+		// 12 x 12
+		float32 Ke[144];
+		b3Mul(Ke, BT, 12, 6, EB, 6, 12);
+		for (u32 i = 0; i < 144; ++i)
+		{
+			Ke[i] *= V;
+		}
+
+		b3SetK(e->K, Ke);
+	}
+
+	// Initialize triangles
+	m_triangles = (b3SoftBodyTriangle*)b3Alloc(4 * m_mesh->tetrahedronCount * sizeof(b3SoftBodyTriangle));
+	for (u32 i = 0; i < m_mesh->tetrahedronCount; ++i)
+	{
+		b3SoftBodyMeshTetrahedron* mt = m_mesh->tetrahedrons + i;
+
+		u32 v1 = mt->v1;
+		u32 v2 = mt->v2;
+		u32 v3 = mt->v3;
+		u32 v4 = mt->v4;
+
+		b3SoftBodyTriangle* t1 = m_triangles + 4 * i + 0;
+		b3SoftBodyTriangle* t2 = m_triangles + 4 * i + 1;
+		b3SoftBodyTriangle* t3 = m_triangles + 4 * i + 2;
+		b3SoftBodyTriangle* t4 = m_triangles + 4 * i + 3;
+
+		t1->v1 = v1;
+		t1->v2 = v2;
+		t1->v3 = v3;
+		t1->tetrahedron = i;
+
+		t2->v1 = v1;
+		t2->v2 = v3;
+		t2->v3 = v4;
+		t2->tetrahedron = i;
+
+		t3->v1 = v1;
+		t3->v2 = v4;
+		t3->v3 = v2;
+		t3->tetrahedron = i;
+
+		t4->v1 = v2;
+		t4->v2 = v4;
+		t4->v3 = v3;
+		t4->tetrahedron = i;
+	}
+}
+
+b3SoftBody::~b3SoftBody()
+{
+	b3Free(m_nodes);
+	b3Free(m_elements);
+	b3Free(m_triangles);
+}
+
+bool b3SoftBody::RayCastSingle(b3SoftBodyRayCastSingleOutput* output, const b3Vec3& p1, const b3Vec3& p2) const
+{
+	b3RayCastInput input;
+	input.p1 = p1;
+	input.p2 = p2;
+	input.maxFraction = 1.0f;
+
+	u32 triangle = ~0;
+	u32 tetrahedron = ~0;
+
+	b3RayCastOutput output0;
+	output0.fraction = B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < 4 * m_mesh->tetrahedronCount; ++i)
+	{
+		b3SoftBodyTriangle* t = m_triangles + i;
+
+		b3Vec3 v1 = m_nodes[t->v1].m_position;
+		b3Vec3 v2 = m_nodes[t->v2].m_position;
+		b3Vec3 v3 = m_nodes[t->v3].m_position;
+
+		b3RayCastOutput subOutput;
+		if (b3RayCast(&subOutput, &input, v1, v2, v3))
+		{
+			if (subOutput.fraction < output0.fraction)
+			{
+				triangle = i;
+				tetrahedron = t->tetrahedron;
+				output0.fraction = subOutput.fraction;
+				output0.normal = subOutput.normal;
+			}
+		}
+	}
+
+	if (tetrahedron != ~0)
+	{
+		output->tetrahedron = tetrahedron;
+		output->v1 = m_triangles[triangle].v1;
+		output->v2 = m_triangles[triangle].v2;
+		output->v3 = m_triangles[triangle].v3;
+		output->fraction = output0.fraction;
+		output->normal = output0.normal;
+
+		return true;
+	}
+
+	return false;
+}
+
+b3SoftBodyNode* b3SoftBody::GetVertexNode(u32 i)
+{
+	B3_ASSERT(i < m_mesh->vertexCount);
+	return m_nodes + i;
+}
+
+float32 b3SoftBody::GetEnergy() const
+{
+	float32 E = 0.0f;
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		E += n->m_mass * b3Dot(n->m_velocity, n->m_velocity);
+	}
+	return 0.5f * E;
+}
+
+void b3SoftBody::ComputeMass()
+{
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+		n->m_mass = 0.0f;
+		n->m_invMass = 0.0f;
+	}
+
+	const float32 inv4 = 1.0f / 4.0f;
+	const float32 rho = m_density;
+
+	for (u32 i = 0; i < m_mesh->tetrahedronCount; ++i)
+	{
+		b3SoftBodyMeshTetrahedron* tetrahedron = m_mesh->tetrahedrons + i;
+
+		b3Vec3 v1 = m_mesh->vertices[tetrahedron->v1];
+		b3Vec3 v2 = m_mesh->vertices[tetrahedron->v2];
+		b3Vec3 v3 = m_mesh->vertices[tetrahedron->v3];
+		b3Vec3 v4 = m_mesh->vertices[tetrahedron->v4];
+
+		float32 volume = b3Volume(v1, v2, v3, v4);
+		B3_ASSERT(volume > 0.0f);
+
+		float32 mass = rho * volume;
+
+		b3SoftBodyNode* n1 = m_nodes + tetrahedron->v1;
+		b3SoftBodyNode* n2 = m_nodes + tetrahedron->v2;
+		b3SoftBodyNode* n3 = m_nodes + tetrahedron->v3;
+		b3SoftBodyNode* n4 = m_nodes + tetrahedron->v4;
+
+		n1->m_mass += inv4 * mass;
+		n2->m_mass += inv4 * mass;
+		n3->m_mass += inv4 * mass;
+		n4->m_mass += inv4 * mass;
+	}
+
+	// Invert
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+		B3_ASSERT(n->m_mass > 0.0f);
+		n->m_invMass = 1.0f / n->m_mass;
+	}
+}
+
+void b3SoftBody::UpdateContacts()
+{
+	B3_PROFILE("Soft Body Update Contacts");
+
+	// Is there a world attached to this cloth?
+	if (m_world == nullptr)
+	{
+		return;
+	}
+
+	// Create contacts 
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		b3Sphere s1;
+		s1.vertex = n->m_position;
+		s1.radius = n->m_radius;
+
+		// Find the deepest penetration
+		b3Shape* bestShape = nullptr;
+		float32 bestSeparation = 0.0f;
+		b3Vec3 bestPoint(0.0f, 0.0f, 0.0f);
+		b3Vec3 bestNormal(0.0f, 0.0f, 0.0f);
+
+		for (b3Body* body = m_world->GetBodyList().m_head; body; body = body->GetNext())
+		{
+			if (n->m_type != e_dynamicSoftBodyNode)
+			{
+				continue;
+			}
+
+			if (body->GetType() != e_staticBody)
+			{
+				//continue;
+			}
+
+			b3Transform xf = body->GetTransform();
+			for (b3Shape* shape = body->GetShapeList().m_head; shape; shape = shape->GetNext())
+			{
+				b3TestSphereOutput output;
+				if (shape->TestSphere(&output, s1, xf))
+				{
+					if (output.separation < bestSeparation)
+					{
+						bestShape = shape;
+						bestSeparation = output.separation;
+						bestPoint = output.point;
+						bestNormal = output.normal;
+					}
+				}
+			}
+		}
+
+		if (bestShape == nullptr)
+		{
+			n->m_bodyContact.active = false;
+			continue;
+		}
+
+		b3Shape* shape = bestShape;
+		b3Body* body = shape->GetBody();
+		float32 separation = bestSeparation;
+		b3Vec3 point = bestPoint;
+		b3Vec3 normal = -bestNormal;
+
+		b3NodeBodyContact* c = &n->m_bodyContact;
+
+		b3NodeBodyContact c0 = *c;
+
+		c->active = true;
+		c->n1 = n;
+		c->s2 = shape;
+		c->normal1 = normal;
+		c->localPoint1.SetZero();
+		c->localPoint2 = body->GetLocalPoint(point);
+		c->t1 = b3Perp(normal);
+		c->t2 = b3Cross(c->t1, normal);
+		c->normalImpulse = 0.0f;
+		c->tangentImpulse.SetZero();
+
+		if (c0.active == true)
+		{
+			c->normalImpulse = c0.normalImpulse;
+			c->tangentImpulse = c0.tangentImpulse;
+		}
+	}
+}
+
+void b3SoftBody::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
+{
+	B3_PROFILE("Soft Body Solve");
+
+	b3SoftBodySolverDef def;
+	def.stack = &m_stackAllocator;
+	def.mesh = m_mesh;
+	def.nodes = m_nodes;
+	def.elements = m_elements;
+
+	b3SoftBodySolver solver(def);
+
+	solver.Solve(dt, gravity, velocityIterations, positionIterations);
+}
+
+void b3SoftBody::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
+{
+	B3_PROFILE("Soft Body Step");
+
+	// Update contacts
+	UpdateContacts();
+
+	// Integrate state, solve constraints. 
+	if (dt > 0.0f)
+	{
+		Solve(dt, m_gravity, velocityIterations, positionIterations);
+	}
+
+	// Clear forces
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		m_nodes[i].m_force.SetZero();
+	}
+}
+
+void b3SoftBody::Draw() const
+{
+	const b3SoftBodyMesh* m = m_mesh;
+
+	for (u32 i = 0; i < m->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		b3Vec3 v = n->m_position;
+
+		if (n->m_type == e_staticSoftBodyNode)
+		{
+			b3Draw_draw->DrawPoint(v, 4.0f, b3Color_white);
+		}
+
+		if (n->m_type == e_dynamicSoftBodyNode)
+		{
+			b3Draw_draw->DrawPoint(v, 4.0f, b3Color_green);
+		}
+	}
+
+	for (u32 i = 0; i < m->tetrahedronCount; ++i)
+	{
+		b3SoftBodyMeshTetrahedron* t = m->tetrahedrons + i;
+
+		b3Vec3 v1 = m_nodes[t->v1].m_position;
+		b3Vec3 v2 = m_nodes[t->v2].m_position;
+		b3Vec3 v3 = m_nodes[t->v3].m_position;
+		b3Vec3 v4 = m_nodes[t->v4].m_position;
+
+		b3Vec3 c = (v1 + v2 + v3 + v4) / 4.0f;
+
+		float32 s = 0.9f;
+
+		v1 = s * (v1 - c) + c;
+		v2 = s * (v2 - c) + c;
+		v3 = s * (v3 - c) + c;
+		v4 = s * (v4 - c) + c;
+
+		// v1, v2, v3
+		b3Draw_draw->DrawTriangle(v1, v2, v3, b3Color_black);
+		
+		b3Vec3 n1 = b3Cross(v2 - v1, v3 - v1);
+		n1.Normalize();
+		b3Draw_draw->DrawSolidTriangle(-n1, v1, v2, v3, b3Color_blue);
+
+		// v1, v3, v4
+		b3Draw_draw->DrawTriangle(v1, v3, v4, b3Color_black);
+		
+		b3Vec3 n2 = b3Cross(v3 - v1, v4 - v1);
+		n2.Normalize();
+		b3Draw_draw->DrawSolidTriangle(-n2, v1, v3, v4, b3Color_blue);
+
+		// v1, v4, v2
+		b3Draw_draw->DrawTriangle(v1, v4, v2, b3Color_black);
+		
+		b3Vec3 n3 = b3Cross(v4 - v1, v2 - v1);
+		n3.Normalize();
+		b3Draw_draw->DrawSolidTriangle(-n3, v1, v4, v2, b3Color_blue);
+
+		// v2, v4, v3
+		b3Draw_draw->DrawTriangle(v2, v4, v3, b3Color_black);
+		
+		b3Vec3 n4 = b3Cross(v4 - v2, v3 - v2);
+		n4.Normalize();
+		b3Draw_draw->DrawSolidTriangle(-n4, v2, v4, v3, b3Color_blue);
+	}
+}
\ No newline at end of file
diff --git a/src/bounce/softbody/softbody_contact_solver.cpp b/src/bounce/softbody/softbody_contact_solver.cpp
new file mode 100644
index 0000000..f7ac4c4
--- /dev/null
+++ b/src/bounce/softbody/softbody_contact_solver.cpp
@@ -0,0 +1,429 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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/softbody/softbody_contact_solver.h>
+#include <bounce/softbody/softbody_node.h>
+#include <bounce/cloth/dense_vec3.h>
+#include <bounce/common/memory/stack_allocator.h>
+#include <bounce/dynamics/shapes/shape.h>
+#include <bounce/dynamics/body.h>
+
+b3SoftBodyContactSolver::b3SoftBodyContactSolver(const b3SoftBodyContactSolverDef& def)
+{
+	m_allocator = def.allocator;
+
+	m_positions = def.positions;
+	m_velocities = def.velocities;
+
+	m_bodyContactCapacity = def.bodyContactCapacity;
+	m_bodyContactCount = 0;
+	m_bodyContacts = (b3NodeBodyContact**)m_allocator->Allocate(m_bodyContactCapacity * sizeof(b3NodeBodyContact*));
+}
+
+b3SoftBodyContactSolver::~b3SoftBodyContactSolver()
+{
+	m_allocator->Free(m_bodyPositionConstraints);
+	m_allocator->Free(m_bodyVelocityConstraints);
+	m_allocator->Free(m_bodyContacts);
+}
+
+void b3SoftBodyContactSolver::Add(b3NodeBodyContact* c)
+{
+	m_bodyContacts[m_bodyContactCount++] = c;
+}
+
+void b3SoftBodyContactSolver::InitializeBodyContactConstraints()
+{
+	m_bodyVelocityConstraints = (b3SoftBodySolverBodyContactVelocityConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactVelocityConstraint));
+	m_bodyPositionConstraints = (b3SoftBodySolverBodyContactPositionConstraint*)m_allocator->Allocate(m_bodyContactCount * sizeof(b3SoftBodySolverBodyContactPositionConstraint));
+	
+	b3DenseVec3& x = *m_positions;
+	b3DenseVec3& v = *m_velocities;
+
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3NodeBodyContact* c = m_bodyContacts[i];
+		b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
+		b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
+
+		vc->indexA = c->n1->m_vertex;
+		vc->bodyB = c->s2->GetBody();
+
+		vc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->n1->m_invMass;
+		vc->invMassB = vc->bodyB->GetInverseMass();
+
+		vc->invIA.SetZero();
+		vc->invIB = vc->bodyB->GetWorldInverseInertia();
+
+		vc->friction = b3MixFriction(c->n1->m_friction, c->s2->GetFriction());
+
+		pc->indexA = c->n1->m_vertex;
+		pc->bodyB = vc->bodyB;
+
+		pc->invMassA = c->n1->m_type == e_staticSoftBodyNode ? 0.0f : c->n1->m_invMass;
+		pc->invMassB = vc->bodyB->m_invMass;
+
+		pc->invIA.SetZero();
+		pc->invIB = vc->bodyB->m_worldInvI;
+
+		pc->radiusA = c->n1->m_radius;
+		pc->radiusB = c->s2->m_radius;
+
+		pc->localCenterA.SetZero();
+		pc->localCenterB = pc->bodyB->m_sweep.localCenter;
+
+		pc->normalA = c->normal1;
+		pc->localPointA = c->localPoint1;
+		pc->localPointB = c->localPoint2;
+	}
+
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3NodeBodyContact* c = m_bodyContacts[i];
+		b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
+		b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
+
+		u32 indexA = vc->indexA;
+		b3Body* bodyB = vc->bodyB;
+
+		float32 mA = vc->invMassA;
+		float32 mB = vc->invMassB;
+
+		b3Mat33 iA = vc->invIA;
+		b3Mat33 iB = vc->invIB;
+
+		b3Vec3 xA = x[indexA];
+		b3Vec3 xB = bodyB->m_sweep.worldCenter;
+
+		b3Quat qA; qA.SetIdentity();
+		b3Quat qB = bodyB->m_sweep.orientation;
+
+		b3Vec3 localCenterA = pc->localCenterA;
+		b3Vec3 localCenterB = pc->localCenterB;
+
+		b3Transform xfA;
+		xfA.rotation = b3QuatMat33(qA);
+		xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
+
+		b3Transform xfB;
+		xfB.rotation = b3QuatMat33(qB);
+		xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
+
+		b3NodeBodyContactWorldPoint wp;
+		wp.Initialize(c, pc->radiusA, xfA, pc->radiusB, xfB);
+
+		vc->normal = wp.normal;
+		vc->tangent1 = c->t1;
+		vc->tangent2 = c->t2;
+		vc->point = wp.point;
+
+		b3Vec3 point = vc->point;
+
+		b3Vec3 rA = point - xA;
+		b3Vec3 rB = point - xB;
+
+		vc->rA = rA;
+		vc->rB = rB;
+
+		vc->normalImpulse = c->normalImpulse;
+		vc->tangentImpulse = c->tangentImpulse;
+
+		{
+			b3Vec3 n = vc->normal;
+
+			b3Vec3 rnA = b3Cross(rA, n);
+			b3Vec3 rnB = b3Cross(rB, n);
+
+			float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
+
+			vc->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
+			vc->velocityBias = 0.0f;
+		}
+
+		{
+			b3Vec3 t1 = vc->tangent1;
+			b3Vec3 t2 = vc->tangent2;
+
+			b3Vec3 rn1A = b3Cross(rA, t1);
+			b3Vec3 rn1B = b3Cross(rB, t1);
+			b3Vec3 rn2A = b3Cross(rA, t2);
+			b3Vec3 rn2B = b3Cross(rB, t2);
+
+			// dot(t1, t2) = 0
+			// J1_l1 * M1 * J2_l1 = J1_l2 * M2 * J2_l2 = 0
+			float32 k11 = mA + mB + b3Dot(iA * rn1A, rn1A) + b3Dot(iB * rn1B, rn1B);
+			float32 k12 = b3Dot(iA * rn1A, rn2A) + b3Dot(iB * rn1B, rn2B);
+			float32 k22 = mA + mB + b3Dot(iA * rn2A, rn2A) + b3Dot(iB * rn2B, rn2B);
+
+			b3Mat22 K;
+			K.x.Set(k11, k12);
+			K.y.Set(k12, k22);
+
+			vc->tangentMass = b3Inverse(K);
+		}
+	}
+}
+
+void b3SoftBodyContactSolver::WarmStart()
+{
+	b3DenseVec3& v = *m_velocities;
+
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
+
+		u32 indexA = vc->indexA;
+		b3Body* bodyB = vc->bodyB;
+
+		b3Vec3 vA = v[indexA];
+		b3Vec3 vB = bodyB->GetLinearVelocity();
+
+		b3Vec3 wA; wA.SetZero();
+		b3Vec3 wB = bodyB->GetAngularVelocity();
+
+		float32 mA = vc->invMassA;
+		float32 mB = vc->invMassB;
+
+		b3Mat33 iA = vc->invIA;
+		b3Mat33 iB = vc->invIB;
+
+		b3Vec3 P = vc->normalImpulse * vc->normal;
+
+		vA -= mA * P;
+		wA -= iA * b3Cross(vc->rA, P);
+
+		vB += mB * P;
+		wB += iB * b3Cross(vc->rB, P);
+
+		b3Vec3 P1 = vc->tangentImpulse.x * vc->tangent1;
+		b3Vec3 P2 = vc->tangentImpulse.y * vc->tangent2;
+
+		vA -= mA * (P1 + P2);
+		wA -= iA * b3Cross(vc->rA, P1 + P2);
+
+		vB += mB * (P1 + P2);
+		wB += iB * b3Cross(vc->rB, P1 + P2);
+
+		v[indexA] = vA;
+
+		bodyB->SetLinearVelocity(vB);
+		bodyB->SetAngularVelocity(wB);
+	}
+}
+
+void b3SoftBodyContactSolver::SolveBodyContactVelocityConstraints()
+{
+	b3DenseVec3& v = *m_velocities;
+
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
+
+		u32 indexA = vc->indexA;
+		b3Body* bodyB = vc->bodyB;
+
+		b3Vec3 vA = v[indexA];
+		b3Vec3 vB = bodyB->GetLinearVelocity();
+
+		b3Vec3 wA; wA.SetZero();
+		b3Vec3 wB = bodyB->GetAngularVelocity();
+
+		float32 mA = vc->invMassA;
+		float32 mB = vc->invMassB;
+
+		b3Mat33 iA = vc->invIA;
+		b3Mat33 iB = vc->invIB;
+
+		b3Vec3 normal = vc->normal;
+		b3Vec3 point = vc->point;
+
+		b3Vec3 rA = vc->rA;
+		b3Vec3 rB = vc->rB;
+
+		// Solve normal constraint.
+		{
+			b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
+			float32 Cdot = b3Dot(normal, dv);
+
+			float32 impulse = vc->normalMass * (-Cdot + vc->velocityBias);
+
+			float32 oldImpulse = vc->normalImpulse;
+			vc->normalImpulse = b3Max(vc->normalImpulse + impulse, 0.0f);
+			impulse = vc->normalImpulse - oldImpulse;
+
+			b3Vec3 P = impulse * normal;
+
+			vA -= mA * P;
+			wA -= iA * b3Cross(rA, P);
+
+			vB += mB * P;
+			wB += iB * b3Cross(rB, P);
+		}
+
+		// Solve tangent constraints.
+		{
+			b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
+
+			b3Vec2 Cdot;
+			Cdot.x = b3Dot(dv, vc->tangent1);
+			Cdot.y = b3Dot(dv, vc->tangent2);
+
+			b3Vec2 impulse = vc->tangentMass * -Cdot;
+			b3Vec2 oldImpulse = vc->tangentImpulse;
+			vc->tangentImpulse += impulse;
+
+			float32 maxImpulse = vc->friction * vc->normalImpulse;
+			if (b3Dot(vc->tangentImpulse, vc->tangentImpulse) > maxImpulse * maxImpulse)
+			{
+				vc->tangentImpulse.Normalize();
+				vc->tangentImpulse *= maxImpulse;
+			}
+
+			impulse = vc->tangentImpulse - oldImpulse;
+
+			b3Vec3 P1 = impulse.x * vc->tangent1;
+			b3Vec3 P2 = impulse.y * vc->tangent2;
+			b3Vec3 P = P1 + P2;
+
+			vA -= mA * P;
+			wA -= iA * b3Cross(rA, P);
+
+			vB += mB * P;
+			wB += iB * b3Cross(rB, P);
+		}
+
+		v[indexA] = vA;
+
+		bodyB->SetLinearVelocity(vB);
+		bodyB->SetAngularVelocity(wB);
+	}
+}
+
+void b3SoftBodyContactSolver::StoreImpulses()
+{
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3NodeBodyContact* c = m_bodyContacts[i];
+		b3SoftBodySolverBodyContactVelocityConstraint* vc = m_bodyVelocityConstraints + i;
+
+		c->normalImpulse = vc->normalImpulse;
+		c->tangentImpulse = vc->tangentImpulse;
+	}
+}
+
+struct b3ClothSolverBodyContactSolverPoint
+{
+	void Initialize(const b3SoftBodySolverBodyContactPositionConstraint* pc, const b3Transform& xfA, const b3Transform& xfB)
+	{
+		b3Vec3 cA = xfA * pc->localPointA;
+		b3Vec3 cB = xfB * pc->localPointB;
+
+		float32 rA = pc->radiusA;
+		float32 rB = pc->radiusB;
+
+		b3Vec3 nA = pc->normalA;
+
+		b3Vec3 pA = cA + rA * nA;
+		b3Vec3 pB = cB - rB * nA;
+
+		point = cB;
+		normal = nA;
+		separation = b3Dot(cB - cA, nA) - rA - rB;
+	}
+
+	b3Vec3 normal;
+	b3Vec3 point;
+	float32 separation;
+};
+
+bool b3SoftBodyContactSolver::SolveBodyContactPositionConstraints()
+{
+	b3DenseVec3& x = *m_positions;
+
+	float32 minSeparation = 0.0f;
+
+	for (u32 i = 0; i < m_bodyContactCount; ++i)
+	{
+		b3SoftBodySolverBodyContactPositionConstraint* pc = m_bodyPositionConstraints + i;
+
+		u32 indexA = pc->indexA;
+		float32 mA = pc->invMassA;
+		b3Mat33 iA = pc->invIA;
+		b3Vec3 localCenterA = pc->localCenterA;
+
+		b3Body* bodyB = pc->bodyB;
+		float32 mB = pc->invMassB;
+		b3Mat33 iB = pc->invIB;
+		b3Vec3 localCenterB = pc->localCenterB;
+
+		b3Vec3 cA = x[indexA];
+		b3Quat qA; qA.SetIdentity();
+
+		b3Vec3 cB = bodyB->m_sweep.worldCenter;
+		b3Quat qB = bodyB->m_sweep.orientation;
+
+		// Solve normal constraint
+		b3Transform xfA;
+		xfA.rotation = b3QuatMat33(qA);
+		xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
+
+		b3Transform xfB;
+		xfB.rotation = b3QuatMat33(qB);
+		xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
+
+		b3ClothSolverBodyContactSolverPoint cpcp;
+		cpcp.Initialize(pc, xfA, xfB);
+
+		b3Vec3 normal = cpcp.normal;
+		b3Vec3 point = cpcp.point;
+		float32 separation = cpcp.separation;
+
+		// Update max constraint error.
+		minSeparation = b3Min(minSeparation, separation);
+
+		// Allow some slop and prevent large corrections.
+		float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
+
+		// Compute effective mass.
+		b3Vec3 rA = point - cA;
+		b3Vec3 rB = point - cB;
+
+		b3Vec3 rnA = b3Cross(rA, normal);
+		b3Vec3 rnB = b3Cross(rB, normal);
+		float32 K = mA + mB + b3Dot(rnA, iA * rnA) + b3Dot(rnB, iB * rnB);
+
+		// Compute normal impulse.
+		float32 impulse = K > 0.0f ? -C / K : 0.0f;
+		b3Vec3 P = impulse * normal;
+
+		cA -= mA * P;
+		qA -= b3Derivative(qA, iA * b3Cross(rA, P));
+		qA.Normalize();
+
+		cB += mB * P;
+		qB += b3Derivative(qB, iB * b3Cross(rB, P));
+		qB.Normalize();
+
+		x[indexA] = cA;
+
+		bodyB->m_sweep.worldCenter = cB;
+		bodyB->m_sweep.orientation = qB;
+	}
+
+	return minSeparation >= -3.0f * B3_LINEAR_SLOP;
+}
\ No newline at end of file
diff --git a/src/bounce/softbody/softbody_mesh.cpp b/src/bounce/softbody/softbody_mesh.cpp
new file mode 100644
index 0000000..2dd3135
--- /dev/null
+++ b/src/bounce/softbody/softbody_mesh.cpp
@@ -0,0 +1,69 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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/softbody/softbody_mesh.h>
+#include <bounce/meshgen/sphere_mesh.h>
+
+b3QSoftBodyMesh::b3QSoftBodyMesh()
+{
+	vertexCount = 0;
+	vertices = nullptr;
+	tetrahedronCount = 0;
+	tetrahedrons = nullptr;
+}
+
+b3QSoftBodyMesh::~b3QSoftBodyMesh()
+{
+	b3Free(vertices);
+	b3Free(tetrahedrons);
+}
+
+void b3QSoftBodyMesh::SetAsSphere(float32 radius, u32 subdivisions)
+{
+	smMesh mesh;
+	smCreateMesh(mesh, subdivisions);
+
+	vertexCount = 1 + mesh.vertexCount;
+	vertices = (b3Vec3*)b3Alloc(vertexCount * sizeof(b3Vec3));
+	vertices[0].SetZero();
+	for (u32 i = 0; i < mesh.vertexCount; ++i)
+	{
+		vertices[1 + i] = mesh.vertices[i];
+	}
+
+	tetrahedronCount = mesh.indexCount / 3;
+	tetrahedrons = (b3SoftBodyMeshTetrahedron*)b3Alloc(tetrahedronCount * sizeof(b3SoftBodyMeshTetrahedron));
+	for (u32 i = 0; i < mesh.indexCount / 3; ++i)
+	{
+		u32 v1 = mesh.indices[3 * i + 0];
+		u32 v2 = mesh.indices[3 * i + 1];
+		u32 v3 = mesh.indices[3 * i + 2];
+
+		b3SoftBodyMeshTetrahedron* t = tetrahedrons + i;
+
+		t->v1 = 1 + v3;
+		t->v2 = 1 + v2;
+		t->v3 = 1 + v1;
+		t->v4 = 0;
+	}
+
+	for (u32 i = 0; i < vertexCount; ++i)
+	{
+		vertices[i] *= radius;
+	}
+}
\ No newline at end of file
diff --git a/src/bounce/softbody/softbody_solver.cpp b/src/bounce/softbody/softbody_solver.cpp
new file mode 100644
index 0000000..cb6ca56
--- /dev/null
+++ b/src/bounce/softbody/softbody_solver.cpp
@@ -0,0 +1,397 @@
+/*
+* Copyright (c) 2016-2019 Irlan Robson https://irlanrobson.github.io
+*
+* 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/softbody/softbody_solver.h>
+#include <bounce/softbody/softbody_mesh.h>
+#include <bounce/softbody/softbody_node.h>
+#include <bounce/softbody/softbody.h>
+#include <bounce/softbody/sparse_mat33.h>
+#include <bounce/softbody/softbody_contact_solver.h>
+
+#include <bounce/dynamics/body.h>
+#include <bounce/dynamics/shapes/shape.h>
+
+// This work is based on the paper "Interactive Virtual Materials" written by 
+// Matthias Mueller Fischer
+// The paper is available here:
+// http://matthias-mueller-fischer.ch/publications/GI2004.pdf
+
+// In order to support velocity constraints on node velocities 
+// we solve Ax = b using a Modified Preconditioned Conjugate Gradient (MPCG) algorithm.
+
+// Number of MPCG iterations, a value that is normally small when small time steps are taken.
+u32 b3_softBodySolverIterations = 0;
+
+// Enables the stiffness warping solver.
+// bool b3_enableStiffnessWarping = true;
+
+b3SoftBodySolver::b3SoftBodySolver(const b3SoftBodySolverDef& def)
+{
+	m_allocator = def.stack;
+	m_mesh = def.mesh;
+	m_nodes = def.nodes;
+	m_elements = def.elements;
+}
+
+b3SoftBodySolver::~b3SoftBodySolver()
+{
+
+}
+
+// https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf
+static B3_FORCE_INLINE void b3ExtractRotation(b3Mat33& out, b3Quat& q, const b3Mat33& A, u32 maxIterations = 20)
+{
+	for (u32 iteration = 0; iteration < maxIterations; ++iteration)
+	{
+		b3Mat33 R = b3QuatMat33(q);
+
+		float32 s = b3Abs(b3Dot(R.x, A.x) + b3Dot(R.y, A.y) + b3Dot(R.z, A.z));
+
+		if (s == 0.0f)
+		{
+			break;
+		}
+
+		float32 inv_s = 1.0f / s + 1.0e-9f;
+
+		b3Vec3 v = b3Cross(R.x, A.x) + b3Cross(R.y, A.y) + b3Cross(R.z, A.z);
+
+		b3Vec3 omega = inv_s * v;
+
+		float32 w = b3Length(omega);
+
+		if (w < 1.0e-9f)
+		{
+			break;
+		}
+
+		b3Quat omega_q(omega / w, w);
+
+		q = omega_q * q;
+		q.Normalize();
+	}
+
+	out = b3QuatMat33(q);
+}
+
+static B3_FORCE_INLINE void b3SolveMPCG(b3DenseVec3& x,
+	const b3SparseMat33& A, const b3DenseVec3& b,
+	const b3DenseVec3& x0, const b3DiagMat33& S, u32 maxIterations = 20)
+{
+	b3DiagMat33 P(A.rowCount);
+	b3DiagMat33 invP(A.rowCount);
+	for (u32 i = 0; i < A.rowCount; ++i)
+	{
+		b3Mat33 D = A(i, i);
+
+		// Sylvester Criterion to ensure PD-ness
+		B3_ASSERT(b3Det(D.x, D.y, D.z) > 0.0f);
+
+		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;
+
+		P[i] = b3Diagonal(xx, yy, zz);
+		invP[i] = b3Diagonal(D.x.x, D.y.y, D.z.z);
+	}
+
+	x = x0;
+
+	float32 delta_0 = b3Dot(S * b, P * (S * b));
+
+	b3DenseVec3 r = S * (b - A * x);
+	b3DenseVec3 c = S * (invP * r);
+
+	float32 delta_new = b3Dot(r, c);
+
+	u32 iteration = 0;
+	for (;;)
+	{
+		if (iteration == maxIterations)
+		{
+			break;
+		}
+
+		if (delta_new <= B3_EPSILON * B3_EPSILON * delta_0)
+		{
+			break;
+		}
+
+		b3DenseVec3 q = S * (A * c);
+
+		float32 alpha = delta_new / b3Dot(c, q);
+
+		x = x + alpha * c;
+		r = r - alpha * q;
+
+		b3DenseVec3 s = invP * r;
+
+		float32 delta_old = delta_new;
+
+		delta_new = b3Dot(r, s);
+
+		float32 beta = delta_new / delta_old;
+
+		c = S * (s + beta * c);
+
+		++iteration;
+	}
+
+	b3_softBodySolverIterations = iteration;
+}
+
+static B3_FORCE_INLINE void b3Mul(float32* C, float32* A, u32 AM, u32 AN, float32* B, u32 BM, u32 BN)
+{
+	B3_ASSERT(AN == BM);
+
+	for (u32 i = 0; i < AM; ++i)
+	{
+		for (u32 j = 0; j < BN; ++j)
+		{
+			C[i + AM * j] = 0.0f;
+
+			for (u32 k = 0; k < AN; ++k)
+			{
+				C[i + AM * j] += A[i + AM * k] * B[k + BM * j];
+			}
+		}
+	}
+}
+
+void b3SoftBodySolver::Solve(float32 dt, const b3Vec3& gravity, u32 velocityIterations, u32 positionIterations)
+{
+	float32 h = dt;
+
+	b3SparseMat33 M(m_mesh->vertexCount);
+	b3DenseVec3 x(m_mesh->vertexCount);
+	b3DenseVec3 p(m_mesh->vertexCount);
+	b3DenseVec3 v(m_mesh->vertexCount);
+	b3DenseVec3 fe(m_mesh->vertexCount);
+	b3DenseVec3 x0(m_mesh->vertexCount);
+	b3DiagMat33 S(m_mesh->vertexCount);
+
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		M(i, i) = b3Diagonal(n->m_mass);
+		x[i] = m_mesh->vertices[i];
+		p[i] = n->m_position;
+		v[i] = n->m_velocity;
+		fe[i] = n->m_force;
+		x0[i] = n->m_velocity;
+
+		// Apply gravity
+		if (n->m_type == e_dynamicSoftBodyNode)
+		{
+			fe[i] += n->m_mass * gravity;
+			S[i].SetIdentity();
+		}
+		else
+		{
+			S[i].SetZero();
+		}
+	}
+
+	// Element assembly
+	b3SparseMat33 K(m_mesh->vertexCount);
+
+	b3DenseVec3 f0(m_mesh->vertexCount);
+	f0.SetZero();
+
+	for (u32 ei = 0; ei < m_mesh->tetrahedronCount; ++ei)
+	{
+		b3SoftBodyMeshTetrahedron* mt = m_mesh->tetrahedrons + ei;
+		b3SoftBodyElement* e = m_elements + ei;
+
+		b3Mat33* Ke = e->K;
+
+		u32 v1 = mt->v1;
+		u32 v2 = mt->v2;
+		u32 v3 = mt->v3;
+		u32 v4 = mt->v4;
+
+		b3Vec3 p1 = p[v1];
+		b3Vec3 p2 = p[v2];
+		b3Vec3 p3 = p[v3];
+		b3Vec3 p4 = p[v4];
+
+		float32 P[16] =
+		{
+			p1.x, p1.y, p1.z, 1.0f,
+			p2.x, p2.y, p2.z, 1.0f,
+			p3.x, p3.y, p3.z, 1.0f,
+			p4.x, p4.y, p4.z, 1.0f
+		};
+
+		float32 P_invP[16];
+		b3Mul(P_invP, P, 4, 4, e->invP, 4, 4);
+
+		b3Mat33 A;
+		for (u32 i = 0; i < 3; ++i)
+		{
+			for (u32 j = 0; j < 3; ++j)
+			{
+				A(i, j) = P_invP[i + 4 * j];
+			}
+		}
+
+		b3Mat33 R;
+		b3ExtractRotation(R, e->q, A);
+
+		b3Mat33 RT = b3Transpose(R);
+
+		u32 vs[4] = { v1, v2, v3, v4 };
+
+		for (u32 i = 0; i < 4; ++i)
+		{
+			u32 vi = vs[i];
+
+			for (u32 j = 0; j < 4; ++j)
+			{
+				u32 vj = vs[j];
+
+				K(vi, vj) += R * Ke[i + 4 * j] * RT;
+			}
+		}
+
+		b3Vec3 x1 = x[v1];
+		b3Vec3 x2 = x[v2];
+		b3Vec3 x3 = x[v3];
+		b3Vec3 x4 = x[v4];
+
+		b3Vec3 xs[4] = { x1, x2, x3, x4 };
+
+		b3Vec3 f0s[4];
+
+		for (u32 i = 0; i < 4; ++i)
+		{
+			f0s[i].SetZero();
+
+			for (u32 j = 0; j < 4; ++j)
+			{
+				f0s[i] += R * Ke[i + 4 * j] * xs[j];
+			}
+		}
+
+		f0[v1] += f0s[0];
+		f0[v2] += f0s[1];
+		f0[v3] += f0s[2];
+		f0[v4] += f0s[3];
+	}
+
+	f0 = -f0;
+
+	b3SparseMat33 A = M + h * h * K;
+
+	b3DenseVec3 b = M * v - h * (K * p + f0 - fe);
+
+	// Solve Ax = b
+	b3DenseVec3 sx(m_mesh->vertexCount);
+	b3SolveMPCG(sx, A, b, x0, S);
+
+	// Solve constraints
+	b3SoftBodyContactSolverDef contactSolverDef;
+	contactSolverDef.allocator = m_allocator;
+	contactSolverDef.positions = &p;
+	contactSolverDef.velocities = &sx;
+	contactSolverDef.bodyContactCapacity = m_mesh->vertexCount;
+
+	b3SoftBodyContactSolver contactSolver(contactSolverDef);
+
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		if (m_nodes[i].m_bodyContact.active)
+		{
+			contactSolver.Add(&m_nodes[i].m_bodyContact);
+		}
+	}
+
+	{
+		contactSolver.InitializeBodyContactConstraints();
+	}
+
+	{
+		contactSolver.WarmStart();
+	}
+
+	// Solve velocity constraints
+	{
+		for (u32 i = 0; i < velocityIterations; ++i)
+		{
+			contactSolver.SolveBodyContactVelocityConstraints();
+		}
+	}
+
+	{
+		contactSolver.StoreImpulses();
+	}
+
+	// Integrate velocities
+	p = p + h * sx;
+
+	// Solve position constraints
+	{
+		bool positionSolved = false;
+		for (u32 i = 0; i < positionIterations; ++i)
+		{
+			bool bodyContactsSolved = contactSolver.SolveBodyContactPositionConstraints();
+
+			if (bodyContactsSolved)
+			{
+				positionSolved = true;
+				break;
+			}
+		}
+	}
+
+	// Synchronize bodies
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		b3NodeBodyContact* c = &n->m_bodyContact;
+
+		if (c->active == false)
+		{
+			continue;
+		}
+
+		b3Body* body = c->s2->GetBody();
+
+		body->SynchronizeTransform();
+
+		body->m_worldInvI = b3RotateToFrame(body->m_invI, body->m_xf.rotation);
+
+		body->SynchronizeShapes();
+	}
+
+	// Copy state buffers back to the nodes
+	for (u32 i = 0; i < m_mesh->vertexCount; ++i)
+	{
+		b3SoftBodyNode* n = m_nodes + i;
+
+		n->m_position = p[i];
+		n->m_velocity = sx[i];
+	}
+}
\ No newline at end of file