diff --git a/examples/testbed/framework/debug_draw.cpp b/examples/testbed/framework/debug_draw.cpp
index b54d8f3..292b122 100644
--- a/examples/testbed/framework/debug_draw.cpp
+++ b/examples/testbed/framework/debug_draw.cpp
@@ -64,7 +64,7 @@ b3Mat44 Camera::BuildProjectionMatrix() const
 b3Transform Camera::BuildWorldTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToMat(m_q);
+	xf.rotation = b3QuatMat33(m_q);
 	xf.position = (m_zoom * xf.rotation.z) - m_center;
 	return xf;
 }
@@ -78,7 +78,7 @@ b3Mat44 Camera::BuildWorldMatrix() const
 b3Transform Camera::BuildViewTransform() const
 {
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToMat(m_q);
+	xf.rotation = b3QuatMat33(m_q);
 	xf.position = (m_zoom * xf.rotation.z) - m_center;
 	return b3Inverse(xf);
 }
@@ -1281,6 +1281,65 @@ void DebugDraw::DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Co
 	m_solid->DrawSphere(radius, color, xf);
 }
 
+void DebugDraw::DrawCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Color& color)
+{
+	float32 height = b3Length(c1 - c2);
+
+	{
+		b3Transform xfc;
+		xfc.rotation.SetIdentity();
+		xfc.position = c1;
+		m_wire->DrawSphere(radius, color, xfc);
+	}
+
+	if (height > 0.0f)
+	{
+		DrawSegment(c1, c2, color);
+		
+		{
+			b3Transform xfc;
+			xfc.rotation.SetIdentity();
+			xfc.position = c2;
+			m_wire->DrawSphere(radius, color, xfc);
+		}
+	}
+}
+
+void DebugDraw::DrawSolidCapsule(const b3Vec3& c1, const b3Vec3& c2, float32 radius, const b3Color& c)
+{
+	float32 height = b3Length(c1 - c2);
+
+	{
+		b3Transform xfc;
+		xfc.rotation.SetIdentity();
+		xfc.position = c1;
+		m_solid->DrawSphere(radius, c, xfc);
+	}
+
+	if (height > 0.0f)
+	{
+		{
+			b3Mat33 R;
+			R.y = (1.0f / height) * (c1 - c2);
+			R.z = b3Perp(R.y);
+			R.x = b3Cross(R.y, R.z);
+
+			b3Transform xfc;
+			xfc.position = 0.5f * (c1 + c2);
+			xfc.rotation = R;
+
+			m_solid->DrawCylinder(radius, height, c, xfc);
+		}
+
+		{
+			b3Transform xfc;
+			xfc.rotation.SetIdentity();
+			xfc.position = c2;
+			m_solid->DrawSphere(radius, c, xfc);
+		}
+	}
+}
+
 void DebugDraw::DrawTransform(const b3Transform& xf)
 {
 	float32 lenght = 1.0f;
diff --git a/examples/testbed/framework/debug_draw.h b/examples/testbed/framework/debug_draw.h
index 868b2c0..ecc7a0d 100644
--- a/examples/testbed/framework/debug_draw.h
+++ b/examples/testbed/framework/debug_draw.h
@@ -103,6 +103,10 @@ public:
 	
 	void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color);
 
+	void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color);
+
+	void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color);
+
 	void DrawAABB(const b3AABB3& aabb, const b3Color& color);
 
 	void DrawTransform(const b3Transform& xf);
diff --git a/examples/testbed/framework/main.cpp b/examples/testbed/framework/main.cpp
index c613636..8d4cc44 100644
--- a/examples/testbed/framework/main.cpp
+++ b/examples/testbed/framework/main.cpp
@@ -64,14 +64,14 @@ static void MouseMove(GLFWwindow* w, double x, double y)
 		if (g_leftDown)
 		{
 			// Negate angles to do positive rotations (CCW) of the world.
-			float32 angleX = 0.005f * B3_PI * -nx;
 			float32 angleY = 0.005f * B3_PI * -ny;
+			float32 angleX = 0.005f * B3_PI * -nx;
+			
+			b3Quat qx = b3QuatRotationX(angleY);
+			b3Quat qy = b3QuatRotationY(angleX);
 
-			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), angleY);
-			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), angleX);
-
-			g_camera.m_q = qy * g_camera.m_q;
 			g_camera.m_q = g_camera.m_q * qx;
+			g_camera.m_q = qy * g_camera.m_q;
 			g_camera.m_q.Normalize();
 		}
 
diff --git a/examples/testbed/framework/test_entries.cpp b/examples/testbed/framework/test_entries.cpp
index bd7f554..d2a31b9 100644
--- a/examples/testbed/framework/test_entries.cpp
+++ b/examples/testbed/framework/test_entries.cpp
@@ -55,9 +55,12 @@
 #include <testbed/tests/body_types.h>
 #include <testbed/tests/varying_friction.h>
 #include <testbed/tests/varying_restitution.h>
-#include <testbed/tests/cloth_test.h>
 #include <testbed/tests/tumbler.h>
-#include <testbed/tests/pendulum.h>
+#include <testbed/tests/single_pendulum.h>
+#include <testbed/tests/multiple_pendulum.h>
+#include <testbed/tests/cloth_test.h>
+#include <testbed/tests/rope_test.h>
+//#include <testbed/tests/tree_test.h>
 
 TestEntry g_tests[] =
 {
@@ -97,9 +100,12 @@ TestEntry g_tests[] =
 	{ "Body Types", &BodyTypes::Create },
 	{ "Varying Friction", &VaryingFriction::Create },
 	{ "Varying Restitution", &VaryingRestitution::Create },
-	{ "Cloth", &Cloth::Create },
 	{ "Tumbler", &Tumbler::Create },
 	{ "Initial Overlap", &InitialOverlap::Create },
-	{ "Pendulum", &Pendulum::Create },
+	{ "Single Pendulum", &SinglePendulum::Create },
+	{ "Multiple Pendulum", &MultiplePendulum::Create },
+	{ "Cloth", &Cloth::Create },
+	{ "Rope", &Rope::Create },
+	//{ "Tree", &Tree::Create },
 	{ NULL, NULL }
 };
\ No newline at end of file
diff --git a/examples/testbed/tests/box_stack.h b/examples/testbed/tests/box_stack.h
index 34f0efa..af150b4 100644
--- a/examples/testbed/tests/box_stack.h
+++ b/examples/testbed/tests/box_stack.h
@@ -33,11 +33,11 @@ public:
 	{
 		g_camera.m_center.Set(2.5f, -2.0f, 5.5f);
 		g_camera.m_zoom = 40.0f;
-		
+
 		{
 			b3BodyDef bdef;
 			bdef.type = b3BodyType::e_staticBody;
-			
+
 			b3Body* body = m_world.CreateBody(bdef);
 
 			b3HullShape hs;
@@ -48,12 +48,12 @@ public:
 			sdef.friction = 1.0f;
 
 			body->CreateShape(sdef);
-		}		
+		}
 
 		b3Vec3 boxScale(1.0f, 1.0f, 1.0f);
-				
+
 		static b3BoxHull boxHull;
-		
+
 		b3Transform m;
 		m.rotation = b3Diagonal(boxScale.x, boxScale.y, boxScale.z);
 		m.position.SetZero();
@@ -75,7 +75,7 @@ public:
 					bdef.position.x = float32(i) * boxScale.x;
 					bdef.position.y = 2.5f * float32(j) * boxScale.y;
 					bdef.position.z = float32(k) * boxScale.z;
-					
+
 					bdef.position += stackOrigin;
 
 					b3Body* body = m_world.CreateBody(bdef);
diff --git a/examples/testbed/tests/capsule_and_hull_collision_1.h b/examples/testbed/tests/capsule_and_hull_collision_1.h
index 34d1243..a0a2282 100644
--- a/examples/testbed/tests/capsule_and_hull_collision_1.h
+++ b/examples/testbed/tests/capsule_and_hull_collision_1.h
@@ -25,14 +25,14 @@ public:
 	CapsuleAndHullCollision1()
 	{
 		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
-		m_xfA.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
+		m_xfA.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
 		
 		m_sA.m_centers[0].Set(1.0f, -1.0f, 0.0f);
 		m_sA.m_centers[1].Set(0.0f, 1.0f, 0.0f);
 		m_sA.m_radius = 2.0f;
 
 		m_xfB.position.Set(0.f, 0.0f, 0.0f);
-		m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
+		m_xfB.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
 
 		b3Transform xf;
 		xf.SetIdentity();
diff --git a/examples/testbed/tests/capsule_and_hull_collision_2.h b/examples/testbed/tests/capsule_and_hull_collision_2.h
index efaaee8..754cf92 100644
--- a/examples/testbed/tests/capsule_and_hull_collision_2.h
+++ b/examples/testbed/tests/capsule_and_hull_collision_2.h
@@ -25,14 +25,14 @@ public:
 	CapsuleAndHullCollision2()
 	{
 		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
-		m_xfA.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
+		m_xfA.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
 
 		m_sA.m_centers[0].Set(0.0f, 0.0f, 0.0f);
 		m_sA.m_centers[1].Set(0.0f, 0.0f, 0.0f);
 		m_sA.m_radius = 0.05f;
 
 		m_xfB.position.Set(0.f, 0.0f, 0.0f);
-		m_xfB.rotation = b3ConvertQuatToMat(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
+		m_xfB.rotation = b3QuatMat33(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
 
 		b3Transform xf;
 		xf.SetIdentity();
diff --git a/examples/testbed/tests/collide_test.h b/examples/testbed/tests/collide_test.h
index 351999d..ea025b2 100644
--- a/examples/testbed/tests/collide_test.h
+++ b/examples/testbed/tests/collide_test.h
@@ -92,7 +92,7 @@ public:
 		if (key == GLFW_KEY_X)
 		{
 			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3ConvertQuatToMat(qx);
+			b3Mat33 xfx = b3QuatMat33(qx);
 
 			m_xfB.rotation = m_xfB.rotation * xfx;
 		}
@@ -100,7 +100,7 @@ public:
 		if (key == GLFW_KEY_Y)
 		{
 			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3ConvertQuatToMat(qy);
+			b3Mat33 xfy = b3QuatMat33(qy);
 
 			m_xfB.rotation = m_xfB.rotation * xfy;
 		}
diff --git a/examples/testbed/tests/distance_test.h b/examples/testbed/tests/distance_test.h
index ec2cc46..9a3a906 100644
--- a/examples/testbed/tests/distance_test.h
+++ b/examples/testbed/tests/distance_test.h
@@ -103,7 +103,7 @@ public:
 		if (key == GLFW_KEY_X)
 		{
 			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfx = b3ConvertQuatToMat(qx);
+			b3Mat33 xfx = b3QuatMat33(qx);
 
 			m_xfB.rotation = m_xfB.rotation * xfx;
 		}
@@ -111,7 +111,7 @@ public:
 		if (key == GLFW_KEY_Y)
 		{
 			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
-			b3Mat33 xfy = b3ConvertQuatToMat(qy);
+			b3Mat33 xfy = b3QuatMat33(qy);
 
 			m_xfB.rotation = m_xfB.rotation * xfy;
 		}
diff --git a/examples/testbed/tests/multiple_pendulum.h b/examples/testbed/tests/multiple_pendulum.h
new file mode 100644
index 0000000..4713aea
--- /dev/null
+++ b/examples/testbed/tests/multiple_pendulum.h
@@ -0,0 +1,149 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MULTIPLE_PENDULUM
+#define MULTIPLE_PENDULUM
+
+class MultiplePendulum : public Test
+{
+public:
+	MultiplePendulum()
+	{
+		g_camera.m_zoom = 10.0f;
+
+		b3Vec3 axis(0.0f, 0.0f, 1.0f);
+
+		b3Body* bs[6];
+		{
+			b3BodyDef bd;
+			bd.type = e_staticBody;
+			bs[0] = m_world.CreateBody(bd);
+		}
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-0.5f, 0.0f, 0.0f);
+			bs[1] = m_world.CreateBody(bd);
+
+			b3CapsuleShape s;
+			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
+			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_radius = 0.05f;
+
+			b3ShapeDef sd;
+			sd.shape = &s;
+			sd.density = 10.0f;
+			bs[1]->CreateShape(sd);
+
+			b3RevoluteJointDef jd;
+			jd.Initialize(bs[0], bs[1], axis, b3Vec3(0.0f, 0.0f, 0.0f), 0.0f, 1.0f);
+			m_world.CreateJoint(jd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-1.5f, 0.0f, 0.0f);
+			bs[2] = m_world.CreateBody(bd);
+
+			b3CapsuleShape s;
+			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
+			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_radius = 0.05f;
+
+			b3ShapeDef sd;
+			sd.shape = &s;
+			sd.density = 10.0f;
+			bs[2]->CreateShape(sd);
+
+			b3RevoluteJointDef jd;
+			jd.Initialize(bs[1], bs[2], axis, b3Vec3(-1.0f, 0.0f, 0.0f), 0.0f, 1.0f);
+			m_world.CreateJoint(jd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-2.5f, 0.0f, 0.0f);
+			bs[3] = m_world.CreateBody(bd);
+
+			b3CapsuleShape s;
+			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
+			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_radius = 0.05f;
+
+			b3ShapeDef sd;
+			sd.shape = &s;
+			sd.density = 100.0f;
+			bs[3]->CreateShape(sd);
+
+			b3RevoluteJointDef jd;
+			jd.Initialize(bs[2], bs[3], axis, b3Vec3(-2.0f, 0.0f, 0.0f), 0.0f, 1.0f);
+			m_world.CreateJoint(jd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-3.5f, 0.0f, 0.0f);
+			bs[4] = m_world.CreateBody(bd);
+
+			b3CapsuleShape s;
+			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
+			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_radius = 0.05f;
+
+			b3ShapeDef sd;
+			sd.shape = &s;
+			sd.density = 1000.0f;
+			bs[4]->CreateShape(sd);
+
+			b3RevoluteJointDef jd;
+			jd.Initialize(bs[3], bs[4], axis, b3Vec3(-3.0f, 0.0f, 0.0f), 0.0f, 1.0f);
+			m_world.CreateJoint(jd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-4.5f, 0.0f, 0.0f);
+			bs[5] = m_world.CreateBody(bd);
+
+			b3CapsuleShape s;
+			s.m_centers[0].Set(0.5f, 0.0f, 0.0f);
+			s.m_centers[1].Set(-0.5f, 0.0f, 0.0f);
+			s.m_radius = 0.05f;
+
+			b3ShapeDef sd;
+			sd.shape = &s;
+			sd.density = 50.0f;
+			bs[5]->CreateShape(sd);
+
+			b3RevoluteJointDef jd;
+			jd.Initialize(bs[4], bs[5], axis, b3Vec3(-4.0f, 0.0f, 0.0f), 0.0f, 1.0f);
+			m_world.CreateJoint(jd);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new MultiplePendulum();
+	}
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/testbed/tests/rope_test.h b/examples/testbed/tests/rope_test.h
new file mode 100644
index 0000000..6699a14
--- /dev/null
+++ b/examples/testbed/tests/rope_test.h
@@ -0,0 +1,118 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be hebd 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 woubd 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 ROPE_TEST_H
+#define ROPE_TEST_H
+
+extern Settings g_settings;
+
+class Rope : public Test
+{
+public:
+	enum
+	{
+		e_count = 10
+	};
+
+	Rope()
+	{
+		g_camera.m_zoom = 30.0f;
+
+		b3Vec3 vs[e_count];
+		float32 ms[e_count];
+		
+		vs[0].Set(0.0f, 0.0f, 0.0f);
+		ms[0] = 0.0f;
+		
+		for (u32 i = 1; i < e_count; ++i)
+		{
+			ms[i] = 1.0f;
+			vs[i].Set(float32(i), 0.0f, 0.0f);
+		}
+
+		b3RopeDef rd;
+		rd.gravity.Set(0.0f, -10.0f, 0.0f);
+		rd.masses = ms;
+		rd.vertices = vs;
+		rd.count = e_count;
+
+		m_rope.Initialize(rd);
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_A)
+		{
+			m_rope.SetGravity(b3Vec3(-10.0f, 0.0f, 0.0f));
+		}
+
+		if (button == GLFW_KEY_D)
+		{
+			m_rope.SetGravity(b3Vec3(10.0f, 0.0f, 0.0f));
+		}
+
+		if (button == GLFW_KEY_S)
+		{
+			m_rope.SetGravity(b3Vec3(0.0f, 0.0f, 10.0f));
+		}
+
+		if (button == GLFW_KEY_W)
+		{
+			m_rope.SetGravity(b3Vec3(0.0f, 0.0f, -10.0f));
+		}
+
+		if (button == GLFW_KEY_Q)
+		{
+			m_rope.SetGravity(b3Vec3(0.0f, 10.0f, 0.0f));
+		}
+
+		if (button == GLFW_KEY_E)
+		{
+			m_rope.SetGravity(b3Vec3(0.0f, -10.0f, 0.0f));
+		}
+	}
+
+	void Step()
+	{
+		float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+
+		if (g_settings.pause)
+		{
+			if (g_settings.singleStep)
+			{
+				g_settings.singleStep = false;
+			}
+			else
+			{
+				dt = 0.0f;
+			}
+		}
+
+		m_rope.Step(dt);
+		m_rope.Draw(g_debugDraw);
+	}
+
+	static Test* Create()
+	{
+		return new Rope();
+	}
+
+	b3Rope m_rope;
+};
+
+#endif
\ No newline at end of file
diff --git a/examples/testbed/tests/pendulum.h b/examples/testbed/tests/single_pendulum.h
similarity index 95%
rename from examples/testbed/tests/pendulum.h
rename to examples/testbed/tests/single_pendulum.h
index 2420c2d..2432572 100644
--- a/examples/testbed/tests/pendulum.h
+++ b/examples/testbed/tests/single_pendulum.h
@@ -22,10 +22,10 @@
 extern Settings g_settings;
 extern DebugDraw* g_debugDraw;
 
-class Pendulum : public Test
+class SinglePendulum : public Test
 {
 public:
-	Pendulum()
+	SinglePendulum()
 	{
 		m_g = -10.0f;
 
@@ -34,7 +34,7 @@ public:
 		m_I = m_m * m_r * m_r;
 		
 		// Initial state
-		m_theta = 0.5f * B3_PI;
+		m_theta = -0.5f * B3_PI;
 		m_omega = 0.0f;
 	}
 
@@ -90,7 +90,7 @@ public:
 
 	static Test* Create()
 	{
-		return new Pendulum();
+		return new SinglePendulum();
 	}
 
 	// Gravity
diff --git a/include/bounce/bounce.h b/include/bounce/bounce.h
index 55d82b0..c5359ad 100644
--- a/include/bounce/bounce.h
+++ b/include/bounce/bounce.h
@@ -54,10 +54,18 @@
 #include <bounce/dynamics/contacts/convex_contact.h>
 #include <bounce/dynamics/contacts/mesh_contact.h>
 
+#include <bounce/dynamics/rope/rope.h>
+#include <bounce/dynamics/cloth/cloth.h>
 #include <bounce/dynamics/body.h>
+
+//#include <bounce/dynamics/tree/joints/tree_weld_joint.h>
+//#include <bounce/dynamics/tree/joints/tree_prismatic_joint.h>
+//#include <bounce/dynamics/tree/joints/tree_revolute_joint.h>
+//#include <bounce/dynamics/tree/joints/tree_spherical_joint.h>
+//#include <bounce/dynamics/tree/tree_body.h>
+//#include <bounce/dynamics/tree/body_tree.h>
+
 #include <bounce/dynamics/world.h>
 #include <bounce/dynamics/world_listeners.h>
 
-#include <bounce/cloth/cloth.h>
-
 #endif
\ No newline at end of file
diff --git a/include/bounce/common/draw.h b/include/bounce/common/draw.h
index 80f5750..7ce84bc 100644
--- a/include/bounce/common/draw.h
+++ b/include/bounce/common/draw.h
@@ -101,6 +101,12 @@ public :
 	// Draw a solid sphere with center and radius.
 	virtual void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0;
 	
+	// Draw a capsule with segment and radius.
+	virtual void DrawCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0;
+
+	// Draw a solid capsule with segment and radius.
+	virtual void DrawSolidCapsule(const b3Vec3& p1, const b3Vec3& p2, float32 radius, const b3Color& color) = 0;
+
 	// Draw a AABB.
 	virtual void DrawAABB(const b3AABB3& aabb, const b3Color& color) = 0;
 
diff --git a/include/bounce/common/geometry.h b/include/bounce/common/geometry.h
index 98b0b21..8cfc325 100644
--- a/include/bounce/common/geometry.h
+++ b/include/bounce/common/geometry.h
@@ -82,7 +82,7 @@ inline b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
 	return P - fraction * plane.normal;
 }
 
-// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v) 
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v) 
 // with respect to a segment AB.
 // The last output value is the divisor.
 inline void b3BarycentricCoordinates(float32 out[3], 
diff --git a/include/bounce/common/math/mat33.h b/include/bounce/common/math/mat33.h
index f8b7bd2..07d3452 100644
--- a/include/bounce/common/math/mat33.h
+++ b/include/bounce/common/math/mat33.h
@@ -56,6 +56,14 @@ struct b3Mat33
 		z += B.z;
 	}
 
+	// Subtract this matrix from a matrix.
+	void operator-=(const b3Mat33& B)
+	{
+		x -= B.x;
+		y -= B.y;
+		z -= B.z;
+	}
+	
 	// Set this matrix to the zero matrix.
 	void SetZero() 
 	{
@@ -81,6 +89,10 @@ struct b3Mat33
 	b3Vec3 x, y, z;
 };
 
+// Usefull constants.
+extern b3Mat33 b3Mat33_zero;
+extern b3Mat33 b3Mat33_identity;
+
 // Add two matrices.
 inline b3Mat33 operator+(const b3Mat33& A, const b3Mat33& B) 
 {
@@ -185,6 +197,11 @@ inline b3Mat33 b3Diagonal(float32 x, float32 y, float32 z)
 // returns the zero matrix.
 b3Mat33 b3Inverse(const b3Mat33& A);
 
+// Invert a symmetric matrix.
+// If the matrix is singular this 
+// returns the zero matrix.
+b3Mat33 b3SymInverse(const b3Mat33& A);
+
 // Return a skew (anti-symmetric) matrix for a vector.
 inline b3Mat33 b3Skew(const b3Vec3& v) 
 {
@@ -228,4 +245,43 @@ inline b3Mat33 b3Basis(const b3Vec3& a)
 	return A;
 }
 
-#endif
+// Rotation about the x-axis.
+inline b3Mat33 b3Mat33RotationX(float32 angle)
+{
+	float32 c = cos(angle);
+	float32 s = sin(angle);
+
+	b3Mat33 R;
+	R.x.Set(1.0f, 0.0f, 0.0f);
+	R.y.Set(0.0f, c, s);
+	R.z.Set(0.0f, -s, c);
+	return R;
+}
+
+// Rotation about the y-axis.
+inline b3Mat33 b3Mat33RotationY(float32 angle)
+{
+	float32 c = cos(angle);
+	float32 s = sin(angle);
+
+	b3Mat33 R;
+	R.x.Set(c, 0.0f, -s);
+	R.y.Set(0.0f, 1.0f, 0.0f);
+	R.z.Set(s, 0.0f, c);
+	return R;
+}
+
+// Rotation about the z-axis.
+inline b3Mat33 b3Mat33RotationZ(float32 angle)
+{
+	float32 c = cos(angle);
+	float32 s = sin(angle);
+
+	b3Mat33 R;
+	R.x.Set(c, s, 0.0f);
+	R.y.Set(-s, c, 0.0f);
+	R.z.Set(0.0f, 0.0f, 1.0f);
+	return R;
+}
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/common/math/quat.h b/include/bounce/common/math/quat.h
index a8d7fa6..4d5ebc1 100644
--- a/include/bounce/common/math/quat.h
+++ b/include/bounce/common/math/quat.h
@@ -147,7 +147,7 @@ inline b3Quat operator+(const b3Quat& a, const b3Quat& b)
 	return b3Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
 }
 
-// Sobtract two quaternions.
+// Subtract two quaternions.
 inline b3Quat operator-(const b3Quat& a, const b3Quat& b)
 {
 	return b3Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
@@ -165,8 +165,8 @@ inline b3Quat operator-(const b3Quat& q)
 	return b3Quat(-q.x, -q.y, -q.z, -q.w);
 }
 
-// Compute a quaternion-quaternion product.
-inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
+// Multiply two quaternions.
+inline b3Quat b3Mul(const b3Quat& a, const b3Quat& b)
 {
 	return b3Quat(
 		a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
@@ -175,7 +175,32 @@ inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
 		a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
 }
 
-// Compute the length of a quaternion.
+// Multiply two quaternions.
+inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
+{
+	return b3Mul(a, b);
+}
+
+// Perform the dot poduct of two quaternions.
+inline float32 b3Dot(const b3Quat& a, const b3Quat& b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+// Return the conjugate of a quaternion.
+// If the quaternion is unit this returns its inverse.
+inline b3Quat b3Conjugate(const b3Quat& q)
+{
+	return b3Quat(-q.x, -q.y, -q.z, q.w);
+}
+
+// Multiply the conjugate of a quaternion times another quaternion.
+inline b3Quat b3MulT(const b3Quat& a, const b3Quat& b)
+{
+	return b3Mul(b3Conjugate(a), b);
+}
+
+// Return the length of a quaternion.
 inline float32 b3Length(const b3Quat& q)
 {
 	return b3Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
@@ -193,18 +218,6 @@ inline b3Quat b3Normalize(const b3Quat& q)
 	return b3Quat(0.0f, 0.0f, 0.0f, 1.0f);
 }
 
-// Perform the dot poduct of two quaternions.
-inline float b3Dot(const b3Quat& a, const b3Quat& b)
-{
-	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
-}
-
-// Conjugate of a quaternion (inverse if the quaternion is unit).
-inline b3Quat b3Conjugate(const b3Quat& q)
-{
-	return b3Quat(-q.x, -q.y, -q.z, q.w);
-}
-
 // Rotate a vector.
 inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
 {
@@ -221,8 +234,8 @@ inline b3Vec3 b3MulT(const b3Quat& q, const b3Vec3& v)
 	return b3Mul(b3Conjugate(q), v);
 }
 
-// Convert a 3-by-3 rotation matrix to an rotation quaternion.
-inline b3Quat b3ConvertMatToQuat(const b3Mat33& m)
+// Convert a 3-by3 rotation matrix to a rotation quaternion.
+inline b3Quat b3Mat33Quat(const b3Mat33& m)
 {
 	// Check the diagonal.
 	float32 trace = m[0][0] + m[1][1] + m[2][2];
@@ -286,8 +299,8 @@ inline b3Quat b3ConvertMatToQuat(const b3Mat33& m)
 	return result;
 }
 
-// Convert an rotation quaternion to a 3-by-3 rotation matrix.
-inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q)
+// Convert a rotation quaternion to a 3-by-3 rotation matrix.
+inline b3Mat33 b3QuatMat33(const b3Quat& q)
 {
 	float32 x = q.x, y = q.y, z = q.z, w = q.w;
 	float32 x2 = x + x, y2 = y + y, z2 = z + z;
@@ -301,4 +314,43 @@ inline b3Mat33 b3ConvertQuatToMat(const b3Quat& q)
 		b3Vec3(         xz + wy,          yz - wx, 1.0f - (xx + yy)));
 }
 
+// Rotation about the x-axis.
+inline b3Quat b3QuatRotationX(float32 angle)
+{
+	float32 x = 0.5f * angle;
+
+	b3Quat q;
+	q.x = sin(x);
+	q.y = 0.0f;
+	q.z = 0.0f;
+	q.w = cos(x);
+	return q;
+}
+
+// Rotation about the y-axis.
+inline b3Quat b3QuatRotationY(float32 angle)
+{
+	float32 x = 0.5f * angle;
+
+	b3Quat q;
+	q.x = 0.0f;
+	q.y = sin(x);
+	q.z = 0.0f;
+	q.w = cos(x);
+	return q;
+}
+
+// Rotation about the z-axis.
+inline b3Quat b3QuatRotationZ(float32 angle)
+{
+	float32 x = 0.5f * angle;
+
+	b3Quat q;
+	q.x = 0.0f;
+	q.y = 0.0f;
+	q.z = sin(x);
+	q.w = cos(x);
+	return q;
+}
+
 #endif
\ No newline at end of file
diff --git a/include/bounce/common/math/transform.h b/include/bounce/common/math/transform.h
index a9a80eb..f3a45a8 100644
--- a/include/bounce/common/math/transform.h
+++ b/include/bounce/common/math/transform.h
@@ -23,49 +23,181 @@
 #include <bounce/common/math/quat.h>
 
 // A transform represents a rigid frame. 
-// It has a translation representing a position
-// and a rotation representing an orientation.
+// It has a translation representing a position 
+// and a rotation matrix representing an orientation 
+// relative to some reference frame.
 struct b3Transform 
 {
 	// Default ctor does nothing for performance.
 	b3Transform() { }
 	
-	// Set this transform from a translation vector and an orientation 
-	// quaternion.
-	b3Transform(const b3Vec3& p, const b3Quat& q)
+	// Set this transform from a rotation quaternion and a translation vector.
+	b3Transform(const b3Quat& _rotation, const b3Vec3& _translation)
 	{
-		position = p;
-		rotation = b3ConvertQuatToMat(q);
+		rotation = b3QuatMat33(_rotation);
+		position = _translation;
 	}
 	
-	// Set this transform to the identity.
+	// Set this transform to the identity transform.
 	void SetIdentity() 
 	{
-		position.SetZero();
 		rotation.SetIdentity();
+		position.SetZero();
 	}
 
-	b3Vec3 position; // in fact a translation
 	b3Mat33 rotation;
+	b3Vec3 position; // in fact a translation
 };
 
+// Multiply a transform times a vector.
+inline b3Vec3 b3Mul(const b3Transform& T, const b3Vec3& v)
+{
+	return b3Mul(T.rotation, v) + T.position;
+}
+
+// Multiply a transform times another transform.
+inline b3Transform b3Mul(const b3Transform& A, const b3Transform& B)
+{
+	// [A y][B x] = [AB Ax+y]
+	// [0 1][0 1]   [0  1   ]
+	b3Transform C;
+	C.rotation = b3Mul(A.rotation, B.rotation);
+	C.position = b3Mul(A.rotation, B.position) + A.position;
+	return C;
+}
+
+// Multiply the transpose of one transform (inverse 
+// transform) times another transform (composed transform).
+inline b3Transform b3MulT(const b3Transform& A, const b3Transform& B) 
+{
+	//[A^-1  -A^-1*y][B x] = [A^-1*B A^-1(x-y)]
+	//[0      1     ][0 1]   [0      1        ]
+	b3Transform C;
+	C.rotation = b3MulT(A.rotation, B.rotation);
+	C.position = b3MulT(A.rotation, B.position - A.position);
+	return C;
+}
+
+// Multiply the transpose of a transform times a vector.
+// If the transform represents a frame then this transforms
+// the vector from one frame to another (inverse transform).
+inline b3Vec3 b3MulT(const b3Transform& A, const b3Vec3& v)
+{
+	//[A^-1  -A^-1*y][x] = A^-1*x - A^-1*y = A^-1 * (x - y)
+	//[0     1      ][1]   
+	return b3MulT(A.rotation, v - A.position);
+}
+
+// Inverse transform.
+inline b3Transform b3Inverse(const b3Transform& T)
+{
+	b3Transform B;
+	B.rotation = b3Transpose(T.rotation);
+	B.position = b3MulT(T.rotation, -T.position);
+	return B;
+}
+
+// Multiply a transform times a vector. If the transform 
+// represents a frame this returns the vector in terms 
+// of the frame.
+inline b3Vec3 operator*(const b3Transform& T, const b3Vec3& v)
+{
+	return b3Mul(T, v);
+}
+
+// Multiply a transform times another transform (composed transform).
+inline b3Transform operator*(const b3Transform& A, const b3Transform& B)
+{
+	return b3Mul(A, B);
+}
+
+// A quaternion-based transform.
+struct b3TransformQT
+{
+	// Default ctor does nothing for performance.
+	b3TransformQT() { }
+
+	// Set this transform from a rotation matrix and a translation vector.
+	b3TransformQT(const b3Mat33& _rotation, const b3Vec3& _translation)
+	{
+		rotation = b3Mat33Quat(_rotation);
+		translation = _translation;
+	}
+
+	// Set this transform to the identity transform.
+	void SetIdentity()
+	{
+		rotation.SetIdentity();
+		translation.SetZero();
+	}
+
+	b3Quat rotation;
+	b3Vec3 translation;
+};
+
+// Convert a quaternion based transform to a matrix based transform. 
+inline b3Transform b3ConvertToTransform(const b3TransformQT& T)
+{
+	return b3Transform(T.rotation, T.translation);
+}
+
+// Multiply a transform times another transform.
+inline b3TransformQT b3Mul(const b3TransformQT& A, const b3TransformQT& B)
+{
+	b3TransformQT C;
+	C.rotation = b3Mul(A.rotation, B.rotation);
+	C.translation = b3Mul(A.rotation, B.translation) + A.translation;
+	return C;
+}
+
+// Multiply the transpose of one transform (inverse 
+// transform) times another transform (composed transform).
+inline b3TransformQT b3MulT(const b3TransformQT& A, const b3TransformQT& B)
+{
+	b3TransformQT C;
+	C.rotation = b3MulT(A.rotation, B.rotation);
+	C.translation = b3MulT(A.rotation, B.translation - A.translation);
+	return C;
+}
+
+inline b3TransformQT operator*(const b3TransformQT& A, const b3TransformQT& B)
+{
+	return b3Mul(A, B);
+}
+
+// Inverse transform a vector.
+inline b3Vec3 b3MulT(const b3TransformQT& A, const b3Vec3& v)
+{
+	return b3MulT(A.rotation, v - A.translation);
+}
+
+// Inverse transform.
+inline b3TransformQT b3Inverse(const b3TransformQT& T)
+{
+	b3TransformQT B;
+	B.rotation = b3Conjugate(T.rotation);
+	B.translation = b3MulT(T.rotation, -T.translation);
+	return B;
+}
+
 // Motion proxy for TOI computation.
 struct b3Sweep
 {
-	b3Vec3 localCenter; // local center
-		
-	b3Vec3 worldCenter0; // last world center
-	b3Quat orientation0; // last orientation
-	float32 t0; // last fraction between [0, 1]
-
-	b3Vec3 worldCenter; // world center
-	b3Quat orientation; // world orientation
-
 	// Get this sweep transform at a given time between [0, 1]
 	b3Transform GetTransform(float32 t) const;
 
 	// Advance to a new initial state.
 	void Advance(float32 t);
+
+	b3Vec3 localCenter; // local center
+
+	b3Quat orientation0; // last orientation
+	b3Vec3 worldCenter0; // last world center
+	
+	float32 t0; // last fraction between [0, 1]
+
+	b3Quat orientation; // world orientation
+	b3Vec3 worldCenter; // world center
 };
 
 inline b3Transform b3Sweep::GetTransform(float32 t) const
@@ -75,7 +207,7 @@ inline b3Transform b3Sweep::GetTransform(float32 t) const
 	q.Normalize();
 
 	b3Transform xf;
-	xf.rotation = b3ConvertQuatToMat(q);
+	xf.rotation = b3QuatMat33(q);
 	xf.position = c - b3Mul(q, localCenter);
 	return xf;
 }
@@ -90,71 +222,4 @@ inline void b3Sweep::Advance(float32 t)
 	t0 = t;
 }
 
-// Multiply a transform times a vector. If the transform 
-// represents a frame this returns the vector in terms 
-// of the frame.
-inline b3Vec3 operator*(const b3Transform& T, const b3Vec3& v)
-{
-	return b3Mul(T.rotation, v) + T.position;
-}
-
-// Multiply a transform times another transform (composed transform).
-// [A y][B x] = [AB Ax+y]
-// [0 1][0 1]   [0  1   ]
-inline b3Transform operator*(const b3Transform& A, const b3Transform& B)
-{
-	b3Transform C;
-	C.rotation = b3Mul(A.rotation, B.rotation);
-	C.position = b3Mul(A.rotation, B.position) + A.position;
-	return C;
-}
-
-// Multiply a transform times a vector.
-inline b3Vec3 b3Mul(const b3Transform& T, const b3Vec3& v)
-{
-	return b3Mul(T.rotation, v) + T.position;
-}
-
-// Multiply a transform times another transform.
-// [A y][B x] = [AB Ax+y]
-// [0 1][0 1]   [0  1   ]
-inline b3Transform b3Mul(const b3Transform& A, const b3Transform& B)
-{
-	b3Transform C;
-	C.rotation = b3Mul(A.rotation, B.rotation);
-	C.position = b3Mul(A.rotation, B.position) + A.position;
-	return C;
-}
-
-// Multiply the transpose of one transform (inverse 
-// transform) times another transform (composed transform).
-//[A^-1  -A^-1*y][B x] = [A^-1*B A^-1(x-y)]
-//[0      1     ][0 1]   [0      1        ]
-inline b3Transform b3MulT(const b3Transform& A, const b3Transform& B) 
-{
-	b3Transform C;
-	C.rotation = b3MulT(A.rotation, B.rotation);
-	C.position = b3MulT(A.rotation, B.position - A.position);
-	return C;
-}
-
-// Multiply the transpose of a transform times a vector.
-// If the transform represents a frame then this transforms
-// the vector from one frame to another (inverse transform).
-//[A^-1  -A^-1*y][x] = A^-1*x - A^-1*y = A^-1 * (x - y)
-//[0     1      ][1]   
-inline b3Vec3 b3MulT(const b3Transform& A, const b3Vec3& v)
-{
-	return b3MulT(A.rotation, v - A.position);
-}
-
-// Inverse transform.
-inline b3Transform b3Inverse(const b3Transform& T)
-{
-	b3Transform B;
-	B.rotation = b3Transpose(T.rotation);
-	B.position = b3MulT(T.rotation, -T.position);
-	return B;
-}
-
-#endif
+#endif
\ No newline at end of file
diff --git a/include/bounce/dynamics/body.h b/include/bounce/dynamics/body.h
index af0c867..b5384c7 100644
--- a/include/bounce/dynamics/body.h
+++ b/include/bounce/dynamics/body.h
@@ -106,12 +106,6 @@ struct b3BodyDef
 class b3Body
 {
 public:
-	// A world manages the body construction.
-	b3Body(const b3BodyDef& def, b3World* world);
-	
-	// A world manages the body destruction.
-	~b3Body() { }
-
 	// Get the type of the body.
 	b3BodyType GetType() const;
 
@@ -307,6 +301,9 @@ private:
 		e_fixedRotationZ = 0x0010,
 	};
 
+	b3Body(const b3BodyDef& def, b3World* world);
+	~b3Body() { }
+
 	// Destroy all shapes associated with the body.
 	void DestroyShapes();
 
@@ -429,7 +426,7 @@ inline void b3Body::SetTransform(const b3Vec3& position, const b3Vec3& axis, flo
 	b3Quat q = b3Quat(axis, angle);
 	
 	m_xf.position = position;
-	m_xf.rotation = b3ConvertQuatToMat(q);
+	m_xf.rotation = b3QuatMat33(q);
 
 	m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
 	m_sweep.orientation = q;
diff --git a/include/bounce/cloth/cloth.h b/include/bounce/dynamics/cloth/cloth.h
similarity index 100%
rename from include/bounce/cloth/cloth.h
rename to include/bounce/dynamics/cloth/cloth.h
diff --git a/include/bounce/dynamics/rope/rope.h b/include/bounce/dynamics/rope/rope.h
new file mode 100644
index 0000000..de3fc7a
--- /dev/null
+++ b/include/bounce/dynamics/rope/rope.h
@@ -0,0 +1,109 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_ROPE_H
+#define B3_ROPE_H
+
+#include <bounce/common/math/transform.h>
+
+class b3Draw;
+
+struct b3RopeBody;
+
+//
+struct b3RopeDef
+{
+	b3RopeDef()
+	{
+		vertices = NULL;
+		masses = NULL;
+		count = 0;
+		gravity.SetZero();
+		linearDamping = 0.6f;
+		angularDamping = 0.6f;
+	}
+
+	//
+	b3Vec3* vertices;
+
+	//
+	float32* masses;
+
+	//
+	u32 count;
+
+	//
+	b3Vec3 gravity;
+
+	//
+	float32 linearDamping;
+
+	//
+	float32 angularDamping;
+};
+
+//
+class b3Rope
+{
+public:
+	//
+	b3Rope();
+	
+	//
+	~b3Rope();
+
+	//
+	void Initialize(const b3RopeDef& def);
+	
+	//
+	void SetOrigin(const b3Vec3& position)
+	{
+		m_p = position;
+	}
+
+	//
+	void SetGravity(const b3Vec3& gravity)
+	{
+		m_gravity = gravity;
+	}
+
+	//
+	void Step(float32 dt);
+
+	//
+	void Draw(b3Draw* draw) const;
+private:
+	//
+	float32 m_kd1, m_kd2;
+
+	//
+	b3Vec3 m_gravity;
+
+	// Base
+	b3Vec3 m_v;
+	b3Vec3 m_w;
+
+	b3Vec3 m_p;
+	b3Quat m_q;
+
+	// 
+	u32 m_count;
+	b3RopeBody* m_links;	
+};
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/dynamics/shapes/shape.h b/include/bounce/dynamics/shapes/shape.h
index 9179556..29d89b2 100644
--- a/include/bounce/dynamics/shapes/shape.h
+++ b/include/bounce/dynamics/shapes/shape.h
@@ -59,12 +59,12 @@ struct b3ShapeDef
 
 struct b3MassData 
 {
+	// The center of mass of the shape relative to the shape's origin. 
+	b3Vec3 center;
+
 	// The mass of the shape in kilograms.
 	float32 mass;
 
-	// The shape center of mass relative to the shape's origin. 
-	b3Vec3 center;
-
 	// The rotational inertia of the shape about the shape's center of mass.
 	b3Mat33 I;
 };
diff --git a/include/bounce/dynamics/spatial.h b/include/bounce/dynamics/spatial.h
new file mode 100644
index 0000000..2c944ca
--- /dev/null
+++ b/include/bounce/dynamics/spatial.h
@@ -0,0 +1,340 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SPATIAL_H
+#define B3_SPATIAL_H
+
+#include <bounce/common/math/mat33.h>
+
+// A 6-by-1 motion vector.
+struct b3MotionVec
+{
+	b3MotionVec() { }
+
+	b3MotionVec(const b3Vec3& _w, const b3Vec3& _v)
+	{
+		w = _w;
+		v = _v;
+	}
+
+	void SetZero()
+	{
+		w.SetZero();
+		v.SetZero();
+	}
+
+	void operator+=(const b3MotionVec& b)
+	{
+		w += b.w;
+		v += b.v;
+	}
+
+	void operator-=(const b3MotionVec& b)
+	{
+		w -= b.w;
+		v -= b.v;
+	}
+	
+	b3Vec3 w, v;
+};
+
+// a + b
+inline b3MotionVec operator+(const b3MotionVec& a, const b3MotionVec& b)
+{
+	return b3MotionVec(a.w + b.w, a.v + b.v);
+}
+
+// a - b
+inline b3MotionVec operator-(const b3MotionVec& a, const b3MotionVec& b)
+{
+	return b3MotionVec(a.w - b.w, a.v - b.v);
+}
+
+// -a 
+inline b3MotionVec operator-(const b3MotionVec& a)
+{
+	return b3MotionVec(-a.w, -a.v);
+}
+
+// a * s
+inline b3MotionVec operator*(const b3MotionVec& a, float32 s)
+{
+	return b3MotionVec(s * a.w, s * a.v);
+}
+
+// s * a
+inline b3MotionVec operator*(float32 s, const b3MotionVec& a)
+{
+	return b3MotionVec(s * a.w, s * a.v);
+}
+
+// a / s
+inline b3MotionVec operator/(const b3MotionVec& a, float32 s)
+{
+	return b3MotionVec(a.w / s, a.v / s);
+}
+
+// a x b
+// [wx  0][w2] = [wx * w2 + 0 * v2] =  [wx * w2]
+// [vx wx][v2]   [vx * w2 + wx * v2]   [vx * w2 + wx * v2]
+inline b3MotionVec b3Cross(const b3MotionVec& a, const b3MotionVec& b)
+{
+	b3MotionVec result;
+	result.w = b3Cross(a.w, b.w);
+	result.v = b3Cross(a.v, b.w) + b3Cross(a.w, b.v);
+	return result;
+}
+
+// A 6-by-1 force vector.
+struct b3ForceVec
+{
+	b3ForceVec() { }
+
+	b3ForceVec(const b3Vec3& _n, const b3Vec3& _f)
+	{
+		n = _n;
+		f = _f;
+	}
+	
+	void SetZero()
+	{
+		n.SetZero();
+		f.SetZero();
+	}
+
+	void operator-=(const b3ForceVec& v)
+	{
+		n -= v.n;
+		f -= v.f;
+	}
+
+	void operator+=(const b3ForceVec& v)
+	{
+		n += v.n;
+		f += v.f;
+	}
+
+	b3Vec3 n, f;
+};
+
+// a + b
+inline b3ForceVec operator+(const b3ForceVec& a, const b3ForceVec& b)
+{
+	return b3ForceVec(a.n + b.n, a.f + b.f);
+}
+
+// a - b
+inline b3ForceVec operator-(const b3ForceVec& a, const b3ForceVec& b)
+{
+	return b3ForceVec(a.n - b.n, a.f - b.f);
+}
+
+// -a
+inline b3ForceVec operator-(const b3ForceVec& a)
+{
+	return b3ForceVec(-a.n, -a.f);
+}
+
+// a * s
+inline b3ForceVec operator*(const b3ForceVec& a, float32 s)
+{
+	return b3ForceVec(s * a.n, s * a.f);
+}
+
+// s * a
+inline b3ForceVec operator*(float32 s, const b3ForceVec& a)
+{
+	return b3ForceVec(s * a.n, s * a.f);
+}
+
+// a / s
+inline b3ForceVec operator/(const b3ForceVec& a, float32 s)
+{
+	return b3ForceVec(a.n / s, a.f / s);
+}
+
+// a^T = [a.b^T, a.a^T]
+// a^T * b = a.b * b.a + a.a * b.b
+inline float32 b3Dot(const b3MotionVec& a, const b3ForceVec& b)
+{
+	return b3Dot(a.v, b.n) + b3Dot(a.w, b.f);
+}
+
+// A 6-by-6 spatial inertia matrix stored as a block matrix.
+// A, B, C, D are the 3-by-3 matrices. D is not stored  
+// because it's defined as D = A^T.
+struct b3SpInertia
+{
+	b3SpInertia() { }
+
+	void SetZero()
+	{
+		A.SetZero();
+		B.SetZero();
+		C.SetZero();
+	}
+
+	// Set this matrix from mass and rotational inertia 
+	// about the local center of mass (zero vector).
+	void SetLocalInertia(float32 m, const b3Mat33& I)
+	{
+		A.SetZero();
+		B = b3Diagonal(m);
+		C = I;
+	}
+
+	void operator-=(const b3SpInertia& M)
+	{
+		A -= M.A;
+		B -= M.B;
+		C -= M.C;
+	}
+
+	void operator+=(const b3SpInertia& M)
+	{
+		A += M.A;
+		B += M.B;
+		C += M.C;
+	}
+	
+	// Solve Ax = b.
+	b3MotionVec Solve(const b3ForceVec& b) const;
+
+	b3Mat33 A, B, C;
+};
+
+inline b3MotionVec b3SpInertia::Solve(const b3ForceVec& b) const
+{
+	// Numerical Recipes, p. 77
+	// Block matrix inversion:
+	// https://en.wikipedia.org/wiki/Block_matrix#Block_matrix_inversion
+	b3Mat33 invA_A, invA_B, invA_C, invA_D;
+	
+	b3Mat33 D = b3Transpose(A);
+	b3Mat33 NinvB = -b3Inverse(B);
+		
+	invA_B = b3Inverse(D * NinvB * A + C);
+	invA_A = invA_B * D * NinvB;
+	invA_D = b3Transpose(invA_A);
+
+	b3Mat33 T = A * invA_A;
+	T[0][0] -= 1.0f;
+	T[1][1] -= 1.0f;
+	T[2][2] -= 1.0f;
+
+	invA_C = NinvB * T;
+
+	b3MotionVec x;
+	x.w = invA_A * b.n + invA_B * b.f;
+	x.v = invA_C * b.n + invA_D * b.f;
+	return x;
+}
+
+// M * v
+inline b3ForceVec operator*(const b3SpInertia& M, const b3MotionVec& v)
+{
+	b3ForceVec result;
+	result.n = M.A * v.w + M.B * v.v;
+	result.f = M.C * v.w + b3MulT(M.A, v.v);
+	return result;
+}
+
+// a * b^T
+inline b3SpInertia b3Outer(const b3ForceVec& a, const b3ForceVec& b)
+{
+	b3SpInertia result;
+	result.A = b3Outer(a.n, b.f);
+	result.B = b3Outer(a.n, b.n);
+	result.C = b3Outer(a.f, b.f);
+	return result;
+}
+
+// A spatial transformation matrix. This is a 
+// 6-by-6 matrix, but we represent it efficiently 
+// with a rotation matrix and a translation vector.
+struct b3SpTransform
+{
+	b3SpTransform() { }
+
+	b3SpTransform(const b3Mat33& _E, const b3Vec3& _r)
+	{
+		E = _E;
+		r = _r;
+	}
+
+	void SetIdentity()
+	{
+		E.SetIdentity();
+		r.SetZero();
+	}
+
+	b3Mat33 E;
+	b3Vec3 r;
+};
+
+// X * v
+inline b3MotionVec b3Mul(const b3SpTransform& X, const b3MotionVec& v)
+{
+	b3MotionVec result;
+	result.w = X.E * v.w;
+	result.v = -b3Cross(X.r, X.E * v.w) + X.E * v.v;
+	return result;
+}
+
+// X^-1 * v
+inline b3MotionVec b3MulT(const b3SpTransform& X, const b3MotionVec& v)
+{
+	b3MotionVec result;
+	result.w = b3MulT(X.E, v.w);
+	result.v = b3MulT(X.E, v.v + b3Cross(X.r, v.w));
+	return result;
+}
+
+// X * v
+inline b3ForceVec b3Mul(const b3SpTransform& X, const b3ForceVec& v)
+{
+	b3ForceVec result;
+	result.n = X.E * v.n;
+	result.f = -b3Cross(X.r, X.E * v.n) + X.E * v.f;
+	return result;
+}
+
+// X^-1 * v
+inline b3ForceVec b3MulT(const b3SpTransform& X, const b3ForceVec& v)
+{
+	b3ForceVec result;
+	result.n = b3MulT(X.E, v.n);
+	result.f = b3MulT(X.E, v.f + b3Cross(X.r, v.n));
+	return result;
+}
+
+// X^-1 * I
+inline b3SpInertia b3MulT(const b3SpTransform& X, const b3SpInertia& I)
+{
+	b3Mat33 E = X.E;
+	b3Mat33 ET = b3Transpose(X.E);
+	b3Mat33 rx = b3Skew(X.r);
+
+	b3SpInertia result;
+	result.A = ET * (I.A - I.B * rx) * E;
+	result.B = ET * I.B * E;
+	result.C = ET * (rx * (I.A - I.B * rx) + I.C - b3Transpose(I.A) * rx) * E;
+	return result;
+}
+
+#endif
\ No newline at end of file
diff --git a/include/bounce/dynamics/world.h b/include/bounce/dynamics/world.h
index ca71d91..54226e9 100644
--- a/include/bounce/dynamics/world.h
+++ b/include/bounce/dynamics/world.h
@@ -76,7 +76,7 @@ public:
 
 	// Remove a joint from the world and deallocate it from the memory.
 	void DestroyJoint(b3Joint* joint);
-
+	 
 	// Simulate a physics step.
 	// The function parameters are the ammount of time to simulate, 
 	// and the number of constraint solver iterations.
diff --git a/src/bounce/collision/gjk/gjk.cpp b/src/bounce/collision/gjk/gjk.cpp
index 4210c3a..7a143e7 100644
--- a/src/bounce/collision/gjk/gjk.cpp
+++ b/src/bounce/collision/gjk/gjk.cpp
@@ -26,7 +26,7 @@
 
 u32 b3_gjkCalls = 0, b3_gjkIters = 0, b3_gjkMaxIters = 0;
 
-// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v) 
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v) 
 // with respect to a segment AB.
 // The last output value is the divisor.
 static B3_FORCE_INLINE void b3Barycentric(float32 out[3], 
@@ -44,7 +44,7 @@ static B3_FORCE_INLINE void b3Barycentric(float32 out[3],
 	out[2] = out[0] + out[1];
 }
 
-// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w) 
+// Convert a point Q from Cartesian coordinates to Barycentric coordinates (u, v, w) 
 // with respect to a triangle ABC.
 // The last output value is the divisor.
 static B3_FORCE_INLINE void b3Barycentric(float32 out[4],
@@ -72,7 +72,7 @@ static B3_FORCE_INLINE void b3Barycentric(float32 out[4],
 	out[3] = out[0] + out[1] + out[2];
 }
 
-// Convert a point Q from euclidean coordinates to barycentric coordinates (u, v, w, x) 
+// 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.
 static B3_FORCE_INLINE void b3Barycentric(float32 out[5],
diff --git a/src/bounce/common/math/mat.cpp b/src/bounce/common/math/mat.cpp
index af561c8..619a958 100644
--- a/src/bounce/common/math/mat.cpp
+++ b/src/bounce/common/math/mat.cpp
@@ -19,6 +19,16 @@
 #include <bounce/common/math/mat22.h>
 #include <bounce/common/math/mat33.h>
 
+b3Mat33 b3Mat33_zero = b3Mat33(
+	b3Vec3(0.0f, 0.0f, 0.0f),
+	b3Vec3(0.0f, 0.0f, 0.0f),
+	b3Vec3(0.0f, 0.0f, 0.0f));
+
+b3Mat33 b3Mat33_identity = b3Mat33(
+	b3Vec3(1.0f, 0.0f, 0.0f),
+	b3Vec3(0.0f, 1.0f, 0.0f),
+	b3Vec3(0.0f, 0.0f, 1.0f));
+
 b3Vec2 b3Mat22::Solve(const b3Vec2& b) const
 {
 	// Cramer's rule
@@ -32,10 +42,10 @@ b3Vec2 b3Mat22::Solve(const b3Vec2& b) const
 		det = 1.0f / det;
 	}
 
-	b3Vec2 x;
-	x.x = det * (a22 * b.x - a12 * b.y);
-	x.y = det * (a11 * b.y - a21 * b.x);
-	return x;
+	b3Vec2 xn;
+	xn.x = det * (a22 * b.x - a12 * b.y);
+	xn.y = det * (a11 * b.y - a21 * b.x);
+	return xn;
 }
 
 b3Mat22 b3Inverse(const b3Mat22& A)
@@ -95,3 +105,32 @@ b3Mat33 b3Inverse(const b3Mat33& A)
 	}
 	return det * b3Adjucate(A);
 }
+
+b3Mat33 b3SymInverse(const b3Mat33& A)
+{
+	float32 det = b3Det(A.x, A.y, A.z);
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	float32 a11 = A.x.x, a12 = A.y.x, a13 = A.z.x;
+	float32 a22 = A.y.y, a23 = A.z.y;
+	float32 a33 = A.z.z;
+
+	b3Mat33 M;
+
+	M.x.x = det * (a22 * a33 - a23 * a23);
+	M.x.y = det * (a13 * a23 - a12 * a33);
+	M.x.z = det * (a12 * a23 - a13 * a22);
+
+	M.y.x = M.x.y;
+	M.y.y = det * (a11 * a33 - a13 * a13);
+	M.y.z = det * (a13 * a12 - a11 * a23);
+
+	M.z.x = M.x.z;
+	M.z.y = M.y.z;
+	M.z.z = det * (a11 * a22 - a12 * a12);
+
+	return M;
+}
\ No newline at end of file
diff --git a/src/bounce/dynamics/body.cpp b/src/bounce/dynamics/body.cpp
index 6c5786e..d9b2cbc 100644
--- a/src/bounce/dynamics/body.cpp
+++ b/src/bounce/dynamics/body.cpp
@@ -77,7 +77,7 @@ b3Body::b3Body(const b3BodyDef& def, b3World* world)
 	m_sweep.t0 = 0.0f;
 
 	m_xf.position = m_sweep.worldCenter;
-	m_xf.rotation = b3ConvertQuatToMat(m_sweep.orientation);
+	m_xf.rotation = b3QuatMat33(m_sweep.orientation);
 	
 	m_linearDamping = def.linearDamping;
 	m_angularDamping = def.angularDamping;
diff --git a/src/bounce/cloth/cloth.cpp b/src/bounce/dynamics/cloth/cloth.cpp
similarity index 99%
rename from src/bounce/cloth/cloth.cpp
rename to src/bounce/dynamics/cloth/cloth.cpp
index a8b53a6..37333f3 100644
--- a/src/bounce/cloth/cloth.cpp
+++ b/src/bounce/dynamics/cloth/cloth.cpp
@@ -16,7 +16,7 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include <bounce/cloth/cloth.h>
+#include <bounce/dynamics/cloth/cloth.h>
 #include <bounce/collision/shapes/mesh.h>
 #include <bounce/common/template/array.h>
 #include <bounce/common/draw.h>
diff --git a/src/bounce/dynamics/contacts/contact.cpp b/src/bounce/dynamics/contacts/contact.cpp
index c25f1dc..f476c58 100644
--- a/src/bounce/dynamics/contacts/contact.cpp
+++ b/src/bounce/dynamics/contacts/contact.cpp
@@ -104,8 +104,14 @@ void b3Contact::Update(b3ContactListener* listener)
 		}
 	}
 
-	// Update the contact state.
+	// Wake the bodies associated with the shapes if the contact has began.
+	if (isOverlapping != wasOverlapping)
+	{
+		bodyA->SetAwake(true);
+		bodyB->SetAwake(true);
+	}
 
+	// Update the contact state.
 	if (isOverlapping == true)
 	{
 		m_flags |= e_overlapFlag;
@@ -115,13 +121,6 @@ void b3Contact::Update(b3ContactListener* listener)
 		m_flags &= ~e_overlapFlag;;
 	}
 
-	// Wake the bodies associated with the shapes if the contact has began.
-	if (isOverlapping != wasOverlapping)
-	{
-		bodyA->SetAwake(true);
-		bodyB->SetAwake(true);
-	}
-
 	// Notify the contact listener the new contact state.
 	if (listener != NULL)
 	{
diff --git a/src/bounce/dynamics/contacts/contact_solver.cpp b/src/bounce/dynamics/contacts/contact_solver.cpp
index 9ad5ffa..059291c 100644
--- a/src/bounce/dynamics/contacts/contact_solver.cpp
+++ b/src/bounce/dynamics/contacts/contact_solver.cpp
@@ -176,11 +176,11 @@ void b3ContactSolver::InitializeConstraints()
 		b3Vec3 wB = m_velocities[indexB].w;
 
 		b3Transform xfA;
-		xfA.rotation = b3ConvertQuatToMat(qA);
+		xfA.rotation = b3QuatMat33(qA);
 		xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
 		
 		b3Transform xfB;
-		xfB.rotation = b3ConvertQuatToMat(qB);
+		xfB.rotation = b3QuatMat33(qB);
 		xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
 
 		for (u32 j = 0; j < manifoldCount; ++j)
@@ -532,11 +532,11 @@ bool b3ContactSolver::SolvePositionConstraints()
 				b3PositionConstraintPoint* pcp = pcm->points + k;
 
 				b3Transform xfA;
-				xfA.rotation = b3ConvertQuatToMat(qA);
+				xfA.rotation = b3QuatMat33(qA);
 				xfA.position = cA - b3Mul(xfA.rotation, localCenterA);
 
 				b3Transform xfB;
-				xfB.rotation = b3ConvertQuatToMat(qB);
+				xfB.rotation = b3QuatMat33(qB);
 				xfB.position = cB - b3Mul(xfB.rotation, localCenterB);
 
 				b3ContactPositionSolverPoint cpcp;
diff --git a/src/bounce/dynamics/contacts/mesh_contact.cpp b/src/bounce/dynamics/contacts/mesh_contact.cpp
index 8a08b4d..3071c93 100644
--- a/src/bounce/dynamics/contacts/mesh_contact.cpp
+++ b/src/bounce/dynamics/contacts/mesh_contact.cpp
@@ -72,7 +72,7 @@ void b3MeshContact::SynchronizeShapes()
 	b3Sweep* sweepA = &bodyA->m_sweep;
 	b3Transform xfA0;
 	xfA0.position = sweepA->worldCenter0;
-	xfA0.rotation = b3ConvertQuatToMat(sweepA->orientation0);
+	xfA0.rotation = b3QuatMat33(sweepA->orientation0);
 		
 	// Calculate the displacement of body A using its position at the last 
 	// time step and the current position.
diff --git a/src/bounce/dynamics/island.cpp b/src/bounce/dynamics/island.cpp
index da22566..bcd25c5 100644
--- a/src/bounce/dynamics/island.cpp
+++ b/src/bounce/dynamics/island.cpp
@@ -83,7 +83,7 @@ void b3Island::Add(b3Joint* j)
 }
 
 // Box2D
-static B3_FORCE_INLINE b3Vec3 b3SolveGyroscopic(const b3Quat& q, const b3Mat33& Ib, const b3Vec3& w1, float32 h)
+static B3_FORCE_INLINE b3Vec3 b3SolveGyro(const b3Quat& q, const b3Mat33& Ib, const b3Vec3& w1, float32 h)
 {
 	// Convert angular velocity to body coordinates
 	b3Vec3 w1b = b3MulT(q, w1);
@@ -149,7 +149,7 @@ void b3Island::Solve(const b3Vec3& gravity, float32 dt, u32 velocityIterations,
 			// I2 * (w2 - w1) + h * cross(w2, I2 * w2) = 0
 			// Toss out I2 from f using local I2 (constant) and local w1 
 			// to remove its time dependency.
-			b3Vec3 w2 = b3SolveGyroscopic(q, b->m_I, w, h);
+			b3Vec3 w2 = b3SolveGyro(q, b->m_I, w, h);
 			b3Vec3 dw2 = w2 - w;
 
 			w += dw1 + dw2;
diff --git a/src/bounce/dynamics/joints/revolute_joint.cpp b/src/bounce/dynamics/joints/revolute_joint.cpp
index 1f513ba..732ed54 100644
--- a/src/bounce/dynamics/joints/revolute_joint.cpp
+++ b/src/bounce/dynamics/joints/revolute_joint.cpp
@@ -180,7 +180,7 @@ void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB,
 	rotation.y = b3Perp(axis);
 	rotation.x = b3Cross(rotation.y, axis);
 
-	b3Quat q = b3ConvertMatToQuat(rotation);
+	b3Quat q = b3Mat33Quat(rotation);
 	float32 len = q.Normalize();
 	B3_ASSERT(len > B3_EPSILON);
 
@@ -651,24 +651,24 @@ bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data)
 
 b3Transform b3RevoluteJoint::GetFrameA() const
 {
-	b3Transform xf(m_localAnchorA, m_localRotationA);
+	b3Transform xf(m_localRotationA, m_localAnchorA);
 	return GetBodyA()->GetWorldFrame(xf);
 }
 
 b3Transform b3RevoluteJoint::GetFrameB() const
 {
-	b3Transform xf(m_localAnchorB, m_localRotationB);
+	b3Transform xf(m_localRotationB, m_localAnchorB);
 	return GetBodyB()->GetWorldFrame(xf);
 }
 
 b3Transform b3RevoluteJoint::GetLocalFrameA() const
 {
-	return b3Transform(m_localAnchorA, m_localRotationA);
+	return b3Transform(m_localRotationA, m_localAnchorA);
 }
 
 b3Transform b3RevoluteJoint::GetLocalFrameB() const
 {
-	return b3Transform(m_localAnchorB, m_localRotationB);
+	return b3Transform(m_localRotationB, m_localAnchorB);
 }
 
 bool b3RevoluteJoint::IsLimitEnabled() const
diff --git a/src/bounce/dynamics/rope/rope.cpp b/src/bounce/dynamics/rope/rope.cpp
new file mode 100644
index 0000000..af8e832
--- /dev/null
+++ b/src/bounce/dynamics/rope/rope.cpp
@@ -0,0 +1,505 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <bounce/dynamics/rope/rope.h>
+#include <bounce/dynamics/spatial.h>
+#include <bounce/common/draw.h>
+
+struct b3RopeBody
+{
+	b3RopeBody() { }
+
+	// J * v
+	b3MotionVec v_J() const
+	{
+		return m_S[0] * m_v.x + m_S[1] * m_v.y + m_S[2] * m_v.z;
+	}
+
+	// 
+	b3Transform X_J() const
+	{
+		// Rigid Body Dynamics Algorithms p. 86
+		// E = mat33(inv(p))
+		b3Quat E = b3Conjugate(m_p);
+
+		b3Transform X;
+		X.rotation = b3QuatMat33(E);
+		X.position.SetZero();
+		return X;
+	}
+
+	// Shared
+
+	// Body
+
+	//
+	float32 m_m, m_I;
+
+	// Joint
+
+	//
+	b3MotionVec m_S[3];
+
+	//
+	b3Transform m_X_i_J;
+
+	// 
+	b3Transform m_X_J_j;
+
+	//
+	b3Quat m_p;
+
+	//
+	b3Vec3 m_v;
+
+	// Temp
+
+	//
+	b3SpTransform m_X_i_j;
+
+	//
+	b3MotionVec m_sv;
+
+	//
+	b3MotionVec m_sc;
+
+	//
+	b3SpInertia m_I_A;
+
+	//
+	b3ForceVec m_F_A;
+
+	//
+	b3ForceVec m_U[3];
+
+	//
+	b3Mat33 m_invD;
+
+	//
+	b3Vec3 m_u;
+
+	//
+	b3Vec3 m_a;
+
+	//
+	b3MotionVec m_sa;
+
+	//
+	b3Transform m_invX;
+
+	//
+	b3Transform m_X;
+};
+
+b3Rope::b3Rope()
+{
+	m_gravity.SetZero();
+	m_kd1 = 0.0f;
+	m_kd2 = 0.0f;
+	m_links = NULL;
+	m_count = 0;
+}
+
+b3Rope::~b3Rope()
+{
+	b3Free(m_links);
+}
+
+void b3Rope::Initialize(const b3RopeDef& def)
+{
+	B3_ASSERT(def.count > 0);
+
+	m_gravity = def.gravity;
+	m_kd1 = def.linearDamping;
+	m_kd2 = def.angularDamping;
+	m_count = def.count;
+	m_links = (b3RopeBody*)b3Alloc(m_count * sizeof(b3RopeBody));
+
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3RopeBody* b = m_links + i;
+		
+		float32 m = def.masses[i];
+		
+		// Simplify r = 1
+		b->m_m = m;
+		b->m_I = m * 0.4f;
+	}
+
+	m_v.SetZero();
+	m_w.SetZero();
+	m_p = def.vertices[0];
+	m_q.SetIdentity();
+
+	m_links[0].m_X.rotation = b3QuatMat33(m_q);
+	m_links[0].m_X.position = m_p;
+
+	for (u32 i = 1; i < m_count; ++i)
+	{
+		b3RopeBody* b = m_links + i;
+		b3RopeBody* b0 = b - 1;
+		b3Vec3 p = def.vertices[i];
+		b3Vec3 p0 = def.vertices[i - 1];
+
+		b->m_X.rotation.SetIdentity();
+		b->m_X.position = p;
+
+		// Set the joint anchor to the parent body position to simulate a rope.
+		b3Transform X_J;
+		X_J.rotation.SetIdentity();
+		X_J.position = p0;
+
+		b->m_X_i_J = b3MulT(X_J, b0->m_X);
+		
+		b->m_X_J_j = b3MulT(b->m_X, X_J);
+
+		b3Vec3 d = -b->m_X_J_j.position;
+
+		b3Vec3 w1(1.0f, 0.0f, 0.0f);
+		b3Vec3 w2(0.0f, 1.0f, 0.0f);
+		b3Vec3 w3(0.0f, 0.0f, 1.0f);
+
+		b3Vec3 v1 = b3Cross(w1, d);
+		b3Vec3 v2 = b3Cross(w2, d);
+		b3Vec3 v3 = b3Cross(w3, d);
+		
+		b->m_S[0].w = w1;
+		b->m_S[0].v = v1;
+
+		b->m_S[1].w = w2;
+		b->m_S[1].v = v2;
+
+		b->m_S[2].w = w3;
+		b->m_S[2].v = v3;
+
+		b->m_p.SetIdentity();
+		b->m_v.SetZero();
+	}
+}
+
+void b3Rope::Step(float32 h)
+{
+	if (m_count == 0)
+	{
+		return;
+	}
+
+	// Propagate down.
+	{
+		b3RopeBody* b = m_links;
+		b3Mat33 I = b3Diagonal(b->m_I);
+
+		b->m_invX = b3Inverse(b->m_X);
+
+		// Convert global velocity to local velocity.
+		b->m_sv.w = b->m_invX.rotation * m_w;
+		b->m_sv.v = b->m_invX.rotation * m_v;
+
+		if (b->m_m == 0.0f)
+		{
+			b->m_I_A.SetZero();
+			b->m_F_A.SetZero();
+		}
+		else
+		{
+			// Uniform inertia results in zero angular momentum.
+			b3ForceVec Pdot;
+			Pdot.n = b3Cross(b->m_sv.w, b->m_m * b->m_sv.v);
+			Pdot.f.SetZero();
+
+			// Convert global force to local force.
+			b3ForceVec F;
+			F.n = b->m_invX.rotation * m_gravity;
+			F.f.SetZero();
+			
+			// Damping force
+			b3ForceVec Fd;
+			Fd.n = -m_kd1 * b->m_m * b->m_sv.v;
+			Fd.f = -m_kd2 * b->m_I * b->m_sv.w;
+
+			b->m_I_A.SetLocalInertia(b->m_m, I);
+			b->m_F_A = Pdot - (F + Fd);
+		}
+	}
+
+	for (u32 i = 1; i < m_count; ++i)
+	{
+		b3RopeBody* link = m_links + i;
+		b3RopeBody* parent = link - 1;
+		b3Mat33 I = b3Diagonal(link->m_I);
+
+		b3Transform X_J = link->X_J();
+		b3Transform X_i_j = link->m_X_J_j * X_J * link->m_X_i_J;
+
+		link->m_invX = X_i_j * parent->m_invX;
+
+		// Flip the translation because r should be the vector 
+		// from the center of mass of the parent link to the 
+		// center of mass of this link in this link's frame.
+		link->m_X_i_j.E = X_i_j.rotation;
+		link->m_X_i_j.r = -X_i_j.position;
+
+		b3MotionVec joint_v = link->v_J();
+		
+		b3MotionVec parent_v = b3Mul(link->m_X_i_j, parent->m_sv);
+
+		link->m_sv = parent_v + joint_v;
+		
+		// v x jv
+		link->m_sc.w = b3Cross(link->m_sv.w, joint_v.w);
+		link->m_sc.v = b3Cross(link->m_sv.v, joint_v.w) + b3Cross(link->m_sv.w, joint_v.v);
+
+		// Uniform inertia results in zero angular momentum.
+		b3ForceVec Pdot;
+		Pdot.n = b3Cross(link->m_sv.w, link->m_m * link->m_sv.v);
+		Pdot.f.SetZero();
+
+		// Damping force
+		b3ForceVec Fd;
+		Fd.n = -m_kd1 * link->m_m * link->m_sv.v;
+		Fd.f = -m_kd2 * link->m_I * link->m_sv.w;
+
+		// Convert global force to local force.
+		b3ForceVec F;
+		F.n = link->m_invX.rotation * m_gravity;
+		F.f.SetZero();
+				
+		link->m_I_A.SetLocalInertia(link->m_m, I);
+		link->m_F_A = Pdot - (F + Fd);
+	}
+	
+	// Propagate up bias forces and inertias.
+	for (u32 j = m_count - 1; j >= 1; --j)
+	{
+		b3RopeBody* link = m_links + j;
+		b3RopeBody* parent = link - 1;
+		b3MotionVec* S = link->m_S;
+		b3MotionVec& c = link->m_sc;
+		
+		b3SpInertia& I_A = link->m_I_A;
+		b3ForceVec& F_A = link->m_F_A;
+
+		b3ForceVec* U = link->m_U;
+		b3Vec3& u = link->m_u;
+				
+		// U
+		U[0] = I_A * S[0];
+		U[1] = I_A * S[1];
+		U[2] = I_A * S[2];
+
+		// D = S^T * U
+		b3Mat33 D;
+
+		D.x.x = b3Dot(S[0], U[0]);
+		D.x.y = b3Dot(S[1], U[0]);
+		D.x.z = b3Dot(S[2], U[0]);
+
+		D.y.x = D.x.y;
+		D.y.y = b3Dot(S[1], U[1]);
+		D.y.z = b3Dot(S[2], U[1]);
+
+		D.z.x = D.x.z;
+		D.z.y = D.y.z;
+		D.z.z = b3Dot(S[2], U[2]);
+
+		// D^-1
+		b3Mat33 invD = b3SymInverse(D);
+		link->m_invD = invD;
+
+		// U * D^-1
+		b3ForceVec U_invD[3];
+		U_invD[0] = invD[0][0] * U[0] + invD[0][1] * U[1] + invD[0][2] * U[2];
+		U_invD[1] = invD[1][0] * U[0] + invD[1][1] * U[1] + invD[1][2] * U[2];
+		U_invD[2] = invD[2][0] * U[0] + invD[2][1] * U[1] + invD[2][2] * U[2];
+
+		// I_a = I_A - U * D^-1 * U^T
+		b3SpInertia M1 = b3Outer(U[0], U_invD[0]);
+		b3SpInertia M2 = b3Outer(U[1], U_invD[1]);
+		b3SpInertia M3 = b3Outer(U[2], U_invD[2]);
+
+		b3SpInertia I_a = link->m_I_A;
+		I_a -= M1;
+		I_a -= M2;
+		I_a -= M3;
+
+		// u = tau - S^T * F_A
+		u[0] = -b3Dot(S[0], F_A);
+		u[1] = -b3Dot(S[1], F_A);
+		u[2] = -b3Dot(S[2], F_A);
+
+		// U * D^-1 * u
+		b3ForceVec U_invD_u = U_invD[0] * u[0] + U_invD[1] * u[1] + U_invD[2] * u[2];
+		
+		// F_a = F_A + I_a * c + U * D^-1 * u
+		b3ForceVec F_a = link->m_F_A + I_a * link->m_sc + U_invD_u;
+
+		b3SpInertia I_a_i = b3MulT(link->m_X_i_j, I_a);
+		b3ForceVec F_a_i = b3MulT(link->m_X_i_j, F_a);
+
+		parent->m_I_A += I_a_i;
+		parent->m_F_A += F_a_i;
+	}
+
+	// Propagate down accelerations
+	{
+		b3RopeBody* body = m_links;
+
+		if (body->m_m == 0.0f)
+		{
+			body->m_sa.SetZero();
+		}
+		else
+		{
+			// a = I^-1 * F 
+			if (m_count == 1)
+			{
+				float32 inv_m = body->m_m > 0.0f ? 1.0f / body->m_m : 0.0f;
+				float32 invI = body->m_I > 0.0f ? 1.0f / body->m_I : 0.0f;
+
+				body->m_sa.w = -invI * body->m_F_A.f;
+				body->m_sa.v = -inv_m * body->m_F_A.n;
+			}
+			else
+			{
+				body->m_sa = body->m_I_A.Solve(-body->m_F_A);
+			}
+		}
+	}
+
+	for (u32 j = 1; j < m_count; ++j)
+	{
+		b3RopeBody* link = m_links + j;
+		b3RopeBody* parent = link - 1;
+		b3MotionVec* S = link->m_S;
+		b3MotionVec c = link->m_sc;
+		b3ForceVec* U = link->m_U;
+		b3Vec3 u = link->m_u;
+		
+		b3MotionVec parent_a = b3Mul(link->m_X_i_j, parent->m_sa);
+		b3MotionVec a = parent_a + c;
+
+		// u - U^T * a
+		b3Vec3 b;
+		b[0] = u[0] - b3Dot(a, U[0]);
+		b[1] = u[1] - b3Dot(a, U[1]);
+		b[2] = u[2] - b3Dot(a, U[2]);
+
+		// D^-1 * b
+		link->m_a = link->m_invD * b;
+		
+		b3MotionVec joint_a = S[0] * link->m_a[0] + S[1] * link->m_a[1] + S[2] * link->m_a[2];
+
+		link->m_sa = a + joint_a;
+	}
+	
+	// Integrate
+	float32 max_w = 8.0f * 2.0f * B3_PI;
+	b3Vec3 min(-max_w, -max_w, -max_w);
+	b3Vec3 max(max_w, max_w, max_w);
+
+	{
+		b3RopeBody* b = m_links;
+
+		// Integrate acceleration
+		
+		// Convert local to global acceleration
+		b3Vec3 v_dot = b3Mul(m_q, b->m_sa.v);
+		b3Vec3 w_dot = b3Mul(m_q, b->m_sa.w);
+
+		m_v += h * v_dot;
+		m_w += h * w_dot;
+		
+		// Integrate velocity		
+		m_p += h * m_v;
+
+		b3Quat q_w(m_w.x, m_w.y, m_w.z, 0.0f);
+		b3Quat q_dot = 0.5f * q_w * m_q;
+		m_q += h * q_dot;
+		m_q.Normalize();
+
+		// Synchronize transform
+		b->m_X.rotation = b3QuatMat33(m_q);
+		b->m_X.position = m_p;
+	}
+	
+	for (u32 i = 1; i < m_count; ++i)
+	{
+		b3RopeBody* link = m_links + i;
+		
+		// Integrate acceleration
+		link->m_v += h * link->m_a;
+
+		// Avoid numerical instability due to large velocities
+		link->m_v = b3Clamp(link->m_v, min, max);
+
+		// 
+		b3Quat q_w(link->m_v.x, link->m_v.y, link->m_v.z, 0.0f);
+		b3Quat q_dot = 0.5f * link->m_p * q_w;
+
+		link->m_p += h * q_dot;
+		link->m_p.Normalize();
+	}
+
+	// Propagate down transforms
+	m_links->m_invX = b3Inverse(m_links->m_X);
+
+	for (u32 j = 1; j < m_count; ++j)
+	{
+		b3RopeBody* link = m_links + j;
+		b3RopeBody* parent = link - 1;
+
+		b3Transform X_J = link->X_J();
+		b3Transform X_i_j = link->m_X_J_j * X_J * link->m_X_i_J;
+
+		link->m_invX = X_i_j * parent->m_invX;
+		link->m_X = b3Inverse(link->m_invX);
+	}
+}
+
+void b3Rope::Draw(b3Draw* draw) const
+{
+	if (m_count == 0)
+	{
+		return;
+	}
+
+	{
+		b3RopeBody* b = m_links;
+		
+		draw->DrawTransform(b->m_X);
+		draw->DrawSolidSphere(b->m_X.position, 0.2f, b3Color_green);
+	}
+
+	for (u32 i = 1; i < m_count; ++i)
+	{
+		b3RopeBody* b = m_links + i;
+		b3RopeBody* b0 = b - 1;
+
+		b3Transform X_J = b0->m_X * b3Inverse(b->m_X_i_J);
+		b3Transform X_J0 = b->m_X * b->m_X_J_j;
+		
+		draw->DrawTransform(X_J);
+		draw->DrawPoint(X_J.position, 5.0f, b3Color_red);
+
+		draw->DrawTransform(X_J0);
+		draw->DrawPoint(X_J0.position, 5.0f, b3Color_red);
+
+		draw->DrawTransform(b->m_X);
+		draw->DrawSolidSphere(b->m_X.position, 0.2f, b3Color_green);
+	}
+}
\ No newline at end of file