first commit

src/bounce/collision/broad_phase.cpp

@@ -0,0 +1,163 @@
+/*
+* 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\collision\broad_phase.h>
+
+b3BroadPhase::b3BroadPhase() 
+{
+	m_moveBufferCapacity = 16;
+	m_moveBuffer = (i32*)b3Alloc(m_moveBufferCapacity * sizeof(i32));
+	memset(m_moveBuffer, NULL, m_moveBufferCapacity * sizeof(i32));
+	m_moveBufferCount = 0;
+
+	m_pairBufferCapacity = 16;
+	m_pairBuffer = (b3Pair*)b3Alloc(m_pairBufferCapacity * sizeof(b3Pair));
+	memset(m_pairBuffer, NULL, m_pairBufferCapacity * sizeof(b3Pair));
+	m_pairBufferCount = 0;
+}
+
+b3BroadPhase::~b3BroadPhase() 
+{
+	b3Free(m_moveBuffer);
+	b3Free(m_pairBuffer);
+}
+
+void b3BroadPhase::BufferMove(i32 proxyId) 
+{
+	// The proxy has been moved. Add it to the buffer of moved proxies.
+	// Check capacity.
+	if (m_moveBufferCount == m_moveBufferCapacity) 
+	{
+		// Duplicate capacity.
+		m_moveBufferCapacity *= 2;
+
+		i32* oldMoveBuffer = m_moveBuffer;
+		m_moveBuffer = (i32*)b3Alloc(m_moveBufferCapacity * sizeof(i32));
+		memcpy(m_moveBuffer, oldMoveBuffer, m_moveBufferCount * sizeof(i32));
+		b3Free(oldMoveBuffer);
+	}
+
+	// Add to move buffer.
+	m_moveBuffer[m_moveBufferCount] = proxyId;
+	++m_moveBufferCount;
+}
+
+bool b3BroadPhase::TestOverlap(i32 proxy1, i32 proxy2) const 
+{
+	return m_tree.TestOverlap(proxy1, proxy2);
+}
+
+i32 b3BroadPhase::CreateProxy(const b3AABB3& aabb, void* userData) 
+{
+	// Later, if the node aabb has changed then it should be reinserted into the tree.
+	// However, this can be expansive due to the hierarchy reconstruction.
+	// Therefore, the original AABB is extended and inserted into the tree,
+	// so we can check later if the new (original) AABB is inside the old (fat) AABB.
+	b3AABB3 fatAABB = aabb;
+	fatAABB.Extend(B3_AABB_EXTENSION);	
+	i32 proxyId = m_tree.InsertNode(fatAABB, userData);
+	BufferMove(proxyId);
+	return proxyId;
+}
+
+void b3BroadPhase::DestroyProxy(i32 proxyId) 
+{
+	return m_tree.RemoveNode(proxyId);
+}
+
+bool b3BroadPhase::MoveProxy(i32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement)
+{
+	if (m_tree.GetAABB(proxyId).Contains(aabb))
+	{
+		// Do nothing if the new AABB is contained in the old AABB.
+		return false;
+	}
+
+	// Update the tree with a fat and motion predicted AABB.
+	const b3Vec3 kExtension(B3_AABB_EXTENSION, B3_AABB_EXTENSION, B3_AABB_EXTENSION);
+
+	// Extend the new (original) AABB.
+	b3AABB3 fatAABB;
+	fatAABB.m_lower = aabb.m_lower - kExtension;
+	fatAABB.m_upper = aabb.m_upper + kExtension;
+
+	if (displacement.x < 0.0f)
+	{
+		fatAABB.m_lower.x += B3_AABB_MULTIPLIER * displacement.x;
+	}
+	else
+	{
+		fatAABB.m_upper.x += B3_AABB_MULTIPLIER * displacement.x;
+	}
+
+	if (displacement.y < 0.0f)
+	{
+		fatAABB.m_lower.y += B3_AABB_MULTIPLIER * displacement.y;
+	}
+	else
+	{
+		fatAABB.m_upper.y += B3_AABB_MULTIPLIER * displacement.y;
+	}
+
+	if (displacement.z < 0.0f)
+	{
+		fatAABB.m_lower.z += B3_AABB_MULTIPLIER * displacement.z;
+	}
+	else
+	{
+		fatAABB.m_upper.z += B3_AABB_MULTIPLIER * displacement.z;
+	}
+
+	// Update proxy with the extented AABB.
+	m_tree.UpdateNode(proxyId, fatAABB);
+	
+	// Buffer the moved proxy.
+	BufferMove(proxyId);
+	
+	// Notify the proxy has moved.
+	return true;
+}
+
+bool b3BroadPhase::Report(i32 proxyId) 
+{
+	if (proxyId == m_queryProxyId) 
+	{
+		// The proxy can't overlap with itself.
+		return true;
+	}
+
+	// Check capacity.
+	if (m_pairBufferCount == m_pairBufferCapacity) 
+	{
+		// Duplicate capacity.
+		m_pairBufferCapacity *= 2;
+		
+		b3Pair* oldPairBuffer = m_pairBuffer;
+		m_pairBuffer = (b3Pair*)b3Alloc(m_pairBufferCapacity * sizeof(b3Pair));
+		memcpy(m_pairBuffer, oldPairBuffer, m_pairBufferCount * sizeof(b3Pair));
+		b3Free(oldPairBuffer);
+	}
+
+	// Add overlapping pair to the pair buffer.
+	m_pairBuffer[m_pairBufferCount].proxy1 = b3Min(proxyId, m_queryProxyId);
+	m_pairBuffer[m_pairBufferCount].proxy2 = b3Max(proxyId, m_queryProxyId);
+	++m_pairBufferCount;
+
+	// Keep looking for overlapping pairs.
+	return true;
+}

src/bounce/collision/distance.cpp

@@ -0,0 +1,238 @@
+/*
+* 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\collision\distance.h>
+#include <bounce\common\math\vec2.h>
+#include <bounce\common\math\vec3.h>
+#include <bounce\common\math\mat22.h>
+
+b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane)
+{
+	return b3Project(P, plane);
+}
+
+b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P,
+	const b3Vec3& A, const b3Vec3& B)
+{
+	float32 wAB[3];
+	b3Barycentric(wAB, A, B, P);
+
+	if (wAB[1] <= 0.0f)
+	{
+		return A;
+	}
+
+	if (wAB[0] <= 0.0f)
+	{
+		return B;
+	}
+
+	float32 s = 1.0f / wAB[2];
+	float32 wA = s * wAB[0];
+	float32 wB = s * wAB[1];
+	return wA * A + wB * B;
+}
+
+b3Vec3 b3ClosestPointOnTriangle(const b3Vec3& P,
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C)
+{
+	// Test vertex regions
+	float32 wAB[3], wBC[3], wCA[3];
+	b3Barycentric(wAB, A, B, P);
+	b3Barycentric(wBC, B, C, P);
+	b3Barycentric(wCA, C, A, P);
+
+	// R A
+	if (wAB[1] <= 0.0f && wCA[0] <= 0.0f)
+	{
+		return A;
+	}
+
+	// R B
+	if (wAB[0] <= 0.0f && wBC[2] <= 0.0f)
+	{
+		return B;
+	}
+
+	// R C
+	if (wBC[0] <= 0.0f && wCA[1] <= 0.0f)
+	{
+		return C;
+	}
+
+	// Test edge regions
+	float32 wABC[4];
+	b3Barycentric(wABC, A, B, C, P);
+
+	// R AB
+	if (wAB[0] > 0.0f && wAB[1] > 0.0f && wABC[2] <= 0.0f)
+	{
+		float32 s = 1.0f / wAB[2];
+		float32 wA = s * wAB[0];
+		float32 wB = s * wAB[1];
+		return wA * A + wB * B;
+	}
+
+	// R BC
+	if (wBC[0] > 0.0f && wBC[1] > 0.0f && wABC[0] <= 0.0f)
+	{
+		float32 s = 1.0f / wBC[2];
+		float32 wB = s * wBC[0];
+		float32 wC = s * wBC[1];
+		return wB * B + wC * C;
+	}
+
+	// R CA
+	if (wCA[0] > 0.0f && wCA[1] > 0.0f && wABC[1] <= 0.0f)
+	{
+		float32 s = 1.0f / wCA[2];
+		float32 wC = s * wCA[0];
+		float32 wA = s * wCA[1];
+		return wC * C + wA * A;
+	}
+
+	if (wABC[3] <= 0.0f)
+	{
+		// ABC degenerates into an edge or vertex.
+		// Give up.
+		return A;
+	}
+
+	// R ABC/ACB
+	float32 divisor = wABC[3];
+	float32 s = 1.0f / divisor;
+	float32 wA = s * wABC[0];
+	float32 wB = s * wABC[1];
+	float32 wC = s * wABC[2];
+	return wA * A + wB * B + wC * C;
+}
+
+void b3ClosestPointsOnLines(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& E1,
+	const b3Vec3& P2, const b3Vec3& E2)
+{
+	// E1 = Q1 - P1
+	// E2 = Q2 - P2
+	// S1 = P1 + x1 * E1
+	// S2 = P2 + x2 * E2
+	// [dot(E1, E1) -dot(E1, E2)][x1] = [-dot(E1, P1 - P2)] 
+	// [dot(E2, E1) -dot(E2, E2)][x2] = [-dot(E2, P1 - P2)]
+	// Ax = b
+	b3Vec3 E3 = P1 - P2;
+
+	b3Mat22 A;
+	A.x.x = b3Dot(E1, E1);
+	A.x.y = b3Dot(E2, E1);
+	A.y.x = -b3Dot(E1, E2);
+	A.y.y = -b3Dot(E2, E2);
+
+	b3Vec2 b;
+	b.x = -b3Dot(E1, E3);
+	b.y = -b3Dot(E2, E3);
+
+	// If the lines are parallel then choose P1 and P2 as 
+	// the closest points.
+	b3Vec2 x = A.Solve(b);
+
+	*C1 = P1 + x.x * E1;
+	*C2 = P2 + x.y * E2;
+}
+
+void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& N1,
+	const b3Vec3& P2, const b3Vec3& N2)
+{
+	float32 a12 = -b3Dot(N1, N2);
+	float32 a21 = -a12;
+
+	float32 det = -1.0f - a12 * a21;
+	if (det == 0.0f)
+	{
+		// Nearly paralell lines.
+		*C1 = P1;
+		*C2 = P2;
+		return;
+	}
+
+	det = 1.0f / det;
+
+	b3Vec3 E3 = P1 - P2;
+
+	b3Vec2 b;
+	b.x = -b3Dot(N1, E3);
+	b.y = -b3Dot(N2, E3);
+
+	b3Vec2 x;
+	x.x = det * (-b.x - a12 * b.y);
+	x.y = det * (b.y - a21 * b.x);
+
+	*C1 = P1 + x.x * N1;
+	*C2 = P2 + x.y * N2;
+}
+
+void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& Q1,
+	const b3Vec3& P2, const b3Vec3& Q2)
+{
+	b3Vec3 E1 = Q1 - P1;
+	float32 L1 = b3Length(E1);
+	
+	b3Vec3 E2 = Q2 - P2;
+	float32 L2 = b3Length(E2);
+
+	if (L1 < B3_LINEAR_SLOP && L2 < B3_LINEAR_SLOP)
+	{
+		*C1 = P1;
+		*C2 = P2;
+		return;
+	}
+
+	if (L1 < B3_LINEAR_SLOP)
+	{
+		*C1 = P1;
+		*C2 = b3ClosestPointOnSegment(P1, P2, Q2);
+		return;
+	}
+
+	if (L2 < B3_LINEAR_SLOP)
+	{
+		*C1 = b3ClosestPointOnSegment(P2, P1, Q1);
+		*C2 = P2;
+		return;
+	}
+
+	// |e1xe2| = sin(theta) * |e1| * |e2|
+	b3Vec3 E1_x_E2 = b3Cross(E1, E2);
+	float32 L = b3Length(E1_x_E2);
+	const float32 kTolerance = 0.005f;
+	if (L < kTolerance * L1 * L2)
+	{
+		*C1 = P1;
+		*C2 = P2;
+	}
+	else
+	{
+		b3Vec3 N1 = (1.0f / L1) * E1;
+		b3Vec3 N2 = (1.0f / L2) * E2;
+		b3ClosestPointsOnNormalizedLines(C1, C2, P1, N1, P2, N2);
+	}
+
+	*C1 = b3ClosestPointOnSegment(*C1, P1, Q1);
+	*C2 = b3ClosestPointOnSegment(*C1, P2, Q2);
+	*C1 = b3ClosestPointOnSegment(*C2, P1, Q1);
+}

src/bounce/collision/gjk/gjk.cpp

@@ -0,0 +1,649 @@
+/*
+* 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\collision\gjk\gjk.h>
+#include <bounce\collision\gjk\gjk_proxy.h>
+
+// Implementation of the GJK (Gilbert-Johnson-Keerthi) algorithm 
+// using Voronoi regions and Barycentric coordinates.
+
+u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters;
+
+b3Vec3 b3Simplex::GetSearchDirection(const b3Vec3& Q) const
+{
+	switch (m_count)
+	{
+	case 1:
+	{
+		return Q - m_vertices[0].point;
+	}
+	case 2:
+	{
+		b3Vec3 A = m_vertices[0].point;
+		b3Vec3 B = m_vertices[1].point;
+
+		b3Vec3 AB = B - A;
+		b3Vec3 AQ = Q - A;
+		b3Vec3 AB_x_AQ = b3Cross(AB, AQ);
+		b3Vec3 PQ = b3Cross(AB_x_AQ, AB);
+		return PQ;
+	}
+	case 3:
+	{
+		b3Vec3 A = m_vertices[0].point;
+		b3Vec3 B = m_vertices[1].point;
+		b3Vec3 C = m_vertices[2].point;
+
+		b3Vec3 AB = B - A;
+		b3Vec3 AC = C - A;
+		b3Vec3 AQ = Q - A;
+		b3Vec3 AB_x_AC = b3Cross(AB, AC);
+		float32 sign = b3Dot(AB_x_AC, AQ);
+		if (sign > 0.0f)
+		{
+			return AB_x_AC;
+		}
+		else
+		{
+			return -AB_x_AC;
+		}
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		return b3Vec3(0.0f, 0.0f, 0.0f);
+	}
+	}
+}
+
+b3Vec3 b3Simplex::GetClosestPoint() const
+{
+	switch (m_count)
+	{
+	case 0:
+		B3_ASSERT(false);
+		return b3Vec3(0.0f, 0.0f, 0.0f);
+	case 1:
+		return m_vertices[0].point;
+	case 2:
+		return m_vertices[0].weight * m_vertices[0].point + m_vertices[1].weight * m_vertices[1].point;
+	case 3:
+		return m_vertices[0].weight * m_vertices[0].point + m_vertices[1].weight * m_vertices[1].point + m_vertices[2].weight * m_vertices[2].point;
+	case 4:
+		return b3Vec3(0.0f, 0.0f, 0.0f);
+	default:
+		B3_ASSERT(false);
+		return b3Vec3(0.0f, 0.0f, 0.0f);
+	}
+}
+
+void b3Simplex::GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const
+{
+	switch (m_count)
+	{
+	case 0:
+		B3_ASSERT(false);
+		break;
+	case 1:
+		*pA = m_vertices[0].pointA;
+		*pB = m_vertices[0].pointB;
+		break;
+
+	case 2:
+		*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA;
+		*pB = m_vertices[0].weight * m_vertices[0].pointB + m_vertices[1].weight * m_vertices[1].pointB;
+		break;
+
+	case 3:
+		*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA + m_vertices[2].weight * m_vertices[2].pointA;
+		*pB = m_vertices[0].weight * m_vertices[0].pointB + m_vertices[1].weight * m_vertices[1].pointB + m_vertices[2].weight * m_vertices[2].pointB;
+		break;
+	case 4:
+		*pA = m_vertices[0].weight * m_vertices[0].pointA + m_vertices[1].weight * m_vertices[1].pointA + m_vertices[2].weight * m_vertices[2].pointA + m_vertices[3].weight * m_vertices[3].pointA;
+		*pB = *pA;
+		break;
+	default:
+		B3_ASSERT(false);
+		break;
+	}
+}
+
+// Closest point on edge AB to Q.
+void b3Simplex::Solve2(const b3Vec3& Q)
+{
+	b3SimplexVertex A = m_vertices[0];
+	b3SimplexVertex B = m_vertices[1];
+
+	// Test vertex regions
+	float32 wAB[3];
+	b3Barycentric(wAB, A.point, B.point, Q);
+
+	// R A
+	if (wAB[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = A;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R B
+	if (wAB[0] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = B;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R AB
+	float32 divisor = wAB[2];
+	float32 s = 1.0f / divisor;
+	m_count = 2;
+	m_vertices[0] = A;
+	m_vertices[0].weight = s * wAB[0];
+	m_vertices[1] = B;
+	m_vertices[1].weight = s * wAB[1];
+}
+
+// Closest point on face ABC to Q.
+// Voronoi regions: A, B, C, AB, AC, BC, ABC, ACB.
+void b3Simplex::Solve3(const b3Vec3& Q)
+{
+	b3SimplexVertex A = m_vertices[0];
+	b3SimplexVertex B = m_vertices[1];
+	b3SimplexVertex C = m_vertices[2];
+
+	// Test vertex regions
+	float32 wAB[3], wBC[3], wCA[3];
+	b3Barycentric(wAB, A.point, B.point, Q);
+	b3Barycentric(wBC, B.point, C.point, Q);
+	b3Barycentric(wCA, C.point, A.point, Q);
+	
+	// Test edge regions		
+	float32 wABC[4];
+	b3Barycentric(wABC, A.point, B.point, C.point, Q);
+
+	// Test vertex regions
+	// R A
+	if (wAB[1] <= 0.0f && wCA[0] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = A;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R B
+	if (wAB[0] <= 0.0f && wBC[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = B;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R C
+	if (wBC[0] <= 0.0f && wCA[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = C;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// Test edge regions		
+	// This is used to help testing if the face degenerates 
+	// into an edge.
+	float32 area = wABC[3];
+
+	// R AB
+	if (wAB[0] > 0.0f && wAB[1] > 0.0f && area * wABC[2] <= 0.0f)
+	{
+		float32 divisor = wAB[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wAB[0];
+		m_vertices[1] = B;
+		m_vertices[1].weight = s * wAB[1];
+		return;
+	}
+
+	// R BC
+	if (wBC[0] > 0.0f && wBC[1] > 0.0f && area * wABC[0] <= 0.0f)
+	{
+		float32 divisor = wBC[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = B;
+		m_vertices[0].weight = s * wBC[0];
+		m_vertices[1] = C;
+		m_vertices[1].weight = s * wBC[1];
+		return;
+	}
+
+	// R CA
+	if (wCA[0] > 0.0f && wCA[1] > 0.0f && area * wABC[1] <= 0.0f)
+	{
+		float32 divisor = wCA[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = C;
+		m_vertices[0].weight = s * wCA[0];
+		m_vertices[1] = A;
+		m_vertices[1].weight = s * wCA[1];
+		return;
+	}
+
+	// R ABC/ACB
+	float32 divisor = wABC[3];
+	if (divisor <= 0.0f)
+	{
+		// Give up.
+		return;
+	}
+
+	B3_ASSERT(divisor > 0.0f);
+	float32 s = 1.0f / divisor;
+	m_count = 3;
+	m_vertices[0] = A;
+	m_vertices[0].weight = s * wABC[0];
+	m_vertices[1] = B;
+	m_vertices[1].weight = s * wABC[1];
+	m_vertices[2] = C;
+	m_vertices[2].weight = s * wABC[2];
+}
+
+// Closest point on tetrahedron A, B, C, D to Q.
+void b3Simplex::Solve4(const b3Vec3& Q)
+{
+	b3SimplexVertex A = m_vertices[0];
+	b3SimplexVertex B = m_vertices[1];
+	b3SimplexVertex C = m_vertices[2];
+	b3SimplexVertex D = m_vertices[3];
+
+	// Test vertex regions
+	float32 wAB[3], wAC[3], wAD[3], wBC[3], wCD[3], wDB[3];
+	b3Barycentric(wAB, A.point, B.point, Q);
+	b3Barycentric(wBC, B.point, C.point, Q);
+	b3Barycentric(wAC, A.point, C.point, Q);
+	b3Barycentric(wAD, A.point, D.point, Q);
+	b3Barycentric(wCD, C.point, D.point, Q);
+	b3Barycentric(wDB, D.point, B.point, Q);
+
+	// Test edge regions
+	float32 wACB[4], wABD[4], wADC[4], wBCD[4];
+	b3Barycentric(wACB, A.point, C.point, B.point, Q);
+	b3Barycentric(wABD, A.point, B.point, D.point, Q);
+	b3Barycentric(wADC, A.point, D.point, C.point, Q);
+	b3Barycentric(wBCD, B.point, C.point, D.point, Q);
+	
+	// Test face regions
+	float32 wABCD[5];
+	b3Barycentric(wABCD, A.point, B.point, C.point, D.point, Q);
+
+	// Test vertex regions
+	
+	// R A
+	if (wAB[1] <= 0.0f && wAC[1] <= 0.0f && wAD[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = A;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R B
+	if (wAB[0] <= 0.0f && wDB[0] <= 0.0f && wBC[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = B;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R C
+	if (wAC[0] <= 0.0f && wBC[0] <= 0.0f && wCD[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = C;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// R D
+	if (wAD[0] <= 0.0f && wCD[0] <= 0.0f && wDB[1] <= 0.0f)
+	{
+		m_count = 1;
+		m_vertices[0] = D;
+		m_vertices[0].weight = 1.0f;
+		return;
+	}
+
+	// Test edge regions
+	
+	// R AB
+	if (wABD[2] <= 0.0f && wACB[1] <= 0.0f && wAB[0] > 0.0f && wAB[1] > 0.0f)
+	{
+		float32 divisor = wAB[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wAB[0];
+		m_vertices[1] = B;
+		m_vertices[1].weight = s * wAB[1];
+		return;
+	}
+
+	// R AC
+	if (wACB[2] <= 0.0f && wADC[1] <= 0.0f && wAC[0] > 0.0f && wAC[1] > 0.0f)
+	{
+		float32 divisor = wAC[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wAC[0];
+		m_vertices[1] = C;
+		m_vertices[1].weight = s * wAC[1];
+		return;
+	}
+
+	// R AD
+	if (wADC[2] <= 0.0f && wABD[1] <= 0.0f && wAD[0] > 0.0f && wAD[1] > 0.0f)
+	{
+		float32 divisor = wAD[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wAD[0];
+		m_vertices[1] = D;
+		m_vertices[1].weight = s * wAD[1];
+		return;
+	}
+
+	// R BC
+	if (wACB[0] <= 0.0f && wBCD[2] <= 0.0f && wBC[0] > 0.0f && wBC[1] > 0.0f)
+	{
+		float32 divisor = wBC[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = B;
+		m_vertices[0].weight = s * wBC[0];
+		m_vertices[1] = C;
+		m_vertices[1].weight = s * wBC[1];
+		return;
+	}
+
+	// R CD
+	if (wADC[0] <= 0.0f && wBCD[0] <= 0.0f && wCD[0] > 0.0f && wCD[1] > 0.0f)
+	{
+		float32 divisor = wCD[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = C;
+		m_vertices[0].weight = s * wCD[0];
+		m_vertices[1] = D;
+		m_vertices[1].weight = s * wCD[1];
+		return;
+	}
+
+	// R DB
+	if (wABD[0] <= 0.0f && wBCD[1] <= 0.0f &&  wDB[0] > 0.0f && wDB[1] > 0.0f)
+	{
+		float32 divisor = wDB[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 2;
+		m_vertices[0] = D;
+		m_vertices[0].weight = s * wDB[0];
+		m_vertices[1] = B;
+		m_vertices[1].weight = s * wDB[1];
+		return;
+	}
+
+	// Test face regions
+	
+	// R ACB 
+	if (wABCD[3] <= 0.0f && wACB[0] > 0.0f && wACB[1] > 0.0f && wACB[2] > 0.0f)
+	{
+		float32 divisor = wACB[0] + wACB[1] + wACB[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 3;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wACB[0];
+		m_vertices[1] = C;
+		m_vertices[1].weight = s * wACB[1];
+		m_vertices[2] = B;
+		m_vertices[2].weight = s * wACB[2];
+		return;
+	}
+
+	// R ABD
+	if (wABCD[2] <= 0.0f && wABD[0] > 0.0f && wABD[1] > 0.0f && wABD[2] > 0.0f)
+	{
+		float32 divisor = wABD[0] + wABD[1] + wABD[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 3;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wABD[0];
+		m_vertices[1] = B;
+		m_vertices[1].weight = s * wABD[1];
+		m_vertices[2] = D;
+		m_vertices[2].weight = s * wABD[2];
+		return;
+	}
+
+	// R ADC
+	if (wABCD[1] <= 0.0f && wADC[0] > 0.0f && wADC[1] > 0.0f && wADC[2] > 0.0f)
+	{
+		float32 divisor = wADC[0] + wADC[1] + wADC[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 3;
+		m_vertices[0] = A;
+		m_vertices[0].weight = s * wADC[0];
+		m_vertices[1] = D;
+		m_vertices[1].weight = s * wADC[1];
+		m_vertices[2] = C;
+		m_vertices[2].weight = s * wADC[2];
+		return;
+	}
+
+	// R BCD
+	if (wABCD[0] <= 0.0f && wBCD[0] > 0.0f && wBCD[1] > 0.0f && wBCD[2] > 0.0f)
+	{
+		float32 divisor = wBCD[0] + wBCD[1] + wBCD[2];
+		B3_ASSERT(divisor > 0.0f);
+		float32 s = 1.0f / divisor;
+		m_count = 3;
+		m_vertices[0] = B;
+		m_vertices[0].weight = s * wBCD[0];
+		m_vertices[1] = C;
+		m_vertices[1].weight = s * wBCD[1];
+		m_vertices[2] = D;
+		m_vertices[2].weight = s * wBCD[2];
+		return;
+	}
+
+	// R ABCD
+	float32 divisor = wABCD[0] + wABCD[1] + wABCD[2] + wABCD[3];
+	if (divisor <= 0.0f)
+	{
+		// Give up.
+		return;
+	}
+
+	B3_ASSERT(divisor > 0.0f);
+	float32 s = 1.0f / divisor;
+	m_count = 4;
+	m_vertices[0].weight = s * wABCD[0];
+	m_vertices[1].weight = s * wABCD[1];
+	m_vertices[2].weight = s * wABCD[2];
+	m_vertices[3].weight = s * wABCD[3];
+}
+
+b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, const b3Transform& xfB, const b3GJKProxy& proxyB)
+{
+	++b3_gjkCalls;
+
+	b3Simplex simplex;
+	
+	// Initialize the simplex.
+	{
+		b3SimplexVertex* v = simplex.m_vertices + 0;
+		b3Vec3 wALocal = proxyA.GetVertex(0);
+		b3Vec3 wBLocal = proxyB.GetVertex(0);
+		v->pointA = b3Mul(xfA, wALocal);
+		v->pointB = b3Mul(xfB, wBLocal);
+		v->point = v->pointB - v->pointA;
+		v->weight = 1.0f;
+		v->indexA = 0;
+		v->indexB = 0;
+		simplex.m_count = 1;
+	}
+
+	// Get simplex vertices as an array.
+	b3SimplexVertex* vertices = simplex.m_vertices;
+
+	// These store the vertices of the last simplex so that we
+	// can check for duplicates and prevent cycling.
+	u32 saveA[4], saveB[4];
+	u32 saveCount = 0;
+
+	// Last iteration squared distance for checking if we're getting close
+	// to the origin and prevent cycling.
+	float32 distSq1 = B3_MAX_FLOAT;
+
+	const b3Vec3 kOrigin(0.0f, 0.0f, 0.0f);
+
+	// Limit number of iterations to prevent cycling.
+	const u32 kMaxIters = 20;
+
+	// Main iteration loop.
+	u32 iter = 0;
+	while (iter < kMaxIters)
+	{
+		// Copy simplex so we can identify duplicates.
+		saveCount = simplex.m_count;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			saveA[i] = vertices[i].indexA;
+			saveB[i] = vertices[i].indexB;
+		}
+
+		// Determine the closest point on the simplex and
+		// remove unused vertices.
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+		case 2:
+			simplex.Solve2(kOrigin);
+			break;
+		case 3:
+			simplex.Solve3(kOrigin);
+			break;
+		case 4:
+			simplex.Solve4(kOrigin);
+			break;
+		default:
+			B3_ASSERT(false);
+			break;
+		}
+
+		// If we have 4 points, then the origin is in the corresponding tethrahedron.
+		if (simplex.m_count == 4)
+		{
+			break;
+		}
+
+		// Compute the closest point.
+		b3Vec3 p = simplex.GetClosestPoint();
+		float32 distSq2 = b3Dot(p, p);
+		// Ensure we're getting close to the origin.
+		if (distSq2 >= distSq1)
+		{
+			//break;
+		}
+		distSq1 = distSq2;
+
+		// Get search direction.
+		b3Vec3 d = simplex.GetSearchDirection(kOrigin);
+
+		// Ensure the search direction is non-zero.
+		if (b3Dot(d, d) < B3_EPSILON * B3_EPSILON)
+		{
+			break;
+		}
+
+		// Compute a tentative new simplex vertex using support points.
+		b3SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = proxyA.GetSupportIndex(b3MulT(xfA.rotation, -d));
+		vertex->pointA = b3Mul(xfA, proxyA.GetVertex(vertex->indexA));
+		vertex->indexB = proxyB.GetSupportIndex(b3MulT(xfB.rotation, d));
+		vertex->pointB = b3Mul(xfB, proxyB.GetVertex(vertex->indexB));
+		vertex->point = vertex->pointB - vertex->pointA;
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+		++b3_gjkIters;
+
+		// Check for duplicate support points. 
+		// This is the main termination criteria.
+		bool duplicate = false;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
+			{
+				duplicate = true;
+				break;
+			}
+		}
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		if (duplicate)
+		{
+			break;
+		}
+
+		// New vertex is needed.
+		++simplex.m_count;
+	}
+
+	b3_gjkMaxIters = b3Max(b3_gjkMaxIters, iter);
+
+	// Prepare output.
+	b3GJKOutput output;
+	simplex.GetClosestPoints(&output.pointA, &output.pointB);
+	output.distance = b3Distance(output.pointA, output.pointB);
+	output.iterations = iter;
+
+	// Output result.
+	return output;
+}

src/bounce/collision/gjk/gjk_cache.cpp

@@ -0,0 +1,284 @@
+/*
+* 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\collision\gjk\gjk_cache.h>
+#include <bounce\collision\gjk\gjk_proxy.h>
+
+extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters;
+u32 b3_gjkCacheHits;
+
+// Implements b3Simplex routines for a cached simplex.
+void b3Simplex::ReadCache(const b3SimplexCache* cache, 
+	const b3Transform& xfA, const b3GJKProxy& proxyA, 
+	const b3Transform& xfB, const b3GJKProxy& proxyB)
+{
+	B3_ASSERT(cache->count <= 4);
+	m_count = (u8)cache->count;
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3SimplexVertex* v = m_vertices + i;
+		v->indexA = cache->indexA[i];
+		v->indexB = cache->indexB[i];
+		b3Vec3 wALocal = proxyA.GetVertex(v->indexA);
+		b3Vec3 wBLocal = proxyB.GetVertex(v->indexB);
+		v->pointA = xfA * wALocal;
+		v->pointB = xfB * wBLocal;
+		v->point = v->pointB - v->pointA;
+		v->weight = 0.0f;
+	}
+
+	// Compute the new simplex metric
+	// If it is substantially different than
+	// old metric then flush the simplex.
+	if (m_count > 1)
+	{
+		float32 metric1 = cache->metric;
+		float32 metric2 = GetMetric();
+		if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < B3_EPSILON)
+		{
+			// Flush
+			m_count = 0;
+		}
+		else
+		{
+			++b3_gjkCacheHits;
+		}
+	}
+
+	// If cache is empty or flushed choose an arbitrary simplex.
+	if (m_count == 0)
+	{
+		b3SimplexVertex* v = m_vertices + 0;
+		b3Vec3 wALocal = proxyA.GetVertex(0);
+		b3Vec3 wBLocal = proxyB.GetVertex(0);
+		v->pointA = b3Mul(xfA, wALocal);
+		v->pointB = b3Mul(xfB, wBLocal);
+		v->point = v->pointB - v->pointA;
+		v->weight = 1.0f;
+		v->indexA = 0;
+		v->indexB = 0;
+		m_count = 1;
+	}
+}
+
+void b3Simplex::WriteCache(b3SimplexCache* cache) const
+{
+	cache->metric = GetMetric();
+	cache->count = u16(m_count);
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		cache->indexA[i] = u8(m_vertices[i].indexA);
+		cache->indexB[i] = u8(m_vertices[i].indexB);
+	}
+}
+
+float32 b3Simplex::GetMetric() const
+{
+	switch (m_count)
+	{
+	case 0:
+		B3_ASSERT(false);
+		return 0.0f;
+	case 1:
+		return 0.0f;
+	case 2:
+		// Magnitude
+		return b3Distance(m_vertices[0].point, m_vertices[1].point);
+	case 3:
+	{
+		// Area
+		b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
+		b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
+		return b3Length(b3Cross(E1, E2));
+	}
+	case 4:
+	{
+		// Volume
+		b3Vec3 E1 = m_vertices[1].point - m_vertices[0].point;
+		b3Vec3 E2 = m_vertices[2].point - m_vertices[0].point;
+		b3Vec3 E3 = m_vertices[3].point - m_vertices[0].point;
+		float32 det = b3Det(E1, E2, E3);
+		float32 sign = b3Sign(det);
+		float32 volume = sign * det;
+		return volume;
+	}
+	default:
+		B3_ASSERT(false);
+		return 0.0f;
+	}
+}
+
+b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
+	const b3Transform& xfB, const b3GJKProxy& proxyB,
+	bool applyRadius, b3SimplexCache* cache)
+{
+	++b3_gjkCalls;
+
+	// Initialize the simplex.
+	b3Simplex simplex;
+	simplex.ReadCache(cache, xfA, proxyA, xfB, proxyB);
+
+	// Get simplex vertices as an array.
+	b3SimplexVertex* vertices = simplex.m_vertices;
+
+	// These store the vertices of the last simplex so that we
+	// can check for duplicates and prevent cycling.
+	u32 saveA[4], saveB[4];
+	u32 saveCount = 0;
+
+	// Last iteration squared distance for checking if we're getting close
+	// to the origin and prevent cycling.
+	float32 distSq1 = B3_MAX_FLOAT;
+
+	const b3Vec3 kOrigin(0.0f, 0.0f, 0.0f);
+
+	// Limit number of iterations to prevent cycling.
+	const u32 kMaxIters = 20;
+
+	// Main iteration loop.
+	u32 iter = 0;
+	while (iter < kMaxIters)
+	{
+		// Copy simplex so we can identify duplicates.
+		saveCount = simplex.m_count;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			saveA[i] = vertices[i].indexA;
+			saveB[i] = vertices[i].indexB;
+		}
+
+		// Determine the closest point on the simplex and
+		// remove unused vertices.
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+		case 2:
+			simplex.Solve2(kOrigin);
+			break;
+		case 3:
+			simplex.Solve3(kOrigin);
+			break;
+		case 4:
+			simplex.Solve4(kOrigin);
+			break;
+		default:
+			B3_ASSERT(false);
+			break;
+		}
+
+		// If we have 4 points, then the origin is in the corresponding tethrahedron.
+		if (simplex.m_count == 4)
+		{
+			break;
+		}
+
+		// Compute the closest point.
+		b3Vec3 p = simplex.GetClosestPoint();
+		float32 distSq2 = b3Dot(p, p);
+		// Ensure we're getting close to the origin.
+		if (distSq2 >= distSq1)
+		{
+			//break;
+		}
+		distSq1 = distSq2;
+
+		// Get search direction.
+		b3Vec3 d = simplex.GetSearchDirection(kOrigin);
+
+		// Ensure the search direction is non-zero.
+		if (b3Dot(d, d) < B3_EPSILON * B3_EPSILON)
+		{
+			break;
+		}
+
+		// Compute a tentative new simplex vertex using support points.
+		b3SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = proxyA.GetSupportIndex(b3MulT(xfA.rotation, -d));
+		vertex->pointA = b3Mul(xfA, proxyA.GetVertex(vertex->indexA));
+		vertex->indexB = proxyB.GetSupportIndex(b3MulT(xfB.rotation, d));
+		vertex->pointB = b3Mul(xfB, proxyB.GetVertex(vertex->indexB));
+		vertex->point = vertex->pointB - vertex->pointA;
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+		++b3_gjkIters;
+
+		// Check for duplicate support points. 
+		// This is the main termination criteria.
+		bool duplicate = false;
+		for (u32 i = 0; i < saveCount; ++i)
+		{
+			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
+			{
+				duplicate = true;
+				break;
+			}
+		}
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		if (duplicate)
+		{
+			break;
+		}
+
+		// New vertex is ok and needed.
+		++simplex.m_count;
+	}
+
+	b3_gjkMaxIters = b3Max(b3_gjkMaxIters, iter);
+
+	// Prepare result.
+	b3GJKOutput output;
+	simplex.GetClosestPoints(&output.pointA, &output.pointB);
+	output.distance = b3Distance(output.pointA, output.pointB);
+	output.iterations = iter;
+
+	// Cache the simplex.
+	simplex.WriteCache(cache);
+
+	// Apply radius if requested.
+	if (applyRadius)
+	{
+		float32 rA = proxyA.m_radius;
+		float32 rB = proxyB.m_radius;
+
+		if (output.distance > rA + rB && output.distance > B3_EPSILON)
+		{
+			// Shapes are still no overlapped.
+			// Move the witness points to the outer surface.
+			output.distance -= rA + rB;
+			b3Vec3 d = output.pointB - output.pointA;
+			b3Vec3 normal = b3Normalize(d);
+			output.pointA += rA * normal;
+			output.pointB -= rB * normal;
+		}
+		else
+		{
+			// Shapes are overlapped when radii are considered.
+			// Move the witness points to the middle.
+			b3Vec3 p = 0.5f * (output.pointA + output.pointB);
+			output.pointA = p;
+			output.pointB = p;
+			output.distance = 0.0f;
+		}
+	}
+
+	// Output result.
+	return output;
+}

src/bounce/collision/gjk/gjk_feature_pair.cpp

@@ -0,0 +1,214 @@
+/*
+* 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\collision\gjk\gjk_cache.h>
+
+b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
+{
+	B3_ASSERT(0 < cache.count && cache.count < 4);
+
+	u32 vertexCount = cache.count;
+	u32 uniqueCount1 = b3UniqueCount(cache.indexA, vertexCount);
+	u32 uniqueCount2 = b3UniqueCount(cache.indexB, vertexCount);
+
+	if (vertexCount == 1)
+	{
+		// VV 
+		b3GJKFeaturePair pair;
+		pair.typeA = b3GJKFeaturePair::e_vertex;
+		pair.typeB = b3GJKFeaturePair::e_vertex;
+		pair.indexA[0] = cache.indexA[0];
+		pair.indexB[0] = cache.indexB[0];
+		return pair;
+	}
+
+	if (vertexCount == 2)
+	{
+		if (uniqueCount1 == 2 && uniqueCount2 == 2)
+		{
+			// EE
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_edge;
+			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexA[1] = cache.indexA[1];
+			pair.indexB[0] = cache.indexB[0];
+			pair.indexB[1] = cache.indexB[1];
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			return pair;
+		}
+
+		if (uniqueCount1 == 1)
+		{
+			// VE
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_vertex;
+			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexB[0] = cache.indexB[0];
+			pair.indexB[1] = cache.indexB[1];
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			return pair;
+		}
+
+		if (uniqueCount2 == 1)
+		{
+			// EV
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_edge;
+			pair.typeB = b3GJKFeaturePair::e_vertex;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexA[1] = cache.indexA[1];
+			pair.indexB[0] = cache.indexB[0];
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			return pair;
+		}
+		B3_ASSERT(false);
+	}
+
+	if (vertexCount == 3)
+	{
+		if (uniqueCount1 == 1 && uniqueCount2 == 3)
+		{
+			// VF
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_vertex;
+			pair.typeB = b3GJKFeaturePair::e_face;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexB[0] = cache.indexB[0];
+			pair.indexB[1] = cache.indexB[1];
+			pair.indexB[2] = cache.indexB[2];
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			B3_ASSERT(pair.indexB[1] != pair.indexB[2]);
+			B3_ASSERT(pair.indexB[2] != pair.indexB[0]);
+			return pair;
+		}
+
+		if (uniqueCount1 == 3 && uniqueCount2 == 1)
+		{
+			// FV
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_face;
+			pair.typeB = b3GJKFeaturePair::e_vertex;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexA[1] = cache.indexA[1];
+			pair.indexA[2] = cache.indexA[2];
+			pair.indexB[0] = cache.indexB[0];
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			B3_ASSERT(pair.indexA[1] != pair.indexA[2]);
+			B3_ASSERT(pair.indexA[2] != pair.indexA[0]);
+			return pair;
+		}
+
+		if (uniqueCount1 == 2 && uniqueCount2 == 2)
+		{
+			// EE
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_edge;
+			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexB[0] = cache.indexB[0];
+
+			if (cache.indexA[0] == cache.indexA[1])
+			{
+				pair.indexA[1] = cache.indexA[2];
+			}
+			else
+			{
+				pair.indexA[1] = cache.indexA[1];
+			}
+
+			if (cache.indexB[0] == cache.indexB[1])
+			{
+				pair.indexB[1] = cache.indexB[2];
+			}
+			else
+			{
+				pair.indexB[1] = cache.indexB[1];
+			}
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			return pair;
+		}
+
+		if (uniqueCount1 == 2 && uniqueCount2 == 3)
+		{
+			// EF
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_edge;
+			pair.typeB = b3GJKFeaturePair::e_face;
+
+			pair.indexA[0] = cache.indexA[0];
+			if (cache.indexA[0] == cache.indexA[1])
+			{
+				pair.indexA[1] = cache.indexA[2];
+			}
+			else
+			{
+				pair.indexA[1] = cache.indexA[1];
+			}
+
+			pair.indexB[0] = cache.indexB[0];
+			pair.indexB[1] = cache.indexB[1];
+			pair.indexB[2] = cache.indexB[2];
+
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			B3_ASSERT(pair.indexB[1] != pair.indexB[2]);
+			B3_ASSERT(pair.indexB[2] != pair.indexB[0]);
+			return pair;
+		}
+
+		if (uniqueCount1 == 3 && uniqueCount2 == 2)
+		{
+			// FE
+			b3GJKFeaturePair pair;
+			pair.typeA = b3GJKFeaturePair::e_face;
+			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.indexA[0] = cache.indexA[0];
+			pair.indexA[1] = cache.indexA[1];
+			pair.indexA[2] = cache.indexA[2];
+
+			pair.indexB[0] = cache.indexB[0];
+			if (cache.indexB[0] == cache.indexB[1])
+			{
+				pair.indexB[1] = cache.indexB[2];
+			}
+			else
+			{
+				pair.indexB[1] = cache.indexB[1];
+			}
+
+			B3_ASSERT(pair.indexA[0] != pair.indexA[1]);
+			B3_ASSERT(pair.indexA[1] != pair.indexA[2]);
+			B3_ASSERT(pair.indexA[2] != pair.indexA[0]);
+			B3_ASSERT(pair.indexB[0] != pair.indexB[1]);
+			return pair;
+		}
+
+		B3_ASSERT(false);
+	}
+
+	B3_ASSERT(false);
+
+	b3GJKFeaturePair pair;
+	pair.typeA = b3GJKFeaturePair::e_unknown;
+	pair.typeB = b3GJKFeaturePair::e_unknown;
+	return pair;
+}

src/bounce/collision/sat/sat.cpp

@@ -0,0 +1,160 @@
+/*
+* 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\collision\sat\sat.h>
+#include <bounce\collision\shapes\hull.h>
+
+// Implementation of the SAT (Separating Axis Test) for 
+// convex hulls. Thanks to Dirk Gregorius for his presentation 
+// at GDC 2013.
+
+float32 b3Project(const b3Hull* hull, const b3Plane& plane)
+{
+	b3Vec3 support = hull->GetVertex(hull->GetSupportVertex(-plane.normal));
+	return b3Distance(support, plane);
+}
+
+// Query minimum separation distance and axis of the first hull planes.
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xfB, xfA);
+
+	// Here greater means less than since is a signed distance.
+	u32 maxIndex = 0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < hullA->faceCount; ++i)
+	{
+		b3Plane plane = xf * hullA->GetPlane(i);
+		float32 separation = b3Project(hullB, plane);
+		if (separation > maxSeparation)
+		{
+			maxIndex = i;
+			maxSeparation = separation;
+		}
+	}
+
+	b3FaceQuery out;
+	out.index = maxIndex;
+	out.separation = maxSeparation;
+	return out;
+}
+
+bool b3IsMinkowskiFace(const b3Vec3& A, const b3Vec3& B, const b3Vec3& B_x_A, const b3Vec3& C, const b3Vec3& D, const b3Vec3& D_x_C)
+{
+	float32 ADC = b3Dot(A, D_x_C);
+	float32 BDC = b3Dot(B, D_x_C);
+
+	float32 CBA = b3Dot(C, B_x_A);
+	float32 DBA = b3Dot(D, B_x_A);
+
+	return CBA * DBA < 0.0f && // Test arc CD against AB plane.
+		ADC * BDC < 0.0f && // Test arc AB against DC plane.
+		CBA * BDC > 0.0f; // Test if arcs AB and CD are on the same hemisphere.
+}
+
+float32 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C1)
+{
+	// Skip over almost parallel edges.
+	b3Vec3 E1_x_E2 = b3Cross(E1, E2);
+	float32 L = b3Length(E1_x_E2);
+
+	const float32 kTol = 0.005f;
+
+	if (L < kTol * b3Sqrt(b3LengthSquared(E1) * b3LengthSquared(E2)))
+	{
+		return -B3_MAX_FLOAT;
+	}
+
+	// Ensure consistent normal orientation to hull B.
+	b3Vec3 N = (1.0f / L) * E1_x_E2;
+	if (b3Dot(N, P1 - C1) < 0.0f)
+	{
+		N = -N;
+	}
+
+	// d = dot(N, P2) - offset = dot(N, P2) - dot(N, P1) = dot(N, P2 - P1) 
+	return b3Dot(N, P2 - P1);
+}
+
+b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Query minimum separation distance and axis of the first hull planes.
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xfB, xfA);
+	b3Vec3 C1 = xf * hullA->centroid;
+
+	u32 maxIndex1 = 0;
+	u32 maxIndex2 = 0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	// Loop through the first hull's unique edges.
+	for (u32 i = 0; i < hullA->edgeCount; i += 2)
+	{
+		const b3HalfEdge* edge1 = hullA->GetEdge(i);
+		const b3HalfEdge* twin1 = hullA->GetEdge(i + 1);
+
+		B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
+
+		b3Vec3 P1 = xf * hullA->GetVertex(edge1->origin);
+		b3Vec3 Q1 = xf * hullA->GetVertex(twin1->origin);
+		b3Vec3 E1 = Q1 - P1;
+
+		// The Gauss Map of edge 1.
+		b3Vec3 U1 = xf.rotation * hullA->GetPlane(edge1->face).normal;
+		b3Vec3 V1 = xf.rotation * hullA->GetPlane(twin1->face).normal;
+
+		// Loop through the second hull's unique edges.
+		for (u32 j = 0; j < hullB->edgeCount; j += 2)
+		{
+			const b3HalfEdge* edge2 = hullB->GetEdge(j);
+			const b3HalfEdge* twin2 = hullB->GetEdge(j + 1);
+
+			B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
+
+			b3Vec3 P2 = hullB->GetVertex(edge2->origin);
+			b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
+			b3Vec3 E2 = Q2 - P2;
+
+			// The Gauss Map of edge 2.
+			b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
+			b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
+
+			// Negate the Gauss Map 2 for account for the MD.
+			if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
+			{
+				float32 separation = b3Project(P1, E1, P2, E2, C1);
+				if (separation > maxSeparation)
+				{
+					maxSeparation = separation;
+					maxIndex1 = i;
+					maxIndex2 = j;
+				}
+			}
+		}
+	}
+
+	b3EdgeQuery out;
+	out.indexA = maxIndex1;
+	out.indexB = maxIndex2;
+	out.separation = maxSeparation;
+	return out;
+}

src/bounce/collision/sat/sat_edge_and_hull.cpp

@@ -0,0 +1,132 @@
+/*
+* 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\collision\sat\sat_edge_and_hull.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+
+float32 b3ProjectEdge(const b3Capsule* hull, const b3Plane& plane)
+{
+	b3Vec3 support = hull->GetVertex(hull->GetSupportVertex(-plane.normal));
+	return b3Distance(support, plane);
+}
+
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Capsule* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Perform computations in the local space of the first hull.
+	b3Transform xf = b3MulT(xfA, xfB);
+
+	// Here greater means less than since is a signed distance.
+	u32 maxIndex = 0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < hullB->faceCount; ++i)
+	{
+		b3Plane plane = b3Mul(xf, hullB->GetPlane(i));
+		float32 separation = b3ProjectEdge(hullA, plane);
+		if (separation > maxSeparation)
+		{
+			maxIndex = i;
+			maxSeparation = separation;
+		}
+	}
+
+	b3FaceQuery out;
+	out.index = maxIndex;
+	out.separation = maxSeparation;
+	return out;
+}
+
+// Qualify the two hull normals against the plane of the ring of the capsule.
+bool b3IsMinkowskiFaceEdge(const b3Vec3& N, const b3Vec3& C, const b3Vec3& D)
+{
+	return b3Dot(N, C) * b3Dot(N, D) < 0.0f;
+}
+
+float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1,
+	const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C2)
+{
+	// Skip over almost parallel edges.
+	b3Vec3 E1_x_E2 = b3Cross(E1, E2);
+	float32 L = b3Length(E1_x_E2);
+	
+	const float32 kTol = 0.005f;
+
+	if (L < kTol * b3Sqrt(b3LengthSquared(E1) * b3LengthSquared(E2)))
+	{
+		return -B3_MAX_FLOAT;
+	}
+
+	// Ensure consistent normal orientation to hull B.
+	b3Vec3 N = (1.0f / L) * E1_x_E2;
+	if (b3Dot(N, P2 - C2) > 0.0f)
+	{
+		N = -N;
+	}
+
+	// d = dot(N, P2) - offset = dot(N, P2) - dot(N, P1) = dot(N, P2 - P1) 
+	return b3Dot(N, P2 - P1);
+}
+
+b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Capsule* hullA, const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Query minimum edge separation.
+	u32 maxIndex = 0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xfB, xfA);
+
+	b3Vec3 P1 = b3Mul(xf, hullA->vertices[0]);
+	b3Vec3 Q1 = b3Mul(xf, hullA->vertices[1]);
+	b3Vec3 E1 = Q1 - P1;
+
+	b3Vec3 C2 = hullB->centroid;
+
+	for (u32 i = 0; i < hullB->edgeCount; i += 2)
+	{
+		const b3HalfEdge* edge2 = hullB->GetEdge(i);
+		const b3HalfEdge* twin2 = hullB->GetEdge(i + 1);
+
+		B3_ASSERT(edge2->twin == i + 1 && twin2->twin == i);
+
+		b3Vec3 P2 = hullB->GetVertex(edge2->origin);
+		b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
+		b3Vec3 E2 = Q2 - P2;
+
+		b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
+		b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
+
+		if (b3IsMinkowskiFaceEdge(E1, U2, V2))
+		{
+			float32 separation = b3ProjectEdge(P1, E1, P2, E2, C2);
+			if (separation > maxSeparation)
+			{
+				maxSeparation = separation;
+				maxIndex = i;
+			}
+		}
+	}
+
+	b3EdgeQuery out;
+	out.indexA = 0;
+	out.indexB = maxIndex;
+	out.separation = maxSeparation;
+	return out;
+}

src/bounce/collision/sat/sat_vertex_and_hull.cpp

@@ -0,0 +1,54 @@
+/*
+* 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\collision\sat\sat_vertex_and_hull.h>
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\hull.h>
+
+float32 b3ProjectVertex(const b3Sphere* hull, const b3Plane& plane)
+{
+	b3Vec3 support = hull->GetVertex(hull->GetSupportVertex(-plane.normal));
+	return b3Distance(support, plane);
+}
+
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Sphere* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Perform computations in the local space of the second hull.
+	b3Vec3 support = b3MulT(xfB, b3Mul(xfA, hullA->vertex));
+
+	u32 maxIndex = 0;
+	float32 maxSeparation = -B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < hullB->faceCount; ++i)
+	{
+		b3Plane plane = hullB->GetPlane(i);
+		float32 separation = b3Distance(support, plane);
+
+		if (separation > maxSeparation)
+		{
+			maxIndex = i;
+			maxSeparation = separation;
+		}
+	}
+	
+	b3FaceQuery out;
+	out.index = maxIndex;
+	out.separation = maxSeparation;
+	return out;
+}

src/bounce/collision/shapes/hull.cpp

@@ -0,0 +1,55 @@
+/*
+* 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\collision\shapes\hull.h>
+
+void b3Hull::Validate() const 
+{
+	for (u32 i = 0; i < faceCount; ++i) 
+	{
+		Validate(faces + i);
+	}
+}
+
+void b3Hull::Validate(const b3Face* face) const 
+{
+	Validate(edges + face->edge);
+}
+
+void b3Hull::Validate(const b3HalfEdge* e) const 
+{
+	u32 edgeIndex = (u32)(e - edges);
+	const b3HalfEdge* twin = edges + e->twin;
+
+	B3_ASSERT(twin->twin == edgeIndex);
+	B3_ASSERT(b3Abs(twin - e) == 1);
+	
+	//B3_ASSERT(edges[e->prev].next == edgeIndex);
+	B3_ASSERT(e->origin != twin->origin);
+
+	u32 count = 0;
+	const b3HalfEdge* start = e;
+	do 
+	{
+		const b3HalfEdge* next = edges + e->next;
+		const b3HalfEdge* twin = edges + next->twin;
+		e = twin;
+		B3_ASSERT(count < edgeCount);
+		++count;
+	} while (e != start);
+}

src/bounce/collision/trees/dynamic_tree.cpp

@@ -0,0 +1,403 @@
+/*
+* 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\collision\trees\dynamic_tree.h>
+
+b3DynamicTree::b3DynamicTree() 
+{
+	m_root = NULL_NODE;
+
+	// Preallocate 32 nodes.
+	m_nodeCapacity = 32;
+	m_nodes = (b3Node*) b3Alloc(m_nodeCapacity * sizeof(b3Node));
+	memset(m_nodes, NULL, m_nodeCapacity * sizeof(b3Node));
+	m_nodeCount = 0;
+
+	// Link the allocated nodes and make the first node 
+	// available the the next allocation.
+	AddToFreeList(m_nodeCount);
+}
+
+b3DynamicTree::~b3DynamicTree() 
+{
+	b3Free(m_nodes);
+}
+
+// Return a node from the pool.
+i32 b3DynamicTree::AllocateNode() 
+{
+	if (m_freeList == NULL_NODE) 
+	{
+		B3_ASSERT(m_nodeCount == m_nodeCapacity);
+
+		// Duplicate capacity.
+		m_nodeCapacity *= 2;
+
+		b3Node* oldNodes = m_nodes;
+		m_nodes = (b3Node*) b3Alloc(m_nodeCapacity * sizeof(b3Node));;
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b3Node));
+		b3Free(oldNodes);
+
+		// Link the (allocated) nodes starting from the new 
+		// node and make the new nodes available the the next allocation.
+		AddToFreeList(m_nodeCount);
+	}
+
+	// Grab the free node.
+	i32 node = m_freeList;
+
+	m_freeList = m_nodes[node].next;
+
+	m_nodes[node].parent = NULL_NODE;
+	m_nodes[node].child1 = NULL_NODE;
+	m_nodes[node].child2 = NULL_NODE;
+	m_nodes[node].height = 0;
+	m_nodes[node].userData = nullptr;
+
+	++m_nodeCount;
+
+	return node;
+}
+
+void b3DynamicTree::FreeNode(i32 node) 
+{
+	B3_ASSERT(node >= 0 && node < m_nodeCapacity);
+	m_nodes[node].next = m_freeList;
+	m_nodes[node].height = -1;
+	m_freeList = node;
+	--m_nodeCount;
+}
+
+void b3DynamicTree::AddToFreeList(i32 node) 
+{
+	// Starting from the given node, relink the linked list of nodes.
+	for (i32 i = node; i < m_nodeCapacity - 1; ++i) 
+	{
+		m_nodes[i].next = i + 1;
+		m_nodes[i].height = -1;
+	}
+
+	m_nodes[m_nodeCapacity - 1].next = NULL_NODE;
+	m_nodes[m_nodeCapacity - 1].height = -1;
+
+	// Make the node available for the next allocation.
+	m_freeList = node;
+}
+
+i32 b3DynamicTree::InsertNode(const b3AABB3& aabb, void* userData) 
+{
+	// Insert into the array.
+	i32 node = AllocateNode();
+	m_nodes[node].aabb = aabb;
+	m_nodes[node].userData = userData;
+	m_nodes[node].height = 0;
+
+	// Insert into the tree.
+	InsertLeaf(node);
+
+	// Return the node ID.
+	return node;
+}
+
+void b3DynamicTree::RemoveNode(i32 proxyId) 
+{
+	// Remove from the tree.
+	RemoveLeaf(proxyId);
+	// Remove from the node array and make it available.
+	FreeNode(proxyId);
+}
+
+void b3DynamicTree::UpdateNode(i32 proxyId, const b3AABB3& aabb)
+{
+	B3_ASSERT(m_root != NULL_NODE);
+	B3_ASSERT(m_nodes[proxyId].IsLeaf());
+	// Remove old AABB from the tree.
+	RemoveLeaf(proxyId);
+	// Insert the new AABB to the tree.
+	m_nodes[proxyId].aabb = aabb;
+	InsertLeaf(proxyId);
+}
+
+i32 b3DynamicTree::HeuristicSearch(const b3AABB3& leafAABB) const 
+{
+	// To find a good branch node, the manhattan distance could be used as heuristic. 
+	// However, the current propagated node and the leaf node volume are incompletely considerable.
+	// Therefore, an approximation of the surface are heuristic (SAH) is used.
+
+	i32 index = m_root;
+	while (!m_nodes[index].IsLeaf()) 
+	{
+		float32 branchArea = m_nodes[index].aabb.SurfaceArea();
+
+		// Minumum cost of pushing the leaf down the tree.
+		b3AABB3 combinedAABB = b3Combine(leafAABB, m_nodes[index].aabb);
+		float32 combinedArea = combinedAABB.SurfaceArea();
+
+		// Cost for creating a new parent node.
+		float32 branchCost = 2.0f * combinedArea;
+
+		float32 inheritanceCost = 2.0f * (combinedArea - branchArea);
+
+		// The branch node child nodes cost.
+		i32 child1 = m_nodes[index].child1;
+		i32 child2 = m_nodes[index].child2;
+
+		// Cost of descending onto child1.
+		float32 childCost1 = 0.0f;
+		if (m_nodes[child1].IsLeaf()) 
+		{
+			b3AABB3 aabb = b3Combine(leafAABB, m_nodes[child1].aabb);
+			childCost1 = aabb.SurfaceArea();
+		}
+		else 
+		{
+			b3AABB3 aabb = b3Combine(leafAABB, m_nodes[child1].aabb);
+			float32 oldArea = m_nodes[child1].aabb.SurfaceArea();
+			float32 newArea = aabb.SurfaceArea();
+			childCost1 = (newArea - oldArea) + inheritanceCost;
+		}
+
+		// Cost of descending onto child1.
+		float32 childCost2 = 0.0f;
+		if (m_nodes[child2].IsLeaf()) 
+		{
+			b3AABB3 aabb = b3Combine(leafAABB, m_nodes[child2].aabb);
+			childCost2 = aabb.SurfaceArea();
+		}
+		else 
+		{
+			b3AABB3 aabb = b3Combine(leafAABB, m_nodes[child2].aabb);
+			float32 oldArea = m_nodes[child2].aabb.SurfaceArea();
+			float32 newArea = aabb.SurfaceArea();
+			childCost2 = (newArea - oldArea) + inheritanceCost;
+		}
+
+		// Choose the node that has the minimum cost.
+		if (branchCost < childCost1 && branchCost < childCost2) 
+		{
+			// The current branch node is the best node and it will be used.
+			break;
+		}
+
+		// Visit the node that has the minimum cost.
+		index = childCost1 < childCost2 ? child1 : child2;
+	}
+	return index;
+}
+
+void b3DynamicTree::InsertLeaf(i32 leaf) 
+{
+	if (m_root == NULL_NODE) 
+	{
+		// If this tree root node is empty then just set the leaf
+		// node to it.
+		m_root = leaf;
+		m_nodes[m_root].parent = NULL_NODE;
+		return;
+	}
+
+	// Get the inserted leaf AABB.
+	b3AABB3 leafAabb = m_nodes[leaf].aabb;
+	
+	// Search for the best branch node of this tree starting from the tree root node.
+	i32 sibling = HeuristicSearch(leafAabb);
+
+	i32 oldParent = m_nodes[sibling].parent;
+	
+	// Create and setup new parent. 
+	i32 newParent = AllocateNode();	
+	m_nodes[newParent].parent = oldParent;
+	m_nodes[newParent].child1 = sibling;
+	m_nodes[sibling].parent = newParent;	
+	m_nodes[newParent].child2 = leaf;
+	m_nodes[leaf].parent = newParent;	
+	m_nodes[newParent].userData = nullptr;
+	m_nodes[newParent].aabb = b3Combine(leafAabb, m_nodes[sibling].aabb);
+	m_nodes[newParent].height = m_nodes[sibling].height + 1;
+
+	if (oldParent != NULL_NODE) 
+	{
+		// The sibling was not the root.
+		// Find which child node of the old parent is the sibling
+		// and link the new parent to it.
+		if (m_nodes[oldParent].child1 == sibling) 
+		{
+			m_nodes[oldParent].child1 = newParent;
+		}
+		else
+		{
+			m_nodes[oldParent].child2 = newParent;
+		}
+	}
+	else 
+	{
+		// If the sibling was the root then the root becomes the created
+		// node.
+		m_root = newParent;
+	}
+
+	// If we have ancestor nodes then adjust its AABBs.
+	WalkBackNodeAndCombineVolumes(newParent);
+}
+
+void b3DynamicTree::RemoveLeaf(i32 leaf) 
+{
+	if (leaf == m_root) 
+	{
+		m_root = NULL_NODE;
+		return;
+	}
+
+	i32 parent = m_nodes[leaf].parent;
+	i32 grandParent = m_nodes[parent].parent;
+	i32 sibling;
+	if (m_nodes[parent].child1 == leaf) 
+	{
+		sibling = m_nodes[parent].child2;
+	}
+	else 
+	{
+		sibling = m_nodes[parent].child1;
+	}
+
+	if (grandParent != NULL_NODE) 
+	{
+		if (m_nodes[grandParent].child1 == parent) 
+		{
+			m_nodes[grandParent].child1 = sibling;
+		}
+		else 
+		{
+			m_nodes[grandParent].child2 = sibling;
+		}
+		m_nodes[sibling].parent = grandParent;
+		
+		// Remove parent node.
+		FreeNode(parent);
+
+		// If we have ancestor then nodes adjust its AABBs.
+		WalkBackNodeAndCombineVolumes(grandParent);
+	}
+	else 
+	{
+		m_root = sibling;
+		m_nodes[sibling].parent = NULL_NODE;		
+		// Remove parent node.
+		FreeNode(parent);
+	}
+}
+
+void b3DynamicTree::WalkBackNodeAndCombineVolumes(i32 node) 
+{
+	while (node != NULL_NODE) 
+	{
+		//@todo node = Balance(node);
+
+		i32 child1 = m_nodes[node].child1;
+		i32 child2 = m_nodes[node].child2;
+
+		B3_ASSERT(child1 != NULL_NODE);
+		B3_ASSERT(child2 != NULL_NODE);
+
+		m_nodes[node].height = 1 + b3Max(m_nodes[child1].height, m_nodes[child2].height);
+		m_nodes[node].aabb = b3Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+		node = m_nodes[node].parent;
+	}
+}
+
+void b3DynamicTree::Validate(i32 nodeID) const 
+{
+	if (nodeID == NULL_NODE) 
+	{
+		return;
+	}
+
+	// The root node has no parent.
+	if (nodeID == m_root) 
+	{
+		B3_ASSERT(m_nodes[nodeID].parent == NULL_NODE);
+	}
+
+	const b3Node* node = m_nodes + nodeID;
+
+	i32 child1 = node->child1;
+	i32 child2 = node->child2;
+
+	if (node->IsLeaf()) 
+	{
+		// Leaf nodes has no children and its height is zero.
+		B3_ASSERT(child1 == NULL_NODE);
+		B3_ASSERT(child2 == NULL_NODE);
+		B3_ASSERT(node->height == 0);
+	}
+	else 
+	{
+		B3_ASSERT(0 <= child1 && child1 < m_nodeCapacity);
+		B3_ASSERT(0 <= child2 && child2 < m_nodeCapacity);
+
+		// The parent of its children is its parent (really?!).
+
+		B3_ASSERT(m_nodes[child1].parent == nodeID);
+		B3_ASSERT(m_nodes[child2].parent == nodeID);
+
+		// Walk down the tree.
+		Validate(child1);
+		Validate(child2);
+	}
+}
+
+void b3DynamicTree::Draw(b3Draw* b3Draw) const
+{
+	b3Color red = b3Color(1.0f, 0.0f, 0.0f);
+	b3Color green = b3Color(0.0f, 1.0f, 0.0f);
+	b3Color blue = b3Color(0.0f, 0.0f, 1.0f);
+	b3Color purple = b3Color(1.0f, 0.0f, 1.0f);
+
+	if (m_nodeCount == 0)
+	{
+		return;
+	}
+
+	b3Stack<i32, 256> stack;
+	stack.Push(m_root);
+
+	while (!stack.IsEmpty())
+	{
+		i32 nodeIndex = stack.Top();
+		stack.Pop();
+
+		if (nodeIndex == NULL_NODE)
+		{
+			continue;
+		}
+
+		const b3Node* node = m_nodes + nodeIndex;
+		if (node->IsLeaf())
+		{
+			b3Draw->DrawAABB(node->aabb, purple);
+		}
+		else
+		{
+			b3Draw->DrawAABB(node->aabb, red);
+			
+			stack.Push(node->child1);
+			stack.Push(node->child2);
+		}
+	}
+}

src/bounce/collision/trees/static_tree.cpp

@@ -0,0 +1,228 @@
+/*
+* 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\collision\trees\static_tree.h>
+#include <bounce\common\template\stack.h>
+#include <algorithm>
+
+b3StaticTree::b3StaticTree()
+{
+	m_nodes = nullptr;
+	m_nodeCount = 0;
+}
+
+b3StaticTree::~b3StaticTree()
+{
+	b3Free(m_nodes);
+}
+
+void b3StaticTree::Build(u32* ids, const b3AABB3* set, u32 N)
+{
+	B3_ASSERT(N > 0);
+
+	// Leafs = N, Internals = N - 1, Total = 2N - 1, if we assume
+	// each leaf node contains exactly 1 object.
+	const u32 kMinObjectsPerLeaf = 1;
+
+	u32 internalCapacity = N - 1;
+	u32 leafCapacity = N;
+	u32 nodeCapacity = 2 * N - 1;
+
+	u32 internalCount = 0;
+	u32 leafCount = 0;
+
+	m_nodes = (b3Node*)b3Alloc(nodeCapacity * sizeof(b3Node));
+	m_nodeCount = 1;
+
+	struct b3Params
+	{
+		u32 node;
+		u32* indices;
+		u32 numObjects;
+	};
+
+	struct b3SortPredicate
+	{
+		const b3AABB3* bs;
+		u32 axis;
+
+		bool operator()(const u32& i, const u32& j) const
+		{
+			const b3AABB3* b1 = bs + i;
+			const b3AABB3* b2 = bs + j;
+
+			b3Vec3 c1 = b1->Centroid();
+			b3Vec3 c2 = b2->Centroid();
+
+			if (c1[axis] < c2[axis])
+			{
+				return true;
+			}
+
+			return false;
+		}
+	};
+
+	b3Stack<b3Params, 256> stack;
+
+	{
+		b3Params params;
+		params.node = 0;
+		params.indices = ids;
+		params.numObjects = N;
+		stack.Push(params);
+	}
+
+	while (stack.Count() > 0)
+	{
+		b3Params params = stack.Top();
+		stack.Pop();
+
+		u32 nodeIndex = params.node;
+		u32* indices = params.indices;
+		u32 numObjects = params.numObjects;
+
+		B3_ASSERT(numObjects > 0);
+		
+		// "Allocate" node
+		b3Node* node = m_nodes + nodeIndex;
+
+		// Enclose set
+		b3AABB3 setAABB = set[indices[0]];
+		for (u32 i = 1; i < numObjects; ++i)
+		{
+			setAABB = b3Combine(setAABB, set[indices[i]]);
+		}
+
+		node->aabb = setAABB;
+
+		if (numObjects <= kMinObjectsPerLeaf)
+		{
+			++leafCount;
+			node->child1 = NULL_NODE_S;
+			node->index = indices[0];
+		}
+		else
+		{
+			++internalCount;
+
+			u32 splitAxis = setAABB.GetLongestAxisIndex();
+			float32 splitPos = setAABB.Centroid()[splitAxis];
+
+			// Sort along longest axis
+			b3SortPredicate pred;
+			pred.axis = splitAxis;
+			pred.bs = set;
+			std::sort(indices, indices + numObjects, pred);
+
+			// Find the object that splits the set in two subsets.
+			u32 left = 0;
+			u32 right = numObjects - 1;
+			u32 middle = left;
+			while (middle < right)
+			{
+				b3Vec3 center = set[indices[middle]].Centroid();
+				if (center[splitAxis] > splitPos)
+				{
+					// Found median.
+					break;
+				}
+				++middle;
+			}
+			
+			B3_ASSERT(middle >= left);
+			B3_ASSERT(middle <= right);
+
+			// Ensure we don't have empty subsets.
+			u32 count1 = middle;
+			u32 count2 = numObjects - middle;
+			if (count1 == 0 || count2 == 0)
+			{
+				// Split at object median.
+				middle = (left + right) / 2;
+				count1 = middle;
+				count2 = numObjects - middle;
+			}
+
+			B3_ASSERT(count1 > 0 && count2 > 0);
+
+			B3_ASSERT(m_nodeCount < nodeCapacity);
+			node->child1 = m_nodeCount;
+			++m_nodeCount;
+
+			B3_ASSERT(m_nodeCount < nodeCapacity);
+			node->child2 = m_nodeCount;
+			++m_nodeCount;
+
+			// Repeat for childs
+			b3Params params1;
+			params1.node = node->child1;
+			params1.indices = indices;
+			params1.numObjects = count1;
+			stack.Push(params1);
+
+			b3Params params2;
+			params2.node = node->child2;
+			params2.indices = indices + middle;
+			params2.numObjects = count2;
+			stack.Push(params2);
+		}
+	}
+	
+	B3_ASSERT(leafCount == leafCapacity);
+	B3_ASSERT(internalCount == internalCapacity);
+	B3_ASSERT(m_nodeCount == nodeCapacity);
+}
+
+void b3StaticTree::Draw(b3Draw* b3Draw) const
+{
+	b3Color red = b3Color(1.0f, 0.0f, 0.0f, 1.0f);
+	b3Color green = b3Color(0.0f, 1.0f, 0.0f, 1.0f);
+	b3Color blue = b3Color(0.0f, 0.0f, 1.0f, 1.0f);
+	b3Color purple = b3Color(1.0f, 0.0f, 1.0f, 1.0f);
+
+	if (m_nodeCount == 0)
+	{
+		return;
+	}
+
+	u32 root = 0;
+
+	b3Stack<u32, 256> stack;
+	stack.Push(root);
+
+	while (!stack.IsEmpty())
+	{
+		u32 nodeIndex = stack.Top();
+
+		stack.Pop();
+
+		const b3Node* node = m_nodes + nodeIndex;
+		if (node->IsLeaf())
+		{
+			b3Draw->DrawAABB(node->aabb, purple);
+		}
+		else
+		{
+			b3Draw->DrawAABB(node->aabb, red);
+			
+			stack.Push(node->child1);
+			stack.Push(node->child2);
+		}
+	}
+}

src/bounce/common/math/mat.cpp

@@ -0,0 +1,182 @@
+/*
+* 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\common\math\mat22.h>
+#include <bounce\common\math\mat33.h>
+
+b3Vec2 b3Mat22::Solve(const b3Vec2& b) const
+{
+	// Cramer's rule
+	float32 a11 = x.x, a12 = y.x;
+	float32	a21 = x.y, a22 = y.y;
+
+	float32 det = a11 * a22 - a12 * a21;
+
+	if (det != 0.0f)
+	{
+		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;
+}
+
+b3Mat22 b3Inverse(const b3Mat22& A)
+{
+	float32 a11 = A.x.x, a12 = A.y.x;
+	float32	a21 = A.x.y, a22 = A.y.y;
+
+	float32 det = a11 * a22 - a12 * a21;
+	
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	
+	b3Mat22 B;
+	B.x.x = det * a22;	B.y.x = -det * a12;
+	B.x.y = -det * a21;	B.y.y = det * a11;
+	return B;
+}
+
+b3Vec3 b3Mat33::Solve(const b3Vec3& b) const
+{
+	// Cramer's rule
+	float32 det = b3Det(x, y, z);
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	
+	b3Vec3 xn;
+	xn.x = det * b3Det(b, y, z);
+	xn.y = det * b3Det(x, b, z);
+	xn.z = det * b3Det(x, y, b);
+	return xn;
+}
+
+b3Mat33 b3Adjucate(const b3Mat33& A)
+{
+	b3Vec3 c1 = b3Cross(A.y, A.z);
+	b3Vec3 c2 = b3Cross(A.z, A.x);
+	b3Vec3 c3 = b3Cross(A.x, A.y);
+
+	b3Mat33 B;
+	B.x.x = c1.x; B.x.y = c2.x; B.x.z = c3.x;
+	B.y.x = c1.y; B.y.y = c2.y; B.y.z = c3.y;
+	B.z.x = c1.z; B.z.y = c2.z; B.z.z = c3.z;
+	return B;
+}
+
+b3Mat33 b3Inverse(const b3Mat33& A)
+{
+	// Cofactor method
+	float32 det = b3Det(A.x, A.y, A.z);
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	return det * b3Adjucate(A);
+}
+
+bool b3Solve(float32* b, float32* A, u32 n)
+{
+	// Gaussian Elimination.
+
+	// Loop through the diagonal elements.
+	for (u32 pivot = 0; pivot < n; ++pivot)
+	{
+		// Find the largest element in the current column.
+		u32 maxRow = pivot;
+		float32 maxElem = b3Abs(A[maxRow + n * pivot]);
+		for (u32 i = pivot + 1; i < n; ++i)
+		{
+			float32 e = b3Abs(A[i + n * pivot]);
+			if (e > maxElem)
+			{
+				maxElem = e;
+				maxRow = i;
+			}
+		}
+
+		// Singularity check.
+		if (b3Abs(maxElem) <= B3_EPSILON)
+		{
+			return false;
+		}
+
+		// Swap rowns if not in the current row.
+		if (maxRow != pivot)
+		{
+			// Swap the row.
+			for (u32 j = 0; j < n; ++j)
+			{
+				float32 a0 = A[maxRow + n * j];
+				A[maxRow + n * j] = A[pivot + n * j];
+				A[pivot + n * j] = a0;
+			}
+
+			// Swap b elements.
+			float32 b0 = b[maxRow];
+			b[maxRow] = b[pivot];
+			b[pivot] = b0;
+		}
+
+		// Divide current row by pivot.
+		float32 invPivot = 1.0f / A[n * pivot + pivot];
+		for (u32 j = 0; j < n; ++j)
+		{
+			A[pivot + n * j] *= invPivot;
+		}
+		b[pivot] *= invPivot;
+
+		// Ensure pivot is 1.
+		A[pivot + n * pivot] = 1.0f;
+
+		// Zero pivot column in other rows. 
+		for (u32 i = pivot + 1; i < n; ++i)
+		{
+			// Subtract multiple of pivot row from current row,
+			// such that pivot column element becomes 0.
+			float32 factor = A[i + n * pivot];
+
+			// Subtract multiple of row.
+			for (u32 j = 0; j < n; ++j)
+			{
+				A[i + n * j] -= factor * A[pivot + n * j];
+			}
+
+			b[i] -= factor * b[pivot];
+		}
+	}
+
+	// Backwards substitution. 
+	u32 p = n - 1;
+	do
+	{
+		--p;		
+		for (u32 j = p + 1; j < n; ++j)
+		{
+			b[p] -= A[p + n*j] * b[j];
+		}
+	} while (p > 0);
+
+	return true;
+}

src/bounce/common/memory/block_pool.cpp

@@ -0,0 +1,130 @@
+/*
+* 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\common\memory\block_pool.h>
+
+b3BlockPool::b3BlockPool(u32 blockSize)
+{
+	m_blockSize = blockSize;
+	m_chunkSize = b3_blockCount * m_blockSize;
+
+	m_chunks = nullptr;
+	m_chunkCount = 0;
+
+	// Pre-allocate some chunks
+	b3Chunk* chunk = (b3Chunk*)b3Alloc(sizeof(b3Chunk) + m_chunkSize);
+	++m_chunkCount;
+	chunk->freeBlocks = (b3Block*)((u8*)chunk + sizeof(b3Chunk));
+
+#ifdef _DEBUG
+	memset(chunk->freeBlocks, 0xcd, m_chunkSize);
+#endif
+
+	// Link the singly-linked list of the new block of the chunk.
+	for (u32 i = 0; i < b3_blockCount - 1; ++i)
+	{
+		b3Block* current = (b3Block*)((u8*)chunk->freeBlocks + i * blockSize);
+		current->next = (b3Block*)((u8*)chunk->freeBlocks + (i + 1) * blockSize);
+	}
+	b3Block* last = (b3Block*)((u8*)chunk->freeBlocks + (b3_blockCount - 1) * blockSize);
+	last->next = nullptr;
+
+	// Push back the new chunk of the singly-linked list of chunks.
+	chunk->next = m_chunks;
+	m_chunks = chunk;
+}
+
+b3BlockPool::~b3BlockPool()
+{
+	b3Chunk* c = m_chunks;
+	while (c)
+	{
+		b3Chunk* quack = c;
+		c = c->next;
+		b3Free(quack);
+		--m_chunkCount;
+	}
+	B3_ASSERT(m_chunkCount == 0);
+}
+
+void* b3BlockPool::Allocate()
+{
+	if (m_chunks)
+	{
+		if (m_chunks->freeBlocks)
+		{
+			b3Block* block = m_chunks->freeBlocks;
+			m_chunks->freeBlocks = block->next;
+			return block;
+		}
+	}
+
+	// Allocate a new chunk of memory.
+	b3Chunk* chunk = (b3Chunk*)b3Alloc(sizeof(b3Chunk) + m_chunkSize);
+	++m_chunkCount;
+	chunk->freeBlocks = (b3Block*)((u8*)chunk + sizeof(b3Chunk));
+
+#ifdef _DEBUG
+	memset(chunk->freeBlocks, 0xcd, m_chunkSize);
+#endif
+
+	// Link the singly-linked list of the new block of the chunk.
+	for (u32 i = 0; i < b3_blockCount - 1; ++i)
+	{
+		b3Block* current = (b3Block*)((u8*)chunk->freeBlocks + i * m_blockSize);
+		current->next = (b3Block*)((u8*)chunk->freeBlocks + (i + 1) * m_blockSize);
+	}
+	b3Block* last = (b3Block*)((u8*)chunk->freeBlocks + (b3_blockCount - 1) * m_blockSize);
+	last->next = nullptr;
+
+	// Push back the new chunk of the singly-linked list of chunks.
+	chunk->next = m_chunks;
+	m_chunks = chunk;
+
+	// Make the free block of the chunk available for the next allocation.
+	b3Block* block = m_chunks->freeBlocks;
+	m_chunks->freeBlocks = block->next;
+	return block;
+}
+
+void b3BlockPool::Free(void* p)
+{
+#ifdef _DEBUG
+	// Verify the block was allocated from this allocator.
+	bool found = false;
+	b3Chunk* c = m_chunks;
+	for (u32 i = 0; i < m_chunkCount; ++i)
+	{
+		b3Chunk* chunk = c;
+		// Memory aabb test.
+		b3Block* blocks = (b3Block*)((u8*)chunk + sizeof(b3Chunk));
+		if ((u8*)blocks <= (u8*)p && (u8*)p + m_blockSize <= (u8*)blocks + m_chunkSize)
+		{
+			found = true;
+			break;
+		}
+		c = c->next;
+	}
+	B3_ASSERT(found);
+	memset(p, 0xfd, m_blockSize);
+#endif
+
+	b3Block* block = (b3Block*)p;
+	block->next = m_chunks->freeBlocks;
+	m_chunks->freeBlocks = block;
+}

src/bounce/common/memory/stack_allocator.cpp

@@ -0,0 +1,83 @@
+/*
+* 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\common\memory\stack_allocator.h>
+
+b3StackAllocator::b3StackAllocator() 
+{
+	m_blockCapacity = 256;
+	m_blocks = (b3Block*)b3Alloc(m_blockCapacity * sizeof(b3Block));
+	m_blockCount = 0;
+	m_allocatedSize = 0;
+}
+
+b3StackAllocator::~b3StackAllocator() 
+{
+	B3_ASSERT(m_allocatedSize == 0);
+	B3_ASSERT(m_blockCount == 0);
+	b3Free(m_blocks);
+}
+
+void* b3StackAllocator::Allocate(u32 size) 
+{
+	if (m_blockCount == m_blockCapacity) 
+	{
+		// Then duplicate capacity if needed.
+		b3Block* oldBlocks = m_blocks;
+		m_blockCapacity *= 2;
+		m_blocks = (b3Block*)b3Alloc(m_blockCapacity * sizeof(b3Block));
+		memcpy(m_blocks, oldBlocks, m_blockCount * sizeof(b3Block));
+		b3Free(oldBlocks);
+	}
+
+	b3Block* block = m_blocks + m_blockCount;
+	block->size = size;
+	if (m_allocatedSize + size > b3_maxStackSize) 
+	{
+		// Allocate with parent allocator.
+		block->data = (u8*) b3Alloc(size);
+		block->parent = true;
+	}
+	else 
+	{
+		// Use this stack memory.
+		block->data = m_memory + m_allocatedSize;
+		block->parent = false;
+		m_allocatedSize += size;
+	}
+	
+	++m_blockCount;
+
+	return block->data;
+}
+
+void b3StackAllocator::Free(void* p) 
+{
+	B3_ASSERT(m_blockCount > 0);
+	b3Block* block = m_blocks + m_blockCount - 1;
+	B3_ASSERT(block->data == p);
+	if (block->parent) 
+	{
+		b3Free(p);
+	}
+	else 
+	{
+		m_allocatedSize -= block->size;
+	}
+	--m_blockCount;
+}

src/bounce/common/settings.cpp

@@ -0,0 +1,48 @@
+/*
+* 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\common\settings.h>
+#include <bounce\common\math\math.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
+
+u32 b3_allocCalls;
+u32 b3_maxAllocCalls;
+
+b3Version b3_version = { 1, 0, 0 };
+
+void* b3Alloc(u32 size) 
+{
+	++b3_allocCalls;
+	b3_maxAllocCalls = b3Max(b3_maxAllocCalls, b3_allocCalls);
+	return malloc(size);
+}
+
+void b3Free(void* block) 
+{
+	return free(block);
+}
+
+void b3Log(const char* text, ...) 
+{
+	va_list args;
+	va_start(args, text);
+	vprintf(text, args);
+	va_end(args);
+}

src/bounce/common/time.cpp

@@ -0,0 +1,78 @@
+/*
+* 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\common\time.h>
+
+// This file contains platform dependent code 
+// and may not compile depending of the OS.
+
+#if (_WIN32 == 1)
+
+#include <Windows.h>
+
+float64 GetCyclesPerSecond()
+{
+	LARGE_INTEGER integer;
+	QueryPerformanceFrequency(&integer);
+	return float64(integer.QuadPart);
+}
+
+u64 GetCycleCount()
+{
+	LARGE_INTEGER integer;
+	QueryPerformanceCounter(&integer);
+	return integer.QuadPart;
+}
+
+#endif
+
+float64 b3Time::m_invFrequency = 0.0;
+
+b3Time::b3Time()
+{
+	m_lastTime = 0;
+	m_curTime = 0;
+
+	if (m_invFrequency == 0.0)
+	{
+		float64 cyclesPerSec = GetCyclesPerSecond();
+		float64 secPerCycles = 1.0 / cyclesPerSec;
+		float64 milisecPerCycles = 1000.0 * secPerCycles;
+		m_invFrequency = milisecPerCycles;
+	}
+
+	m_lastRealTime = GetCycleCount();
+}
+
+void b3Time::Update() 
+{
+	// Retrieve the current time in units of cycles.
+	u64 curTime = GetCycleCount();
+	u64 dt = curTime - m_lastRealTime;
+	m_lastRealTime = curTime;
+		
+	float64 dtMs = m_invFrequency * float64(dt);
+	
+	UpdateBy(dtMs);
+}
+
+void b3Time::UpdateBy(float64 delta)
+{
+	m_lastTime = m_curTime;
+	m_curTime += delta;
+}

src/bounce/dynamics/body.cpp

@@ -0,0 +1,361 @@
+/*
+* 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\body.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\joints\joint.h>
+#include <bounce\dynamics\contacts\contact.h>
+
+b3Body::b3Body(const b3BodyDef& def, b3World* world) 
+{
+	m_world = world;
+	m_type = def.type;
+	m_flags = 0;
+	
+	if (def.awake)
+	{
+		m_flags |= e_awakeFlag;
+	}
+	
+	if (m_type == e_dynamicBody) 
+	{
+		m_mass = 1.0f;
+		m_invMass = 1.0f;
+
+		if (def.fixedRotationX)
+		{
+			m_flags |= e_fixedRotationX;
+		}
+
+		if (def.fixedRotationY)
+		{
+			m_flags |= e_fixedRotationY;
+		}
+
+		if (def.fixedRotationZ)
+		{
+			m_flags |= e_fixedRotationZ;
+		}
+	}
+	else 
+	{
+		m_mass = 0.0f;
+		m_invMass = 0.0f;
+	}
+		
+	m_I.SetZero();
+	m_invI.SetZero();
+	m_worldInvI.SetZero();
+
+	m_force.SetZero();
+	m_torque.SetZero();
+	
+	m_linearVelocity = def.linearVelocity;
+	m_angularVelocity = def.angularVelocity;
+
+	m_sweep.localCenter.SetZero();
+	m_sweep.worldCenter = def.position;
+	m_sweep.orientation = def.orientation;
+	m_sweep.worldCenter0 = def.position;
+	m_sweep.orientation0 = def.orientation;
+	m_sweep.t0 = 0.0f;
+
+	m_xf.position = m_sweep.worldCenter;
+	m_xf.rotation = b3ConvertQuatToRot(m_sweep.orientation);
+	
+	m_gravityScale = def.gravityScale;
+	m_userData = def.userData;
+	m_islandID = -1;
+	m_sleepTime = 0.0f;
+}
+
+b3Shape* b3Body::CreateShape(const b3ShapeDef& def) 
+{
+	// Create the shape with the definition.
+	b3Shape* shape = b3Shape::Create(def);
+	shape->m_body = this;
+	shape->m_isSensor = def.isSensor;
+	shape->m_userData = def.userData;
+	shape->m_density = def.density;
+	shape->m_friction = def.friction;
+	shape->m_restitution = def.restitution;
+	
+	// Add the shape to this body shape list.
+	m_shapeList.PushFront(shape);
+	
+	// Since a new shape was added the new mass properties of 
+	// this body need to be recomputed.
+	if (shape->m_density > 0.0f) 
+	{
+		ResetMass();
+	}
+
+	// Compute the world AABB of the new shape and assign a broad-phase proxy to it.
+	b3Transform xf = shape->GetTransform();
+	
+	b3AABB3 aabb;
+	shape->ComputeAABB(&aabb, xf);
+	shape->m_broadPhaseID = m_world->m_contactMan.m_broadPhase.CreateProxy(aabb, shape);
+
+	// Tell the world that a new shape was added so new contacts can be created.
+	m_world->m_flags |= b3World::e_shapeAddedFlag;
+
+	return shape;
+}
+
+void b3Body::DestroyContacts() 
+{
+	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
+	{
+		s->DestroyContacts();
+	}
+}
+
+void b3Body::DestroyJoints() 
+{
+	b3JointEdge* je = m_jointEdges.m_head;
+	while (je)
+	{
+		b3JointEdge* tmp = je;
+		je = je->m_next;
+		m_world->m_jointMan.Destroy(tmp->joint);
+	}
+}
+
+void b3Body::DestroyShape(b3Shape* shape) 
+{
+	// Remove the shape from this body shape list.
+	B3_ASSERT(shape->m_body == this);
+	m_shapeList.Remove(shape);
+
+	// Destroy any contacts associated with the shape.
+	shape->DestroyContacts();
+	
+	// Destroy the broad-phase proxy associated with the shape.
+	m_world->m_contactMan.m_broadPhase.DestroyProxy(shape->m_broadPhaseID);
+	
+	// Destroy the shape.
+	b3Shape::Destroy(shape);
+
+	// Recalculate the new inertial properties of this body.
+	ResetMass();
+}
+
+void b3Body::DestroyShapes() 
+{
+	// Destroy all shapes that belong to this body.
+	b3Shape* s = m_shapeList.m_head;
+	while (s) 
+	{
+		b3Shape* s0 = s;
+		s = s->m_next;
+		s0->DestroyContacts();
+		m_world->m_contactMan.m_broadPhase.DestroyProxy(s0->m_broadPhaseID);
+		m_shapeList.Remove(s0);
+		b3Shape::Destroy(s0);
+	}
+}
+
+void b3Body::SynchronizeTransform()
+{
+	m_xf = m_sweep.GetTransform(1.0f);
+}
+
+void b3Body::SynchronizeShapes() 
+{
+	b3Transform xf0;
+	xf0.position = m_sweep.worldCenter0;
+	xf0.rotation = b3ConvertQuatToRot(m_sweep.orientation0);
+	
+	b3Transform xf1 = m_xf;
+	
+	b3Vec3 displacement = xf1.position - xf0.position;
+
+	// Update the AABBs of all shapes.
+	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
+	{
+		b3AABB3 aabb;
+		s->ComputeAABB(&aabb, xf1);		
+		m_world->m_contactMan.m_broadPhase.MoveProxy(s->m_broadPhaseID, aabb, displacement);
+	}
+}
+
+void b3Body::ResetMass() 
+{
+	// Set mass and inertia tensor to zero.
+	m_mass = 0.0f;
+	m_invMass = 0.0f;
+	m_I.SetZero();
+	m_invI.SetZero();
+	m_worldInvI.SetZero();
+
+	// Static and kinematic bodies have zero mass.
+	if (m_type == e_staticBody || m_type == e_kinematicBody)
+	{
+		return;
+	}
+
+	// Compute mass data for each shape. Each shape contributes to 
+	// this body mass data.
+	b3Vec3 localCenter;
+	localCenter.SetZero();
+	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
+	{
+		if (s->m_density == 0.0f) 
+		{
+			continue;
+		}
+		
+		b3MassData massData;
+		s->ComputeMass(&massData, s->m_density);
+		localCenter += massData.mass * massData.center;
+		m_mass += massData.mass;
+		m_I += massData.I;
+	}
+
+	if (m_mass > 0.0f) 
+	{
+		m_invMass = 1.0f / m_mass;
+		localCenter *= m_invMass;
+		m_invI = b3Inverse(m_I);
+		m_worldInvI = b3RotateToFrame(m_invI, m_xf.rotation);
+	}
+	else 
+	{
+		// Dynamic bodies have positive mass.
+		m_mass = 1.0f;
+		m_invMass = 1.0f;
+	}
+
+	// Fix rotation.
+	if (m_flags & e_fixedRotationX)
+	{
+		m_I.y.y = 0.0f;
+		m_I.z.y = 0.0f;
+		m_I.y.z = 0.0f;
+		m_I.z.z = 0.0f;
+
+		m_invI.y.y = 0.0f;
+		m_invI.z.y = 0.0f;
+		m_invI.y.z = 0.0f;
+		m_invI.z.z = 0.0f;
+
+		m_worldInvI.y.y = 0.0f;
+		m_worldInvI.z.y = 0.0f;
+		m_worldInvI.y.z = 0.0f;
+		m_worldInvI.z.z = 0.0f;
+	}
+	
+	if (m_flags & e_fixedRotationY)
+	{
+		m_I.x.x = 0.0f;
+		m_I.x.z = 0.0f;
+		m_I.z.x = 0.0f;
+		m_I.z.z = 0.0f;
+
+		m_invI.x.x = 0.0f;
+		m_invI.x.z = 0.0f;
+		m_invI.z.x = 0.0f;
+		m_invI.z.z = 0.0f;
+		
+		m_worldInvI.x.x = 0.0f;
+		m_worldInvI.x.z = 0.0f;
+		m_worldInvI.z.x = 0.0f;
+		m_worldInvI.z.z = 0.0f;
+	}
+	
+	if (m_flags & e_fixedRotationZ)
+	{
+		m_I.x.x = 0.0f;
+		m_I.x.y = 0.0f;
+		m_I.y.x = 0.0f;
+		m_I.y.y = 0.0f;
+
+		m_invI.x.x = 0.0f;
+		m_invI.x.y = 0.0f;
+		m_invI.y.x = 0.0f;
+		m_invI.y.y = 0.0f;
+
+		m_worldInvI.x.x = 0.0f;
+		m_worldInvI.x.y = 0.0f;
+		m_worldInvI.y.x = 0.0f;
+		m_worldInvI.y.y = 0.0f;
+	}
+
+	// Update center of mass.
+	m_sweep.localCenter = localCenter;
+	
+	b3Vec3 worldCenter0 = m_sweep.worldCenter;
+	m_sweep.worldCenter = b3Mul(m_xf, localCenter);
+	m_sweep.worldCenter0 = m_sweep.worldCenter;
+
+	// Update center of mass velocity.
+	m_linearVelocity += b3Cross(m_angularVelocity, m_sweep.worldCenter - worldCenter0);
+}
+
+bool b3Body::ShouldCollide(const b3Body* other) const
+{
+	if (m_type != e_dynamicBody && other->m_type != e_dynamicBody)
+	{
+		// At least one body should be kinematic or dynamic.
+		return false;
+	}
+
+	// Check if there are joints that connects
+	// this body with the other body and if the joint was configured 
+	// to let not their collision occur.
+	for (b3JointEdge* je = m_jointEdges.m_head; je; je = je->m_next)
+	{
+		b3Joint* j = je->joint;
+		if (je->other == other)
+		{
+			if (j->m_collideLinked == false)
+			{
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+void b3Body::Dump() const
+{
+	i32 bodyIndex = m_islandID;
+
+	b3Log("		{\n");
+	b3Log("		b3BodyDef bd;\n");
+	b3Log("		bd.type = (b3BodyType) %d;\n", m_type);
+	b3Log("		bd.position.Set(%f, %f, %f);\n", m_sweep.worldCenter.x, m_sweep.worldCenter.y, m_sweep.worldCenter.z);
+	b3Log("		bd.orientation.Set(%f, %f, %f, %f);\n", m_sweep.orientation.x, m_sweep.orientation.y, m_sweep.orientation.z, m_sweep.orientation.w);
+	b3Log("		bd.linearVelocity.Set(%f, %f, %f);\n", m_linearVelocity.x, m_linearVelocity.y, m_linearVelocity.z);
+	b3Log("		bd.angularVelocity.Set(%f, %f, %f);\n", m_angularVelocity.x, m_angularVelocity.y, m_angularVelocity.z);
+	b3Log("		bd.gravityScale = %f;\n", m_gravityScale);
+	b3Log("		bd.awake = %d;\n", m_flags & e_awakeFlag);
+	b3Log("		\n");
+	b3Log("		bodies[%d] = world.CreateBody(bd);\n");
+	b3Log("		\n");
+	for (b3Shape* s = m_shapeList.m_head; s; s = s->m_next)
+	{
+		b3Log("		{\n");
+		s->Dump(bodyIndex);
+		b3Log("		}\n");
+	}
+	b3Log("		}\n");
+}

src/bounce/dynamics/contact_manager.cpp

@@ -0,0 +1,306 @@
+/*
+* 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\contact_manager.h>
+#include <bounce\dynamics\contacts\convex_contact.h>
+#include <bounce\dynamics\contacts\mesh_contact.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\world_listeners.h>
+
+b3ContactManager::b3ContactManager() : 
+	m_convexBlocks(sizeof(b3ConvexContact)),
+	m_meshBlocks(sizeof(b3MeshContact))
+{
+	m_contactListener = nullptr;
+	m_contactFilter = nullptr;
+}
+
+void b3ContactManager::AddPair(void* dataA, void* dataB) 
+{
+	b3Shape* shapeA = (b3Shape*)dataA;
+	b3Shape* shapeB = (b3Shape*)dataB;
+
+	b3Body* bodyA = shapeA->GetBody();
+	b3Body* bodyB = shapeB->GetBody();
+	
+	if (bodyA == bodyB) 
+	{
+		// Two shapes that belong to the same body cannot collide.
+		return;
+	}
+
+	// Check if there is a contact between the two shapes.
+	// Search the list A or B. The shorter if possible.
+	for (b3ContactEdge* ce = shapeB->m_contactEdges.m_head; ce; ce = ce->m_next)
+	{
+		if (ce->other == shapeA)
+		{
+			b3Contact* c = ce->contact;
+			
+			b3Shape* sA = c->GetShapeA();
+			b3Shape* sB = c->GetShapeB();
+
+			if (sA == shapeA && sB == shapeB)
+			{
+				// A contact already exists.
+				return;
+			}
+
+			if (sB == shapeA && sA == shapeB)
+			{
+				// A contact already exists.
+				return;
+			}
+		}
+	}
+
+	// Check if a joint prevents collision between the bodies.
+	if (bodyA->ShouldCollide(bodyB) == false)
+	{
+		// The bodies must not collide with each other.
+		return;
+	}
+
+	// Check if the contact filter prevents the collision.
+	if (m_contactFilter)
+	{
+		if (m_contactFilter->ShouldCollide(shapeA, shapeB) == false)
+		{
+			return;
+		}
+	}
+
+	// Allocate a new contact.
+	b3Contact* c = Create(shapeA, shapeB);
+
+	// Get the shapes from the contact again
+	// because contact creation will swap the shapes if typeA > typeB.
+	shapeA = c->GetShapeA();
+	shapeB = c->GetShapeB();
+	bodyA = shapeA->GetBody();
+	bodyB = shapeB->GetBody();
+
+	c->m_flags = 0;
+	b3OverlappingPair* pair = &c->m_pair;
+
+	// Initialize edge A
+	pair->edgeA.contact = c;
+	pair->edgeA.other = shapeB;
+	
+	// Add edge A to shape A's contact list.
+	shapeA->m_contactEdges.PushFront(&pair->edgeA);
+
+	// Initialize edge B
+	pair->edgeB.contact = c;
+	pair->edgeB.other = shapeA;
+
+	// Add edge B to shape B's contact list.
+	shapeB->m_contactEdges.PushFront(&pair->edgeB);
+
+	// Awake the bodies if both are not sensors.
+	if (!shapeA->IsSensor() && !shapeB->IsSensor()) 
+	{
+		bodyA->SetAwake(true);
+		bodyB->SetAwake(true);
+	}
+
+	// Add the contact to the world contact list.
+	m_contactList.PushFront(c);
+	
+	if (c->m_type == b3ContactType::e_meshContact)
+	{
+		// Add the contact to the world mesh contact list.
+		b3MeshContact* mc = (b3MeshContact*)c;
+		
+		// Find new shape-child overlapping pairs.
+		mc->FindNewPairs();
+
+		b3MeshContactLink* link = &mc->m_link;
+		link->m_c = mc;
+		m_meshContactList.PushFront(link);
+	}
+}
+
+void b3ContactManager::SynchronizeShapes()
+{
+	b3MeshContactLink* c = m_meshContactList.m_head;
+	while (c)
+	{
+		c->m_c->SynchronizeShapes();
+		c = c->m_next;
+	}
+}
+
+// Find potentially overlapping shape pairs.
+void b3ContactManager::FindNewContacts()
+{
+	m_broadPhase.FindNewPairs(this);
+
+	b3MeshContactLink* c = m_meshContactList.m_head;
+	while (c)
+	{
+		c->m_c->FindNewPairs();
+		c = c->m_next;
+	}
+}
+
+void b3ContactManager::UpdateContacts() 
+{	
+	// Update the state of all contacts.
+	b3Contact* c = m_contactList.m_head;
+	while (c)
+	{
+		b3OverlappingPair* pair = &c->m_pair;
+
+		b3Shape* shapeA = pair->shapeA;
+		i32 proxyA = shapeA->m_broadPhaseID;
+		b3Body* bodyA = shapeA->m_body;
+		
+		b3Shape* shapeB = pair->shapeB;
+		i32 proxyB = shapeB->m_broadPhaseID;
+		b3Body* bodyB = shapeB->m_body;
+		
+		// Check if the bodies must not collide with each other.
+		if (bodyA->ShouldCollide(bodyB) == false)
+		{
+			b3Contact* quack = c;
+			c = c->m_next;
+			Destroy(quack);
+			continue;
+		}
+
+		// Check for external filtering.
+		if (m_contactFilter)
+		{
+			if (m_contactFilter->ShouldCollide(shapeA, shapeB) == false)
+			{
+				// The user has stopped the contact.
+				b3Contact* quack = c;
+				c = c->m_next;
+				Destroy(quack);
+				continue;
+			}
+		}
+
+		// At least one body must be dynamic or kinematic.
+		bool activeA = bodyA->IsAwake() && bodyA->m_type != e_staticBody;
+		bool activeB = bodyB->IsAwake() && bodyB->m_type != e_staticBody;
+		if (activeA == false && activeB == false) 
+		{
+			c = c->m_next;
+			continue;
+		}
+
+		// Destroy the contact if the shape AABBs are not overlapping.
+		bool overlap = m_broadPhase.TestOverlap(proxyA, proxyB);
+		if (overlap == false)
+		{
+			b3Contact* quack = c;
+			c = c->m_next;
+			Destroy(quack);
+			continue;
+		}
+
+		// The contact persists.
+		c->Update(m_contactListener);
+
+		c = c->m_next;
+	}
+}
+
+b3Contact* b3ContactManager::Create(b3Shape* shapeA, b3Shape* shapeB) 
+{
+	b3ShapeType typeA = shapeA->GetType();
+	b3ShapeType typeB = shapeB->GetType();
+
+	if (typeA > typeB) 
+	{
+		b3Swap(typeA, typeB);
+		b3Swap(shapeA, shapeB);
+	}
+
+	B3_ASSERT(typeA <= typeB);
+
+	b3Contact* c = nullptr;
+	if (typeA != e_meshShape && typeB != e_meshShape) 
+	{
+		void* block = m_convexBlocks.Allocate();
+		b3ConvexContact* cxc = new (block) b3ConvexContact(shapeA, shapeB);
+		c = cxc;
+	}
+	else 
+	{
+		if (typeB == e_meshShape) 
+		{
+			void* block = m_meshBlocks.Allocate();
+			b3MeshContact* mxc = new (block) b3MeshContact(shapeA, shapeB);
+			c = mxc;
+		}
+		else 
+		{
+			// Collisions between meshes are not implemented.
+			//B3_ASSERT(false);
+		}
+	}
+
+	// The shapes might be swapped.
+	c->m_pair.shapeA = shapeA;
+	c->m_pair.shapeB = shapeB;
+	return c;
+}
+
+void b3ContactManager::Destroy(b3Contact* c) 
+{
+	// Report to the contact listener that the contact will be destroyed.
+	if (m_contactListener) 
+	{
+		if (c->IsOverlapping()) 
+		{
+			m_contactListener->EndContact(c);
+		}
+	}
+	
+	b3OverlappingPair* pair = &c->m_pair;
+	
+	b3Shape* shapeA = c->GetShapeA();
+	b3Shape* shapeB = c->GetShapeB();
+	
+	shapeA->m_contactEdges.Remove(&pair->edgeA);
+	shapeB->m_contactEdges.Remove(&pair->edgeB);
+
+	// Remove the contact from the world contact list.
+	m_contactList.Remove(c);
+
+	if (c->m_type == e_convexContact)
+	{
+		b3ConvexContact* cc = (b3ConvexContact*)c;
+		cc->~b3ConvexContact();
+		m_convexBlocks.Free(cc);
+	}
+	else
+	{
+		b3MeshContact* mc = (b3MeshContact*)c;
+		
+		// Remove the contact from the world mesh contact list.
+		m_meshContactList.Remove(&mc->m_link);
+		
+		mc->~b3MeshContact();
+		m_meshBlocks.Free(mc);
+	}
+}

src/bounce/dynamics/contacts/collide/clip.cpp

@@ -0,0 +1,280 @@
+/*
+* 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\contacts\collide\clip.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+
+void b3BuildEdge(b3ClipVertex vOut[2],
+	const b3Capsule* hull)
+{
+	vOut[0].position = hull->vertices[0];
+	vOut[0].pair = b3MakePair(0, B3_NULL_EDGE, 0, B3_NULL_EDGE);
+	
+	vOut[1].position = hull->vertices[1];
+	vOut[1].pair = b3MakePair(1, B3_NULL_EDGE, 0, B3_NULL_EDGE);
+}
+
+void b3BuildPolygon(b3ClipPolygon& pOut,
+	const b3Transform& xf, u32 index, const b3Hull* hull)
+{
+	B3_ASSERT(pOut.Count() == 0);
+
+	const b3Face* face = hull->GetFace(index);
+	const b3HalfEdge* begin = hull->GetEdge(face->edge);
+	const b3HalfEdge* edge = begin;
+	do
+	{
+		const b3HalfEdge* twin = hull->GetEdge(edge->twin);
+		u32 edgeId = twin->twin;
+		u32 previousEdgeId = twin->next;
+
+		b3ClipVertex clipVertex;
+		clipVertex.position = b3Mul(xf, hull->GetVertex(edge->origin));
+		clipVertex.pair = b3MakePair(previousEdgeId, B3_NULL_EDGE, edgeId, B3_NULL_EDGE);
+
+		pOut.PushBack(clipVertex);
+
+		edge = hull->GetEdge(edge->next);
+	} while (edge != begin);
+
+	B3_ASSERT(pOut.Count() > 2);
+}
+
+// Sutherland-Hodgman clipping.
+u32 b3ClipEdgeToPlane(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3ClipPlane& plane)
+{
+	u32 numOut = 0;
+
+	float32 distance1 = b3Distance(vIn[0].position, plane.plane);
+	float32 distance2 = b3Distance(vIn[1].position, plane.plane);
+
+	// If the points are behind the plane keep them
+	if (distance1 <= 0.0f)
+	{
+		vOut[numOut++] = vIn[0];
+	}
+
+	if (distance2 <= 0.0f)
+	{
+		vOut[numOut++] = vIn[1];
+	}
+
+	// If the points are on opposite sides keep intersection
+	if (distance1 <= 0.0f && distance2 > 0.0f)
+	{
+		float32 fraction = distance1 / (distance1 - distance2);
+		vOut[numOut].position = vIn[0].position + fraction * (vIn[1].position - vIn[0].position);
+		vOut[numOut].pair = b3MakePair(vIn[0].pair.inEdgeA, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
+		++numOut;
+	}
+	else if (distance1 > 0.0f && distance2 <= 0.0f)
+	{
+		float32 fraction = distance1 / (distance1 - distance2);
+		vOut[numOut].position = vIn[0].position + fraction * (vIn[1].position - vIn[0].position);
+		vOut[numOut].pair = b3MakePair(vIn[1].pair.inEdgeA, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
+		++numOut;
+	}
+
+	return numOut;
+}
+
+// Sutherland-Hodgman clipping.
+void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
+	const b3ClipPolygon& pIn, const b3ClipPlane& plane)
+{
+	B3_ASSERT(pIn.Count() > 0);
+	B3_ASSERT(pOut.Count() == 0);
+
+	b3ClipVertex v1 = pIn.Back();
+	float32 distance1 = b3Distance(v1.position, plane.plane);
+
+	for (u32 i = 0; i < pIn.Count(); ++i)
+	{
+		b3ClipVertex v2 = pIn[i];
+		float32 distance2 = b3Distance(v2.position, plane.plane);
+
+		if (distance1 <= 0.0f && distance2 <= 0.0f)
+		{
+			// Both vertices are behind or lying on the plane.
+			// Keep v2
+			pOut.PushBack(v2);
+		}
+		else if (distance1 <= 0.0f && distance2 > 0.0f)
+		{
+			// v1 is behind and v2 in front
+			// Keep intersection
+			float32 fraction = distance1 / (distance1 - distance2);
+
+			b3ClipVertex vertex;
+			vertex.position = v1.position + fraction * (v2.position - v1.position);
+			vertex.pair = b3MakePair(v1.pair.inEdgeA, B3_NULL_EDGE, B3_NULL_EDGE, plane.id);
+
+			pOut.PushBack(vertex);
+		}
+		else if (distance1 > 0.0f && distance2 <= 0.0f)
+		{
+			// v2 is behind and v1 in front
+			// Keep intersection and v2
+			float32 fraction = distance1 / (distance1 - distance2);
+
+			b3ClipVertex vertex;
+			vertex.position = v1.position + fraction * (v2.position - v1.position);
+			vertex.pair = b3MakePair(v1.pair.inEdgeA, plane.id, B3_NULL_EDGE, B3_NULL_EDGE);
+
+			pOut.PushBack(vertex);
+			pOut.PushBack(v2);
+		}
+
+		// Make v2 as the starting vertex of the next edge
+		v1 = v2;
+		distance1 = distance2;
+	}
+}
+
+// Clip a segment to edge side planes.
+u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3Capsule* hull)
+{
+	// Start from somewhere.
+	vOut[0] = vIn[0];
+	vOut[1] = vIn[1];
+	u32 numOut = 0;
+
+	b3Vec3 P1 = hull->vertices[0];
+	b3Vec3 Q1 = hull->vertices[1];
+	b3Vec3 E1 = Q1 - P1;
+
+	B3_ASSERT(b3Dot(E1, E1) > B3_EPSILON * B3_EPSILON);
+	b3ClipPlane clipPlane1;
+	clipPlane1.plane.normal = b3Normalize(E1);
+	clipPlane1.plane.offset = b3Dot(clipPlane1.plane.normal, Q1);
+	clipPlane1.id = 0;
+
+	b3ClipVertex clipEdge1[2];
+	numOut = b3ClipEdgeToPlane(clipEdge1, vOut, clipPlane1);
+
+	vOut[0] = clipEdge1[0];
+	vOut[1] = clipEdge1[1];
+
+	if (numOut < 2)
+	{
+		return numOut;
+	}
+
+	b3ClipPlane clipPlane2;
+	clipPlane2.plane.normal = -clipPlane1.plane.normal;
+	clipPlane2.plane.offset = b3Dot(clipPlane2.plane.normal, P1);
+	clipPlane2.id = 1;
+
+	b3ClipVertex clipEdge2[2];
+	numOut = b3ClipEdgeToPlane(clipEdge2, vOut, clipPlane2);
+
+	vOut[0] = clipEdge2[0];
+	vOut[1] = clipEdge2[1];
+
+	if (numOut < 2)
+	{
+		return numOut;
+	}
+
+	return numOut;
+}
+
+// Clip a segment to face side planes.
+u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3Transform& xf, u32 index, const b3Hull* hull)
+{
+	// Start from somewhere.
+	vOut[0] = vIn[0];
+	vOut[1] = vIn[1];
+	u32 numOut = 0;
+
+	const b3Face* face = hull->GetFace(index);
+	const b3HalfEdge* begin = hull->GetEdge(face->edge);
+	const b3HalfEdge* edge = begin;
+	do
+	{
+		const b3HalfEdge* twin = hull->GetEdge(edge->twin);
+		u32 edgeId = u32(twin->twin);
+
+		b3Plane plane = hull->GetEdgeSidePlane(edgeId);
+		plane.offset += B3_HULL_RADIUS_SUM;
+
+		b3ClipPlane clipPlane;
+		clipPlane.id = edgeId;
+		clipPlane.plane = b3Mul(xf, plane);
+
+		b3ClipVertex clipEdge[2];
+		numOut = b3ClipEdgeToPlane(clipEdge, vOut, clipPlane);
+
+		vOut[0] = clipEdge[0];
+		vOut[1] = clipEdge[1];
+
+		if (numOut == 0)
+		{
+			return numOut;
+		}
+
+		edge = hull->GetEdge(edge->next);
+	} while (edge != begin);
+
+	// Now vOut contains the clipped points.
+	return numOut;
+}
+
+// Clip a polygon to face side planes.
+void b3ClipPolygonToFace(b3ClipPolygon& pOut,
+	const b3ClipPolygon& pIn, const b3Transform& xf, u32 index, const b3Hull* hull)
+{
+	B3_ASSERT(pIn.Count() > 0);
+	B3_ASSERT(pOut.Count() == 0);
+
+	// Start from somewhere.
+	pOut = pIn;
+
+	const b3Face* face = hull->GetFace(index);
+	const b3HalfEdge* begin = hull->GetEdge(face->edge);
+	const b3HalfEdge* edge = begin;
+	do
+	{
+		const b3HalfEdge* twin = hull->GetEdge(edge->twin);
+		u32 edgeId = u32(twin->twin);
+
+		b3Plane plane = hull->GetEdgeSidePlane(edgeId);
+		plane.offset += B3_HULL_RADIUS_SUM;
+
+		b3ClipPlane clipPlane;
+		clipPlane.id = edgeId;
+		clipPlane.plane = b3Mul(xf, plane);
+
+		b3StackArray<b3ClipVertex, 32> clipPolygon;
+		b3ClipPolygonToPlane(clipPolygon, pOut, clipPlane);
+		pOut = clipPolygon;
+
+		if (pOut.IsEmpty())
+		{
+			return;
+		}
+
+		edge = hull->GetEdge(edge->next);
+	} while (edge != begin);
+
+	// Now pOut contains the clipped points.
+}

src/bounce/dynamics/contacts/collide/collide.cpp

@@ -0,0 +1,187 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\dynamics\shapes\mesh_shape.h>
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+#include <bounce\collision\shapes\mesh.h>
+#include <bounce\collision\distance.h>
+
+void b3ShapeGJKProxy::Set(const b3Shape* shape, u32 index)
+{
+	switch (shape->GetType())
+	{
+	case e_sphereShape:
+	{
+		const b3SphereShape* sphere = (b3SphereShape*)shape;
+		m_count = 1;
+		m_vertices = &sphere->m_center;
+		m_radius = sphere->m_radius;
+		break;
+	}
+	case e_capsuleShape:
+	{
+		const b3CapsuleShape* capsule = (b3CapsuleShape*)shape;
+		m_count = 2;
+		m_vertices = capsule->m_centers;
+		m_radius = capsule->m_radius;
+		break;
+	}
+	case e_hullShape:
+	{
+		const b3HullShape* hull = (b3HullShape*)shape;
+		m_count = hull->m_hull->vertexCount;
+		m_vertices = hull->m_hull->vertices;
+		m_radius = hull->m_radius;
+		break;
+	}
+	case e_meshShape:
+	{
+		const b3MeshShape* mesh = (b3MeshShape*)shape;
+
+		B3_ASSERT(index >= 0);
+		B3_ASSERT(index < mesh->m_mesh->triangleCount);
+
+		const b3Triangle& triangle = mesh->m_mesh->GetTriangle(index);
+
+		m_buffer[0] = mesh->m_mesh->vertices[triangle.v1];
+		m_buffer[1] = mesh->m_mesh->vertices[triangle.v2];
+		m_buffer[2] = mesh->m_mesh->vertices[triangle.v3];
+
+		m_count = 3;
+		m_vertices = m_buffer;
+		m_radius = mesh->m_radius;
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+}
+
+bool b3TestOverlap(const b3Transform& xfA, u32 indexA, const b3Shape* shapeA,
+	const b3Transform& xfB, u32 indexB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	b3ShapeGJKProxy proxyA(shapeA, indexA);
+	b3ShapeGJKProxy proxyB(shapeB, indexB);
+	
+	b3GJKOutput distance = b3GJK(xfA, proxyA, xfB, proxyB, true, &cache->simplexCache);
+
+	const float32 kTol = 2.0f * B3_EPSILON;
+	return distance.distance <= kTol;
+}
+
+void b3CollideSphereAndSphereShapes(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	B3_NOT_USED(cache);
+	b3SphereShape* hullA = (b3SphereShape*)shapeA;
+	b3SphereShape* hullB = (b3SphereShape*)shapeB;
+	b3CollideSphereAndSphere(manifold, xfA, hullA, xfB, hullB);
+}
+
+void b3CollideSphereAndHullShapes(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	B3_NOT_USED(cache);
+	b3SphereShape* hullA = (b3SphereShape*)shapeA;
+	b3HullShape* hullB = (b3HullShape*)shapeB;
+	b3CollideSphereAndHull(manifold, xfA, hullA, xfB, hullB);
+}
+
+void b3CollideSphereAndCapsuleShapes(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	B3_NOT_USED(cache);
+	b3SphereShape* hullA = (b3SphereShape*)shapeA;
+	b3CapsuleShape* hullB = (b3CapsuleShape*)shapeB;
+	b3CollideSphereAndCapsule(manifold, xfA, hullA, xfB, hullB);
+}
+
+void b3CollideCapsuleAndCapsuleShapes(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	B3_NOT_USED(cache);
+	b3CapsuleShape* hullA = (b3CapsuleShape*)shapeA;
+	b3CapsuleShape* hullB = (b3CapsuleShape*)shapeB;
+	b3CollideCapsuleAndCapsule(manifold, xfA, hullA, xfB, hullB);
+}
+
+void b3CollideCapsuleAndHullShapes(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	b3CapsuleShape* hullA = (b3CapsuleShape*)shapeA;
+	b3HullShape* hullB = (b3HullShape*)shapeB;
+	b3CollideCapsuleAndHull(manifold, xfA, hullA, xfB, hullB);
+}
+
+void b3CollideHullAndHullShapes(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache)
+{
+	b3HullShape* hullA = (b3HullShape*)shapeA;
+	b3HullShape* hullB = (b3HullShape*)shapeB;
+	b3CollideHullAndHull(manifold, xfA, hullA, xfB, hullB, cache);
+}
+
+void b3CollideShapeAndShape(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB, 
+	b3ConvexCache* cache)
+{
+	typedef void(*b3CollideFunction)(b3Manifold&, 
+		const b3Transform&, const class b3Shape*,
+		const b3Transform&, const class b3Shape*,
+		b3ConvexCache*);
+
+	static const b3CollideFunction s_CollideMatrix[e_maxShapes][e_maxShapes] =
+	{
+		{ &b3CollideSphereAndSphereShapes,	&b3CollideSphereAndCapsuleShapes,	&b3CollideSphereAndHullShapes  },
+		{ nullptr,							&b3CollideCapsuleAndCapsuleShapes,	&b3CollideCapsuleAndHullShapes },
+		{ nullptr,							nullptr,							&b3CollideHullAndHullShapes	   },
+	};
+
+	b3ShapeType typeA = shapeA->GetType();
+	b3ShapeType typeB = shapeB->GetType();
+
+	B3_ASSERT(typeA <= typeB);
+	
+	b3CollideFunction CollideFunc = s_CollideMatrix[typeA][typeB];
+	
+	B3_ASSERT(CollideFunc);
+	CollideFunc(manifold, xfA, shapeA, xfB, shapeB, cache);
+}

src/bounce/dynamics/contacts/collide/collide_capsule_and_hull.cpp

@@ -0,0 +1,234 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\collide\clip.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+
+void b3BuildEdgeContact(b3Manifold& manifold,
+	const b3Transform& xf1, const b3Capsule* hull1,
+	const b3Transform& xf2, u32 index2, const b3Hull* hull2)
+{
+	b3Vec3 P1 = b3Mul(xf1, hull1->GetVertex(0));
+	b3Vec3 Q1 = b3Mul(xf1, hull1->GetVertex(1));
+	b3Vec3 E1 = Q1 - P1;
+	b3Vec3 N1 = b3Normalize(E1);
+	
+	const b3HalfEdge* edge2 = hull2->GetEdge(index2);
+	const b3HalfEdge* twin2 = hull2->GetEdge(index2 + 1);
+
+	b3Vec3 C2 = b3Mul(xf2, hull2->centroid);
+	b3Vec3 P2 = b3Mul(xf2, hull2->GetVertex(edge2->origin));
+	b3Vec3 Q2 = b3Mul(xf2, hull2->GetVertex(twin2->origin));
+	b3Vec3 E2 = Q2 - P2;
+	b3Vec3 N2 = b3Normalize(E2);
+
+	b3Vec3 E3 = P1 - P2;
+
+	b3Vec2 b;
+	b.x = -b3Dot(N1, E3);
+	b.y = -b3Dot(N2, E3);
+
+	float32 a12 = -b3Dot(N1, N2), a21 = -a12;
+
+	float32 det = -1.0f - a12 * a21;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	b3Vec2 x;
+	x.x = det * (-b.x - a12 * b.y);
+	x.y = det * (b.y - a21 * b.x);
+
+	b3Vec3 point1 = P1 + x.x * N1;
+	b3Vec3 point2 = P2 + x.y * N2;
+
+	b3Vec3 axis = b3Cross(E1, E2);
+	b3Vec3 normal = b3Normalize(axis);
+	if (b3Dot(normal, P2 - C2) > 0.0f)
+	{
+		normal = -normal;
+	}
+
+	b3FeaturePair pair = b3MakePair(0, 1, index2, index2 + 1);
+	
+	manifold.pointCount = 1;
+	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+	manifold.points[0].key = b3MakeKey(pair);
+	manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+	manifold.points[0].localPoint = b3MulT(xf1, point1);
+	manifold.points[0].localPoint2 = b3MulT(xf2, point2);
+
+	manifold.center = 0.5f * (point1 + hull1->radius * normal + point2 - B3_HULL_RADIUS * normal);
+	manifold.normal = normal;
+	manifold.tangent1 = b3Perp(normal);
+	manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+}
+
+void b3BuildFaceContact(b3Manifold& manifold,
+	const b3Transform& xf1, const b3Capsule* hull1,
+	const b3Transform& xf2, u32 index2, const b3Hull* hull2)
+{
+	// Clip edge 1 against the side planes of the face 2.
+	b3Capsule tempHull1;
+	tempHull1.vertices[0] = b3Mul(xf1, hull1->vertices[0]);
+	tempHull1.vertices[1] = b3Mul(xf1, hull1->vertices[1]);
+	tempHull1.radius = hull1->radius;
+
+	b3ClipVertex edge1[2];
+	b3BuildEdge(edge1, &tempHull1);
+
+	b3ClipVertex clipEdge1[2];
+	u32 clipCount = b3ClipEdgeToFace(clipEdge1, edge1, xf2, index2, hull2);
+	
+	// Project.
+	if (clipCount == 2)
+	{
+		float32 r1 = hull1->radius;
+		float32 r2 = B3_HULL_RADIUS;
+		float32 totalRadius = r1 + r2;
+
+		b3Plane localPlane2 = hull2->GetPlane(index2);
+		b3Plane plane2 = b3Mul(xf2, localPlane2);
+		const b3Face* face2 = hull2->GetFace(index2);
+		const b3HalfEdge* edge2 = hull2->GetEdge(face2->edge);
+		b3Vec3 localPoint2 = hull2->GetVertex(edge2->origin);
+
+		b3Vec3 cp1 = b3ClosestPointOnPlane(clipEdge1[0].position, plane2);
+		b3Vec3 cp2 = b3ClosestPointOnPlane(clipEdge1[1].position, plane2);
+
+		float32 s1 = b3Distance(clipEdge1[0].position, plane2);
+		float32 s2 = b3Distance(clipEdge1[1].position, plane2);
+
+		if (s1 <= totalRadius && s2 <= totalRadius)
+		{
+			b3Vec3 normal = -plane2.normal;
+			b3Vec3 p1 = 0.5f * (clipEdge1[0].position + r1 * normal + cp1 - r2 * normal);
+			b3Vec3 p2 = 0.5f * (clipEdge1[1].position + r1 * normal + cp2 - r2 * normal);
+
+			manifold.pointCount = 2;
+			
+			manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+			manifold.points[0].key = b3MakeKey(clipEdge1[0].pair);
+			manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+			manifold.points[0].localPoint = b3MulT(xf1, clipEdge1[0].position);
+			manifold.points[0].localPoint2 = b3MulT(xf2, cp1);
+
+			manifold.points[1].triangleKey = B3_NULL_TRIANGLE;
+			manifold.points[1].key = b3MakeKey(clipEdge1[1].pair);
+			manifold.points[1].localNormal = b3MulT(xf1.rotation, normal);
+			manifold.points[1].localPoint = b3MulT(xf1, clipEdge1[1].position);
+			manifold.points[1].localPoint2 = b3MulT(xf2, cp2);
+
+			manifold.center = 0.5f * (p1 + p2);
+			manifold.normal = normal;
+			manifold.tangent1 = b3Perp(normal);
+			manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+		}
+	}
+}
+
+void b3CollideCapsuleAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3CapsuleShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB)
+{
+	b3Capsule hullA;
+	hullA.vertices[0] = sA->m_centers[0];
+	hullA.vertices[1] = sA->m_centers[1];
+	hullA.radius = sA->m_radius;
+	const b3Hull* hullB = sB->m_hull;
+
+	b3ShapeGJKProxy proxyA(sA, 0);
+	b3ShapeGJKProxy proxyB(sB, 0);
+	b3GJKOutput distance = b3GJK(xfA, proxyA, xfB, proxyB);
+	float32 totalRadius = hullA.radius + B3_HULL_RADIUS;
+	if (distance.distance > totalRadius)
+	{
+		return;
+	}
+
+	if (distance.distance > 0.0f)
+	{
+		// Define incident face.
+		b3Vec3 NA = (distance.pointB - distance.pointA) / distance.distance;
+		b3Vec3 localNA = b3MulT(xfB.rotation, NA);
+
+		// Search reference face.
+		u32 indexB = hullB->GetSupportFace(-localNA);
+		b3Vec3 localNB = hullB->GetPlane(indexB).normal;
+		b3Vec3 NB = b3Mul(xfB.rotation, localNB);
+
+		// Paralell vectors |v1xv2| = sin(theta)
+		const float32 kTol = 0.005f;
+		b3Vec3 axis = b3Cross(NA, NB);
+		float32 L = b3Dot(axis, axis);
+		if (L < kTol * kTol)
+		{
+			// Reference face found.
+			// Try to build a face contact.
+			b3BuildFaceContact(manifold, xfA, &hullA, xfB, indexB, hullB);
+			if (manifold.pointCount == 2)
+			{
+				return;
+			}
+		}
+		
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, NA);
+		manifold.points[0].localPoint = b3MulT(xfA, distance.pointA);
+		manifold.points[0].localPoint2 = b3MulT(xfB, distance.pointB);
+
+		manifold.center = 0.5f * (distance.pointA + sA->m_radius * NA + distance.pointB - B3_HULL_RADIUS * NA);
+		manifold.normal = NA;
+		manifold.tangent1 = b3Perp(NA);
+		manifold.tangent2 = b3Cross(manifold.tangent1, NA);
+
+		return;
+	}
+	
+	b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfA, &hullA, xfB, hullB);
+	if (faceQueryB.separation > totalRadius)
+	{
+		return;
+	}
+
+	b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, &hullA, xfB, hullB);
+	if (edgeQuery.separation > totalRadius)
+	{
+		return;
+	}
+
+	// Now the hulls are overlapping.
+	const float32 kRelEdgeTol = 0.95f;
+	const float32 kAbsTol = 0.05f;
+	if (edgeQuery.separation > kRelEdgeTol * faceQueryB.separation + kAbsTol)
+	{
+		b3BuildEdgeContact(manifold, xfA, &hullA, xfB, edgeQuery.indexB, hullB);
+	}
+	else
+	{
+		b3BuildFaceContact(manifold, xfA, &hullA, xfB, faceQueryB.index, hullB);
+	}
+}

src/bounce/dynamics/contacts/collide/collide_capsules.cpp

@@ -0,0 +1,143 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\collide\clip.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\collision\shapes\capsule.h>
+
+bool b3AreParalell(const b3Capsule& hullA, const b3Capsule& hullB)
+{
+	b3Vec3 E1 = hullA.vertices[1] - hullA.vertices[0];
+	float32 L1 = b3Length(E1);
+	if (L1 < B3_LINEAR_SLOP)
+	{
+		return false;
+	}
+
+	b3Vec3 E2 = hullB.vertices[1] - hullB.vertices[0];
+	float32 L2 = b3Length(E2);
+	if (L2 < B3_LINEAR_SLOP)
+	{
+		return false;
+	}
+
+	// |e1 x e2| = sin(theta) * |e1| * |e2|
+	const float32 kTol = 0.005f;
+	b3Vec3 N = b3Cross(E1, E2);
+	return b3Length(N) < kTol * L1 * L2;
+}
+
+void b3CollideCapsuleAndCapsule(b3Manifold& manifold,
+	const b3Transform& xfA, const b3CapsuleShape* sA,
+	const b3Transform& xfB, const b3CapsuleShape* sB)
+{
+	b3Capsule hullA;
+	hullA.vertices[0] = b3Mul(xfA, sA->m_centers[0]);
+	hullA.vertices[1] = b3Mul(xfA, sA->m_centers[1]);
+	hullA.radius = sA->m_radius;
+
+	b3Capsule hullB;
+	hullB.vertices[0] = b3Mul(xfB, sB->m_centers[0]);
+	hullB.vertices[1] = b3Mul(xfB, sB->m_centers[1]);
+	hullB.radius = sB->m_radius;
+
+	float32 totalRadius = hullA.radius + hullB.radius;
+
+	if (b3AreParalell(hullA, hullB))
+	{
+		// Clip edge A against the side planes of edge B.
+		b3ClipVertex edgeA[2];
+		b3BuildEdge(edgeA, &hullA);
+
+		b3ClipVertex clipEdgeA[2];
+		u32 clipCount = b3ClipEdgeToFace(clipEdgeA, edgeA, &hullB);
+
+		float32 totalRadius = hullA.radius + hullB.radius;
+
+		if (clipCount == 2)
+		{
+			b3Vec3 cp1 = b3ClosestPointOnSegment(clipEdgeA[0].position, hullB.vertices[0], hullB.vertices[1]);
+			b3Vec3 cp2 = b3ClosestPointOnSegment(clipEdgeA[1].position, hullB.vertices[0], hullB.vertices[1]);
+
+			float32 d1 = b3Distance(clipEdgeA[0].position, cp1);
+			float32 d2 = b3Distance(clipEdgeA[1].position, cp2);
+
+			if (d1 <= totalRadius && d2 <= totalRadius)
+			{
+				b3Vec3 n1 = (cp1 - clipEdgeA[0].position) / d1;
+				b3Vec3 n2 = (cp2 - clipEdgeA[1].position) / d2;
+
+				b3Vec3 p1 = 0.5f * (clipEdgeA[0].position + hullA.radius * n1 + cp1 - hullB.radius * n1);
+				b3Vec3 p2 = 0.5f * (clipEdgeA[1].position + hullA.radius * n2 + cp2 - hullB.radius * n2);
+
+				b3Vec3 center = 0.5f * (p1 + p2);
+				b3Vec3 normal = b3Normalize(n1 + n2);
+
+				manifold.pointCount = 2;
+
+				manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+				manifold.points[0].key = b3MakeKey(clipEdgeA[0].pair);
+				manifold.points[0].localNormal = b3MulT(xfA.rotation, n1);
+				manifold.points[0].localPoint = b3MulT(xfA, clipEdgeA[0].position);
+				manifold.points[0].localPoint2 = b3MulT(xfB, cp1);
+
+				manifold.points[1].triangleKey = B3_NULL_TRIANGLE;
+				manifold.points[1].key = b3MakeKey(clipEdgeA[1].pair);
+				manifold.points[1].localNormal = b3MulT(xfA.rotation, n2);
+				manifold.points[1].localPoint = b3MulT(xfA, clipEdgeA[1].position);
+				manifold.points[1].localPoint2 = b3MulT(xfB, cp2);
+
+				manifold.center = center;
+				manifold.normal = normal;
+				manifold.tangent1 = b3Perp(normal);
+				manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+
+				return;
+			}
+		}
+	}
+
+	b3Vec3 pointA, pointB;
+	b3ClosestPointsOnSegments(&pointA, &pointB, hullA.vertices[0], hullA.vertices[1], hullB.vertices[0], hullB.vertices[1]);
+	if (b3DistanceSquared(pointA, pointB) > totalRadius * totalRadius)
+	{
+		return;
+	}
+
+	float32 distance = b3Distance(pointA, pointB);
+
+	if (distance > 0.0f)
+	{
+		b3Vec3 normal = (pointB - pointA) / distance;
+		b3Vec3 center = 0.5f * (pointA + hullA.radius * normal + pointB - hullB.radius * normal);
+
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, normal);
+		manifold.points[0].localPoint = b3MulT(xfA, pointA);
+		manifold.points[0].localPoint2 = b3MulT(xfB, pointB);
+		
+		manifold.center = center;
+		manifold.normal = normal;
+		manifold.tangent1 = b3Perp(normal);
+		manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+	}
+}

src/bounce/dynamics/contacts/collide/collide_hulls.cpp

@@ -0,0 +1,258 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\collide\clip.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\contacts\contact_cluster.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\collision\shapes\hull.h>
+
+void b3BuildEdgeContact(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB, 
+	const b3EdgeQuery& query)
+{
+	u32 indexA = query.indexA;
+
+	const b3HalfEdge* edge1 = hullA->GetEdge(indexA);
+	const b3HalfEdge* twin1 = hullA->GetEdge(indexA + 1);
+
+	b3Vec3 C1 = b3Mul(xfA, hullA->centroid);
+	b3Vec3 P1 = b3Mul(xfA, hullA->GetVertex(edge1->origin));
+	b3Vec3 Q1 = b3Mul(xfA, hullA->GetVertex(twin1->origin));
+	b3Vec3 E1 = Q1 - P1;
+
+	u32 indexB = query.indexB;
+	
+	const b3HalfEdge* edge2 = hullB->GetEdge(indexB);
+	const b3HalfEdge* twin2 = hullB->GetEdge(indexB + 1);
+
+	b3Vec3 C2 = b3Mul(xfB, hullB->centroid);
+	b3Vec3 P2 = b3Mul(xfB, hullB->GetVertex(edge2->origin));
+	b3Vec3 Q2 = b3Mul(xfB, hullB->GetVertex(twin2->origin));
+	b3Vec3 E2 = Q2 - P2;
+
+	b3Vec3 N = b3Cross(E1, E2);
+	if (b3Dot(N, P1 - C1) < 0.0f)
+	{
+		N = -N;
+	}
+	N.Normalize();
+
+	b3Vec3 PA, PB;
+	b3ClosestPointsOnLines(&PA, &PB, P1, E1, P2, E2);
+
+	b3FeaturePair pair = b3MakePair(indexA, indexA + 1, indexB, indexB + 1);
+
+	manifold.pointCount = 1;
+	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+	manifold.points[0].key = b3MakeKey(pair);
+	manifold.points[0].localNormal = b3MulT(xfA.rotation, N);
+	manifold.points[0].localPoint = b3MulT(xfA, PA);
+	manifold.points[0].localPoint2 = b3MulT(xfB, PB);
+
+	manifold.center = 0.5f * (PA + B3_HULL_RADIUS * N + PB - B3_HULL_RADIUS * N);
+	manifold.normal = N;
+	manifold.tangent1 = b3Perp(N);
+	manifold.tangent2 = b3Cross(manifold.tangent1, N);
+}
+
+void b3BuildFaceContact(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB,
+	const b3FaceQuery& query, bool flipNormal)
+{
+	// 1. Define the reference face plane (A).
+	u32 indexA = query.index;
+	const b3Face* faceA = hullA->GetFace(indexA);
+	const b3HalfEdge* edgeA = hullA->GetEdge(faceA->edge);
+	b3Plane localPlaneA = hullA->GetPlane(indexA);
+	b3Vec3 localNormalA = localPlaneA.normal;
+	b3Vec3 localPointA = hullA->GetVertex(edgeA->origin);
+	b3Plane planeA = b3Mul(xfA, localPlaneA);
+
+	// 2. Find the incident face polygon (B).	
+	
+	// Put the reference plane normal in the frame of the incident hull (B).
+	b3Vec3 normalA = b3MulT(xfB.rotation, planeA.normal);
+
+	// Find the support polygon in the *negated* direction.
+	b3StackArray<b3ClipVertex, 32> polygonB;
+	u32 indexB = hullB->GetSupportFace(-normalA);
+	b3BuildPolygon(polygonB, xfB, indexB, hullB);
+
+	// 3. Clip incident face polygon (B) against the reference face (A) side planes.
+	b3StackArray<b3ClipVertex, 32> clipPolygonB;
+	b3ClipPolygonToFace(clipPolygonB, polygonB, xfA, indexA, hullA);
+	if (clipPolygonB.IsEmpty())
+	{
+		return;
+	}
+
+	// 4. Project the clipped polygon on the reference plane for reduction.
+	// Ensure the deepest point is contained in the reduced polygon.
+	b3StackArray<b3ClusterVertex, 32> polygonA;
+	
+	u32 minIndex = 0;
+	float32 minSeparation = B3_MAX_FLOAT;
+
+	for (u32 i = 0; i < clipPolygonB.Count(); ++i)
+	{
+		b3ClipVertex vB = clipPolygonB[i];
+		float32 separation = b3Distance(vB.position, planeA);
+
+		if (separation <= B3_HULL_RADIUS_SUM)
+		{
+			if (separation < minSeparation)
+			{
+				minIndex = polygonA.Count();
+				minSeparation = separation;
+			}
+
+			b3ClusterVertex vA;
+			vA.position = b3Project(vB.position, planeA);
+			vA.clipIndex = i;
+			polygonA.PushBack(vA);
+		}
+	}
+
+	if (polygonA.IsEmpty())
+	{
+		return;
+	}
+
+	// 5. Reduce.
+	b3StackArray<b3ClusterVertex, 32> reducedPolygonA;
+	b3ReducePolygon(reducedPolygonA, polygonA, minIndex);
+	B3_ASSERT(!reducedPolygonA.IsEmpty());
+
+	// 6. Build face contact.
+	b3Vec3 normal = planeA.normal;
+	manifold.center.SetZero();
+
+	u32 pointCount = reducedPolygonA.Count();
+	for (u32 i = 0; i < pointCount; ++i)
+	{
+		u32 clipIndex = reducedPolygonA[i].clipIndex;
+		b3ClipVertex vB = clipPolygonB[clipIndex];
+		b3Vec3 vA = b3ClosestPointOnPlane(vB.position, planeA);
+
+		b3ManifoldPoint* cp = manifold.points + i;
+
+		if (flipNormal)
+		{
+			b3FeaturePair pair;
+			pair.inEdgeA = vB.pair.inEdgeB;
+			pair.outEdgeA = vB.pair.outEdgeB;
+			pair.inEdgeB = vB.pair.inEdgeA;
+			pair.outEdgeB = vB.pair.outEdgeA;
+
+			cp->triangleKey = B3_NULL_TRIANGLE;
+			cp->key = b3MakeKey(pair);
+			cp->localNormal = b3MulT(xfB.rotation, -normal);
+			cp->localPoint = b3MulT(xfB, vB.position);
+			cp->localPoint2 = b3MulT(xfA, vA);
+			
+			manifold.center += 0.5f * (vA + B3_HULL_RADIUS * normal + vB.position - B3_HULL_RADIUS * normal);
+		}
+		else
+		{
+			cp->triangleKey = B3_NULL_TRIANGLE;
+			cp->key = b3MakeKey(vB.pair);
+			cp->localNormal = b3MulT(xfA.rotation, normal);
+			cp->localPoint = b3MulT(xfA, vA);
+			cp->localPoint2 = b3MulT(xfB, vB.position);
+			
+			manifold.center += 0.5f * (vA + B3_HULL_RADIUS * normal + vB.position - B3_HULL_RADIUS * normal);
+		}
+	}
+	
+	if (flipNormal)
+	{
+		localNormalA = -localNormalA;
+		normal = -normal;
+	}
+
+	manifold.center /= float32(pointCount);
+	manifold.normal = normal;
+	manifold.tangent1 = b3Perp(normal);
+	manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+	manifold.pointCount = pointCount;
+}
+
+void b3CollideHulls(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	b3FaceQuery faceQueryA = b3QueryFaceSeparation(xfA, hullA, xfB, hullB);
+	if (faceQueryA.separation > B3_HULL_RADIUS_SUM)
+	{
+		return;
+	}
+
+	b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfB, hullB, xfA, hullA);
+	if (faceQueryB.separation > B3_HULL_RADIUS_SUM)
+	{
+		return;
+	}
+
+	b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, hullA, xfB, hullB);
+	if (edgeQuery.separation > B3_HULL_RADIUS_SUM)
+	{
+		return;
+	}
+
+	const float32 kRelEdgeTol = 0.90f;
+	const float32 kRelFaceTol = 0.98f;
+	const float32 kAbsTol = 0.05f;
+
+	if (edgeQuery.separation > kRelEdgeTol * b3Max(faceQueryA.separation, faceQueryB.separation) + kAbsTol)
+	{
+		b3BuildEdgeContact(manifold, xfA, hullA, xfB, hullB, edgeQuery);
+	}
+	else
+	{
+		if (faceQueryA.separation > kRelFaceTol * faceQueryB.separation + kAbsTol)
+		{
+			b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, faceQueryA, false);
+		}
+		else
+		{
+			b3BuildFaceContact(manifold, xfB, hullB, xfA, hullA, faceQueryB, true);
+		}
+	}
+}
+
+//
+bool b3_enableConvexCache = true;
+u32 b3_convexCalls, b3_convexCacheHits;
+
+void b3CollideHullAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3HullShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB,
+	b3FeatureCache* cache);
+
+void b3CollideHullAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3HullShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB, 
+	b3ConvexCache* cache)
+{
+	++b3_convexCalls;
+	b3CollideHullAndHull(manifold, xfA, sA, xfB, sB, &cache->featureCache);
+}

src/bounce/dynamics/contacts/collide/collide_hulls_cache.cpp

@@ -0,0 +1,363 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\collide\clip.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\collision\shapes\hull.h>
+
+extern u32 b3_convexCacheHits;
+
+void b3BuildEdgeContact(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB,
+	const b3EdgeQuery& query);
+
+void b3BuildFaceContact(b3Manifold& manifold,
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB,
+	const b3FaceQuery& query, bool flipNormal);
+
+void b3RebuildEdgeContact(b3Manifold& manifold,
+	const b3Transform& xfA, u32 indexA, const b3HullShape* sA,
+	const b3Transform& xfB, u32 indexB, const b3HullShape* sB)
+{
+	const b3Hull* hullA = sA->m_hull;
+	const b3Hull* hullB = sB->m_hull;
+
+	const b3HalfEdge* edge1 = hullA->GetEdge(indexA);
+	const b3HalfEdge* twin1 = hullA->GetEdge(indexA + 1);
+
+	b3Vec3 C1 = b3Mul(xfA, hullA->centroid);
+	b3Vec3 P1 = b3Mul(xfA, hullA->GetVertex(edge1->origin));
+	b3Vec3 Q1 = b3Mul(xfA, hullA->GetVertex(twin1->origin));
+	b3Vec3 E1 = Q1 - P1;
+
+	const b3HalfEdge* edge2 = hullB->GetEdge(indexB);
+	const b3HalfEdge* twin2 = hullB->GetEdge(indexB + 1);
+
+	b3Vec3 C2 = b3Mul(xfB, hullB->centroid);
+	b3Vec3 P2 = b3Mul(xfB, hullB->GetVertex(edge2->origin));
+	b3Vec3 Q2 = b3Mul(xfB, hullB->GetVertex(twin2->origin));
+	b3Vec3 E2 = Q2 - P2;
+
+	b3Vec3 PA, PB;
+	b3ClosestPointsOnLines(&PA, &PB, P1, E1, P2, E2);
+
+	// Check if the closest points are still lying on the opposite segments 
+	// using Barycentric coordinates.
+	float32 wB[3];
+	b3Barycentric(wB, P1, Q1, PB);
+
+	float32 wA[3];
+	b3Barycentric(wA, P2, Q2, PA);
+
+	if (wB[1] > 0.0f && wB[1] <= wB[2] &&
+		wA[1] > 0.0f && wA[1] <= wA[2])
+	{
+		b3Vec3 N = b3Cross(E1, E2);
+		if (b3Dot(N, P1 - C1) < 0.0f)
+		{
+			N = -N;
+		}
+		N.Normalize();
+
+		b3FeaturePair pair = b3MakePair(indexA, indexA + 1, indexB, indexB + 1);
+
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = b3MakeKey(pair);
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, N);
+		manifold.points[0].localPoint = b3MulT(xfA, PA);
+		manifold.points[0].localPoint2 = b3MulT(xfB, PB);
+		
+		manifold.center = 0.5f * (PA + B3_HULL_RADIUS * N + PB - B3_HULL_RADIUS * N);
+		manifold.normal = N;
+		manifold.tangent1 = b3Perp(N);
+		manifold.tangent2 = b3Cross(manifold.tangent1, N);
+	}
+}
+
+void b3RebuildFaceContact(b3Manifold& manifold,
+	const b3Transform& xfA, u32 indexA, const b3HullShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB, bool flipNormal)
+{
+	const b3Hull* hullA = sA->m_hull;
+	const b3Hull* hullB = sB->m_hull;
+
+	b3FaceQuery query;
+	query.index = indexA;
+	
+	b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, query, flipNormal);
+}
+
+void b3CollideCache(b3Manifold& manifold,
+	const b3Transform& xfA, const b3HullShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB,
+	b3FeatureCache* cache)
+{
+	B3_ASSERT(cache->m_featurePair.state == b3SATCacheType::e_empty);
+
+	const b3Hull* hullA = sA->m_hull;
+	const b3Hull* hullB = sB->m_hull;
+
+	b3FaceQuery faceQueryA = b3QueryFaceSeparation(xfA, hullA, xfB, hullB);
+	if (faceQueryA.separation > B3_HULL_RADIUS_SUM)
+	{
+		// Write a separation cache.
+		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair.type = b3SATFeaturePair::e_faceA;
+		cache->m_featurePair.indexA = faceQueryA.index;
+		cache->m_featurePair.indexB = faceQueryA.index;
+		return;
+	}
+
+	b3FaceQuery faceQueryB = b3QueryFaceSeparation(xfB, hullB, xfA, hullA);
+	if (faceQueryB.separation > B3_HULL_RADIUS_SUM)
+	{
+		// Write a separation cache.
+		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair.type = b3SATFeaturePair::e_faceB;
+		cache->m_featurePair.indexA = faceQueryB.index;
+		cache->m_featurePair.indexB = faceQueryB.index;
+		return;
+	}
+
+	b3EdgeQuery edgeQuery = b3QueryEdgeSeparation(xfA, hullA, xfB, hullB);
+	if (edgeQuery.separation > B3_HULL_RADIUS_SUM)
+	{
+		// Write a separation cache.
+		cache->m_featurePair.state = b3SATCacheType::e_separation;
+		cache->m_featurePair.type = b3SATFeaturePair::e_edgeA;
+		cache->m_featurePair.indexA = edgeQuery.indexA;
+		cache->m_featurePair.indexB = edgeQuery.indexB;
+		return;
+	}
+
+	const float32 kRelEdgeTol = 0.90f;
+	const float32 kRelFaceTol = 0.98f;
+	const float32 kAbsTol = 0.05f;
+
+	if (edgeQuery.separation > kRelEdgeTol * b3Max(faceQueryA.separation, faceQueryB.separation) + kAbsTol)
+	{
+		b3BuildEdgeContact(manifold, xfA, hullA, xfB, hullB, edgeQuery);
+		if(manifold.pointCount > 0)
+		{	
+			// Write an overlap cache.
+			cache->m_featurePair.state = b3SATCacheType::e_overlap;
+			cache->m_featurePair.type = b3SATFeaturePair::e_edgeA;
+			cache->m_featurePair.indexA = edgeQuery.indexA;
+			cache->m_featurePair.indexB = edgeQuery.indexB;		
+		}
+	}
+	else
+	{
+		if (faceQueryA.separation > kRelFaceTol * faceQueryB.separation + kAbsTol)
+		{
+			b3BuildFaceContact(manifold, xfA, hullA, xfB, hullB, faceQueryA, false);
+			if(manifold.pointCount > 0)
+			{
+				// Write an overlap cache.
+				cache->m_featurePair.state = b3SATCacheType::e_overlap;
+				cache->m_featurePair.type = b3SATFeaturePair::e_faceA;
+				cache->m_featurePair.indexA = faceQueryA.index;
+				cache->m_featurePair.indexB = faceQueryA.index;
+			}
+		}
+		else
+		{
+			b3BuildFaceContact(manifold, xfB, hullB, xfA, hullA, faceQueryB, true);
+			if(manifold.pointCount > 0)
+			{
+				// Write an overlap cache.
+				cache->m_featurePair.state = b3SATCacheType::e_overlap;
+				cache->m_featurePair.type = b3SATFeaturePair::e_faceB;
+				cache->m_featurePair.indexA = faceQueryB.index;
+				cache->m_featurePair.indexB = faceQueryB.index;
+			}
+		}
+	}
+}
+
+b3SATCacheType b3FeatureCache::ReadState(
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// If the cache was empty or flushed choose an arbitrary feature pair.
+	if (m_featurePair.state == b3SATCacheType::e_empty)
+	{
+		m_featurePair.state = b3SATCacheType::e_separation;
+		m_featurePair.type = b3SATFeaturePair::e_faceA;
+		m_featurePair.indexA = 0;
+		m_featurePair.indexB = 0;
+	}
+
+	switch (m_featurePair.type)
+	{
+	case b3SATFeaturePair::e_edgeA:
+	{
+		return ReadEdge(xfA, hullA, xfB, hullB);
+	}
+	case b3SATFeaturePair::e_faceA:
+	{
+		return ReadFace(xfA, hullA, xfB, hullB);
+	}
+	case b3SATFeaturePair::e_faceB:
+	{
+		return ReadFace(xfB, hullB, xfA, hullA);
+	}
+	default:
+	{
+		return b3SATCacheType::e_empty;
+	}
+	}
+}
+
+b3SATCacheType b3FeatureCache::ReadFace(
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xfB, xfA);
+	b3Plane plane = xf * hullA->GetPlane(m_featurePair.indexA);
+	float32 separation = b3Project(hullB, plane);
+	if (separation > B3_HULL_RADIUS_SUM)
+	{
+		return e_separation;
+	}
+	return e_overlap;
+}
+
+b3SATCacheType b3FeatureCache::ReadEdge(
+	const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB)
+{
+	u32 i = m_featurePair.indexA;
+	u32 j = m_featurePair.indexB;
+
+	// Query minimum separation distance and axis of the first hull planes.
+	// Perform computations in the local space of the second hull.
+	b3Transform xf = b3MulT(xfB, xfA);
+	b3Vec3 C1 = xf * hullA->centroid;
+
+	const b3HalfEdge* edge1 = hullA->GetEdge(i);
+	const b3HalfEdge* twin1 = hullA->GetEdge(i + 1);
+
+	B3_ASSERT(edge1->twin == i + 1 && twin1->twin == i);
+
+	b3Vec3 P1 = xf * hullA->GetVertex(edge1->origin);
+	b3Vec3 Q1 = xf * hullA->GetVertex(twin1->origin);
+	b3Vec3 E1 = Q1 - P1;
+
+	// The Gauss Map of edge 1.
+	b3Vec3 U1 = xf.rotation * hullA->GetPlane(edge1->face).normal;
+	b3Vec3 V1 = xf.rotation * hullA->GetPlane(twin1->face).normal;
+
+	const b3HalfEdge* edge2 = hullB->GetEdge(j);
+	const b3HalfEdge* twin2 = hullB->GetEdge(j + 1);
+
+	B3_ASSERT(edge2->twin == j + 1 && twin2->twin == j);
+
+	b3Vec3 P2 = hullB->GetVertex(edge2->origin);
+	b3Vec3 Q2 = hullB->GetVertex(twin2->origin);
+	b3Vec3 E2 = Q2 - P2;
+
+	// The Gauss Map of edge 2.
+	b3Vec3 U2 = hullB->GetPlane(edge2->face).normal;
+	b3Vec3 V2 = hullB->GetPlane(twin2->face).normal;
+
+	// Negate the Gauss Map 2 for account for the MD.
+	if (b3IsMinkowskiFace(U1, V1, -E1, -U2, -V2, -E2))
+	{
+		float32 separation = b3Project(P1, E1, P2, E2, C1);
+		if (separation > B3_HULL_RADIUS_SUM)
+		{
+			return b3SATCacheType::e_separation;
+		}
+		else
+		{
+			return b3SATCacheType::e_overlap;
+		}
+	}
+
+	// We can't determine the cache type 
+	// therefore must run SAT.
+	return b3SATCacheType::e_empty;
+}
+
+void b3CollideHullAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3HullShape* sA,
+	const b3Transform& xfB, const b3HullShape* sB,
+	b3FeatureCache* cache)
+{
+	const b3Hull* hullA = sA->m_hull;
+	const b3Hull* hullB = sB->m_hull;
+
+	// Read cache
+	b3SATCacheType state0 = cache->m_featurePair.state;
+	b3SATCacheType state1 = cache->ReadState(xfA, hullA, xfB, hullB);
+	
+	if (state0 == b3SATCacheType::e_separation &&
+		state1 == b3SATCacheType::e_separation)
+	{
+		// Separation cache hit.
+		++b3_convexCacheHits;
+		return;
+	}
+	else if (state0 == b3SATCacheType::e_overlap &&
+			 state1 == b3SATCacheType::e_overlap)
+	{
+		// Try to rebuild or reclip the features.
+		switch (cache->m_featurePair.type)
+		{
+		case b3SATFeaturePair::e_edgeA:
+		{
+			b3RebuildEdgeContact(manifold, xfA, cache->m_featurePair.indexA, sA, xfB, cache->m_featurePair.indexB, sB);
+			break;
+		}
+		case b3SATFeaturePair::e_faceA:
+		{
+			b3RebuildFaceContact(manifold, xfA, cache->m_featurePair.indexA, sA, xfB, sB, false);
+			break;
+		}
+		case b3SATFeaturePair::e_faceB:
+		{
+			b3RebuildFaceContact(manifold, xfB, cache->m_featurePair.indexA, sB, xfA, sA, true);
+			break;
+		}
+		default:
+		{
+			break;
+		}
+		}
+
+		if (manifold.pointCount > 0)
+		{
+			// Overlap cache hit.
+			++b3_convexCacheHits;
+			return;
+		}
+	}
+
+	// Separation cache miss.
+	// Overlap cache miss.
+	// Flush the cache.
+	cache->m_featurePair.state = b3SATCacheType::e_empty;
+	b3CollideCache(manifold, xfA, sA, xfB, sB, cache);
+}

src/bounce/dynamics/contacts/collide/collide_sphere_and_capsule.cpp

@@ -0,0 +1,147 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\capsule.h>
+
+void b3CollideSphereAndCapsule(b3Manifold& manifold,
+	const b3Transform& xfA, const b3SphereShape* sA,
+	const b3Transform& xfB, const b3CapsuleShape* sB)
+{
+	b3Vec3 Q = b3Mul(xfA, sA->m_center);
+
+	b3Vec3 A = b3Mul(xfB, sB->m_centers[0]);
+	b3Vec3 B = b3Mul(xfB, sB->m_centers[1]);
+	b3Vec3 AB = B - A;
+	
+	// Barycentric coordinates for Q
+	float32 u = b3Dot(B - Q, AB);
+	float32 v = b3Dot(Q - A, AB);
+	
+	float32 radius = sA->m_radius + sB->m_radius;
+
+	if (v <= 0.0f)
+	{
+		// A
+		b3Vec3 P = A;
+		b3Vec3 d = P - Q;
+		float32 dd = b3Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+
+		b3Vec3 n(0.0f, 1.0f, 0.0f);
+		float32 len = b3Length(d);
+		if (len > B3_EPSILON)
+		{
+			n = d / len;
+		}
+		
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
+		manifold.points[0].localPoint = sA->m_center;
+		manifold.points[0].localPoint2 = sB->m_centers[0];
+
+		manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
+		manifold.normal = n;
+		manifold.tangent1 = b3Perp(n);
+		manifold.tangent2 = b3Cross(manifold.tangent1, n);
+		return;
+	}
+
+	if (u <= 0.0f)
+	{
+		// B
+		b3Vec3 P = B;
+		b3Vec3 d = P - Q;
+		float32 dd = b3Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+
+		b3Vec3 n(0.0f, 1.0f, 0.0f);
+		float32 len = b3Length(d);
+		if (len > B3_EPSILON)
+		{
+			n = d / len;
+		}
+
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
+		manifold.points[0].localPoint = sA->m_center;
+		manifold.points[0].localPoint2 = sB->m_centers[1];
+
+		manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
+		manifold.normal = n;
+		manifold.tangent1 = b3Perp(n);
+		manifold.tangent2 = b3Cross(manifold.tangent1, n);
+		return;
+	}
+
+	// AB
+	//float32 s = u + v;
+	float32 s = b3Dot(AB, AB);
+	//B3_ASSERT(s > 0.0f);
+	b3Vec3 P;
+	if (s < B3_LINEAR_SLOP * B3_LINEAR_SLOP)
+	{
+		P = A;
+	}
+	else
+	{
+		P = (u * A + v * B) / s;
+	}
+	
+	b3Vec3 d = P - Q;
+	float32 dd = b3Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return;
+	}
+	
+	b3Vec3 QA = A - Q;
+	b3Vec3 e = b3Cross(AB, QA);
+	b3Vec3 n = b3Cross(AB, e);
+	if (b3Dot(n, QA) < 0.0f)
+	{
+		n = -n;
+	}
+	n.Normalize();
+	
+	manifold.pointCount = 1;
+	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+	manifold.points[0].key = 2;
+	manifold.points[0].localNormal = b3MulT(xfA.rotation, n);
+	manifold.points[0].localPoint = sA->m_center;
+	manifold.points[0].localPoint2 = b3MulT(xfB, P);
+
+	manifold.center = 0.5f * (P + sA->m_radius * n + Q - sB->m_radius * n);
+	manifold.normal = n;
+	manifold.tangent1 = b3Perp(n);
+	manifold.tangent2 = b3Cross(manifold.tangent1, n);
+}

src/bounce/dynamics/contacts/collide/collide_sphere_and_hull.cpp

@@ -0,0 +1,91 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include <bounce\dynamics\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\hull.h>
+
+void b3CollideSphereAndHull(b3Manifold& manifold,
+	const b3Transform& xf1, const b3SphereShape* s1, 
+	const b3Transform& xf2, const b3HullShape* s2) 
+{
+	b3ShapeGJKProxy proxy1(s1, 0);	
+	b3ShapeGJKProxy proxy2(s2, 0);	
+	b3GJKOutput distance = b3GJK(xf1, proxy1, xf2, proxy2);	
+	float32 totalRadius = s1->m_radius + s2->m_radius;
+	if (distance.distance > totalRadius)
+	{
+		return;
+	}
+	
+	if (distance.distance > 0.0f)
+	{
+		b3Vec3 p1 = distance.pointA;
+		b3Vec3 p2 = distance.pointB;
+		b3Vec3 normal = (p2 - p1) / distance.distance;
+		
+		manifold.pointCount = 1;
+		manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+		manifold.points[0].key = 0;
+		manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+		manifold.points[0].localPoint = s1->m_center;
+		manifold.points[0].localPoint2 = b3MulT(xf2, p2);
+		
+		manifold.center = 0.5f * (p1 + s1->m_radius * normal + p2 - s2->m_radius * normal);
+		manifold.normal = normal;
+		manifold.tangent1 = b3Perp(normal);
+		manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+
+		return;
+	}
+
+	b3Sphere hull1;
+	hull1.vertex = s1->m_center;
+	hull1.radius = s1->m_radius;
+	
+	const b3Hull* hull2 = s2->m_hull;
+
+	b3FaceQuery faceQuery = b3QueryFaceSeparation(xf1, &hull1, xf2, hull2);
+	if (faceQuery.separation > totalRadius)
+	{
+		return;
+	}
+
+	b3Plane localPlane = hull2->GetPlane(faceQuery.index);
+	b3Plane plane = b3Mul(xf2, localPlane);
+	b3Vec3 cp1 = b3Mul(xf1, hull1.vertex);
+	b3Vec3 cp2 = b3ClosestPointOnPlane(cp1, plane);
+	
+	// Ensure normal orientation to shape B
+	b3Vec3 normal = -plane.normal;
+	
+	manifold.pointCount = 1;
+	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+	manifold.points[0].key = 1;
+	manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+	manifold.points[0].localPoint = s1->m_center;
+	manifold.points[0].localPoint2 = b3MulT(xf2, cp2);
+	
+	manifold.center = 0.5f * (cp1 + s1->m_radius * normal + cp2 - B3_HULL_RADIUS * normal);
+	manifold.normal = normal;
+	manifold.tangent1 = b3Perp(normal);
+	manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+}

src/bounce/dynamics/contacts/collide/collide_spheres.cpp

@@ -0,0 +1,63 @@
+/*
+* 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\contacts\collide\collide.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\collision\shapes\sphere.h>
+
+void b3CollideSphereAndSphere(b3Manifold& manifold,
+	const b3Transform& xf1, const b3SphereShape* s1,
+	const b3Transform& xf2, const b3SphereShape* s2)
+{
+	b3Vec3 c1 = b3Mul(xf1, s1->m_center);
+	b3Vec3 c2 = b3Mul(xf2, s2->m_center);
+	b3Vec3 d = c2 - c1;
+	float32 dd = b3Dot(d, d);
+	float32 totalRadius = s1->m_radius + s2->m_radius;
+	if (dd > totalRadius * totalRadius)
+	{
+		return;
+	}
+
+	float32 distance = b3Length(d);
+	
+	b3Vec3 normal;
+	if (distance > B3_EPSILON)
+	{
+		normal = d / distance;
+	}
+	else
+	{
+		normal.Set(0.0f, 1.0f, 0.0f);
+	}
+	
+	b3Vec3 point = 0.5f * (c1 + s1->m_radius * normal + c2 - s2->m_radius * normal);
+	
+	manifold.pointCount = 1;
+	manifold.points[0].triangleKey = B3_NULL_TRIANGLE;
+	manifold.points[0].key = 0;
+	manifold.points[0].localNormal = b3MulT(xf1.rotation, normal);
+	manifold.points[0].localPoint = s1->m_center;
+	manifold.points[0].localPoint2 = s2->m_center;
+
+	manifold.center = point;
+	manifold.normal = normal;
+	manifold.tangent1 = b3Perp(normal);
+	manifold.tangent2 = b3Cross(manifold.tangent1, normal);
+}

src/bounce/dynamics/contacts/contact.cpp

@@ -0,0 +1,154 @@
+/*
+* 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\contacts\contact.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\world_listeners.h>
+
+const b3Manifold* b3Contact::GetManifold(u32 index) const
+{
+	B3_ASSERT(index < m_manifoldCount);
+	return m_manifolds + index;
+}
+
+b3Manifold* b3Contact::GetManifold(u32 index)
+{
+	B3_ASSERT(index < m_manifoldCount);
+	return m_manifolds + index;
+}
+
+void b3Contact::GetWorldManifold(b3WorldManifold* out, u32 index) const
+{
+	B3_ASSERT(index < m_manifoldCount);
+	b3Manifold* m = m_manifolds + index;
+
+	const b3Shape* shapeA = GetShapeA();
+	b3Transform xfA = shapeA->GetTransform();
+
+	const b3Shape* shapeB = GetShapeB();
+	b3Transform xfB = shapeB->GetTransform();
+
+	out->Initialize(m, xfA, shapeA->m_radius, xfB, shapeB->m_radius);
+}
+
+void b3Contact::Update(b3ContactListener* listener) 
+{
+	b3Shape* shapeA = GetShapeA();
+	b3Body* bodyA = shapeA->GetBody();
+	i32 proxyA = shapeA->m_broadPhaseID;
+	b3Transform xfA = shapeA->GetTransform();
+
+	b3Shape* shapeB = GetShapeB();
+	b3Body* bodyB = shapeB->GetBody();
+	i32 proxyB = shapeB->m_broadPhaseID;
+	b3Transform xfB = shapeB->GetTransform();
+
+	b3World* world = bodyA->GetWorld();
+
+	bool wasOverlapping = IsOverlapping();
+	bool isOverlapping = false;
+	bool isSensorContact = shapeA->IsSensor() || shapeB->IsSensor();
+	
+	if (isSensorContact == true)
+	{
+		isOverlapping = TestOverlap();
+		m_manifoldCount = 0;
+	}
+	else 
+	{
+		// Copy the old contact points.
+		b3Manifold oldManifolds[B3_MAX_MANIFOLDS];
+		u32 oldManifoldCount = m_manifoldCount;
+		memcpy(oldManifolds, m_manifolds, oldManifoldCount * sizeof(b3Manifold));
+		
+		// Clear all contact points.
+		m_manifoldCount = 0;
+		for (u32 i = 0; i < m_manifoldCapacity; ++i)
+		{
+			m_manifolds[i].GuessImpulses();
+		}
+
+		// Generate new contact points for the solver.
+		Collide();
+		
+		// Initialize the new built contact points for warm starting the solver.
+		if (world->m_warmStarting == true)
+		{
+			for (u32 i = 0; i < m_manifoldCount; ++i)
+			{
+				b3Manifold* m2 = m_manifolds + i;
+				for (u32 j = 0; j < oldManifoldCount; ++j)
+				{
+					const b3Manifold* m1 = oldManifolds + j;
+					m2->FindImpulses(*m1);
+				}
+			}
+		}
+
+		// The shapes are overlapping if at least one contact 
+		// point was built.
+		for (u32 i = 0; i < m_manifoldCount; ++i)
+		{
+			if (m_manifolds[i].pointCount > 0)
+			{
+				isOverlapping = true;
+				break;
+			}
+		}
+	}
+	
+	// Update the contact state.
+
+	if (isOverlapping == true)
+	{
+		m_flags |= e_overlapFlag;
+	}
+	else
+	{
+		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 != nullptr) 
+	{
+		if (wasOverlapping == false && isOverlapping == true)
+		{
+			listener->BeginContact(this);
+		}
+
+		if (wasOverlapping == true && isOverlapping == false)
+		{
+			listener->EndContact(this);
+		}
+
+		if (isSensorContact == false && isOverlapping == true)
+		{
+			listener->PreSolve(this);
+		}
+	}
+}

src/bounce/dynamics/contacts/contact_cluster.cpp

@@ -0,0 +1,519 @@
+/*
+* 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\contacts\contact_cluster.h>
+#include <bounce\collision\distance.h>
+
+inline void AddCluster(b3Array<b3Cluster>& clusters, const b3Vec3& centroid)
+{
+	const float32 kTol = 0.05f;
+	for (u32 i = 0; i < clusters.Count(); ++i)
+	{
+		b3Vec3 c = clusters[i].centroid;
+		float32 dd = b3DistanceSquared(centroid, c);
+		if (dd < kTol * kTol)
+		{
+			// Merge the clusters.
+			clusters[i].centroid = 0.5f * (clusters[i].centroid + centroid);
+			return;
+		}
+	}
+
+	b3Cluster c;
+	c.centroid = centroid;
+	clusters.PushBack(c);
+}
+
+void b3InitializeClusters(b3Array<b3Cluster>& outClusters, const b3Array<b3Observation>& inObs)
+{
+	B3_ASSERT(outClusters.IsEmpty());
+	
+	const u32 kMaxClusters = 3;
+
+	if (inObs.Count() <= kMaxClusters)
+	{
+		for (u32 i = 0; i < inObs.Count(); ++i)
+		{
+			const b3Observation& o = inObs[i];
+			AddCluster(outClusters, o.point);
+		}
+		return;
+	}
+
+	B3_ASSERT(inObs.Count() > 3);
+
+	// This is used to skip observations that were 
+	// assigned to a cluster.
+	b3StackArray<bool, 64> chosens;
+	chosens.Resize(inObs.Count());
+	for (u32 i = 0; i < inObs.Count(); ++i)
+	{
+		chosens[i] = false;
+	}
+
+	{
+		u32 index = 0;
+		const b3Observation& o = inObs[index];
+		b3Vec3 A = o.point;
+		AddCluster(outClusters, A);
+		chosens[index] = true;
+	}
+
+	{
+		b3Vec3 A = outClusters[0].centroid;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < inObs.Count(); ++i)
+		{
+			if (chosens[i])	{ continue;	}
+
+			const b3Observation& o = inObs[i];
+
+			b3Vec3 B = o.point;
+			float32 dd = b3DistanceSquared(A, B);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		const b3Observation& o = inObs[index];
+		b3Vec3 B = o.point;
+		AddCluster(outClusters, B);
+		chosens[index] = true;
+	}
+
+	B3_ASSERT(outClusters.Count() == 1 || outClusters.Count() == 2);
+
+	if (outClusters.Count() == 1)
+	{
+		b3Vec3 A = outClusters[0].centroid;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < inObs.Count(); ++i)
+		{
+			if (chosens[i])	{ continue; }
+
+			const b3Observation& o = inObs[i];
+			
+			b3Vec3 B = o.point;
+
+			float32 dd = b3DistanceSquared(A, B);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		const b3Observation& o = inObs[index];
+		b3Vec3 B = o.point;
+		AddCluster(outClusters, B);
+		chosens[index] = true;
+		return;
+	}
+
+	B3_ASSERT(outClusters.Count() == 2);
+
+	{
+		b3Vec3 A = outClusters[0].centroid;
+		b3Vec3 B = outClusters[1].centroid;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < inObs.Count(); ++i)
+		{
+			if (chosens[i]) { continue; }
+
+			const b3Observation& o = inObs[i];
+			
+			b3Vec3 C = o.point;
+			b3Vec3 Q = b3ClosestPointOnSegment(C, A, B);
+
+			float32 dd = b3DistanceSquared(C, Q);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		const b3Observation& o = inObs[index];
+		b3Vec3 C = o.point;
+		AddCluster(outClusters, C);
+		chosens[index] = true;
+	}
+}
+
+inline void b3MoveObsToCluster(b3Array<b3Observation>& observations, u32 fromCluster, u32 toCluster)
+{
+	for (u32 i = 0; i < observations.Count(); ++i)
+	{
+		b3Observation& obs = observations[i];
+		if (obs.cluster == fromCluster)
+		{
+			obs.cluster = toCluster;
+		}
+	}
+}
+
+inline u32 b3BestCluster(const b3Array<b3Cluster>& clusters, const b3Vec3& point)
+{
+	u32 bestIndex = 0;
+	float32 bestValue = B3_MAX_FLOAT;
+	for (u32 i = 0; i < clusters.Count(); ++i)
+	{
+		b3Vec3 centroid = clusters[i].centroid;
+		float32 metric = b3DistanceSquared(point, centroid);
+		if (metric < bestValue)
+		{
+			bestValue = metric;
+			bestIndex = i;
+		}
+	}
+	return bestIndex;
+}
+
+void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outObservations,
+	const b3Array<b3Cluster>& inClusters, const b3Array<b3Observation>& inObservations)
+{
+	//B3_ASSERT(!inObservations.IsEmpty());
+	B3_ASSERT(outObservations.IsEmpty());
+
+	//B3_ASSERT(!inClusters.IsEmpty());
+	B3_ASSERT(outClusters.IsEmpty());
+
+	// Temporary data
+	b3StackArray<b3Cluster, 32> clusters;
+	clusters.Swap(inClusters);
+	
+	b3StackArray<b3Observation, 32> observations;
+	observations.Swap(inObservations);
+
+	// Termination criteria for k-means clustering
+	const u32 kMaxIters = 10;
+
+	u32 iter = 0;
+	while (iter < kMaxIters)
+	{
+		// Assign each observation to the closest cluster centroid.
+		for (u32 i = 0; i < observations.Count(); ++i)
+		{
+			b3Observation& obs = observations[i];
+			obs.cluster = b3BestCluster(clusters, obs.point);
+		}
+
+		// Compute the new cluster centroids.
+		for (u32 i = 0; i < clusters.Count(); ++i)
+		{
+			b3Cluster& cluster = clusters[i];
+			
+			b3Vec3 centroid;
+			centroid.SetZero();
+
+			u32 pointCount = 0;
+			for (u32 j = 0; j < observations.Count(); ++j)
+			{
+				const b3Observation& obs = observations[j];
+				if (obs.cluster == i)
+				{
+					centroid += obs.point;
+					++pointCount;
+				}
+			}
+
+			if (pointCount > 0)
+			{
+				centroid *= 1.0f / float32(pointCount);
+				cluster.centroid = centroid;
+			}
+		}
+
+		++iter;
+	}
+
+	// Remove empty clusters.
+	outObservations.Swap(observations);
+	
+	u32 numOut = 0;
+
+	for (u32 i = 0; i < clusters.Count(); ++i)
+	{
+		u32 pointCount = 0;
+		for (u32 j = 0; j < outObservations.Count(); ++j)
+		{
+			const b3Observation& obs = outObservations[j];
+			if (obs.cluster == i)
+			{
+				++pointCount;
+			}
+		}
+
+		if (pointCount > 0)
+		{
+			// Transfer the clusters observations into the newly cluster.
+			b3MoveObsToCluster(outObservations, i, numOut);
+			outClusters.PushBack(clusters[i]);
+			++numOut;
+		}
+	}
+}
+
+void b3ReducePolygon(b3ClusterPolygon& pOut, 
+	const b3ClusterPolygon& pIn, u32 startIndex)
+{
+	B3_ASSERT(startIndex < pIn.Count());
+
+	B3_ASSERT(pOut.Count() == 0);
+	
+	pOut.Reserve(pIn.Count());
+
+	if (pIn.Count() <= B3_MAX_MANIFOLD_POINTS)
+	{
+		for (u32 i = 0; i < pIn.Count(); ++i)
+		{
+			pOut.PushBack(pIn[i]);
+		}
+		return;
+	}
+
+	B3_ASSERT(pIn.Count() > B3_MAX_MANIFOLD_POINTS);
+	
+	b3StackArray<bool, 32> chosens;
+	chosens.Resize(pIn.Count());
+	for (u32 i = 0; i < chosens.Count(); ++i)
+	{
+		chosens[i] = false;
+	}
+
+	{
+		u32 index = startIndex;
+		pOut.PushBack(pIn[index]);
+		chosens[index] = true;
+	}
+
+	{
+		b3Vec3 A = pOut[0].position;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < pIn.Count(); ++i)
+		{
+			if (chosens[i]) { continue; }
+
+			b3Vec3 B = pIn[i].position;
+			b3Vec3 d = B - A;
+			float32 dd = b3Dot(d, d);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		pOut.PushBack(pIn[index]);
+		chosens[index] = true;
+	}
+
+	{
+		b3Vec3 A = pOut[0].position;
+		b3Vec3 B = pOut[1].position;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < pIn.Count(); ++i)
+		{
+			if (chosens[i]) { continue; }
+
+			b3Vec3 C = pIn[i].position;
+			b3Vec3 Q = b3ClosestPointOnSegment(C, A, B);
+			b3Vec3 d = Q - C;
+			float32 dd = b3Dot(d, d);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		pOut.PushBack(pIn[index]);
+		chosens[index] = true;
+	}
+
+	{
+		b3Vec3 A = pOut[0].position;
+		b3Vec3 B = pOut[1].position;
+		b3Vec3 C = pOut[2].position;
+
+		u32 index = 0;
+		float32 max = -B3_MAX_FLOAT;
+		for (u32 i = 0; i < pIn.Count(); ++i)
+		{
+			if (chosens[i]) { continue; }
+
+			b3Vec3 D = pIn[i].position;
+			b3Vec3 Q = b3ClosestPointOnTriangle(D, A, B, C);
+			b3Vec3 d = Q - D;
+			float32 dd = b3Dot(d, d);
+			if (dd > max)
+			{
+				max = dd;
+				index = i;
+			}
+		}
+
+		pOut.PushBack(pIn[index]);
+		chosens[index] = true;
+	}
+
+	B3_ASSERT(pOut.Count() <= B3_MAX_MANIFOLD_POINTS);
+}
+
+
+u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 numIn,
+	const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB)
+{
+	u32 numOut = 0;
+
+	// Initialize normals
+	b3StackArray<b3Observation, 32> tempObservations;
+	for (u32 i = 0; i < numIn; ++i)
+	{
+		b3WorldManifold wm;
+		wm.Initialize(inManifolds + i, xfA, radiusA, xfB, radiusB);
+
+		for (u32 j = 0; j < wm.pointCount; ++j)
+		{
+			b3Observation obs;
+			obs.manifold = i;
+			obs.manifoldPoint = j;
+			obs.point = wm.points[j].normal;
+			obs.cluster = B3_NULL_CLUSTER;
+
+			tempObservations.PushBack(obs);
+		}
+	}
+
+	// Initialize clusters
+	b3StackArray<b3Cluster, 3> tempClusters;
+	b3InitializeClusters(tempClusters, tempObservations);
+
+	// Cluster 
+	b3StackArray<b3Cluster, 3> clusters;
+	b3StackArray<b3Observation, 32> observations;
+	b3Clusterize(clusters, observations, tempClusters, tempObservations);
+
+	B3_ASSERT(clusters.Count() <= 3);
+
+	for (u32 i = 0; i < clusters.Count(); ++i)
+	{		
+		// Gather manifold points.
+		b3Vec3 center;
+		center.SetZero();
+		
+		b3Vec3 normal;
+		normal.SetZero();
+
+		b3StackArray<b3ClusterVertex, 32> polygonB;
+		for (u32 j = 0; j < observations.Count(); ++j)
+		{
+			b3Observation& o = observations[j];
+			if (o.cluster != i)
+			{
+				continue;
+			}
+
+			const b3Manifold* m = inManifolds + o.manifold;
+			const b3ManifoldPoint* mp = m->points + o.manifoldPoint;
+
+			b3WorldManifoldPoint wmp;
+			wmp.Initialize(mp, xfA, radiusA, xfB, radiusB);
+
+			b3ClusterVertex cv;
+			cv.position = wmp.point;
+			cv.clipIndex = j;
+			polygonB.PushBack(cv);
+
+			center += wmp.point;
+			normal += o.point;
+		}
+
+		if (polygonB.IsEmpty())
+		{
+			continue;
+		}
+
+		center /= float32(polygonB.Count());
+		//normal /= float32(polygonB.Count());
+		normal.Normalize();
+
+		B3_ASSERT(numOut < B3_MAX_MANIFOLDS);
+		b3Manifold* manifold = outManifolds + numOut;
+		++numOut;
+
+		manifold->center = center;
+		manifold->normal = normal;
+		manifold->tangent1 = b3Perp(normal);
+		manifold->tangent2 = b3Cross(manifold->tangent1, normal);
+
+		// Reduce.
+		// Ensure deepest point is contained.
+		u32 minIndex = 0;
+		float32 minSeparation = B3_MAX_FLOAT;
+		for (u32 j = 0; j < polygonB.Count(); ++j)
+		{
+			const b3Observation* o = observations.Get(polygonB[j].clipIndex);
+			const b3Manifold* inManifold = inManifolds + o->manifold;
+			const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
+
+			b3WorldManifoldPoint wmp;
+			wmp.Initialize(inPoint, xfA, radiusA, xfB, radiusB);
+
+			float32 separation = wmp.separation;
+			if (separation < minSeparation)
+			{
+				minIndex = j;
+				minSeparation = separation;
+			}
+
+			polygonB[j].position = polygonB[j].position + separation * normal;
+		}
+		
+		b3StackArray<b3ClusterVertex, 32> reducedB;
+		b3ReducePolygon(reducedB, polygonB, minIndex);
+		for (u32 j = 0; j < reducedB.Count(); ++j)
+		{
+			b3ClusterVertex v = reducedB[j];
+			u32 inIndex = v.clipIndex;
+
+			const b3Observation* o = observations.Get(inIndex);
+			const b3Manifold* inManifold = inManifolds + o->manifold;
+			const b3ManifoldPoint* inPoint = inManifold->points + o->manifoldPoint;
+
+			manifold->points[j] = *inPoint;
+		}
+
+		manifold->pointCount = reducedB.Count();
+	}
+
+	B3_ASSERT(numOut <= B3_MAX_MANIFOLDS);
+	return numOut;
+}

src/bounce/dynamics/contacts/contact_solver.cpp

@@ -0,0 +1,582 @@
+/*
+* 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\contacts\contact_solver.h>
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\memory\stack_allocator.h>
+
+// This solver implements PGS for solving velocity constraints and 
+// NGS for solving position constraints.
+
+b3ContactSolver::b3ContactSolver(const b3ContactSolverDef* def)
+{
+	m_allocator = def->allocator;
+	m_count = def->count;
+	m_positions = def->positions;
+	m_velocities = def->velocities;
+	m_contacts = def->contacts;
+	m_positionConstraints = (b3ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b3ContactPositionConstraint));
+	m_velocityConstraints = (b3ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b3ContactVelocityConstraint));
+	m_dt = def->dt;
+	m_invDt = m_dt != 0.0f ? 1.0f / m_dt : 0.0f;
+}
+
+b3ContactSolver::~b3ContactSolver()
+{
+	// Reverse free.
+	for (u32 index1 = m_count; index1 > 0; --index1)
+	{
+		u32 i1 = index1 - 1;
+		b3ContactPositionConstraint* pc = m_positionConstraints + i1;
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i1;
+		
+		for (u32 index2 = pc->manifoldCount; index2 > 0; --index2)
+		{
+			u32 i2 = index2 - 1;
+			b3PositionConstraintManifold* pcm = pc->manifolds + i2;
+			b3VelocityConstraintManifold* vcm = vc->manifolds + i2;
+			
+			m_allocator->Free(vcm->points);
+			m_allocator->Free(pcm->points);
+		}
+
+		m_allocator->Free(vc->manifolds);
+		m_allocator->Free(pc->manifolds);
+	}
+
+	m_allocator->Free(m_velocityConstraints);
+	m_allocator->Free(m_positionConstraints);
+}
+
+void b3ContactSolver::InitializeConstraints()
+{
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3Contact* c = m_contacts[i];
+
+		b3Shape* shapeA = c->GetShapeA();
+		b3Shape* shapeB = c->GetShapeB();
+
+		b3Body* bodyA = shapeA->GetBody();
+		b3Body* bodyB = shapeB->GetBody();
+
+		u32 manifoldCount = c->m_manifoldCount;
+		b3Manifold* manifolds = c->m_manifolds;
+
+		b3ContactPositionConstraint* pc = m_positionConstraints + i;
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+		pc->indexA = bodyA->m_islandID;
+		pc->invMassA = bodyA->m_invMass;
+		pc->invIA = bodyA->m_worldInvI;
+		pc->localCenterA = bodyA->m_sweep.localCenter;
+		pc->radiusA = shapeA->m_radius;
+
+		pc->indexB = bodyB->m_islandID;
+		pc->invMassB = bodyB->m_invMass;
+		pc->invIB = bodyB->m_worldInvI;
+		pc->localCenterB = bodyB->m_sweep.localCenter;
+		pc->radiusB = shapeB->m_radius;
+
+		pc->manifoldCount = manifoldCount;
+		pc->manifolds = (b3PositionConstraintManifold*)m_allocator->Allocate(manifoldCount * sizeof(b3PositionConstraintManifold));
+
+		vc->indexA = bodyA->m_islandID;
+		vc->invMassA = bodyA->m_invMass;
+		vc->invIA = bodyA->m_worldInvI;
+
+		vc->indexB = bodyB->m_islandID;
+		vc->invMassB = bodyB->m_invMass;
+		vc->invIB = bodyB->m_worldInvI;
+
+		vc->friction = b3MixFriction(shapeA->m_friction, shapeB->m_friction);
+		vc->restitution = b3MixRestitution(shapeA->m_restitution, shapeB->m_restitution);
+
+		vc->manifoldCount = manifoldCount;
+		vc->manifolds = (b3VelocityConstraintManifold*)m_allocator->Allocate(manifoldCount * sizeof(b3VelocityConstraintManifold));
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3Manifold* m = manifolds + j;
+			b3PositionConstraintManifold* pcm = pc->manifolds + j;
+			b3VelocityConstraintManifold* vcm = vc->manifolds + j;
+
+			pcm->pointCount = m->pointCount;
+			pcm->points = (b3PositionConstraintPoint*)m_allocator->Allocate(pcm->pointCount * sizeof(b3PositionConstraintPoint));
+			
+			vcm->pointCount = m->pointCount;
+			vcm->points = (b3VelocityConstraintPoint*)m_allocator->Allocate(vcm->pointCount * sizeof(b3VelocityConstraintPoint));
+			
+			vcm->tangentImpulse = m->tangentImpulse;
+			vcm->motorImpulse = m->motorImpulse;
+			
+			for (u32 k = 0; k < m->pointCount; ++k)
+			{
+				b3ManifoldPoint* cp = m->points + k;
+				b3PositionConstraintPoint* pcp = pcm->points + k;
+				b3VelocityConstraintPoint* vcp = vcm->points + k;
+
+				pcp->localNormalA = cp->localNormal;
+				pcp->localPointA = cp->localPoint;
+				pcp->localPointB = cp->localPoint2;
+
+				vcp->normalImpulse = cp->normalImpulse;
+			}
+		}
+	}
+
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3Contact* c = m_contacts[i];
+		u32 manifoldCount = c->m_manifoldCount;
+
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b3ContactPositionConstraint* pc = m_positionConstraints + i;
+
+		float32 radiusA = pc->radiusA;
+		float32 radiusB = pc->radiusB;
+
+		u32 indexA = vc->indexA;
+		float32 mA = vc->invMassA;
+		b3Mat33 iA = vc->invIA;
+
+		u32 indexB = vc->indexB;
+		float32 mB = vc->invMassB;
+		b3Mat33 iB = vc->invIB;
+
+		b3Vec3 xA = m_positions[indexA].x;
+		b3Quat qA = m_positions[indexA].q;
+		b3Vec3 xB = m_positions[indexB].x;
+		b3Quat qB = m_positions[indexB].q;
+
+		b3Vec3 vA = m_velocities[indexA].v;
+		b3Vec3 wA = m_velocities[indexA].w;
+		b3Vec3 vB = m_velocities[indexB].v;
+		b3Vec3 wB = m_velocities[indexB].w;
+
+		b3Transform xfA(xA, qA);
+		b3Transform xfB(xB, qB);
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3Manifold* m = c->m_manifolds + j;
+			b3PositionConstraintManifold* pcm = pc->manifolds + j;
+			b3VelocityConstraintManifold* vcm = vc->manifolds + j;
+
+			b3WorldManifold wm;
+			wm.Initialize(m, xfA, radiusA, xfB, radiusB);
+
+			vcm->center = wm.center;
+			vcm->normal = wm.normal;
+			vcm->tangent1 = wm.tangent1;
+			vcm->tangent2 = wm.tangent2;
+
+			u32 pointCount = wm.pointCount;
+
+			for (u32 k = 0; k < pointCount; ++k)
+			{
+				b3ManifoldPoint* mp = m->points + k;
+				b3WorldManifoldPoint* wmp = wm.points + k;
+				
+				b3PositionConstraintPoint* pcp = pcm->points + k;
+				b3VelocityConstraintPoint* vcp = vcm->points + k;
+
+				b3Vec3 normal = wmp->normal;
+				b3Vec3 point = wmp->point;
+				b3Vec3 rA = point - xA;
+				b3Vec3 rB = point - xB;
+
+				vcp->rA = rA;
+				vcp->rB = rB;
+
+				// Add normal constraint.
+				{
+					vcp->normal = normal;
+
+					// Compute effective mass.
+					b3Vec3 rnA = b3Cross(rA, normal);
+					b3Vec3 rnB = b3Cross(rB, normal);
+					float32 K = mA + mB + b3Dot(iA * rnA, rnA) + b3Dot(iB * rnB, rnB);
+					vcp->normalMass = K > 0.0f ? 1.0f / K : 0.0f;
+
+					// Add restitution to the velocity constraint.
+					b3Vec3 dv = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
+					float32 vn = b3Dot(normal, dv);
+					vcp->velocityBias = 0.0f;
+					if (vn < -B3_VELOCITY_THRESHOLD)
+					{
+						vcp->velocityBias = -vc->restitution * vn;
+					}
+				}
+			}
+
+			if (pointCount > 0)
+			{
+				b3Vec3 rA = vcm->center - xA;
+				b3Vec3 rB = vcm->center - xB;
+
+				vcm->rA = rA;
+				vcm->rB = rB;
+
+				// Add friction constraint.
+				{
+					b3Vec3 t1 = vcm->tangent1;
+					b3Vec3 t2 = vcm->tangent2;
+
+					// Compute effective mass.
+					// Identities used:
+					// I = I^T because I is symmetric. Its inverse is also symmetric.
+					// dot(a * u, b * v) = a * b * dot(u, v)
+					// dot(t1, t2) = 0
+					b3Vec3 rt1A = b3Cross(rA, t1);
+					b3Vec3 rt1B = b3Cross(rB, t1);
+					b3Vec3 rt2A = b3Cross(rA, t2);
+					b3Vec3 rt2B = b3Cross(rB, t2);
+
+					float32 kTan11 = mA + mB + b3Dot(iA * rt1A, rt1A) + b3Dot(iB * rt2B, rt2B);
+					float32 kTan12 = b3Dot(iA * rt1A, rt2A) + b3Dot(iB * rt1B, rt2B);
+					float32 kTan21 = kTan12;
+					float32 kTan22 = mA + mB + b3Dot(iA * rt2A, rt2A) + b3Dot(iB * rt2B, rt2B);
+
+					b3Mat22 K;
+					K.x.x = kTan11;
+					K.x.y = kTan12;
+					K.y.x = kTan21;
+					K.y.y = kTan22;
+
+					vcm->tangentMass = K;
+				}
+
+				// Add motor constraint.
+				{
+					float32 mass = b3Dot(vcm->normal, (iA + iB) * vcm->normal);
+					vcm->motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
+				}
+			}
+		}
+	}
+}
+
+void b3ContactSolver::WarmStart()
+{
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+		u32 indexA = vc->indexA;
+		float32 mA = vc->invMassA;
+		b3Mat33 iA = vc->invIA;
+
+		u32 indexB = vc->indexB;
+		float32 mB = vc->invMassB;
+		b3Mat33 iB = vc->invIB;
+
+		u32 manifoldCount = vc->manifoldCount;
+
+		b3Vec3 vA = m_velocities[indexA].v;
+		b3Vec3 wA = m_velocities[indexA].w;
+		b3Vec3 vB = m_velocities[indexB].v;
+		b3Vec3 wB = m_velocities[indexB].w;
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3VelocityConstraintManifold* vcm = vc->manifolds + j;
+			u32 pointCount = vcm->pointCount;
+
+			for (u32 k = 0; k < pointCount; ++k)
+			{
+				b3VelocityConstraintPoint* vcp = vcm->points + k;
+
+				b3Vec3 P = vcp->normalImpulse * vcp->normal;
+
+				vA -= mA * P;
+				wA -= iA * b3Cross(vcp->rA, P);
+
+				vB += mB * P;
+				wB += iB * b3Cross(vcp->rB, P);
+			}
+
+			if (pointCount > 0)
+			{
+				b3Vec3 P1 = vcm->tangentImpulse.x * vcm->tangent1;
+				b3Vec3 P2 = vcm->tangentImpulse.y * vcm->tangent2;
+				b3Vec3 P3 = vcm->motorImpulse * vcm->normal;
+
+				vA -= mA * (P1 + P2);
+				wA -= iA * (b3Cross(vcm->rA, P1 + P2) + P3);
+
+				vB += mB * (P1 + P2);
+				wB += iB * (b3Cross(vcm->rB, P1 + P2) + P3);
+			}
+		}
+
+		m_velocities[indexA].v = vA;
+		m_velocities[indexA].w = wA;
+		m_velocities[indexB].v = vB;
+		m_velocities[indexB].w = wB;
+	}
+}
+
+void b3ContactSolver::SolveVelocityConstraints()
+{
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
+		u32 manifoldCount = vc->manifoldCount;
+
+		u32 indexA = vc->indexA;
+		float32 mA = vc->invMassA;
+		b3Mat33 iA = vc->invIA;
+
+		u32 indexB = vc->indexB;
+		float32 mB = vc->invMassB;
+		b3Mat33 iB = vc->invIB;
+
+		b3Vec3 vA = m_velocities[indexA].v;
+		b3Vec3 wA = m_velocities[indexA].w;
+		b3Vec3 vB = m_velocities[indexB].v;
+		b3Vec3 wB = m_velocities[indexB].w;
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3VelocityConstraintManifold* vcm = vc->manifolds + j;
+			u32 pointCount = vcm->pointCount;
+
+			float32 normalImpulse = 0.0f;
+
+			for (u32 k = 0; k < pointCount; ++k)
+			{
+				b3VelocityConstraintPoint* vcp = vcm->points + k;
+				B3_ASSERT(vcp->normalImpulse >= 0.0f);
+
+				// Solve normal constraint.
+				{
+					b3Vec3 dv = vB + b3Cross(wB, vcp->rB) - vA - b3Cross(wA, vcp->rA);
+					float32 Cdot = b3Dot(vcp->normal, dv);
+
+					float32 impulse = -vcp->normalMass * (Cdot - vcp->velocityBias);
+
+					float32 oldImpulse = vcp->normalImpulse;
+					vcp->normalImpulse = b3Max(vcp->normalImpulse + impulse, 0.0f);
+					impulse = vcp->normalImpulse - oldImpulse;
+
+					b3Vec3 P = impulse * vcp->normal;
+
+					vA -= mA * P;
+					wA -= iA * b3Cross(vcp->rA, P);
+
+					vB += mB * P;
+					wB += iB * b3Cross(vcp->rB, P);
+
+					normalImpulse += vcp->normalImpulse;
+				}
+			}
+			
+			if (pointCount > 0)
+			{
+				// Solve tangent constraints.
+				{
+					b3Vec3 dv = vB + b3Cross(wB, vcm->rB) - vA - b3Cross(wA, vcm->rA);
+					
+					b3Vec2 Cdot;
+					Cdot.x = b3Dot(dv, vcm->tangent1);
+					Cdot.y = b3Dot(dv, vcm->tangent2);
+
+					b3Vec2 impulse = vcm->tangentMass.Solve(-Cdot);
+					b3Vec2 oldImpulse = vcm->tangentImpulse;
+					vcm->tangentImpulse += impulse;
+					
+					float32 maxImpulse = vc->friction * normalImpulse;
+					if (b3Dot(vcm->tangentImpulse, vcm->tangentImpulse) > maxImpulse * maxImpulse)
+					{
+						vcm->tangentImpulse.Normalize();
+						vcm->tangentImpulse *= maxImpulse;
+					}
+					
+					impulse = vcm->tangentImpulse - oldImpulse;
+
+					b3Vec3 P1 = impulse.x * vcm->tangent1;
+					b3Vec3 P2 = impulse.y * vcm->tangent2;
+					b3Vec3 P = P1 + P2;
+
+					vA -= mA * P;
+					wA -= iA * b3Cross(vcm->rA, P);
+
+					vB += mB * P;
+					wB += iB * b3Cross(vcm->rB, P);
+				}
+
+				// Solve motor constraint.
+				{
+					float32 Cdot = b3Dot(vcm->normal, wB - wA);
+					float32 impulse = -vcm->motorMass * Cdot;
+					float32 oldImpulse = vcm->motorImpulse;
+					float32 maxImpulse = vc->friction * normalImpulse;
+					vcm->motorImpulse = b3Clamp(vcm->motorImpulse + impulse, -maxImpulse, maxImpulse);
+					impulse = vcm->motorImpulse - oldImpulse;
+
+					b3Vec3 P = impulse * vcm->normal;
+
+					wA -= iA * P;
+					wB += iB * P;
+				}
+			}
+		}
+
+		m_velocities[indexA].v = vA;
+		m_velocities[indexA].w = wA;
+		m_velocities[indexB].v = vB;
+		m_velocities[indexB].w = wB;
+	}
+}
+
+void b3ContactSolver::StoreImpulses()
+{
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3Contact* c = m_contacts[i];
+		b3Manifold* manifolds = c->m_manifolds;
+		u32 manifoldCount = c->m_manifoldCount;
+
+		b3ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3Manifold* m = manifolds + j;
+			u32 pointCount = m->pointCount;
+
+			b3VelocityConstraintManifold* vcm = vc->manifolds + j;
+			m->tangentImpulse = vcm->tangentImpulse;
+			m->motorImpulse = vcm->motorImpulse;
+
+			for (u32 k = 0; k < pointCount; ++k)
+			{
+				b3ManifoldPoint* cp = m->points + k;
+				b3VelocityConstraintPoint* vcp = vcm->points + k;
+				cp->normalImpulse = vcp->normalImpulse;
+			}
+		}
+	}
+}
+
+struct b3ContactPositionSolverPoint
+{
+	void Initialize(const b3ContactPositionConstraint* pc, const b3PositionConstraintPoint* pcp, const b3Transform& xfA, const b3Transform& xfB)
+	{
+		normal = b3Mul(xfA.rotation, pcp->localNormalA);
+		b3Vec3 p1 = b3Mul(xfA, pcp->localPointA);
+		b3Vec3 p2 = b3Mul(xfB, pcp->localPointB);
+		point = 0.5f * (p1 + pc->radiusA * normal + p2 - pc->radiusB * normal);
+		separation = b3Dot(p2 - p1, normal) - pc->radiusA - pc->radiusB;
+	}
+
+	b3Vec3 normal;
+	b3Vec3 point;
+	float32 separation;
+};
+
+bool b3ContactSolver::SolvePositionConstraints()
+{
+	float32 minSeparation = 0.0f;
+
+	for (u32 i = 0; i < m_count; ++i)
+	{
+		b3ContactPositionConstraint* pc = m_positionConstraints + i;
+
+		u32 indexA = pc->indexA;
+		float32 mA = pc->invMassA;
+		b3Mat33 iA = pc->invIA;
+		b3Vec3 localCenterA = pc->localCenterA;
+
+		u32 indexB = pc->indexB;
+		float32 mB = pc->invMassB;
+		b3Mat33 iB = pc->invIB;
+		b3Vec3 localCenterB = pc->localCenterB;
+
+		b3Vec3 xA = m_positions[indexA].x;
+		b3Quat qA = m_positions[indexA].q;
+
+		b3Vec3 xB = m_positions[indexB].x;
+		b3Quat qB = m_positions[indexB].q;
+
+		u32 manifoldCount = pc->manifoldCount;
+
+		for (u32 j = 0; j < manifoldCount; ++j)
+		{
+			b3PositionConstraintManifold* pcm = pc->manifolds + j;
+			u32 pointCount = pcm->pointCount;
+
+			// Solve normal constraints
+			for (u32 k = 0; k < pointCount; ++k)
+			{
+				b3PositionConstraintPoint* pcp = pcm->points + k;
+
+				b3Transform xfA;
+				xfA.rotation = b3ConvertQuatToRot(qA);
+				xfA.position = xA - b3Mul(xfA.rotation, localCenterA);
+
+				b3Transform xfB;
+				xfB.rotation = b3ConvertQuatToRot(qB);
+				xfB.position = xB - b3Mul(xfB.rotation, localCenterB);
+
+				b3ContactPositionSolverPoint cpcp;
+				cpcp.Initialize(pc, pcp, xfA, xfB);
+
+				b3Vec3 normal = cpcp.normal;
+				b3Vec3 point = cpcp.point;
+				float32 separation = cpcp.separation;
+
+				b3Vec3 rA = point - xA;
+				b3Vec3 rB = point - xB;
+
+				// Update max constraint error.
+				minSeparation = b3Min(minSeparation, separation);
+
+				// Prevent large corrections and allow some slop.
+				float32 C = b3Clamp(B3_BAUMGARTE * (separation + B3_LINEAR_SLOP), -B3_MAX_LINEAR_CORRECTION, 0.0f);
+
+				// Compute effective mass.
+				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;
+
+				xA -= mA * P;
+				qA -= b3Derivative(qA, iA * b3Cross(rA, P));
+				qA.Normalize();
+
+				xB += mB * P;
+				qB += b3Derivative(qB, iB * b3Cross(rB, P));
+				qB.Normalize();
+			}
+		}
+
+		m_positions[indexA].x = xA;
+		m_positions[indexA].q = qA;
+
+		m_positions[indexB].x = xB;
+		m_positions[indexB].q = qB;
+	}
+
+	return minSeparation >= -3.0f * B3_LINEAR_SLOP;
+}

src/bounce/dynamics/contacts/convex_contact.cpp

@@ -0,0 +1,61 @@
+/*
+* 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\contacts\convex_contact.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\world.h>
+
+b3ConvexContact::b3ConvexContact(b3Shape* shapeA, b3Shape* shapeB)
+{
+	m_type = e_convexContact;
+
+	m_manifoldCapacity = 1;
+	m_manifolds = &m_stackManifold;
+	m_manifoldCount = 0;
+
+	m_cache.simplexCache.count = 0;
+	m_cache.featureCache.m_featurePair.state = b3SATCacheType::e_empty;
+}
+
+bool b3ConvexContact::TestOverlap()
+{
+	b3Shape* shapeA = GetShapeA();
+	b3Transform xfA = shapeA->GetTransform();
+
+	b3Shape* shapeB = GetShapeB();
+	b3Transform xfB = shapeB->GetTransform();
+
+	return b3TestOverlap(xfA, 0, shapeA, xfB, 0, shapeB, &m_cache);
+}
+
+void b3ConvexContact::Collide()
+{
+	b3Shape* shapeA = GetShapeA();
+	b3Transform xfA = shapeA->GetTransform();
+
+	b3Shape* shapeB = GetShapeB();
+	b3Transform xfB = shapeB->GetTransform();
+
+	B3_ASSERT(m_manifoldCount == 0);
+	b3CollideShapeAndShape(m_stackManifold, xfA, shapeA, xfB, shapeB, &m_cache);
+	m_manifoldCount = 1;
+}
+
+void b3ConvexContact::SynchronizeShapes()
+{
+}

src/bounce/dynamics/contacts/manifold.cpp

@@ -0,0 +1,86 @@
+/*
+* 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\contacts\manifold.h>
+
+void b3Manifold::GuessImpulses()
+{
+	pointCount = 0;
+	tangentImpulse.SetZero();
+	motorImpulse = 0.0f;
+	for (u32 i = 0; i < B3_MAX_MANIFOLD_POINTS; ++i)
+	{
+		b3ManifoldPoint* p = points + i;
+		p->normalImpulse = 0.0f;
+		p->persisting = 0;
+	}
+}
+
+void b3Manifold::FindImpulses(const b3Manifold& oldManifold)
+{
+	tangentImpulse = oldManifold.tangentImpulse;
+	motorImpulse = oldManifold.motorImpulse;
+
+	for (u32 i = 0; i < oldManifold.pointCount; ++i)
+	{
+		const b3ManifoldPoint* p1 = oldManifold.points + i;
+		for (u32 j = 0; j < pointCount; ++j)
+		{
+			b3ManifoldPoint* p2 = points + j;
+			if (p2->triangleKey == p1->triangleKey && p2->key == p1->key)
+			{
+				p2->normalImpulse = p1->normalImpulse;
+				p2->persisting = 1;
+				break;
+			}
+		}
+	}
+}
+
+void b3WorldManifoldPoint::Initialize(const b3ManifoldPoint* mp,
+	const b3Transform& xfA, float32 radiusA,
+	const b3Transform& xfB, float32 radiusB)
+{
+	normal = b3Mul(xfA.rotation, mp->localNormal);
+	b3Vec3 p1 = b3Mul(xfA, mp->localPoint);
+	b3Vec3 p2 = b3Mul(xfB, mp->localPoint2);
+	point = 0.5f * (p1 + radiusA * normal + p2 - radiusB * normal);
+	separation = b3Dot(p2 - p1, normal) - radiusA - radiusB;
+}
+
+void b3WorldManifold::Initialize(const b3Manifold* manifold,
+	const b3Transform& xfA, float32 radiusA,
+	const b3Transform& xfB, float32 radiusB)
+{
+	if (manifold->pointCount > 0)
+	{
+		center = manifold->center;
+		normal = manifold->normal;
+		tangent1 = manifold->tangent1;
+		tangent2 = manifold->tangent2;
+	}
+
+	pointCount = manifold->pointCount;
+
+	for (u32 i = 0; i < pointCount; ++i)
+	{
+		const b3ManifoldPoint* mp = manifold->points + i;
+		b3WorldManifoldPoint* wmp = points + i;
+		wmp->Initialize(mp, xfA, radiusA, xfB, radiusB);
+	}
+}

src/bounce/dynamics/contacts/mesh_contact.cpp

@@ -0,0 +1,269 @@
+/*
+* 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\contacts\mesh_contact.h>
+#include <bounce\dynamics\contacts\contact_cluster.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\shapes\mesh_shape.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\collision\shapes\mesh.h>
+#include <bounce\collision\shapes\triangle_hull.h>
+#include <bounce\common\memory\stack_allocator.h>
+
+b3MeshContact::b3MeshContact(b3Shape* shapeA, b3Shape* shapeB)
+{
+	m_type = e_meshContact;
+
+	m_manifoldCapacity = B3_MAX_MANIFOLDS;
+	m_manifolds = m_stackManifolds;
+	m_manifoldCount = 0;
+
+	b3Transform xfA = shapeA->GetTransform();
+	b3Transform xfB = shapeB->GetTransform();
+
+	b3Transform xf = b3MulT(xfB, xfA);
+
+	b3AABB3 fatAABB;
+	shapeA->ComputeAABB(&fatAABB, xf);
+	fatAABB.Extend(B3_AABB_EXTENSION);
+
+	m_aabbA = fatAABB;
+	m_aabbMoved = true;
+
+	// Pre-allocate some indices
+	m_triangleCapacity = 16;
+	m_triangles = (b3TriangleCache*)b3Alloc(m_triangleCapacity * sizeof(b3TriangleCache));
+	m_triangleCount = 0;
+}
+
+b3MeshContact::~b3MeshContact()
+{
+	b3Free(m_triangles);
+}
+
+void b3MeshContact::SynchronizeShapes()
+{
+	b3Shape* shapeA = GetShapeA();
+	b3Body* bodyA = shapeA->GetBody();
+	b3Transform xfA = bodyA->m_xf;
+
+	b3Shape* shapeB = GetShapeB();
+	b3Transform xfB = shapeB->GetTransform();
+
+	b3Sweep* sweepA = &bodyA->m_sweep;
+	b3Transform xfA0;
+	xfA0.position = sweepA->worldCenter0;
+	xfA0.rotation = b3ConvertQuatToRot(sweepA->orientation0);
+		
+	// Calculate the displacement of the body A. 
+	// using its position at the last time step and the current position.
+	// Could use displacement = velocity * dt.
+	b3Vec3 displacement = xfA.position - xfA0.position;
+
+	// Compute the AABB in the reference frame of shape B.
+	b3Transform xf = b3MulT(xfB, xfA);
+	
+	b3AABB3 aabb;
+	shapeA->ComputeAABB(&aabb, xf);
+
+	// Update the AABB with the new (transformed) AABB and
+	// buffer move.
+	m_aabbMoved = MoveAABB(aabb, displacement);
+}
+
+bool b3MeshContact::MoveAABB(const b3AABB3& aabb, const b3Vec3& displacement)
+{
+	// Do nothing if the new AABB is contained in the old AABB.
+	if (m_aabbA.Contains(aabb))
+	{
+		// Do nothing if the new AABB is contained in the old AABB.
+		return false;
+	}
+
+	// Update the AABB with a fat and motion predicted AABB.
+
+	// Extend the new (original) AABB.
+	b3AABB3 fatAABB = aabb;
+	fatAABB.Extend(B3_AABB_EXTENSION);
+
+	if (displacement.x < 0.0f)
+	{
+		fatAABB.m_lower.x += B3_AABB_MULTIPLIER * displacement.x;
+	}
+	else
+	{
+		fatAABB.m_upper.x += B3_AABB_MULTIPLIER * displacement.x;
+	}
+
+	if (displacement.y < 0.0f)
+	{
+		fatAABB.m_lower.y += B3_AABB_MULTIPLIER * displacement.y;
+	}
+	else
+	{
+		fatAABB.m_upper.y += B3_AABB_MULTIPLIER * displacement.y;
+	}
+
+	if (displacement.z < 0.0f)
+	{
+		fatAABB.m_lower.z += B3_AABB_MULTIPLIER * displacement.z;
+	}
+	else
+	{
+		fatAABB.m_upper.z += B3_AABB_MULTIPLIER * displacement.z;
+	}
+
+	// Update proxy with the extented AABB.
+	m_aabbA = fatAABB;
+
+	// Notify the proxy has moved.
+	return true;
+}
+
+void b3MeshContact::FindNewPairs()
+{
+	// Reuse the overlapping buffer if the AABB didn't move
+	// significantly.
+	if (m_aabbMoved == false)
+	{
+		return;
+	}
+
+	// Clear the index cache.
+	m_triangleCount = 0;
+
+	const b3MeshShape* meshShapeB = (b3MeshShape*)GetShapeB();
+	const b3Mesh* meshB = meshShapeB->m_mesh;
+	const b3StaticTree* tree = &meshB->tree;
+
+	// Query and update the overlapping buffer.
+	tree->QueryAABB(this, m_aabbA);
+}
+
+bool b3MeshContact::Report(u32 proxyId)
+{
+	b3MeshShape* meshShapeB = (b3MeshShape*)GetShapeB();
+	const b3Mesh* meshB = meshShapeB->m_mesh;
+	const b3StaticTree* treeB = &meshB->tree;
+
+	u32 triangleIndex = treeB->GetUserData(proxyId);
+
+	// Add the triangle to the overlapping buffer.
+	if (m_triangleCount == m_triangleCapacity)
+	{
+		b3TriangleCache* oldElements = m_triangles;
+		m_triangleCapacity *= 2;
+		m_triangles = (b3TriangleCache*)b3Alloc(m_triangleCapacity * sizeof(b3TriangleCache));
+		memcpy(m_triangles, oldElements, m_triangleCount * sizeof(b3TriangleCache));
+		b3Free(oldElements);
+	}
+
+	B3_ASSERT(m_triangleCount  < m_triangleCapacity);
+
+	b3TriangleCache* cache = m_triangles + m_triangleCount;
+	cache->index = triangleIndex;
+	cache->cache.simplexCache.count = 0;
+	cache->cache.featureCache.m_featurePair.state = b3SATCacheType::e_empty;
+	
+	++m_triangleCount;
+
+	// Keep looking for triangles.
+	return true;
+}
+
+bool b3MeshContact::TestOverlap()
+{
+	b3Shape* shapeA = GetShapeA();
+	b3Transform xfA = shapeA->GetTransform();
+	u32 indexA = 0;
+
+	b3Shape* shapeB = GetShapeB();
+	b3Transform xfB = shapeB->GetTransform();
+
+	b3MeshShape* meshShapeB = (b3MeshShape*)shapeB;
+	const b3Mesh* meshB = meshShapeB->m_mesh;
+	const b3StaticTree* treeB = &meshB->tree;
+
+	// Test if at least one triangle of the shape B overlaps the shape A.
+	for (u32 i = 0; i < m_triangleCount; ++i)
+	{
+		b3TriangleCache* cache = m_triangles + i;
+		u32 indexB = cache->index;
+		bool overlap = b3TestOverlap(xfA, indexA, shapeA, xfB, indexB, shapeB, &cache->cache);
+		if (overlap == true)
+		{
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void b3MeshContact::Collide()
+{
+	B3_ASSERT(m_manifoldCount == 0);
+
+	b3Shape* shapeA = GetShapeA();
+	b3Body* bodyA = shapeA->GetBody();
+	b3Transform xfA = shapeA->GetTransform();
+
+	b3Shape* shapeB = GetShapeB();
+	b3MeshShape* meshShapeB = (b3MeshShape*)shapeB;
+	b3Transform xfB = shapeB->GetTransform();
+
+	b3World* world = bodyA->GetWorld();
+	b3StackAllocator* allocator = &world->m_stackAllocator;
+
+	b3Manifold* tempManifolds = (b3Manifold*)allocator->Allocate(m_triangleCount * sizeof(b3Manifold));
+	u32 tempCount = 0;
+
+	const b3Mesh* meshB = meshShapeB->m_mesh;
+	for (u32 i = 0; i < m_triangleCount; ++i)
+	{
+		b3TriangleCache* triangleCache = m_triangles + i;
+		u32 triangleIndex = triangleCache->index;
+		b3Triangle* triangle = meshB->triangles + triangleIndex;
+
+		b3Vec3 v1 = meshB->vertices[triangle->v1];
+		b3Vec3 v2 = meshB->vertices[triangle->v2];
+		b3Vec3 v3 = meshB->vertices[triangle->v3];
+
+		b3TriangleHull hullB(v1, v2, v3);
+
+		b3HullShape shapeB;
+		shapeB.m_hull = &hullB;
+		shapeB.m_radius = B3_HULL_RADIUS;
+
+		b3Manifold* manifold = tempManifolds + tempCount;
+		manifold->GuessImpulses();
+		b3CollideShapeAndShape(*manifold, xfA, shapeA, xfB, &shapeB, &triangleCache->cache);
+		
+		for (u32 j = 0; j < manifold->pointCount; ++j)
+		{
+			manifold->points[j].triangleKey = triangleIndex;
+		}
+		
+		++tempCount;
+	}
+
+	B3_ASSERT(m_manifoldCount == 0);
+	m_manifoldCount = b3Clusterize(m_stackManifolds, tempManifolds, tempCount, xfA, shapeA->m_radius, xfB, B3_HULL_RADIUS);
+	allocator->Free(tempManifolds);
+}

src/bounce/dynamics/draw_world.cpp

@@ -0,0 +1,281 @@
+/*
+* 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\common\draw.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\body.h>
+
+#include <bounce\dynamics\contacts\convex_contact.h>
+#include <bounce\dynamics\contacts\mesh_contact.h>
+
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\dynamics\shapes\mesh_shape.h>
+
+#include <bounce\dynamics\joints\mouse_joint.h>
+#include <bounce\dynamics\joints\spring_joint.h>
+#include <bounce\dynamics\joints\revolute_joint.h>
+#include <bounce\dynamics\joints\sphere_joint.h>
+#include <bounce\dynamics\joints\cone_joint.h>
+
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+#include <bounce\collision\shapes\mesh.h>
+
+void b3World::DebugDraw() const
+{
+	B3_ASSERT(m_debugDraw);
+
+	u32 flags = m_debugDraw->m_flags;
+
+	b3Color black(0.0f, 0.0f, 0.0f, 1.0f);
+	b3Color white(1.0f, 1.0f, 1.0f, 1.0f);
+	b3Color red(1.0f, 0.0f, 0.0f, 1.0f);
+	b3Color green(0.0f, 1.0f, 0.0f, 1.0f);
+	b3Color blue(0.0f, 0.0f, 1.0f, 1.0f);
+	b3Color yellow(1.0f, 1.0f, 0.0f, 1.0f);
+	b3Color purple(1.0f, 0.0f, 1.0f, 1.0f);
+
+	if (flags & b3Draw::e_centerOfMassesFlag)
+	{
+		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+		{
+			const b3Transform& xf = b->GetTransform();
+			m_debugDraw->DrawTransform(xf);
+		}
+	}
+
+	if (flags & b3Draw::e_shapesFlag)
+	{
+		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+		{
+			const b3Transform& xf = b->GetTransform();
+			for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next)
+			{
+				DrawShape(xf, s);
+			}
+		}
+	}
+
+	if (flags & b3Draw::e_aabbsFlag)
+	{
+		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+		{
+			for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next)
+			{
+				const b3AABB3& aabb = m_contactMan.m_broadPhase.GetAABB(s->m_broadPhaseID);
+				m_debugDraw->DrawAABB(aabb, purple);
+			}
+		}
+	}
+
+	if (flags & b3Draw::e_jointsFlag)
+	{
+		for (b3Joint* j = m_jointMan.m_jointList.m_head; j; j = j->m_next)
+		{
+			DrawJoint(j);
+		}
+	}
+
+	for (b3Contact* c = m_contactMan.m_contactList.m_head; c; c = c->m_next)
+	{
+		const b3Shape* shapeA = c->GetShapeA();
+		const b3Shape* shapeB = c->GetShapeB();
+
+		u32 manifoldCount = c->m_manifoldCount;
+		const b3Manifold* manifolds = c->m_manifolds;
+
+		for (u32 i = 0; i < manifoldCount; ++i)
+		{
+			const b3Manifold* m = manifolds + i;
+
+			b3WorldManifold wm;
+			wm.Initialize(m, shapeA->GetTransform(), shapeA->m_radius, shapeB->GetTransform(), shapeB->m_radius);
+
+			if (wm.pointCount > 0)
+			{
+				b3Vec3 n = wm.normal;
+				b3Vec3 t1 = wm.tangent1;
+				b3Vec3 t2 = wm.tangent2;
+				b3Vec3 p = wm.center;
+				float32 Pt1 = m->tangentImpulse.x;
+				float32 Pt2 = m->tangentImpulse.y;
+				
+				if (flags & b3Draw::e_contactPointsFlag)
+				{
+					m_debugDraw->DrawPoint(p, yellow);
+				}
+
+				if (flags & b3Draw::e_contactNormalsFlag)
+				{
+					m_debugDraw->DrawSegment(p, p + n, yellow);
+				}
+
+				if (flags & b3Draw::e_contactTangentsFlag)
+				{
+					m_debugDraw->DrawSegment(p, p + t1, yellow);
+					m_debugDraw->DrawSegment(p, p + t2, yellow);
+				}
+			}
+
+			for (u32 j = 0; j < wm.pointCount; ++j)
+			{
+				const b3ManifoldPoint* mp = m->points + j;
+				const b3WorldManifoldPoint* wmp = wm.points + j;
+
+				b3Vec3 n = wmp->normal;
+				b3Vec3 p = wmp->point;
+				float32 Pn = mp->normalImpulse;
+				
+				if (flags & b3Draw::e_contactPointsFlag)
+				{
+					m_debugDraw->DrawPoint(p, mp->persisting ? green : red);
+				}
+				
+				if (flags & b3Draw::e_contactNormalsFlag)
+				{
+					m_debugDraw->DrawSegment(p, p + n, white);
+				}
+			}
+		}
+	}
+}
+
+void b3World::DrawShape(const b3Transform& xf, const b3Shape* shape) const
+{
+	b3Color wireColor(0.0f, 0.0f, 0.0f);
+	switch (shape->GetType())
+	{
+	case e_sphereShape:
+	{
+		const b3SphereShape* sphere = (b3SphereShape*)shape;
+		b3Vec3 c = xf * sphere->m_center;
+		m_debugDraw->DrawPoint(c, wireColor);
+		break;
+	}
+	case e_capsuleShape:
+	{
+		const b3CapsuleShape* capsule = (b3CapsuleShape*)shape;
+		b3Vec3 c1 = xf * capsule->m_centers[0];
+		b3Vec3 c2 = xf * capsule->m_centers[1];
+		
+		m_debugDraw->DrawPoint(c1, wireColor);
+		m_debugDraw->DrawPoint(c2, wireColor);
+		m_debugDraw->DrawSegment(c1, c2, wireColor);
+		break;
+	}
+	case e_hullShape:
+	{
+		const b3HullShape* hs = (b3HullShape*)shape;
+		const b3Hull* hull = hs->m_hull;
+
+		for (u32 i = 0; i < hull->faceCount; ++i)
+		{
+			b3Vec3 polygon[B3_MAX_HULL_FEATURES];
+			u32 vCount = 0;
+
+			// Build convex polygon for loop
+			const b3Face* face = hull->GetFace(i);
+			const b3HalfEdge* begin = hull->GetEdge(face->edge);
+			const b3HalfEdge* edge = begin;
+			do
+			{
+				polygon[vCount++] = xf * hull->GetVertex(edge->origin);
+				edge = hull->GetEdge(edge->next);
+			} while (edge != begin);
+
+			m_debugDraw->DrawPolygon(polygon, vCount, wireColor);
+		}
+
+		break;
+	}
+	case e_meshShape:
+	{
+		const b3MeshShape* ms = (b3MeshShape*)shape;
+		const b3Mesh* mesh = ms->m_mesh;
+		for (u32 i = 0; i < mesh->triangleCount; ++i)
+		{
+			const b3Triangle* triangle = mesh->triangles + i;
+
+			b3Vec3 vs[3];
+			vs[0] = xf * mesh->vertices[triangle->v1];
+			vs[1] = xf * mesh->vertices[triangle->v2];
+			vs[2] = xf * mesh->vertices[triangle->v3];
+
+			m_debugDraw->DrawPolygon(vs, 3, wireColor);
+		}
+		break;
+	}
+	default:
+	{
+		break;
+	}
+	};
+}
+
+void b3World::DrawJoint(const b3Joint* joint) const
+{
+	b3JointType type = joint->GetType();
+
+	switch (type)
+	{
+	case e_mouseJoint:
+	{
+		b3MouseJoint* o = (b3MouseJoint*)joint;
+		o->Draw(m_debugDraw);
+		break;
+	}
+	case e_springJoint:
+	{
+		b3SpringJoint* o = (b3SpringJoint*)joint;
+		o->Draw(m_debugDraw);
+		break;
+	}
+	case e_revoluteJoint:
+	{
+		b3RevoluteJoint* o = (b3RevoluteJoint*)joint;
+		o->Draw(m_debugDraw);
+		break;
+	}
+	case e_sphereJoint:
+	{
+		b3SphereJoint* o = (b3SphereJoint*)joint;
+		o->Draw(m_debugDraw);
+		break;
+	}
+	case e_coneJoint:
+	{
+		b3ConeJoint* o = (b3ConeJoint*)joint;
+		o->Draw(m_debugDraw);
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+}
+
+void b3World::DrawContact(const b3Contact* c) const
+{
+
+}

src/bounce/dynamics/island.cpp

@@ -0,0 +1,302 @@
+/*
+* 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\island.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\time_step.h>
+#include <bounce\dynamics\joints\joint.h>
+#include <bounce\dynamics\joints\joint_solver.h>
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\dynamics\contacts\contact_solver.h>
+#include <bounce\common\memory\stack_allocator.h>
+#include <bounce\common\time.h>
+
+b3Island::b3Island(b3StackAllocator* allocator, u32 bodyCapacity, u32 contactCapacity, u32 jointCapacity) 
+{
+	m_allocator = allocator;
+	m_bodyCapacity = bodyCapacity;
+	m_contactCapacity = contactCapacity;
+	m_jointCapacity = jointCapacity;
+	
+	m_bodies = (b3Body**)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Body*));
+	m_velocities = (b3Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Velocity));
+	m_positions = (b3Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b3Position));
+	m_contacts = (b3Contact**)m_allocator->Allocate(m_contactCapacity * sizeof(b3Contact*));
+	m_joints = (b3Joint**)m_allocator->Allocate(m_jointCapacity * sizeof(b3Joint*));
+
+	m_bodyCount = 0;
+	m_contactCount = 0;
+	m_jointCount = 0;
+}
+
+b3Island::~b3Island() 
+{
+	// @note Reverse order of construction.
+	m_allocator->Free(m_joints);
+	m_allocator->Free(m_contacts);
+	m_allocator->Free(m_positions);
+	m_allocator->Free(m_velocities);
+	m_allocator->Free(m_bodies);
+}
+
+void b3Island::Clear() 
+{
+	m_bodyCount = 0;
+	m_contactCount = 0;
+	m_jointCount = 0;
+}
+
+void b3Island::Add(b3Body* b) 
+{
+	B3_ASSERT(m_bodyCount < m_bodyCapacity);
+	b->m_islandID = m_bodyCount;
+	m_bodies[m_bodyCount] = b;
+	++m_bodyCount;
+}
+
+void b3Island::Add(b3Contact* c) 
+{
+	B3_ASSERT(m_contactCount < m_contactCapacity);
+	m_contacts[m_contactCount] = c;
+	++m_contactCount;
+}
+
+void b3Island::Add(b3Joint* j) 
+{
+	B3_ASSERT(m_jointCount < m_jointCapacity);
+	m_joints[m_jointCount] = j;
+	++m_jointCount;
+}
+
+void b3Island::Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32 velocityIterations, u32 positionIterations, u32 flags)
+{
+	float32 h = dt;
+
+	// 1. Integrate velocities
+	for (u32 i = 0; i < m_bodyCount; ++i) 
+	{
+		b3Body* b = m_bodies[i];
+
+		b3Vec3 v = b->m_linearVelocity;
+		b3Vec3 w = b->m_angularVelocity;
+		b3Vec3 x = b->m_sweep.worldCenter;
+		b3Quat q = b->m_sweep.orientation;
+
+		// Remember the positions for CCD
+		b->m_sweep.worldCenter0 = b->m_sweep.worldCenter;
+		b->m_sweep.orientation0 = b->m_sweep.orientation;
+
+		if (b->m_type == e_dynamicBody) 
+		{
+			b3Vec3 force = b->m_force + b->m_gravityScale * gravity;
+			
+			// Integrate forces
+			v += h * b->m_invMass * force;
+			// Clear forces
+			b->m_force.SetZero();
+			
+			// Integrate torques
+			// @todo add gyroscopic term
+			w += h * b3Mul(b->m_worldInvI, b->m_torque);
+			// Clear torques
+			b->m_torque.SetZero();
+			
+			// Apply some damping
+			// Box2D.
+			// ODE: dv/dt + c * v = 0
+			// Solution: v(t) = v0 * exp(-c * t)
+			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+			// v2 = exp(-c * dt) * v1
+			// Pade approximation:
+			// v2 = v1 * 1 / (1 + c * dt)
+			v *= 1.0f / (1.0f + h * 0.1f);
+			w *= 1.0f / (1.0f + h * 0.1f);
+		}
+
+		m_velocities[i].v = v;
+		m_velocities[i].w = w;
+		m_positions[i].x = x;
+		m_positions[i].q = q;
+	}
+
+	b3JointSolverDef jointSolverDef;
+	jointSolverDef.joints = m_joints;
+	jointSolverDef.count = m_jointCount;
+	jointSolverDef.positions = m_positions;
+	jointSolverDef.velocities = m_velocities;
+	jointSolverDef.dt = h;
+	b3JointSolver jointSolver(&jointSolverDef);
+
+	b3ContactSolverDef contactSolverDef;
+	contactSolverDef.allocator = m_allocator;
+	contactSolverDef.contacts = m_contacts;
+	contactSolverDef.count = m_contactCount;
+	contactSolverDef.positions = m_positions;
+	contactSolverDef.velocities = m_velocities;
+	contactSolverDef.dt = h;
+	b3ContactSolver contactSolver(&contactSolverDef);
+
+	// 2. Initialize constraints
+	{
+		b3Time time;
+
+		contactSolver.InitializeConstraints();
+
+		if (flags & e_warmStartBit)
+		{
+			contactSolver.WarmStart();
+		}
+
+		jointSolver.InitializeConstraints();
+
+		if (flags & e_warmStartBit)
+		{
+			jointSolver.WarmStart();
+		}
+
+		time.Update();
+		profile->solver.initializeContacts = time.GetElapsedMilis();
+	}
+
+	// 3. Solve velocity constraints
+	{
+		b3Time time;
+
+		for (u32 i = 0; i < velocityIterations; ++i)
+		{
+			jointSolver.SolveVelocityConstraints();
+			contactSolver.SolveVelocityConstraints();
+		}
+
+		if (flags & e_warmStartBit)
+		{
+			contactSolver.StoreImpulses();
+		}
+
+		time.Update();
+		profile->solver.solveVelocity = time.GetElapsedMilis();
+	}
+
+	// 4. Integrate positions
+	for (u32 i = 0; i < m_bodyCount; ++i) 
+	{
+		b3Vec3 x = m_positions[i].x;
+		b3Quat q = m_positions[i].q;
+		b3Vec3 v = m_velocities[i].v;
+		b3Vec3 w = m_velocities[i].w;
+
+		// Prevent numerical instability due to large velocity changes.		
+		b3Vec3 translation = h * v;
+		if (b3Dot(translation, translation) > B3_MAX_TRANSLATION_SQUARED)
+		{
+			float32 ratio = B3_MAX_TRANSLATION / b3Length(translation);
+			v *= ratio;
+		}
+
+		b3Vec3 rotation = h * w;
+		if (b3Dot(rotation, rotation) > B3_MAX_ROTATION_SQUARED)
+		{
+			float32 ratio = B3_MAX_ROTATION / b3Length(rotation);
+			w *= ratio;
+		}
+
+		// Integrate position 
+		x += h * v;
+		// Integrate orientation
+		q = b3Integrate(q, w, h);
+
+		m_positions[i].x = x;
+		m_positions[i].q = q;
+		m_velocities[i].v = v;
+		m_velocities[i].w = w;
+	}
+
+	// 5. Solve position constraints
+	{
+		b3Time time;
+		
+		bool positionsSolved = false;
+		for (u32 i = 0; i < positionIterations; ++i) 
+		{
+			bool contactsSolved = contactSolver.SolvePositionConstraints();
+			bool jointsSolved = jointSolver.SolvePositionConstraints();
+			if (contactsSolved && jointsSolved)
+			{
+				// Early out if the position errors are small.
+				positionsSolved = true;
+				break;
+			}
+		}
+		
+		time.Update();
+		profile->solver.solvePosition = time.GetElapsedMilis();
+	}
+
+	// 6. Copy state buffers back to the bodies
+	for (u32 i = 0; i < m_bodyCount; ++i) 
+	{
+		b3Body* b = m_bodies[i];
+		b->m_sweep.worldCenter = m_positions[i].x;
+		b->m_sweep.orientation = m_positions[i].q;
+		b->m_sweep.orientation.Normalize();
+		b->m_linearVelocity = m_velocities[i].v;
+		b->m_angularVelocity = m_velocities[i].w;	
+		b->SynchronizeTransform();
+		// Transform body inertia to world inertia
+		b->m_worldInvI = b3RotateToFrame(b->m_invI, b->m_xf.rotation);
+	}
+
+	// 7. Put bodies under unconsiderable motion to sleep
+	if (flags & e_sleepBit) 
+	{
+		float32 minSleepTime = B3_MAX_FLOAT;
+		for (u32 i = 0; i < m_bodyCount; ++i) 
+		{
+			b3Body* b = m_bodies[i];
+			if (b->m_type == e_staticBody) 
+			{
+				continue;
+			}
+
+			// Compute the linear and angular speed of the body.
+			float32 sqrLinVel = b3Dot(b->m_linearVelocity, b->m_linearVelocity);
+			float32 sqrAngVel = b3Dot(b->m_angularVelocity, b->m_angularVelocity);
+
+			if (sqrLinVel > B3_SLEEP_LINEAR_TOL || sqrAngVel > B3_SLEEP_ANGULAR_TOL) 
+			{
+				minSleepTime = 0.0f;
+				b->m_sleepTime = 0.0f;
+			}
+			else 
+			{
+				b->m_sleepTime += h;
+				minSleepTime = b3Min(minSleepTime, b->m_sleepTime);
+			}
+		}
+
+		// Put the island to sleep so long as the minimum found sleep time
+		// is below the threshold. 
+		if (minSleepTime >= B3_TIME_TO_SLEEP) 
+		{
+			for (u32 i = 0; i < m_bodyCount; ++i) 
+			{
+				m_bodies[i]->SetAwake(false);
+			}
+		}
+	}
+}

src/bounce/dynamics/joint_manager.cpp

@@ -0,0 +1,81 @@
+/*
+* 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\joint_manager.h>
+#include <bounce\dynamics\joints\joint.h>
+#include <bounce\dynamics\body.h>
+
+b3JointManager::b3JointManager() 
+{
+}
+
+b3Joint* b3JointManager::Create(const b3JointDef* def) 
+{
+	b3Body* bodyA = def->bodyA;
+	b3Body* bodyB = def->bodyB;
+
+	// There must not be one joint linking the same bodies.
+	B3_ASSERT(bodyA != bodyB);
+	if (bodyA == bodyB)
+	{
+		return nullptr;
+	}
+
+	// Allocate the new joint.
+	b3Joint* j = b3Joint::Create(def);
+	j->m_flags = 0;
+	j->m_collideLinked = def->collideLinked;
+	j->m_userData = def->userData;
+
+	// Add the joint to body A's joint edge list
+	j->m_pair.bodyA = bodyA;
+	j->m_pair.edgeA.other = bodyB;
+	j->m_pair.edgeA.joint = j;
+	bodyA->m_jointEdges.PushFront(&j->m_pair.edgeA);
+
+	// Add the joint to body B's joint edge list
+	j->m_pair.bodyB = bodyB;
+	j->m_pair.edgeB.other = bodyA;
+	j->m_pair.edgeB.joint = j;
+	bodyB->m_jointEdges.PushFront(&j->m_pair.edgeB);
+
+	// Add the joint to the world joint list
+	m_jointList.PushFront(j);
+
+	// Creating a joint doesn't awake the bodies.
+
+	return j;
+}
+
+void b3JointManager::Destroy(b3Joint* j) 
+{
+	b3Body* bodyA = j->GetBodyA();
+	b3Body* bodyB = j->GetBodyB();
+
+	// Remove the joint from body A's joint list.
+	bodyA->m_jointEdges.Remove(&j->m_pair.edgeA);
+
+	// Remove the joint from body B's joint list.
+	bodyB->m_jointEdges.Remove(&j->m_pair.edgeB);
+
+	// Remove the joint from the world joint list.
+	m_jointList.Remove(j);
+	
+	// Destroy the joint.
+	b3Joint::Destroy(j);
+}

src/bounce/dynamics/joints/cone_joint.cpp

@@ -0,0 +1,363 @@
+/*
+* 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\joints\cone_joint.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\draw.h>
+
+// C = dot(u2, u1) - cos(0.5 * angle) > 0
+// Cdot = dot(u2, omega1 x u1) + dot(u1, omega2 x u2)
+// Cycle:
+// dot(u1 x u2, omega1) + dot(u2 x u1, omega2) = 
+// dot(-u2 x u1, omega1) + dot(u2 x u1, omega2)
+// n = u2 x u1
+// J = [0 -n 0 n]
+
+void b3ConeJointDef::Initialize(b3Body* bA, b3Body* bB,
+	const b3Vec3& axis, const b3Vec3& anchor, float32 angle)
+{
+	bodyA = bA;
+	bodyB = bB;
+	
+	b3Transform xf;
+	xf.rotation.y = axis;
+	xf.rotation.z = b3Perp(axis);
+	xf.rotation.x = b3Cross(xf.rotation.z, xf.rotation.y);
+	xf.position = anchor;
+
+	localFrameA = bodyA->GetLocalFrame(xf);
+	localFrameB = bodyB->GetLocalFrame(xf);
+	coneAngle = angle;
+}
+
+b3ConeJoint::b3ConeJoint(const b3ConeJointDef* def)
+{
+	m_type = e_coneJoint;
+	m_localFrameA = def->localFrameA;
+	m_localFrameB = def->localFrameB;
+	m_enableLimit = def->enableLimit;
+	m_coneAngle = def->coneAngle;
+	m_limitState = e_inactiveLimit;
+	m_limitImpulse = 0.0f;
+	m_limitAxis.SetZero();
+	m_impulse.SetZero();
+}
+
+void b3ConeJoint::InitializeConstraints(const b3SolverData* data)
+{
+	b3Body* m_bodyA = GetBodyA();
+	b3Body* m_bodyB = GetBodyB();
+
+	m_indexA = m_bodyA->m_islandID;
+	m_indexB = m_bodyB->m_islandID;
+
+	m_mA = m_bodyA->m_invMass;
+	m_mB = m_bodyB->m_invMass;
+
+	m_iA = m_bodyA->m_worldInvI;
+	m_iB = m_bodyB->m_worldInvI;
+
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	b3Transform xfA = m_bodyA->GetWorldFrame(m_localFrameA);
+	b3Transform xfB = m_bodyB->GetWorldFrame(m_localFrameB);
+
+	// Add point-to-point constraint.
+	{
+		m_rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
+		m_rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
+
+		// Compute effective mass matrix.
+		b3Mat33 M = b3Diagonal(m_mA + m_mB);
+		b3Mat33 RA = b3Skew(m_rA);
+		b3Mat33 RAT = b3Transpose(RA);
+		b3Mat33 RB = b3Skew(m_rB);
+		b3Mat33 RBT = b3Transpose(RB);
+		m_mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
+	}
+
+	// Add limit constraint.
+	if (m_enableLimit)
+	{
+		b3Vec3 u1 = xfA.rotation.y;
+		b3Vec3 u2 = xfB.rotation.y;
+
+		m_limitAxis = b3Cross(u2, u1);
+		
+		float32 mass = b3Dot((m_iA + m_iB) * m_limitAxis, m_limitAxis);
+		m_limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
+		
+		// C = cone - angle >= 0
+		float32 cosine = b3Dot(u2, u1);
+		float32 sine = b3Length(m_limitAxis);
+		float32 angle = atan2(sine, cosine);
+		if (0.5f * m_coneAngle < angle)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_limitState = e_atLowerLimit;
+				m_limitImpulse = 0.0f;
+			}
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_limitImpulse = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+	}
+}
+
+void b3ConeJoint::WarmStart(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+	
+	{
+		vA -= m_mA * m_impulse;
+		wA -= m_iA * b3Cross(m_rA, m_impulse);
+
+		vB += m_mB * m_impulse;
+		wB += m_iB * b3Cross(m_rB, m_impulse);
+	}
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b3Vec3 P = m_limitImpulse * m_limitAxis;
+		wA -= m_iA * P;
+		wB += m_iB * P;
+	}
+
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+void b3ConeJoint::SolveVelocityConstraints(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	// Solve point-to-point constraint.
+	{
+		b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
+		b3Vec3 P = m_mass.Solve(-Cdot);
+		
+		m_impulse += P;
+
+		vA -= m_mA * P;
+		wA -= m_iA * b3Cross(m_rA, P);
+
+		vB += m_mB * P;
+		wB += m_iB * b3Cross(m_rB, P);
+	}
+
+	// Solve limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 impulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			float32 Cdot = b3Dot(m_limitAxis, wB - wA);
+			impulse = -m_limitMass * Cdot;
+			m_limitImpulse += impulse;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 Cdot = b3Dot(m_limitAxis, wB - wA);
+			impulse = -m_limitMass * Cdot;
+			float32 oldImpulse = m_limitImpulse;
+			m_limitImpulse = b3Max(m_limitImpulse + impulse, 0.0f);
+			impulse = m_limitImpulse - oldImpulse;
+		}
+		
+		b3Vec3 P = impulse * m_limitAxis;
+
+		wA -= m_iA * P;
+		wB += m_iB * P;
+	}
+
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+bool b3ConeJoint::SolvePositionConstraints(const b3SolverData* data)
+{
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	float32 mA = m_mA;
+	b3Mat33 iA = m_iA;
+	float32 mB = m_mB;
+	b3Mat33 iB = m_iB;
+
+	// Solve point-to-point constraint.
+	float32 linearError = 0.0f;
+	{
+		b3Vec3 rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
+		b3Vec3 rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
+
+		b3Vec3 C = xB + rB - xA - rA;
+
+		linearError = b3Length(C);
+
+		b3Mat33 M = b3Diagonal(mA + mB);
+		b3Mat33 RA = b3Skew(rA);
+		b3Mat33 RAT = b3Transpose(RA);
+		b3Mat33 RB = b3Skew(rB);
+		b3Mat33 RBT = b3Transpose(RB);
+		b3Mat33 mass = M + RA * iA * RAT + RB * iB * RBT;
+		
+		b3Vec3 P = mass.Solve(-C);
+
+		xA -= mA * P;
+		qA -= b3Derivative(qA, iA * b3Cross(rA, P));
+		qA.Normalize();
+
+		xB += mB * P;
+		qB += b3Derivative(qB, iB * b3Cross(rB, P));
+		qB.Normalize();
+	}
+
+	// Solve limit constraint.
+	float32 limitError = 0.0f;
+	if (m_enableLimit)
+	{
+		// Compute Jacobian
+		b3Vec3 u1 = b3Mul(qA, m_localFrameA.rotation.y);
+		b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.y);
+		b3Vec3 limitAxis = b3Cross(u2, u1);
+		
+		// Compute effective mass.
+		float32 mass = b3Dot((iA + iB) * limitAxis, limitAxis);
+		float32 limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
+
+		// Compute joint angle.
+		float32 cosine = b3Dot(u2, u1);
+		float32 sine = b3Length(limitAxis);
+		float32 angle = atan2(sine, cosine);
+		
+		float32 limitImpulse = 0.0f;
+
+		if (0.5f * m_coneAngle < angle)
+		{
+			float32 C = 0.5f * m_coneAngle - angle;
+			limitError = -C;
+			
+			// Allow some slop and prevent large corrections
+			C = b3Clamp(C + B3_ANGULAR_SLOP, -B3_MAX_ANGULAR_CORRECTION, 0.0f);
+			limitImpulse = -C * limitMass;
+		}
+		
+		b3Vec3 P = limitImpulse * limitAxis;
+
+		qA -= b3Derivative(qA, iA * P);
+		qA.Normalize();
+
+		qB += b3Derivative(qB, iB * P);
+		qB.Normalize();
+	}
+
+	data->positions[m_indexA].x = xA;
+	data->positions[m_indexA].q = qA;
+	data->positions[m_indexB].x = xB;
+	data->positions[m_indexB].q = qB;
+
+	return linearError <= B3_LINEAR_SLOP && limitError <= B3_ANGULAR_SLOP;
+}
+
+const b3Transform& b3ConeJoint::GetFrameA() const
+{
+	return m_localFrameA;
+}
+
+void b3ConeJoint::SetFrameA(const b3Transform& frame)
+{
+	m_localFrameA = frame;
+}
+
+const b3Transform& b3ConeJoint::GetFrameB() const
+{
+	return m_localFrameB;
+}
+
+void b3ConeJoint::SetFrameB(const b3Transform& frame)
+{
+	m_localFrameB = frame;
+}
+
+bool b3ConeJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b3ConeJoint::SetEnableLimit(bool bit)
+{
+	if (bit != m_enableLimit)
+	{
+		GetBodyA()->SetAwake(true);
+		GetBodyB()->SetAwake(true);
+		m_limitImpulse = 0.0f;
+		m_limitState = e_inactiveLimit;
+		m_enableLimit = bit;
+	}
+}
+
+float32 b3ConeJoint::GetLowerLimit() const
+{
+	return m_coneAngle;
+}
+
+void b3ConeJoint::SetLimit(float32 angle)
+{
+	if (angle != m_coneAngle || angle != m_coneAngle)
+	{
+		GetBodyA()->SetAwake(true);
+		GetBodyB()->SetAwake(true);
+		m_limitImpulse = 0.0f;
+		m_coneAngle = angle;
+	}
+}
+
+void b3ConeJoint::Draw(b3Draw* b3Draw) const
+{
+	b3Transform xfA = GetBodyA()->GetWorldFrame(m_localFrameA);
+	b3Draw->DrawTransform(xfA);
+	b3Transform xfB = GetBodyB()->GetWorldFrame(m_localFrameB);
+	b3Draw->DrawTransform(xfB);
+}

src/bounce/dynamics/joints/joint.cpp

@@ -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 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\joints\joint.h>
+#include <bounce\dynamics\joints\mouse_joint.h>
+#include <bounce\dynamics\joints\spring_joint.h>
+#include <bounce\dynamics\joints\revolute_joint.h>
+#include <bounce\dynamics\joints\sphere_joint.h>
+#include <bounce\dynamics\joints\cone_joint.h>
+
+b3Joint* b3Joint::Create(const b3JointDef* def)
+{
+	b3Joint* joint = nullptr;
+	switch (def->type)
+	{
+	case e_mouseJoint:
+	{
+		void* block = b3Alloc(sizeof(b3MouseJoint));
+		joint = new (block) b3MouseJoint((b3MouseJointDef*)def);
+		break;
+	}
+	case e_springJoint:
+	{
+		void* block = b3Alloc(sizeof(b3SpringJoint));
+		joint = new (block) b3SpringJoint((b3SpringJointDef*)def);
+		break;
+	}
+	case e_revoluteJoint:
+	{
+		void* block = b3Alloc(sizeof(b3RevoluteJoint));
+		joint = new (block) b3RevoluteJoint((b3RevoluteJointDef*)def);
+		break;
+	}
+	case e_sphereJoint:
+	{
+		void* block = b3Alloc(sizeof(b3SphereJoint));
+		joint = new (block) b3SphereJoint((b3SphereJointDef*)def);
+		break;
+	}
+	case e_coneJoint:
+	{
+		void* block = b3Alloc(sizeof(b3ConeJoint));
+		joint = new (block) b3ConeJoint((b3ConeJointDef*)def);
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+	return joint;
+}
+
+void b3Joint::Destroy(b3Joint* joint)
+{
+	B3_ASSERT(joint);
+
+	b3JointType type = joint->GetType();
+	switch (type)
+	{
+	case e_mouseJoint:
+	{
+		b3MouseJoint* o = (b3MouseJoint*)joint;
+		o->~b3MouseJoint();
+		b3Free(joint);
+		break;
+	}
+	case e_springJoint:
+	{
+		b3SpringJoint* o = (b3SpringJoint*)joint;
+		o->~b3SpringJoint();
+		b3Free(joint);
+		break;
+	}
+	case b3JointType::e_revoluteJoint:
+	{
+		b3RevoluteJoint* o = (b3RevoluteJoint*)joint;
+		o->~b3RevoluteJoint();
+		b3Free(joint);
+		break;
+	}
+	case b3JointType::e_sphereJoint:
+	{
+		b3SphereJoint* o = (b3SphereJoint*)joint;
+		o->~b3SphereJoint();
+		b3Free(joint);
+		break;
+	}
+	case b3JointType::e_coneJoint:
+	{
+		b3ConeJoint* o = (b3ConeJoint*)joint;
+		o->~b3ConeJoint();
+		b3Free(joint);
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+}

src/bounce/dynamics/joints/joint_solver.cpp

@@ -0,0 +1,69 @@
+/*
+* 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\joints\joint_solver.h>
+#include <bounce\dynamics\joints\joint.h>
+
+b3JointSolver::b3JointSolver(const b3JointSolverDef* def) 
+{
+	m_count = def->count;
+	m_joints = def->joints;
+	m_solverData.dt = def->dt;
+	m_solverData.invdt = def->dt > 0.0f ? 1.0f / def->dt : 0.0f;
+	m_solverData.positions = def->positions;
+	m_solverData.velocities = def->velocities;
+}
+
+void b3JointSolver::InitializeConstraints() 
+{
+	for (u32 i = 0; i < m_count; ++i) 
+	{
+		b3Joint* j = m_joints[i];
+		j->InitializeConstraints(&m_solverData);
+	}
+}
+
+void b3JointSolver::WarmStart() 
+{
+	for (u32 i = 0; i < m_count; ++i) 
+	{
+		b3Joint* j = m_joints[i];
+		j->WarmStart(&m_solverData);
+	}
+}
+
+void b3JointSolver::SolveVelocityConstraints() 
+{
+	for (u32 i = 0; i < m_count; ++i) 
+	{
+		b3Joint* j = m_joints[i];
+		j->SolveVelocityConstraints(&m_solverData);
+	}
+}
+
+bool b3JointSolver::SolvePositionConstraints() 
+{
+	bool jointsSolved = true;
+	for (u32 i = 0; i < m_count; ++i) 
+	{
+		b3Joint* j = m_joints[i];
+		bool jointSolved = j->SolvePositionConstraints(&m_solverData);
+		jointsSolved = jointsSolved && jointSolved;
+	}
+	return jointsSolved;
+}

src/bounce/dynamics/joints/mouse_joint.cpp

@@ -0,0 +1,114 @@
+/*
+* 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\joints\mouse_joint.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\draw.h>
+
+b3MouseJoint::b3MouseJoint(const b3MouseJointDef* def) 
+{
+	m_type = e_mouseJoint;
+	m_worldAnchorA = def->worldAnchorA;
+	m_localAnchorB = def->localAnchorB;	
+	m_maxForce = def->maxForce;
+	m_impulse.SetZero();
+}
+
+void b3MouseJoint::InitializeConstraints(const b3SolverData* data) 
+{
+	b3Body* m_bodyB = GetBodyB();
+
+	m_indexB = m_bodyB->m_islandID;
+	m_mB = m_bodyB->m_invMass;
+	m_iB = m_bodyB->m_worldInvI;
+
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	b3Vec3 worldAnchorB = b3Mul(qB, m_localAnchorB) + xB;
+	
+	m_C = worldAnchorB - m_worldAnchorA;	
+	m_rB = worldAnchorB - xB;
+
+	b3Mat33 M = b3Diagonal(m_mB);
+	b3Mat33 RB = b3Skew(m_rB);
+	b3Mat33 RBT = b3Transpose(RB);
+	m_mass = M + RB * m_iB * RBT;
+}
+
+void b3MouseJoint::WarmStart(const b3SolverData* data) 
+{
+	data->velocities[m_indexB].v += m_mB * m_impulse;
+	data->velocities[m_indexB].w += b3Mul(m_iB, b3Cross(m_rB, m_impulse));
+}
+
+void b3MouseJoint::SolveVelocityConstraints(const b3SolverData* data) 
+{
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	b3Vec3 Cdot = vB + b3Cross(wB, m_rB);
+
+	b3Vec3 impulse = m_mass.Solve(-(Cdot + data->invdt * B3_BAUMGARTE * m_C));
+	b3Vec3 oldImpulse = m_impulse;
+	m_impulse += impulse;
+
+	// Prevent large reaction impulses.
+	float32 maxImpulse = data->dt * m_maxForce;
+	float32 sqrImpulse = b3Dot(m_impulse, m_impulse);
+	if (sqrImpulse > maxImpulse * maxImpulse)
+	{
+		float32 ratio = maxImpulse / b3Sqrt(sqrImpulse);
+		m_impulse *= ratio;
+	}
+	
+	impulse = m_impulse - oldImpulse;
+
+	vB += m_mB * impulse;
+	wB += b3Mul(m_iB, b3Cross(m_rB, impulse));
+	
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+bool b3MouseJoint::SolvePositionConstraints(const b3SolverData* data) 
+{
+	// There is no position correction for spring joints.
+	return true;
+}
+
+// Get the world space anchor point on the first body (usually the mouse world space position).
+b3Vec3 b3MouseJoint::GetWorldAnchorB() const 
+{
+	return b3Mul(GetBodyB()->GetTransform(), m_localAnchorB);
+}
+
+void b3MouseJoint::Draw(b3Draw* b3Draw) const 
+{
+	b3Color red = b3Color(1.0f, 0.0f, 0.0f, 1.0f);
+	b3Color green = b3Color(0.0f, 1.0f, 0.0f, 1.0f);
+	b3Color yellow = b3Color(1.0f, 1.0f, 0.0f, 1.0f);
+	
+	b3Transform xfB = GetBodyB()->m_xf;
+	b3Vec3 worldAnchorA = m_worldAnchorA;
+	b3Vec3 worldAnchorB = b3Mul(xfB, m_localAnchorB);
+
+	b3Draw->DrawPoint(worldAnchorA, green);
+	b3Draw->DrawPoint(worldAnchorB, red);
+	b3Draw->DrawSegment(worldAnchorA, worldAnchorB, yellow);
+}

src/bounce/dynamics/joints/revolute_joint.cpp

@@ -0,0 +1,675 @@
+/*
+* 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\joints\revolute_joint.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\draw.h>
+
+// C1 = p2 - p1
+// C2 = dot(u2, w1)
+// C3 = dot(v2, w1)
+// Cdot = dot(u2, omega1 x w1) + dot(w1, omega2 x u2)
+// Cyclic identity:
+// Cdot = dot(w1 x u2, omega1) + dot(u2 x w1, omega2) =
+// dot(-u2 x w1, omega1) + dot(u2 x w1, omega2) 
+
+// n1 = cross(u2, w1)^T
+// n2 = cross(v2, w1)^T
+
+// J = [-I  skew(r1) I -skew(r2)]
+//     [0   -n1     0  n1]
+//     [0   -n2     0  n2]
+
+// W = [i1  0      0]
+//     [0   m1 0   0]
+//     [0   0  i2  0]
+//     [0   0  0  m2]
+
+void b3RevoluteJointDef::Initialize(b3Body* bA, b3Body* bB,
+	const b3Vec3& axis, const b3Vec3& anchor,
+	float32 lower, float32 upper)
+{
+	b3Transform xf;
+	xf.rotation.z = axis;
+	xf.rotation.y = b3Perp(axis);
+	xf.rotation.x = b3Cross(xf.rotation.y, axis);
+	xf.position = anchor;
+
+	bodyA = bA;
+	bodyB = bB;
+	localFrameA = bodyA->GetLocalFrame(xf);
+	localFrameB = bodyB->GetLocalFrame(xf);
+	lowerAngle = lower;
+	upperAngle = upper;
+	B3_ASSERT(lowerAngle <= upperAngle);
+}
+
+b3RevoluteJoint::b3RevoluteJoint(const b3RevoluteJointDef* def)
+{
+	m_type = e_revoluteJoint;
+	m_localFrameA = def->localFrameA;
+	m_localFrameB = def->localFrameB;
+	m_enableLimit = def->enableLimit;
+	m_lowerAngle = def->lowerAngle;
+	m_upperAngle = def->upperAngle;
+	B3_ASSERT(m_lowerAngle <= m_upperAngle);
+	m_enableMotor = def->enableMotor;
+	m_motorSpeed = def->motorSpeed;
+	m_maxMotorTorque = def->maxMotorTorque;
+
+	m_impulse[0] = 0.0f;
+	m_impulse[1] = 0.0f;
+	m_impulse[2] = 0.0f;
+	m_impulse[3] = 0.0f;
+	m_impulse[4] = 0.0f;
+	m_nA.SetZero();
+	m_nB.SetZero();
+
+	m_motorImpulse = 0.0f;
+
+	m_limitState = e_inactiveLimit;
+	m_limitImpulse = 0.0f;
+	m_limitAxis.SetZero();
+}
+
+void b3RevoluteJoint::InitializeConstraints(const b3SolverData* data)
+{
+	b3Body* m_bodyA = GetBodyA();
+	b3Body* m_bodyB = GetBodyB();
+
+	m_indexA = m_bodyA->m_islandID;
+	m_indexB = m_bodyB->m_islandID;
+	m_mA = m_bodyA->m_invMass;
+	m_mB = m_bodyB->m_invMass;
+	m_iA = m_bodyA->m_worldInvI;
+	m_iB = m_bodyB->m_worldInvI;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	float32 mA = m_mA;
+	b3Mat33 iA = m_iA;
+	float32 mB = m_mB;
+	b3Mat33 iB = m_iB;
+
+	b3Transform xfA = m_bodyA->GetWorldFrame(m_localFrameA);
+	b3Vec3 u1 = xfA.rotation.x;
+	b3Vec3 v1 = xfA.rotation.y;
+	b3Vec3 w1 = xfA.rotation.z;
+
+	b3Transform xfB = m_bodyB->GetWorldFrame(m_localFrameB);
+	b3Vec3 u2 = xfB.rotation.x;
+	b3Vec3 v2 = xfB.rotation.y;
+	b3Vec3 w2 = xfB.rotation.z;
+
+	// Add motor constraint.
+	if (m_enableMotor || m_enableLimit)
+	{
+		m_limitAxis = w1;
+		float32 mass = b3Dot(m_limitAxis, (iA + iB) * m_limitAxis);
+		m_motorMass = mass > 0.0f ? 1.0f / mass : 0.0f;
+	}
+		
+	// Add limit constraint.
+	if (m_enableLimit)
+	{
+		float32 cosine = b3Dot(u2, u1);
+		float32 sine = b3Dot(u2, v1);
+		float32 angle = atan2(sine, cosine);
+
+		if (b3Abs(m_upperAngle - m_lowerAngle) < 2.0f * B3_ANGULAR_SLOP)
+		{
+			if (m_limitState != e_equalLimits)
+			{
+				m_limitState = e_equalLimits;
+				m_limitImpulse = 0.0f;
+			}
+		}
+		else if (angle <= m_lowerAngle)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_limitState = e_atLowerLimit;
+				m_limitImpulse = 0.0f;
+			}
+		}
+		else if (angle >= m_upperAngle)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_limitState = e_atUpperLimit;
+				m_limitImpulse = 0.0f;
+			}
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_limitImpulse = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+	}
+
+	// Add point-to-point and axes-to-axes constraints.
+	{
+		m_rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
+		m_rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
+
+		m_nA = b3Cross(u2, w1);
+		m_nB = b3Cross(v2, w1);
+
+		// Compute effective mass matrix.
+		b3Vec3 rA = m_rA, rB = m_rB;
+		b3Vec3 nA = m_nA, nB = m_nB;
+
+		b3Mat33 M = b3Diagonal(mA + mB);
+		b3Mat33 I = iA + iB;
+		b3Mat33 RA = b3Skew(rA);
+		b3Mat33 RAT = b3Transpose(RA);
+		b3Mat33 RB = b3Skew(rB);
+		b3Mat33 RBT = b3Transpose(RB);
+
+		// Identities and properties used:
+		// 1x3 * 3x3 = transpose(3x3)*1x3 
+		// 1x3 * 3x1 = dot(1x3, 3x1)
+		// K = transpose(K) (SPD)
+		b3Mat33 e11 = M + RA * iA * RAT + RB * iB * RBT;
+		b3Vec3 e21 = b3Cross(rA, iA * -nA) + b3Cross(-rB, iB * nA);
+		b3Vec3 e31 = b3Cross(rA, iA * -nB) + b3Cross(-rB, iB * nB);
+		
+		b3Vec3 e12 = e21;
+		float32 e22 = b3Dot(nA, I * nA);
+		float32 e32 = b3Dot(nA, I * nB);
+		
+		b3Vec3 e13 = e31;
+		float32 e23 = e32;
+		float32 e33 = b3Dot(nB, I * nB);
+
+		b3Mat<5, 5> K;
+
+		K(0, 0) = e11(0, 0);
+		K(1, 0) = e11(1, 0);
+		K(2, 0) = e11(2, 0);
+
+		K(0, 1) = e11(0, 1);
+		K(1, 1) = e11(1, 1);
+		K(2, 1) = e11(2, 1);
+
+		K(0, 2) = e11(0, 2);
+		K(1, 2) = e11(1, 2);
+		K(2, 2) = e11(2, 2);
+
+		K(3, 0) = e21.x;
+		K(3, 1) = e21.y;
+		K(3, 2) = e21.z;
+
+		K(4, 0) = e31.x;
+		K(4, 1) = e31.y;
+		K(4, 2) = e31.z;
+
+		K(0, 3) = e12.x;
+		K(1, 3) = e12.y;
+		K(2, 3) = e12.z;
+
+		K(0, 4) = e13.x;
+		K(1, 4) = e13.y;
+		K(2, 4) = e13.z;
+
+		K(3, 3) = e22;
+		K(3, 4) = e23;
+		K(4, 3) = e32;
+		K(4, 4) = e33;
+
+		m_mass = K;
+	}
+}
+
+void b3RevoluteJoint::WarmStart(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		b3Vec3 P = m_motorImpulse * m_limitAxis;
+		wA -= m_iA * P;
+		wB += m_iB * P;
+	}
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b3Vec3 P = m_limitImpulse * m_limitAxis;
+		wA -= m_iA * P;
+		wB += m_iB * P;
+	}
+
+	{
+		b3Vec3 P1(m_impulse[0], m_impulse[1], m_impulse[2]);
+		b3Vec3 P2 = m_impulse[3] * m_nA + m_impulse[4] * m_nB;
+
+		vA -= m_mA * P1;
+		wA -= m_iA * (b3Cross(m_rA, P1) + P2);
+
+		vB += m_mB * P1;
+		wB += m_iB * (b3Cross(m_rB, P1) + P2);
+	}
+
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+void b3RevoluteJoint::SolveVelocityConstraints(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	float32 mA = m_mA;
+	b3Mat33 iA = m_iA;
+	float32 mB = m_mB;
+	b3Mat33 iB = m_iB;
+
+	// Solve motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 Cdot = b3Dot(m_limitAxis, wB - wA) - m_motorSpeed;
+		float32 impulse = -m_motorMass * Cdot;
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = data->dt * m_maxMotorTorque;
+		m_motorImpulse = b3Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		b3Vec3 P = impulse * m_limitAxis;
+
+		wA -= iA * P;
+		wB += iB * P;
+	}
+
+	// Solve limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 Cdot = b3Dot(m_limitAxis, wB - wA);
+		float32 impulse = -m_motorMass * Cdot;
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_limitImpulse += impulse;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 oldImpulse = m_limitImpulse;
+			m_limitImpulse = b3Max(m_limitImpulse + impulse, 0.0f);
+			impulse = m_limitImpulse - oldImpulse;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 oldImpulse = m_limitImpulse;
+			m_limitImpulse = b3Min(m_limitImpulse + impulse, 0.0f);
+			impulse = m_limitImpulse - oldImpulse;
+		}
+
+		b3Vec3 P = impulse * m_limitAxis;
+
+		wA -= iA * P;
+		wB += iB * P;
+	}
+
+	// Solve point-to-point and axes-to-axes constraint.
+	{
+		b3Vec3 rA = m_rA;
+		b3Vec3 rB = m_rB;
+		b3Vec3 Cdot1 = vB + b3Cross(wB, rB) - vA - b3Cross(wA, rA);
+
+		b3Vec3 dw = wB - wA;
+		
+		b3Vec3 nA = m_nA;
+		float32 Cdot2 = b3Dot(nA, dw);
+
+		b3Vec3 nB = m_nB;
+		float32 Cdot3 = b3Dot(nB, dw);
+
+		b3Vec<5> Cdot;
+		Cdot[0] = Cdot1.x;
+		Cdot[1] = Cdot1.y;
+		Cdot[2] = Cdot1.z;
+		Cdot[3] = Cdot2;
+		Cdot[4] = Cdot3;
+
+		// Copy the effective mass so it can be destroyed in the 
+		// linear solver.
+		b3Mat<5, 5> mass = m_mass;
+		b3Vec<5> impulse = -Cdot;
+		if (b3Solve(impulse.e, mass.e, 5))
+		{
+			m_impulse += impulse;
+
+			b3Vec3 P1(impulse[0], impulse[1], impulse[2]);
+			b3Vec3 P2 = impulse[3] * nA + impulse[4] * nB;
+
+			vA -= mA * P1;
+			wA -= iA * (b3Cross(rA, P1) + P2);
+
+			vB += mB * P1;
+			wB += iB * (b3Cross(rB, P1) + P2);
+		}
+	}
+
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+bool b3RevoluteJoint::SolvePositionConstraints(const b3SolverData* data)
+{
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	float32 mA = m_mA;
+	b3Mat33 iA = m_iA;
+	float32 mB = m_mB;
+	b3Mat33 iB = m_iB;
+
+	// Solve limit constraint.
+	float32 limitError = 0.0f;
+	if (m_enableLimit)
+	{
+		b3Vec3 limitAxis = b3Mul(qA, m_localFrameA.rotation.z);
+		
+		// Compute fresh effective mass.
+		float32 mass = b3Dot((iA + iB) * limitAxis, limitAxis);
+		float32 limitMass = mass > 0.0f ? 1.0f / mass : 0.0f;
+
+		// Compute joint angle.
+		b3Vec3 u1 = b3Mul(qA, m_localFrameA.rotation.x);
+		b3Vec3 v1 = b3Mul(qA, m_localFrameA.rotation.y);
+		b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.x);
+		
+		float32 cosine = b3Dot(u2, u1);
+		float32 sine = b3Dot(u2, v1);
+		float32 angle = atan2(sine, cosine);
+
+		float32 limitImpulse = 0.0f;
+
+		if (b3Abs(m_upperAngle - m_lowerAngle) < 2.0f * B3_ANGULAR_SLOP)
+		{
+			float32 C = angle - m_lowerAngle;
+			limitError = b3Abs(C);
+			
+			// Prevent large corrections
+			C = b3Clamp(C, -B3_MAX_ANGULAR_CORRECTION, B3_MAX_ANGULAR_CORRECTION);
+			limitImpulse = -C * limitMass;
+		}
+		else if (angle <= m_lowerAngle)
+		{
+			float32 C = angle - m_lowerAngle;
+			limitError = -C;
+
+			// Allow some slop and prevent large corrections
+			C = b3Clamp(C + B3_ANGULAR_SLOP, -B3_MAX_ANGULAR_CORRECTION, 0.0f);
+			limitImpulse = -C * limitMass;
+		}
+		else if (angle >= m_upperAngle)
+		{
+			float32 C = angle - m_upperAngle;
+			limitError = C;
+
+			// Allow some slop and prevent large corrections
+			C = b3Clamp(C - B3_ANGULAR_SLOP, 0.0f, B3_MAX_ANGULAR_CORRECTION);
+			limitImpulse = -C * limitMass;
+		}
+
+		b3Vec3 P = limitImpulse * limitAxis;
+
+		qA -= b3Derivative(qA, iA * P);
+		qA.Normalize();
+
+		qB += b3Derivative(qB, iB * P);
+		qB.Normalize();
+	}
+
+	// Solve point-to-point and axes-to-axes constraints.
+	float32 linearError = 0.0f;
+	float32 angularError1 = 0.0f;
+	float32 angularError2 = 0.0f;
+	{
+		b3Vec3 rA = b3Mul(qA, m_localFrameA.position - m_localCenterA);
+		b3Vec3 rB = b3Mul(qB, m_localFrameB.position - m_localCenterB);
+		b3Vec3 C1 = xB + rB - xA - rA;
+		linearError = b3Length(C1);
+
+		b3Vec3 w1 = b3Mul(qA, m_localFrameA.rotation.z);
+
+		b3Vec3 u2 = b3Mul(qB, m_localFrameB.rotation.x);
+		float32 C2 = b3Dot(u2, w1);
+		angularError1 = b3Abs(C2);
+
+		b3Vec3 v2 = b3Mul(qB, m_localFrameB.rotation.y);
+		float32 C3 = b3Dot(v2, w1);
+		angularError2 = b3Abs(C3);
+
+		b3Vec<5> C;
+		C[0] = C1.x;
+		C[1] = C1.y;
+		C[2] = C1.z;
+		C[3] = C2;
+		C[4] = C3;
+
+		// Compute effective mass matrix.
+		b3Vec3 nA = b3Cross(u2, w1);
+		b3Vec3 nB = b3Cross(v2, w1);
+
+		b3Mat33 RA = b3Skew(rA);
+		b3Mat33 RAT = b3Transpose(RA);
+		b3Mat33 RB = b3Skew(rB);
+		b3Mat33 RBT = b3Transpose(RB);
+		b3Mat33 M = b3Diagonal(mA + mB);
+		b3Mat33 I = iA + iB;
+
+		b3Mat33 e11 = M + RA * iA * RAT + RB * iB * RBT;
+		b3Vec3 e21 = b3Cross(rA, iA * -nA) + b3Cross(-rB, iB * nA);
+		b3Vec3 e31 = b3Cross(rA, iA * -nB) + b3Cross(-rB, iB * nB);
+		
+		b3Vec3 e12 = e21;
+		float32 e22 = b3Dot(nA, I * nA);
+		float32 e32 = b3Dot(nA, I * nB);
+
+		b3Vec3 e13 = e31;
+		float32 e23 = e32;
+		float32 e33 = b3Dot(nB, I * nB);
+
+		b3Mat<5, 5> K;
+
+		K(0, 0) = e11(0, 0);
+		K(1, 0) = e11(1, 0);
+		K(2, 0) = e11(2, 0);
+
+		K(0, 1) = e11(0, 1);
+		K(1, 1) = e11(1, 1);
+		K(2, 1) = e11(2, 1);
+
+		K(0, 2) = e11(0, 2);
+		K(1, 2) = e11(1, 2);
+		K(2, 2) = e11(2, 2);
+
+		K(3, 0) = e21.x;
+		K(3, 1) = e21.y;
+		K(3, 2) = e21.z;
+
+		K(4, 0) = e31.x;
+		K(4, 1) = e31.y;
+		K(4, 2) = e31.z;
+
+		K(0, 3) = e12.x;
+		K(1, 3) = e12.y;
+		K(2, 3) = e12.z;
+
+		K(0, 4) = e13.x;
+		K(1, 4) = e13.y;
+		K(2, 4) = e13.z;
+
+		K(3, 3) = e22;
+		K(3, 4) = e23;
+		K(4, 3) = e32;
+		K(4, 4) = e33;
+
+		b3Vec<5> impulse = -C;
+		if (b3Solve(impulse.e, K.e, 5))
+		{
+			b3Vec3 P1(impulse[0], impulse[1], impulse[2]);
+			b3Vec3 P2 = impulse[3] * nA + impulse[4] * nB;
+
+			xA -= mA * P1;
+			qA -= b3Derivative(qA, iA * (b3Cross(rA, P1) + P2));
+			qA.Normalize();
+
+			xB += mB * P1;
+			qB += b3Derivative(qB, iB * (b3Cross(rB, P1) + P2));
+			qB.Normalize();
+		}
+	}
+
+	data->positions[m_indexA].x = xA;
+	data->positions[m_indexA].q = qA;
+	data->positions[m_indexB].x = xB;
+	data->positions[m_indexB].q = qB;
+
+	return linearError <= B3_LINEAR_SLOP &&
+		angularError1 <= B3_ANGULAR_SLOP &&
+		angularError2 <= B3_ANGULAR_SLOP &&
+		limitError <= B3_ANGULAR_SLOP;
+}
+
+const b3Transform& b3RevoluteJoint::GetFrameA() const
+{
+	return m_localFrameA;
+}
+
+void b3RevoluteJoint::SetFrameA(const b3Transform& frame)
+{
+	m_localFrameA = frame;
+}
+
+const b3Transform& b3RevoluteJoint::GetFrameB() const
+{
+	return m_localFrameB;
+}
+
+void b3RevoluteJoint::SetFrameB(const b3Transform& frame)
+{
+	m_localFrameB = frame;
+}
+
+bool b3RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b3RevoluteJoint::SetEnableLimit(bool bit)
+{
+	if (bit != m_enableLimit)
+	{
+		GetBodyA()->SetAwake(true);
+		GetBodyB()->SetAwake(true);
+		m_limitImpulse = 0.0f;
+		m_limitState = e_inactiveLimit;
+		m_enableLimit = bit;
+	}
+}
+
+float32 b3RevoluteJoint::GetLowerLimit() const
+{
+	return m_lowerAngle;
+}
+
+float32 b3RevoluteJoint::GetUpperLimit() const
+{
+	return m_upperAngle;
+}
+
+void b3RevoluteJoint::SetLimits(float32 lower, float32 upper)
+{
+	B3_ASSERT(lower <= upper);
+	if (lower != m_lowerAngle || upper != m_upperAngle)
+	{
+		GetBodyA()->SetAwake(true);
+		GetBodyB()->SetAwake(true);
+		m_limitImpulse = 0.0f;
+		m_lowerAngle = lower;
+		m_upperAngle = upper;
+	}
+}
+
+bool b3RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b3RevoluteJoint::SetEnableMotor(bool bit)
+{
+	if (bit != m_enableMotor)
+	{
+		GetBodyA()->SetAwake(true);
+		GetBodyB()->SetAwake(true);
+		m_motorImpulse = 0.0f;
+		m_enableMotor = bit;
+	}
+}
+
+float32 b3RevoluteJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+void b3RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+	GetBodyA()->SetAwake(true);
+	GetBodyB()->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+float32 b3RevoluteJoint::GetMaxMotorTorque() const
+{
+	return m_maxMotorTorque;
+}
+
+void b3RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+	GetBodyA()->SetAwake(true);
+	GetBodyB()->SetAwake(true);
+	m_maxMotorTorque = torque;
+}
+
+void b3RevoluteJoint::Draw(b3Draw* b3Draw) const
+{
+	b3Transform xfA = GetBodyA()->GetWorldFrame(m_localFrameA);
+	b3Draw->DrawTransform(xfA);
+	b3Transform xfB = GetBodyB()->GetWorldFrame(m_localFrameB);
+	b3Draw->DrawTransform(xfB);
+}

src/bounce/dynamics/joints/sphere_joint.cpp

@@ -0,0 +1,158 @@
+/*
+* 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\joints\sphere_joint.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\draw.h>
+
+void b3SphereJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+}
+
+b3SphereJoint::b3SphereJoint(const b3SphereJointDef* def)
+{
+	m_type = b3JointType::e_sphereJoint;
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_impulse.SetZero();
+}
+
+void b3SphereJoint::InitializeConstraints(const b3SolverData* data)
+{
+	b3Body* m_bodyA = GetBodyA();
+	b3Body* m_bodyB = GetBodyB();
+
+	m_indexA = m_bodyA->m_islandID;
+	m_indexB = m_bodyB->m_islandID;
+	m_mA = m_bodyA->m_invMass;
+	m_mB = m_bodyB->m_invMass;
+	m_iA = m_bodyA->m_worldInvI;
+	m_iB = m_bodyB->m_worldInvI;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	// Compute effective mass for the block solver
+	m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
+
+	b3Mat33 RA = b3Skew(m_rA);
+	b3Mat33 RAT = b3Transpose(RA);
+	b3Mat33 RB = b3Skew(m_rB);
+	b3Mat33 RBT = b3Transpose(RB);
+	b3Mat33 M = b3Diagonal(m_mA + m_mB);
+
+	m_mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
+}
+
+void b3SphereJoint::WarmStart(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	vA -= m_mA * m_impulse;
+	wA -= m_iA * b3Cross(m_rA, m_impulse);
+
+	vB += m_mB * m_impulse;
+	wB += m_iB * b3Cross(m_rB, m_impulse);
+	
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+void b3SphereJoint::SolveVelocityConstraints(const b3SolverData* data)
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	b3Vec3 Cdot = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
+	b3Vec3 impulse = m_mass.Solve(-Cdot);
+	
+	m_impulse += impulse;
+
+	vA -= m_mA * impulse;
+	wA -= m_iA * b3Cross(m_rA, impulse);
+
+	vB += m_mB * impulse;
+	wB += m_iB * b3Cross(m_rB, impulse);
+	
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+bool b3SphereJoint::SolvePositionConstraints(const b3SolverData* data)
+{
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	// Compute effective mass
+	b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
+	b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
+
+	b3Mat33 M = b3Diagonal(m_mA + m_mB);
+	b3Mat33 RA = b3Skew(rA);
+	b3Mat33 RAT = b3Transpose(RA);
+	b3Mat33 RB = b3Skew(rB);
+	b3Mat33 RBT = b3Transpose(RB);
+
+	b3Mat33 mass = M + RA * m_iA * RAT + RB * m_iB * RBT;
+	
+	b3Vec3 C = xB + rB - xA - rA;
+	b3Vec3 impulse = mass.Solve(-C);
+
+	xA -= m_mA * impulse;
+	qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse)));
+	qA.Normalize();
+
+	xB += m_mB * impulse;
+	qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse)));
+	qB.Normalize();
+
+	data->positions[m_indexA].x = xA;
+	data->positions[m_indexA].q = qA;
+	data->positions[m_indexB].x = xB;
+	data->positions[m_indexB].q = qB;
+
+	return b3Length(C) <= B3_LINEAR_SLOP;
+}
+
+void b3SphereJoint::Draw(b3Draw* b3Draw) const
+{
+	b3Vec3 pA = GetBodyA()->GetWorldPoint(m_localAnchorA);
+	b3Draw->DrawPoint(pA, b3Color(1.0f, 0.0f, 0.0f, 1.0f));
+	b3Vec3 pB = GetBodyB()->GetWorldPoint(m_localAnchorB);
+	b3Draw->DrawPoint(pB, b3Color(0.0f, 1.0f, 0.0f, 1.0f));
+	
+	b3Draw->DrawSegment(pA, pB, b3Color(1.0f, 1.0f, 0.0f, 1.0f));
+}

src/bounce/dynamics/joints/spring_joint.cpp

@@ -0,0 +1,214 @@
+/*
+* 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\joints\spring_joint.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\common\draw.h>
+
+// C = ||x2 + r2 - x1 - r1|| - length
+// Cdot = dot(n, v2 + w2 x r2 - v1 - w1 x r1)
+// J = [-n^T -cross(n, r1)^T n^T cross(n, r2)^T]
+
+void b3SpringJointDef::Initialize(b3Body* bA, b3Body* bB, const b3Vec3& anchorA, const b3Vec3& anchorB)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchorA);
+	localAnchorB = bodyB->GetLocalPoint(anchorB);
+	length = b3Distance(anchorA, anchorB);
+}
+
+b3SpringJoint::b3SpringJoint(const b3SpringJointDef* def) 
+{
+	m_type = e_springJoint;
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_length = def->length;
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+	m_impulse = 0.0f;
+}
+
+void b3SpringJoint::InitializeConstraints(const b3SolverData* data) 
+{
+	b3Body* m_bodyA = GetBodyA();
+	b3Body* m_bodyB = GetBodyB();
+
+	m_indexA = m_bodyA->m_islandID;
+	m_indexB = m_bodyB->m_islandID;
+
+	m_mA = m_bodyA->m_invMass;
+	m_mB = m_bodyB->m_invMass;
+
+	m_iA = m_bodyA->m_worldInvI;
+	m_iB = m_bodyB->m_worldInvI;
+
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	// Handle singularity 
+	m_n = xB + m_rB - xA - m_rA;
+	float32 length = b3Length(m_n);
+	if (length > B3_LINEAR_SLOP)
+	{
+		m_n = 1.0f / length * m_n;
+	}
+	else
+	{
+		m_n.SetZero();
+	}
+
+	if (m_frequencyHz > 0.0f)
+	{
+		// Compute spring parameters
+		float32 C = length - m_length;
+		
+		// Angular frequency
+		float32 omega = 2.0f * B3_PI * m_frequencyHz;
+
+		// Damping coefficient
+		float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+
+		// Spring stiffness
+		float32 s = m_mass * omega * omega;
+
+		// Box2D's Soft Constraints talk
+		float32 h = data->dt;
+		m_gamma = h * (d + h * s);
+		m_bias = h * C * s * m_gamma;
+
+	}
+	else
+	{
+		m_bias = 0.0f;
+		m_gamma = 0.0f;
+	}
+
+	// Compute effective mass
+	m_rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
+
+	b3Vec3 rnA = b3Cross(m_rA, m_n);
+	b3Vec3 rnB = b3Cross(m_rB, m_n);
+
+	float32 mass = m_mA + m_mB + b3Dot(m_iA * rnA, rnA) + b3Dot(m_iB * rnB, rnB) + m_gamma;
+	m_mass = mass > 0.0f ? 1.0f / mass : 0.0f;
+}
+
+void b3SpringJoint::WarmStart(const b3SolverData* data) 
+{
+	b3Vec3 P = m_impulse * m_n;
+	
+	data->velocities[m_indexA].v -= m_mA * P;
+	data->velocities[m_indexA].w -= m_iA * b3Cross(m_rA, P);
+	data->velocities[m_indexB].v += m_mB * P;
+	data->velocities[m_indexB].w += m_iB * b3Cross(m_rB, P);
+}
+
+void b3SpringJoint::SolveVelocityConstraints(const b3SolverData* data) 
+{
+	b3Vec3 vA = data->velocities[m_indexA].v;
+	b3Vec3 wA = data->velocities[m_indexA].w;
+	b3Vec3 vB = data->velocities[m_indexB].v;
+	b3Vec3 wB = data->velocities[m_indexB].w;
+
+	b3Vec3 dv = vB + b3Cross(wB, m_rB) - vA - b3Cross(wA, m_rA);
+	float32 Cdot = b3Dot(m_n, dv);
+	
+	float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+	m_impulse += impulse;
+	
+	b3Vec3 P = impulse * m_n;
+
+	vA -= m_mA * P;
+	wA -= m_iA * b3Cross(m_rA, P);
+	
+	vB += m_mB * P;
+	wB += m_iB * b3Cross(m_rB, P);
+
+	data->velocities[m_indexA].v = vA;
+	data->velocities[m_indexA].w = wA;
+	data->velocities[m_indexB].v = vB;
+	data->velocities[m_indexB].w = wB;
+}
+
+bool b3SpringJoint::SolvePositionConstraints(const b3SolverData* data) 
+{
+	if (m_frequencyHz > 0.0f)
+	{
+		// There is no position correction for spring joints.
+		B3_NOT_USED(data);
+		return true;
+	}
+
+	b3Vec3 xA = data->positions[m_indexA].x;
+	b3Quat qA = data->positions[m_indexA].q;
+	b3Vec3 xB = data->positions[m_indexB].x;
+	b3Quat qB = data->positions[m_indexB].q;
+
+	b3Vec3 rA = b3Mul(qA, m_localAnchorA - m_localCenterA);
+	b3Vec3 rB = b3Mul(qB, m_localAnchorB - m_localCenterB);
+	
+	b3Vec3 n = xB + rB - xA - rA;
+	float32 length = b3Length(n);
+	float32 C = length - m_length;
+	C = b3Clamp(C, -B3_MAX_LINEAR_CORRECTION, B3_MAX_LINEAR_CORRECTION);
+	
+	// Compute effective mass
+	n.Normalize();
+	
+	b3Vec3 rnA = b3Cross(rA, n);
+	b3Vec3 rnB = b3Cross(rB, n);
+	float32 kMass = m_mA + m_mB + b3Dot(m_iA * rnA, rnA) + b3Dot(m_iB * rnB, rnB);
+	float32 mass = kMass > 0.0f ? 1.0f / kMass : 0.0f;
+	float32 lambda = -mass * C;
+
+	b3Vec3 impulse = lambda * n;
+		
+	xA -= m_mA * impulse;
+	qA -= b3Derivative(qA, b3Mul(m_iA, b3Cross(rA, impulse)));
+
+	xB += m_mB * impulse;
+	qB += b3Derivative(qB, b3Mul(m_iB, b3Cross(rB, impulse)));
+
+	data->positions[m_indexA].x = xA;
+	data->positions[m_indexA].q = qA;
+	data->positions[m_indexB].x = xB;
+	data->positions[m_indexB].q = qB;
+	
+	return b3Abs(C) < B3_LINEAR_SLOP;
+}
+
+void b3SpringJoint::Draw(b3Draw* b3Draw) const 
+{
+	b3Color red = b3Color(1.0f, 0.0f, 0.0f, 1.0f);
+	b3Color green = b3Color(0.0f, 1.0f, 0.0f, 1.0f);
+	b3Color blue = b3Color(0.0f, 0.0f, 1.0f, 1.0f);
+
+	b3Vec3 pA = GetBodyA()->GetWorldPoint(m_localAnchorA);
+	b3Vec3 pB = GetBodyB()->GetWorldPoint(m_localAnchorB);
+	
+	b3Draw->DrawPoint(pA, green);
+	b3Draw->DrawPoint(pB, green);
+	b3Draw->DrawSegment(pA, pB, blue);
+}

src/bounce/dynamics/shapes/capsule_shape.cpp

@@ -0,0 +1,302 @@
+/*
+* 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\shapes\capsule_shape.h>
+
+b3CapsuleShape::b3CapsuleShape() 
+{
+	m_type = e_capsuleShape;
+	m_radius = 0.0f;
+	m_centers[0].SetZero();
+	m_centers[1].SetZero();
+}
+
+b3CapsuleShape::~b3CapsuleShape() 
+{
+}
+
+void b3CapsuleShape::Swap(const b3CapsuleShape& other) 
+{
+	m_centers[0] = other.m_centers[0];
+	m_centers[1] = other.m_centers[1];
+	m_radius = other.m_radius;
+}
+
+void b3CapsuleShape::ComputeMass(b3MassData* massData, float32 density) const 
+{
+	b3Vec3 A = m_centers[0];
+	b3Vec3 B = m_centers[1];
+
+	b3Vec3 d = B - A;
+	float32 h = b3Length(d);
+	B3_ASSERT(h > B3_LINEAR_SLOP);
+	float32 h2 = h * h;
+
+	float32 r = m_radius;
+	float32 r2 = r * r;
+	float32 r3 = r2 * r;
+
+	// Cylinder inertia about the capsule center of mass
+	b3MassData Ic_Cylinder;
+	{
+		// Cylinder mass
+		float32 volume = B3_PI * r2 * h;
+		float32 mass = density * volume;
+
+		// Cylinder inertia about the center of mass (same as capsule center of mass)
+		float32 x = (1.0f / 12.0f) * mass * (3.0f * r2 + h2);
+		float32 y = 0.5f * mass * r2;
+		float32 z = x;
+
+		Ic_Cylinder.center = 0.5f * (A + B);
+		Ic_Cylinder.mass = mass;
+		Ic_Cylinder.I = b3Diagonal(x, y, z);
+	}
+
+	// Hemisphere inertia about the capsule center of mass
+	b3MassData Ic_Hemisphere;
+	{
+		// Hemisphere volume and mass
+		float32 volume = (2.0f / 3.0f) * B3_PI * r3;
+		float32 mass = density * volume;
+		
+		// Ic = Io + m * d^2
+		// Io = Ic - m * d^2
+
+		// Hemisphere inertia about the origin
+		float32 Io = (2.0f / 5.0f) * mass * r2;
+		
+		// Hemisphere center of mass relative to origin
+		float32 d1 = (3.0f / 8.0f) * r;
+		
+		// Hemisphere inertia about the hemisphere center of mass
+		float32 Ic = Io - (mass * d1 * d1);
+
+		// Hemisphere center of mass relative to the capsule center of mass
+		float32 d2 = d1 + 0.5f * h;
+		
+		// Hemisphere inertia about the capsule center of mass
+		float32 x = Ic + (mass * d2 * d2);
+		float32 y = Io;
+		float32 z = x;
+
+		Ic_Hemisphere.center.Set(0.0f, d2, 0.0f);
+		Ic_Hemisphere.mass = mass;
+		Ic_Hemisphere.I = b3Diagonal(x, y, z);
+	}
+
+	// Capsule inertia about the capsule center of mass
+	b3MassData Ic_Capsule;
+	{
+		// Capsule center of mass
+		Ic_Capsule.center = 0.5f * (A + B);
+		// Inertia about the capsule center of mass
+		// taking two hemispheres into account
+		Ic_Capsule.mass = Ic_Cylinder.mass + 2.0f * Ic_Hemisphere.mass;
+		Ic_Capsule.I = Ic_Cylinder.I + 2.0f * Ic_Hemisphere.I;
+	}
+	
+	// Capsule inertia about the reference frame of the cylinder
+	// Center of mass doesn't change
+	B3_ASSERT(h > B3_LINEAR_SLOP);
+	b3Mat33 R;
+	R.y = (1.0f / h) * d;
+	R.x = b3Perp(R.y);
+	R.z = b3Cross(R.y, R.x);
+
+	b3Mat33 Ic = b3RotateToFrame(Ic_Capsule.I, R);
+	
+	// Inertia about the center of mass
+	massData->center = Ic_Capsule.center;
+	massData->mass = Ic_Capsule.mass;
+	massData->I = Ic;
+}
+
+void b3CapsuleShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const 
+{
+	b3Vec3 c1 = b3Mul(xf, m_centers[0]);
+	b3Vec3 c2 = b3Mul(xf, m_centers[1]);
+	b3Vec3 r(m_radius, m_radius, m_radius);
+	aabb->m_lower = b3Min(c1, c2) - r;
+	aabb->m_upper = b3Max(c1, c2) + r;
+}
+
+bool b3CapsuleShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+{
+	// The point in the frame of the capsule
+	b3Vec3 Q = b3MulT(xf, point);
+
+	b3Vec3 A = m_centers[0];
+	b3Vec3 B = m_centers[1];
+	b3Vec3 AB = B - A;
+
+	// Barycentric coordinates for Q
+	float32 u = b3Dot(B - Q, AB);
+	float32 v = b3Dot(Q - A, AB);
+
+	if (v <= 0.0f)
+	{
+		// A
+		if (b3DistanceSquared(A, Q) > m_radius * m_radius)
+		{
+			return false;
+		}
+		return true;
+	}
+
+	if (u <= 0.0f)
+	{
+		// B
+		if (b3DistanceSquared(B, Q) > m_radius * m_radius)
+		{
+			return false;
+		}
+		return true;
+	}
+
+	// AB
+	float32 s = b3Dot(AB, AB);
+	B3_ASSERT(s > 0.0f);
+	b3Vec3 P = (1.0f / s) * (u * A + v * B);
+	if (b3DistanceSquared(P, Q) > m_radius * m_radius)
+	{
+		return false;
+	}
+
+	return true;
+}
+
+bool b3CapsuleShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const 
+{
+	b3Vec3 A = input.p1;
+	b3Vec3 B = input.p2;
+
+	b3Vec3 n = B - A;
+	float32 nn = b3Dot(n, n);
+	
+	// Check for short segment.
+	if (nn < B3_EPSILON * B3_EPSILON)
+	{
+		return false;
+	}
+
+	b3Vec3 P = b3Mul(xf, m_centers[0]);
+	b3Vec3 Q = b3Mul(xf, m_centers[1]);
+
+	b3Vec3 d = Q - P;
+	float32 dd = b3Dot(d, d);
+	
+	// Check for short segment.
+	if (dd < B3_EPSILON * B3_EPSILON)
+	{
+		return false;
+	}
+
+	// Solve quadratic equation.
+	b3Vec3 m = A - P;
+	float32 nd = b3Dot(n, d);
+	float32 mm = b3Dot(m, m);
+	float32 md = b3Dot(m, d);
+	float32 mn = b3Dot(m, n);
+	
+	float32 a = dd * nn - nd * nd;
+	if (b3Abs(a) < 2.0f * (B3_EPSILON * B3_EPSILON))
+	{
+		return false;
+	}
+
+	float32 b = dd * mn - nd * md;
+	float32 c = dd * (mm - m_radius * m_radius) - md * md;
+
+	float32 disc = b * b - a * c;
+
+	// Check for negative discriminant.
+	if (disc < 0.0f)
+	{
+		return false;
+	}
+
+	// Find minimum intersection of the line with the cylinder.
+	float32 t = -b - b3Sqrt(disc);
+	t /= a;
+
+	// Is the intersection on the segment?
+	if (t < 0.0f || t > 1.0f)
+	{
+		return false;
+	}
+	
+	// Hemisphere check.
+	float32 tp = md + t * nd;
+
+	b3Vec3 C;
+	if (tp < 0.0f)
+	{
+		// P
+		C = P;
+	}
+	else if (tp > dd)
+	{
+		// Q
+		C = Q;
+	}
+	else
+	{
+		// PQ
+		// Intersection is on the cylinder.
+		b3Vec3 X = (1.0f - t) * A + t * B;
+		b3Vec3 e2 = b3Cross(d, X - P);
+		b3Vec3 e3 = b3Cross(e2, d);
+		e3.Normalize();
+
+		output->fraction = t;
+		output->normal = e3;
+		return true;
+	}
+
+	// Intersection is on hemisphere at C. 
+	d = n;
+	a = nn;
+
+	m = A - C;
+	b = b3Dot(m, d);
+	c = b3Dot(m, m) - m_radius * m_radius;
+
+	disc = b * b - a * c;
+
+	// Check for negative discriminant.
+	if (disc < 0.0f)
+	{
+		return false;
+	}
+
+	// Find the minimum time of impact of the line with the hemisphere.
+	t = -b - b3Sqrt(disc);
+
+	// Is the intersection point on the segment?
+	if (t > 0.0f && t <= input.maxFraction * a)
+	{
+		// Finish solution.
+		t /= a;
+		output->fraction = t;
+		output->normal = b3Normalize(m + t * d);
+		return true;
+	}
+
+	return false;
+}

src/bounce/dynamics/shapes/hull_shape.cpp

@@ -0,0 +1,264 @@
+/*
+* 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\shapes\hull_shape.h>
+#include <bounce\collision\shapes\hull.h>
+#include <bounce\common\template\array.h>
+
+b3HullShape::b3HullShape()
+{
+	m_type = e_hullShape;
+	m_radius = B3_HULL_RADIUS;
+	m_hull = nullptr;
+}
+
+b3HullShape::~b3HullShape()
+{
+}
+
+void b3HullShape::Swap(const b3HullShape& other)
+{
+	m_radius = other.m_radius;
+	m_hull = other.m_hull;
+}
+
+void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
+{
+	// Build triangles for hull
+	b3StackArray<b3Triangle, 256> triangles;
+	
+	// Use a small buffer for polygons.
+	u32 polygon[B3_MAX_HULL_FEATURES];
+	u32 vCount = 0;
+
+	// Convert polygons to triangles
+	for (u32 i = 0; i < m_hull->faceCount; ++i)
+	{
+		// Build convex polygon for loop
+		const b3Face* face = m_hull->GetFace(i);
+		const b3HalfEdge* begin = m_hull->GetEdge(face->edge);
+		const b3HalfEdge* edge = begin;
+		do
+		{
+			polygon[vCount++] = u32(edge->origin);
+			edge = m_hull->GetEdge(edge->next);
+		} while (edge != begin);
+		
+		// Triangulate convex polygon
+		B3_ASSERT(vCount > 2);
+		for (u32 j = 1; j < vCount - 1; ++j)
+		{
+			b3Triangle triangle;
+			triangle.v1 = polygon[0];
+			triangle.v2 = polygon[j];
+			triangle.v3 = polygon[j + 1];
+			triangles.PushBack(triangle);
+		}
+
+		vCount = 0;
+	}
+	vCount = 0;
+
+	// Compute mass data
+	b3Vec3 center(0.0f, 0.0f, 0.0f);
+	float32 volume = 0.0f;
+	b3Mat33 I;
+	I.SetZero();
+
+	// Pick reference point not too away from the origin 
+	// to minimize floating point rounding errors.
+	b3Vec3 v1(0.0f, 0.0f, 0.0f);
+
+	const float32 inv4 = 0.25f;
+	const float32 inv6 = 1.0f / 6.0f;
+	const float32 inv60 = 1.0f / 60.0f;
+	const float32 inv120 = 1.0f / 120.0f;
+
+	b3Vec3 diag(0.0f, 0.0f, 0.0f);
+	b3Vec3 offDiag(0.0f, 0.0f, 0.0f);
+
+	for (u32 i = 0; i < triangles.Count(); ++i)
+	{
+		const b3Triangle* triangle = triangles.Get(i);
+
+		b3Vec3 v2 = m_hull->GetVertex(triangle->v1);
+		b3Vec3 v3 = m_hull->GetVertex(triangle->v2);
+		b3Vec3 v4 = m_hull->GetVertex(triangle->v3);
+		b3Vec3 tetraCenter = inv4 * (v1 + v2 + v3 + v4);
+
+		b3Vec3 e1 = v2 - v1;
+		b3Vec3 e2 = v3 - v1;
+		b3Vec3 e3 = v4 - v1;
+		float32 det = b3Det(e1, e2, e3);
+		float32 tetraVolume = inv6 * det;
+
+		// Volume weighted center of mass
+		center += tetraVolume * tetraCenter;
+		volume += tetraVolume;
+
+		// Volume weighted inertia tensor
+		// https://github.com/melax/sandbox
+		for (u32 j = 0; j < 3; ++j)
+		{
+			u32 j1 = (j + 1) % 3;
+			u32 j2 = (j + 2) % 3;
+
+			diag[j] += inv60 * det *
+				(e1[j] * e2[j] + e2[j] * e3[j] + e3[j] * e1[j] +
+				 e1[j] * e1[j] + e2[j] * e2[j] + e3[j] * e3[j]);
+
+			offDiag[j] += inv120 * det  *
+				(e1[j1] * e2[j2] + e2[j1] * e3[j2] + e3[j1] * e1[j2] +
+				 e1[j1] * e3[j2] + e2[j1] * e1[j2] + e3[j1] * e2[j2] +
+				 e1[j1] * e1[j2] * 2.0f + e2[j1] * e2[j2] * 2.0f + e3[j1] * e3[j2] * 2.0f);
+		}
+	}
+
+	B3_ASSERT(volume > 0.0f);
+	float32 invVolume = 0.0f;
+	if (volume != 0.0f)
+	{
+		invVolume = 1.0f / volume;
+	}
+	else
+	{
+		invVolume = 0.0f;
+	}
+
+	diag = invVolume * diag;
+	offDiag = invVolume * offDiag;
+
+	I.x.Set(diag.y + diag.z, -offDiag.z, -offDiag.y);
+	I.y.Set(-offDiag.z, diag.x + diag.z, -offDiag.x);
+	I.z.Set(-offDiag.y, -offDiag.x, diag.x + diag.y);
+
+	massData->center = invVolume * center;
+	massData->mass = density * volume;
+	massData->I = massData->mass * I;
+}
+
+void b3HullShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const
+{
+	aabb->Compute(m_hull->vertices, m_hull->vertexCount, xf);
+	aabb->Extend(m_radius);
+}
+
+bool b3HullShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+{
+	// Put the point into the hull's frame of reference.
+	b3Vec3 p = b3MulT(xf, point);
+	for (u32 i = 0; i < m_hull->faceCount; ++i)
+	{
+		float32 d = b3Distance(p, m_hull->planes[i]);
+		if (d > m_radius)
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool b3HullShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const
+{
+	u32 planeCount = m_hull->faceCount;
+	const b3Plane* planes = m_hull->planes;
+
+	// Put the segment into the poly's frame of reference.
+	b3Vec3 p1 = b3MulT(xf.rotation, input.p1 - xf.position);
+	b3Vec3 p2 = b3MulT(xf.rotation, input.p2 - xf.position);
+	b3Vec3 d = p2 - p1;
+
+	float32 lower = 0.0f;
+	float32 upper = input.maxFraction;
+
+	i32 index = -1;
+
+	// s(lower) = p1 + lower * d, 0 <= lower <= kupper
+	// The segment intersects the plane if a 'lower' exists
+	// for which s(lower) is inside all half-spaces.
+
+	// Solve line segment to plane:
+	// dot(n, s(lower)) = offset
+	// dot(n, p1 + lower * d) = offset
+	// dot(n, p1) + dot(n, lower * d) = offset
+	// dot(n, p1) + lower * dot(n, d) = offset
+	// lower * dot(n, d) = offset - dot(n, p1)
+	// lower = (offset - dot(n, p1)) / dot(n, d)
+
+	for (u32 i = 0; i < planeCount; ++i)
+	{
+		float32 numerator = planes[i].offset - b3Dot(planes[i].normal, p1);
+		float32 denominator = b3Dot(planes[i].normal, d);
+
+		if (denominator == 0.0f)
+		{
+			// s is parallel to this half-space.
+			if (numerator < 0.0f)
+			{
+				// s is outside of this half-space.
+				// dot(n, p1) and dot(n, p2) < 0.
+				return false;
+			}
+		}
+		else
+		{
+			// Original predicates:
+			// lower < numerator / denominator, for denominator < 0
+			// upper < numerator / denominator, for denominator < 0
+			// Optimized predicates:
+			// lower * denominator > numerator
+			// upper * denominator > numerator
+			if (denominator < 0.0f)
+			{
+				// s enters this half-space.
+				if (numerator < lower * denominator)
+				{
+					// Increase lower.
+					lower = numerator / denominator;
+					index = i;
+				}
+			}
+			else
+			{
+				// s exits the half-space.	
+				if (numerator < upper * denominator)
+				{
+					// Decrease upper.
+					upper = numerator / denominator;
+				}
+			}
+
+			// Exit if intersection becomes empty.
+			if (upper < lower)
+			{
+				return false;
+			}
+		}
+	}
+
+	B3_ASSERT(lower >= 0.0f && lower <= input.maxFraction);
+
+	if (index >= 0)
+	{
+		output->fraction = lower;
+		output->normal = b3Mul(xf.rotation, planes[index].normal);
+		return true;
+	}
+
+	return false;
+}

src/bounce/dynamics/shapes/mesh_shape.cpp

@@ -0,0 +1,170 @@
+/*
+* 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\shapes\mesh_shape.h>
+#include <bounce\collision\shapes\mesh.h>
+
+b3MeshShape::b3MeshShape() 
+{
+	m_type = e_meshShape;
+	m_radius = B3_HULL_RADIUS;
+	m_mesh = nullptr;
+}
+
+b3MeshShape::~b3MeshShape() 
+{
+}
+
+void b3MeshShape::Swap(const b3MeshShape& other) 
+{
+	m_radius = other.m_radius;
+	m_mesh = other.m_mesh;
+}
+
+void b3MeshShape::ComputeMass(b3MassData* massData, float32 density) const 
+{
+	B3_NOT_USED(density);
+	u32 n = m_mesh->vertexCount;
+	b3Vec3 c(0.0f, 0.0f, 0.0f);
+	for (u32 i = 0; i < n; ++i)
+	{
+		c += m_mesh->vertices[i];
+	}
+	if (n > 0)
+	{
+		c = 1.0f / float32(n) * c;
+	}	
+	massData->center = c;
+	massData->mass = 0.0f;
+	massData->I.SetZero();
+}
+
+void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf) const 
+{
+	output->Compute(m_mesh->vertices, m_mesh->vertexCount, xf);
+	output->Extend(m_radius);
+}
+
+void b3MeshShape::ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index) const
+{
+	*output = m_mesh->GetTriangleAABB(index);
+}
+
+bool b3MeshShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+{
+	B3_NOT_USED(point);
+	B3_NOT_USED(xf);
+	return false;
+}
+
+bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf, u32 index) const
+{
+	B3_ASSERT(index < m_mesh->triangleCount);
+	b3Triangle* triangle = m_mesh->triangles + index;
+
+	b3Vec3 p1 = input.p1;
+	b3Vec3 p2 = input.p2;
+	b3Vec3 d = p2 - p1;
+
+	b3Vec3 v1 = m_mesh->vertices[triangle->v1];
+	b3Vec3 v2 = m_mesh->vertices[triangle->v2];
+	b3Vec3 v3 = m_mesh->vertices[triangle->v3];
+
+	b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
+	n.Normalize();
+	float32 offset = b3Dot(n, v1);
+
+	float32 numerator = b3Dot(n, v1 - p1);
+	float32 denominator = b3Dot(n, d);
+
+	if (denominator == 0.0f)
+	{
+		return false;
+	}
+
+	float32 t = numerator / denominator;
+	if (t < 0.0f || input.maxFraction < t)
+	{
+		return false;
+	}
+
+	b3Vec3 q = p1 + t * d;
+
+	float32 w[4];
+	b3Barycentric(w, v1, v2, v3, q);
+
+	if (w[0] > 0.0f && w[1] > 0.0f && w[2] > 0.0f)
+	{
+		output->fraction = t;
+		if (numerator > 0.0f)
+		{
+			output->normal = -n;
+		}
+		else
+		{
+			output->normal = n;
+		}
+		return true;
+	}
+
+	return false;
+}
+
+struct b3MeshRayCastCallback
+{
+	float32 Report(const b3RayCastInput& input, u32 proxyId)
+	{
+		u32 index = mesh->m_mesh->tree.GetUserData(proxyId);
+		b3RayCastOutput indexOutput;
+		if (mesh->RayCast(&indexOutput, input, xf, index))
+		{
+			hit = true;
+			if (indexOutput.fraction < output.fraction)
+			{
+				output = indexOutput;
+			}
+		}
+		return 1.0f;
+	}
+
+	b3Transform xf;
+	const b3MeshShape* mesh;
+	bool hit;
+	b3RayCastOutput output;
+};
+
+bool b3MeshShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const 
+{
+	// Put the ray into the mesh's frame of reference.
+	b3RayCastInput subInput;
+	subInput.p1 = b3MulT(xf.rotation, input.p1 - xf.position);
+	subInput.p2 = b3MulT(xf.rotation, input.p2 - xf.position);
+	subInput.maxFraction = input.maxFraction;
+
+	b3MeshRayCastCallback callback;
+	callback.mesh = this;
+	callback.hit = false;
+	callback.output.fraction = B3_MAX_FLOAT;
+
+	m_mesh->tree.QueryRay(&callback, subInput);
+
+	output->fraction = callback.output.fraction;
+	output->normal = b3Mul(xf.rotation, callback.output.normal);
+
+	return callback.hit;
+}

src/bounce/dynamics/shapes/shape.cpp

@@ -0,0 +1,291 @@
+/*
+* 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\shapes\shape.h>
+#include <bounce\dynamics\shapes\sphere_shape.h>
+#include <bounce\dynamics\shapes\capsule_shape.h>
+#include <bounce\dynamics\shapes\hull_shape.h>
+#include <bounce\dynamics\shapes\mesh_shape.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+#include <bounce\collision\shapes\mesh.h>
+
+void b3Shape::SetSensor(bool flag)
+{
+	if (flag != m_isSensor)
+	{
+		if (m_body)
+		{
+			m_body->SetAwake(true);
+		}
+		m_isSensor = flag;
+	}
+}
+
+b3Transform b3Shape::GetTransform() const 
+{
+	return m_body->GetTransform();
+}
+
+void b3Shape::DestroyContacts()
+{
+	b3World* world = m_body->GetWorld();
+	b3ContactEdge* ce = m_contactEdges.m_head;
+	while (ce)
+	{
+		b3ContactEdge* tmp = ce;
+		ce = ce->m_next;
+		world->m_contactMan.Destroy(tmp->contact);
+	}
+}
+
+void b3Shape::Dump(i32 bodyIndex) const
+{
+	switch (m_type)
+	{
+	case e_sphereShape:
+	{
+		b3SphereShape* sphere = (b3SphereShape*) this;
+		b3Log("		b3SphereShape shape;\n");
+		b3Log("		shape.m_center.Set(%f, %f, %f);\n", sphere->m_center.x, sphere->m_center.y, sphere->m_center.z);
+		b3Log("		shape.m_radius = %f;\n", sphere->m_radius);
+		break;
+	}
+	case e_capsuleShape:
+	{
+		b3CapsuleShape* capsule = (b3CapsuleShape*) this;
+		b3Log("		b3CapsuleShape shape;\n");
+		b3Log("		shape.m_centers[0].Set(%f, %f, %f);\n", capsule->m_centers[0].x, capsule->m_centers[0].y, capsule->m_centers[0].z);
+		b3Log("		shape.m_centers[1].Set(%f, %f, %f);\n", capsule->m_centers[1].x, capsule->m_centers[1].y, capsule->m_centers[1].z);
+		b3Log("		shape.m_radius = %f;\n", capsule->m_radius);
+		break;
+	}
+	case e_hullShape:
+	{
+		b3HullShape* hs = (b3HullShape*) this;
+		const b3Hull* h = hs->m_hull;
+		
+		b3Log("		u8* marker = (u8*) b3Alloc(%d);\n", h->GetSize());
+		b3Log("		\n");
+		b3Log("		b3Hull* h = (b3Hull*)marker;\n");
+		b3Log("		marker += 1 * sizeof(b3Hull);\n");
+		b3Log("		h->vertices = (b3Vec3*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Vec3);\n", h->vertexCount);
+		b3Log("		h->edges = (b3HalfEdge*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3HalfEdge);\n", h->edgeCount);
+		b3Log("		h->faces = (b3Face*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Face);\n", h->faceCount);
+		b3Log("		h->planes = (b3Plane*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Plane);\n", h->faceCount);
+		b3Log("		\n");
+		b3Log("		h->centroid.Set(%f, %f, %f);\n", h->centroid.x, h->centroid.y, h->centroid.z);
+		b3Log("		\n");
+		b3Log("		h->vertexCount = %d;\n", h->vertexCount);
+		for (u32 i = 0; i < h->vertexCount; ++i)
+		{
+			const b3Vec3* v = h->vertices + i;
+			b3Log("		h->vertices[%d].Set(%f, %f, %f);\n", i, v->x, v->y, v->z);
+		}
+		b3Log("		\n");
+		b3Log("		h->edgeCount = %d;\n", h->edgeCount);
+		for (u32 i = 0; i < h->edgeCount; ++i)
+		{
+			const b3HalfEdge* e = h->edges + i;
+			b3Log("		h->edges[%d].origin = %d;\n", i, e->origin);
+			b3Log("		h->edges[%d].twin = %d;\n", i, e->twin);
+			b3Log("		h->edges[%d].face = %d;\n", i, e->face);
+			b3Log("		h->edges[%d].next = %d;\n", i, e->next);
+		}
+		b3Log("		\n");
+		b3Log("		h->faceCount = %d;\n", h->faceCount);
+		for (u32 i = 0; i < h->faceCount; ++i)
+		{
+			const b3Face* f = h->faces + i;
+			b3Log("		h->faces[%d].edge = %d;\n", i, f->edge);
+		}
+		b3Log("		\n");
+		for (u32 i = 0; i < h->faceCount; ++i)
+		{
+			const b3Plane* p = h->planes + i;
+			b3Log("		h->planes[%d].normal.Set(%f, %f, %f);\n", i, p->normal.x, p->normal.y, p->normal.z);
+			b3Log("		h->planes[%d].offset = %f;\n", i, p->offset);
+		}
+		b3Log("		\n");
+		b3Log("		h->Validate();\n");
+		b3Log("		\n");
+		b3Log("		b3HullShape shape;\n");
+		b3Log("		shape.m_hull = h;\n");
+		b3Log("		shape.m_radius = %f;\n", m_radius);
+		break;
+	}
+	case e_meshShape:
+	{
+		b3MeshShape* ms = (b3MeshShape*) this;
+		const b3Mesh* m = ms->m_mesh;
+		
+		b3Log("		u8* marker = (u8*) b3Alloc(%d);\n", m->GetSize());
+		b3Log("		\n");
+		b3Log("		b3Mesh* m = (b3Hull*)marker;\n");
+		b3Log("		marker += 1 * sizeof(b3Mesh);\n");
+		b3Log("		m->vertices = (b3Vec3*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Vec3);\n", m->vertexCount);
+		b3Log("		m->triangles = (b3Triangle*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Triangle);\n", m->triangleCount);
+		b3Log("		m->planes = (b3Plane*)marker;\n");
+		b3Log("		marker += %d * sizeof(b3Plane);\n", 2 * m->triangleCount);
+		b3Log("		\n");
+		for (u32 i = 0; i < m->vertexCount; ++i)
+		{
+			const b3Vec3* v = m->vertices + i;
+			b3Log("		m->vertices[%d].Set(%f, %f, %f);\n", i, v->x, v->y, v->z);
+		}
+		b3Log("		\n");
+		for (u32 i = 0; i < m->triangleCount; ++i)
+		{
+			const b3Triangle* t = m->triangles + i;
+			b3Log("		m->triangles[%d].v1 = %d;\n", i, t->v1);
+			b3Log("		m->triangles[%d].v2 = %d;\n", i, t->v2);
+			b3Log("		m->triangles[%d].v3 = %d;\n", i, t->v3);
+		}
+		b3Log("		\n");
+		b3Log("		\n");
+		b3Log("		m->BuildTree();\n");		
+		b3Log("		\n");
+		b3Log("		b3MeshShape shape;\n");
+		b3Log("		shape.m_mesh = m;\n");
+		b3Log("		shape.m_radius = %f;\n", m_radius);
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	};
+
+	b3Log("		\n");
+	b3Log("		b3ShapeDef sd;\n");
+	b3Log("		sd.shape = &shape;\n");
+	b3Log("		sd.density = %f;\n", m_density);
+	b3Log("		sd.restitution = %f;\n", m_restitution);
+	b3Log("		sd.friction = %f;\n", m_friction);
+	b3Log("		sd.sensor = %d;\n", m_isSensor);
+	b3Log("		\n");
+	b3Log("		bodies[%d]->CreateShape(sd);\n", bodyIndex);
+}
+
+b3Shape* b3Shape::Create(const b3ShapeDef& def)
+{
+	b3Shape* shape = nullptr;
+	switch (def.shape->GetType())
+	{
+	case e_sphereShape:
+	{
+		// Grab pointer to the specific memory.
+		b3SphereShape* sphere1 = (b3SphereShape*)def.shape;
+		void* mem = b3Alloc(sizeof(b3SphereShape));
+		b3SphereShape* sphere2 = new (mem)b3SphereShape();
+		// Clone the polyhedra.
+		sphere2->Swap(*sphere1);
+		shape = sphere2;
+		break;
+	}
+	case e_capsuleShape:
+	{
+		// Grab pointer to the specific memory.
+		b3CapsuleShape* caps1 = (b3CapsuleShape*)def.shape;
+		void* block = b3Alloc(sizeof(b3CapsuleShape));
+		b3CapsuleShape* caps2 = new (block)b3CapsuleShape();
+		caps2->Swap(*caps1);
+		shape = caps2;
+		break;
+	}
+	case e_hullShape:
+	{
+		// Grab pointer to the specific memory.
+		b3HullShape* hull1 = (b3HullShape*)def.shape;
+		void* block = b3Alloc(sizeof(b3HullShape));
+		b3HullShape* hull2 = new (block)b3HullShape();
+		hull2->Swap(*hull1);
+		shape = hull2;
+		break;
+	}
+	case e_meshShape:
+	{
+		// Grab pointer to the specific memory.
+		b3MeshShape* mesh1 = (b3MeshShape*)def.shape;
+		void* block = b3Alloc(sizeof(b3MeshShape));
+		b3MeshShape* mesh2 = new (block) b3MeshShape();
+		// Clone the mesh.
+		mesh2->Swap(*mesh1);
+		shape = mesh2;
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		break;
+	}
+	}
+
+	return shape;
+}
+
+void b3Shape::Destroy(b3Shape* shape)
+{
+	// Free the shape from the memory.
+	switch (shape->GetType())
+	{
+	case e_sphereShape:
+	{
+		b3SphereShape* sphere = (b3SphereShape*)shape;
+		sphere->~b3SphereShape();
+		b3Free(shape);
+		break;
+	}
+	case e_capsuleShape:
+	{
+		b3CapsuleShape* caps = (b3CapsuleShape*)shape;
+		caps->~b3CapsuleShape();
+		b3Free(shape);
+		break;
+	}
+	case e_hullShape:
+	{
+		b3HullShape* hull = (b3HullShape*)shape;
+		hull->~b3HullShape();
+		b3Free(shape);
+		break;
+	}
+	case e_meshShape:
+	{
+		b3MeshShape* mesh = (b3MeshShape*)shape;
+		mesh->~b3MeshShape();
+		b3Free(shape);
+		break;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+	}
+	}
+}

src/bounce/dynamics/shapes/sphere_shape.cpp

@@ -0,0 +1,120 @@
+/*
+* 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\shapes\sphere_shape.h>
+
+b3SphereShape::b3SphereShape() 
+{
+	m_type = e_sphereShape;
+	m_radius = 0.0f;
+	m_center.SetZero();
+}
+
+b3SphereShape::~b3SphereShape() 
+{
+}
+
+void b3SphereShape::Swap(const b3SphereShape& other) 
+{
+	m_center = other.m_center;
+	m_radius = other.m_radius;
+}
+
+void b3SphereShape::ComputeMass(b3MassData* massData, float32 density) const 
+{
+	// Compute inertia about the origin
+	float32 volume = (4.0f / 3.0f) * B3_PI * m_radius * m_radius * m_radius;
+	float32 mass = density * volume;
+	b3Mat33 Io = b3Diagonal(mass * (2.0f / 5.0f) * m_radius * m_radius);
+	// Move inertia to the sphere center
+	massData->center = m_center;
+	massData->mass = mass;
+	massData->I = b3MoveToCOM(Io, mass, m_center);
+}
+
+void b3SphereShape::ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const 
+{
+	b3Vec3 center = b3Mul(xf, m_center);
+	b3Vec3 r(m_radius, m_radius, m_radius);
+	aabb->m_lower = center - r;
+	aabb->m_upper = center + r;
+}
+
+bool b3SphereShape::TestPoint(const b3Vec3& point, const b3Transform& xf) const
+{
+	b3Vec3 center = b3Mul(xf, m_center);
+	float32 rr = m_radius * m_radius;
+	b3Vec3 d = point - center;
+	float32 dd = b3Dot(d, d);
+	return dd <= rr;
+}
+
+bool b3SphereShape::RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const 
+{
+	// dot(x - c, x - c) - r^2 = 0
+	// S = p1 + t * (p2 - p1)
+	// Solve for t:
+	// dot(p1 + t * d - c, P + t * d - c) - r^2 =
+	// dot(d, d) * t^2 + 2 * dot(m, d) * t + dot(m, m) - r^2 = 0
+	// m = p1 - c
+	// d = p2 - p1
+	b3Vec3 d = input.p2 - input.p1;
+	float32 a = b3Dot(d, d);
+	
+	// Check for short segment.
+	if (a < B3_EPSILON * B3_EPSILON)
+	{
+		return false;
+	}
+		
+	// Solve quadratic equation
+	// a * t^2 + 2 * b * t + c = 0
+	// a = dot(d, d)
+	// b = dot(m, d)
+	// c = dot(m, m) - r^2
+	// t = -b +/- sqrt(b * b - a * c) / a
+	b3Vec3 m = input.p1 - b3Mul(xf, m_center);
+	float32 b = b3Dot(m, d);
+	float32 c = b3Dot(m, m) - m_radius * m_radius;
+	
+	float32 disc = b * b - a * c;
+
+	// Check for negative discriminant.
+	// Does the ray misses the sphere completely?
+	if (disc < 0.0f)
+	{
+		return false;
+	}
+
+	// Find the minimum time of impact of the line with the sphere.
+	// t_min = -b - sqrt(disc)
+	// t_max = -b + sqrt(disc) 
+	float32 t = -b - b3Sqrt(disc);
+	
+	// Is the intersection point on the segment?
+	if (t > 0.0f && t <= input.maxFraction * a) 
+	{
+		// Finish solution.
+		t /= a;
+		output->fraction = t;
+		output->normal = b3Normalize(m + t * d);
+		return true;
+	}
+	
+	return false;
+}

src/bounce/dynamics/world.cpp

@@ -0,0 +1,398 @@
+/*
+* 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\world.h>
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\island.h>
+#include <bounce\dynamics\world_listeners.h>
+#include <bounce\dynamics\shapes\shape.h>
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\dynamics\joints\joint.h>
+#include <bounce\dynamics\time_step.h>
+#include <bounce\common\time.h>
+
+extern u32 b3_allocCalls;
+extern u32 b3_maxAllocCalls;
+
+b3World::b3World() : m_bodyBlocks(sizeof(b3Body))
+{
+	b3_allocCalls = 0;
+	b3_maxAllocCalls = 0;
+	m_debugDraw = nullptr;
+	memset(&m_profile, 0, sizeof(b3Profile));
+
+	m_flags = e_clearForcesFlag;
+	m_sleeping = false;
+	m_warmStarting = true;
+	m_gravity.Set(0.0f, -9.8f, 0.0f);
+}
+
+b3World::~b3World()
+{
+	b3Body* b = m_bodyList.m_head;
+	while (b)
+	{
+		// Shapes and joints use b3Alloc.
+		b->DestroyShapes();
+		b->DestroyJoints();
+		b = b->m_next;
+	}
+	b3_allocCalls = 0;
+	b3_maxAllocCalls = 0;
+}
+
+void b3World::SetSleeping(bool flag)
+{
+	m_sleeping = flag;
+	if (m_sleeping == false)
+	{
+		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+		{
+			b->SetAwake(true);
+		}
+	}
+}
+
+void b3World::SetDebugDraw(b3Draw* debugDraw)
+{
+	m_debugDraw = debugDraw;
+}
+
+b3Body* b3World::CreateBody(const b3BodyDef& def)
+{
+	void* mem = m_bodyBlocks.Allocate();
+	b3Body* b = new(mem) b3Body(def, this);
+	m_bodyList.PushFront(b);	
+	return b;
+}
+
+void b3World::DestroyBody(b3Body* b)
+{
+	b->DestroyShapes();
+	b->DestroyJoints();
+	b->DestroyContacts();
+	
+	m_bodyList.Remove(b);
+	b->~b3Body();
+	m_bodyBlocks.Free(b);
+}
+
+b3Joint* b3World::CreateJoint(const b3JointDef& def)
+{
+	return m_jointMan.Create(&def);
+}
+
+void b3World::DestroyJoint(b3Joint* j)
+{
+	m_jointMan.Destroy(j);
+}
+
+void b3World::Step(float32 dt, u32 velocityIterations, u32 positionIterations)
+{
+	memset(&m_profile, 0, sizeof(b3Profile));
+
+	b3Time stepTime;
+
+	if (m_flags & e_shapeAddedFlag)
+	{
+		// If new shapes were added new contacts might be created.
+		m_contactMan.FindNewContacts();
+		m_flags &= ~e_shapeAddedFlag;
+	}
+
+	{
+		// Update contacts. This is where some contacts might be destroyed.
+		b3Time time;
+		m_contactMan.UpdateContacts();
+		time.Update();
+		m_profile.collide.narrowphase = time.GetElapsedMilis();
+	}
+
+	{
+		b3Time time;
+		if (dt > 0.0f)
+		{
+			Solve(dt, velocityIterations, positionIterations);
+		}
+		time.Update();
+	}
+
+	{
+		//todo
+		//SolveTOI
+	}
+
+	stepTime.Update();
+	m_profile.total = stepTime.GetElapsedMilis();
+}
+
+void b3World::Solve(float32 dt, u32 velocityIterations, u32 positionIterations)
+{
+	// Clear all visited flags for the depth first search.
+	for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+	{
+		b->m_flags &= ~b3Body::e_islandFlag;
+	}
+
+	for (b3Joint* j = m_jointMan.m_jointList.m_head; j; j = j->m_next)
+	{
+		j->m_flags &= ~b3Joint::e_islandFlag;
+	}
+
+	for (b3Contact* c = m_contactMan.m_contactList.m_head; c; c = c->m_next)
+	{
+		c->m_flags &= ~b3Contact::e_islandFlag;
+	}
+
+	u32 islandFlags = 0;
+	islandFlags |= m_warmStarting * b3Island::e_warmStartBit;
+	islandFlags |= m_sleeping * b3Island::e_sleepBit;
+
+	b3Vec3 externalForce = m_gravity;
+
+	// Create a worst case island.
+	b3Island island(&m_stackAllocator, m_bodyList.m_count, m_contactMan.m_contactList.m_count, m_jointMan.m_jointList.m_count);
+
+	// Build and simulate awake islands.
+	u32 stackSize = m_bodyList.m_count;
+	b3Body** stack = (b3Body**)m_stackAllocator.Allocate(stackSize * sizeof(b3Body*));
+	for (b3Body* seed = m_bodyList.m_head; seed; seed = seed->m_next)
+	{
+		// The seed must not be on an island.
+		if (seed->m_flags & b3Body::e_islandFlag)
+		{
+			continue;
+		}
+
+		// Bodies that are sleeping are not solved for performance.
+		if (!(seed->m_flags & b3Body::e_awakeFlag))
+		{
+			continue;
+		}
+
+		// The seed must be dynamic or kinematic.
+		if (seed->m_type == e_staticBody)
+		{
+			continue;
+		}
+
+		// Perform a depth first search on this body constraint graph.
+		island.Clear();
+		u32 stackCount = 0;
+		stack[stackCount++] = seed;
+		seed->m_flags |= b3Body::e_islandFlag;
+
+		while (stackCount > 0)
+		{
+			// Add this body to the island.
+			b3Body* b = stack[--stackCount];
+			island.Add(b);
+			
+			// This body must be awake.
+			b->m_flags |= b3Body::e_awakeFlag;
+
+			// Don't propagate islands across static bodies to keep them small.
+			if (b->m_type == e_staticBody)
+			{
+				continue;
+			}
+
+			// Search all contacts connected to this body.
+			for (b3Shape* s = b->m_shapeList.m_head; s; s = s->m_next)
+			{
+				for (b3ContactEdge* ce = s->m_contactEdges.m_head; ce; ce = ce->m_next)
+				{
+					b3Contact* contact = ce->contact;
+
+					// The contact must not be on an island.
+					if (contact->m_flags & b3Contact::e_islandFlag)
+					{
+						continue;
+					}
+
+					// The contact must be overlapping.
+					if (!(contact->m_flags & b3Contact::e_overlapFlag))
+					{
+						continue;
+					}
+
+					// A sensor can't respond to contacts. 
+					bool sensorA = contact->GetShapeA()->m_isSensor;
+					bool sensorB = contact->GetShapeB()->m_isSensor;
+					if (sensorA || sensorB)
+					{
+						continue;
+					}
+
+					// Add contact to the island and mark it.
+					island.Add(contact);
+					contact->m_flags |= b3Contact::e_islandFlag;
+
+					b3Body* other = ce->other->GetBody();
+
+					// Skip adjacent vertex if it was visited.
+					if (other->m_flags & b3Body::e_islandFlag)
+					{
+						continue;
+					}
+
+					// Add the other body to the island and propagate through it.
+					B3_ASSERT(stackCount < stackSize);
+					stack[stackCount++] = other;
+					other->m_flags |= b3Body::e_islandFlag;
+				}
+			}
+
+			// Search all joints connected to this body.
+			for (b3JointEdge* je = b->m_jointEdges.m_head; je; je = je->m_next)
+			{
+				b3Joint* joint = je->joint;
+
+				// The joint must not be on an island.
+				if (joint->m_flags & b3Joint::e_islandFlag)
+				{
+					continue;
+				}
+
+				// Add joint to the island and mark it.
+				island.Add(joint);
+				joint->m_flags |= b3Joint::e_islandFlag;
+
+				b3Body* other = je->other;
+
+				// The other body must not be on an island.
+				if (other->m_flags & b3Body::e_islandFlag)
+				{
+					continue;
+				}
+
+				// Push the other body onto the stack and mark it.
+				B3_ASSERT(stackCount < stackSize);
+				stack[stackCount++] = other;
+				other->m_flags |= b3Body::e_islandFlag;
+			}
+		}
+
+		// Integrate velocities, clear forces and torques, solve constraints, integrate positions.
+		island.Solve(&m_profile, externalForce, dt, velocityIterations, positionIterations, islandFlags);
+		
+		// Allow static bodies to participate in other islands.
+		for (u32 i = 0; i < island.m_bodyCount; ++i)
+		{
+			b3Body* b = island.m_bodies[i];
+			if (b->m_type == e_staticBody)
+			{
+				b->m_flags &= ~b3Body::e_islandFlag;
+			}
+		}
+	}
+
+	m_stackAllocator.Free(stack);
+
+	{
+		b3Time time;
+		for (b3Body* b = m_bodyList.m_head; b; b = b->m_next)
+		{
+			// If a body didn't participate on a island then it didn't move
+			// at all.
+			if ((b->m_flags & b3Body::e_islandFlag) == 0)
+			{
+				continue;
+			}
+
+			// Update shape AABBs.
+			b->SynchronizeShapes();
+		}
+
+		// Notify the contacts the shapes may have moved.
+		m_contactMan.SynchronizeShapes();
+
+		time.Update();
+		m_contactMan.FindNewContacts();
+		time.Update();
+		m_profile.collide.broadphase = time.GetElapsedMilis();
+	}
+}
+
+struct b3CastRayCallback
+{
+	float32 Report(const b3RayCastInput& input, i32 proxyId)
+	{
+		// Get shape associated with the proxy.
+		void* userData = broadPhase->GetUserData(proxyId);
+		b3Shape* shape = (b3Shape*)userData;
+		
+		// Calculate transformation from shape local space to world space.
+		b3Transform xf = shape->GetTransform();
+
+		b3RayCastOutput output;
+		bool hit = shape->RayCast(&output, input, xf);
+		if (hit)
+		{
+			// Ray hits shape.
+			float32 fraction = output.fraction;
+			float32 w1 = 1.0f - fraction;
+			float32 w2 = fraction;
+			
+			b3Vec3 point = w1 * input.p1 + w2 * input.p2;
+			b3Vec3 normal = output.normal;
+
+			// Report the intersection to the user and get the new ray cast fraction.
+			return listener->ReportShape(shape, point, normal, fraction);
+		}
+
+		// Continue search from where we stopped.
+		return input.maxFraction;
+	}
+
+	b3RayCastListener* listener;
+	const b3BroadPhase* broadPhase;
+};
+
+void b3World::CastRay(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const
+{
+	b3RayCastInput input;
+	input.p1 = p1;
+	input.p2 = p2;
+	input.maxFraction = 1.0f;
+	
+	b3CastRayCallback callback;
+	callback.listener = listener;
+	callback.broadPhase = &m_contactMan.m_broadPhase;
+	m_contactMan.m_broadPhase.QueryRay(&callback, input);
+}
+
+struct b3CastAABBCallback
+{
+	bool Report(i32 proxyID)
+	{
+		b3Shape* shape = (b3Shape*)broadPhase->GetUserData(proxyID);
+		return listener->ReportShape(shape);
+	}
+
+	b3QueryListener* listener;
+	const b3BroadPhase* broadPhase;
+};
+
+void b3World::CastAABB(b3QueryListener* listener, const b3AABB3& aabb) const
+{
+	b3CastAABBCallback callback;
+	callback.listener = listener;
+	callback.broadPhase = &m_contactMan.m_broadPhase;
+	m_contactMan.m_broadPhase.QueryAABB(&callback, aabb);
+}

src/bounce/quickhull/qh_hull.cpp

@@ -0,0 +1,772 @@
+/*
+* 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\quickhull\qh_hull.h>
+#include <bounce\common\template\stack.h>
+#include <bounce\common\draw.h>
+
+float32 qhFindAABB(u32 iMin[3], u32 iMax[3], const b3Array<b3Vec3>& vertices)
+{
+	b3Vec3 min(B3_MAX_FLOAT, B3_MAX_FLOAT, B3_MAX_FLOAT);
+	iMin[0] = 0;
+	iMin[1] = 0;
+	iMin[2] = 0;
+	
+	b3Vec3 max(-B3_MAX_FLOAT, -B3_MAX_FLOAT, -B3_MAX_FLOAT);
+	iMax[0] = 0;
+	iMax[1] = 0;
+	iMax[2] = 0;
+
+	for (u32 i = 0; i < vertices.Count(); ++i)
+	{
+		b3Vec3 p = vertices[i];
+
+		for (u32 j = 0; j < 3; ++j)
+		{
+			if (p[j] < min[j])
+			{
+				min[j] = p[j];
+				iMin[j] = i;
+			}
+
+			if (p[j] > max[j])
+			{
+				max[j] = p[j];
+				iMax[j] = i;
+			}
+		}
+	}
+
+	return 3.0f * (b3Abs(max.x) + b3Abs(max.y) + b3Abs(max.z)) * B3_EPSILON;
+}
+
+qhHull::qhHull()
+{
+}
+
+qhHull::~qhHull()
+{
+}
+
+void qhHull::Construct(void* memory, const b3Array<b3Vec3>& vs)
+{
+	u32 V = vs.Count();
+	u32 E = 3 * V - 6;
+	u32 HE = 2 * E;
+	u32 F = 2 * V - 4;
+
+	HE *= 2;
+	F *= 2;
+
+	// Euler's formula
+	// V - E + F = 2
+	
+	m_freeVertices = nullptr;
+	qhVertex* vertices = (qhVertex*)memory;
+	for (u32 i = 0; i < V; ++i)
+	{
+		FreeVertex(vertices + i);
+	}
+
+	m_freeEdges = nullptr;
+	qhHalfEdge* edges = (qhHalfEdge*)((u8*)vertices + V * sizeof(qhVertex));
+	for (u32 i = 0; i < HE; ++i)
+	{
+		FreeEdge(edges + i);
+	}
+	
+	m_freeFaces = nullptr;
+	qhFace* faces = (qhFace*)((u8*)edges + HE * sizeof(qhHalfEdge));
+	for (u32 i = 0; i < F; ++i)
+	{
+		qhFace* f = faces + i;
+		f->conflictList.head = nullptr;
+		f->conflictList.count = 0;
+		FreeFace(f);
+	}
+		
+	m_faceList.head = nullptr;
+	m_faceList.count = 0;
+	m_iteration = 0;
+
+	if (!BuildInitialHull(vs))
+	{
+		return;
+	}
+
+	qhVertex* eye = NextVertex();
+	while (eye)
+	{
+		AddVertex(eye);
+		eye = NextVertex();
+		++m_iteration;
+	}
+}
+
+bool qhHull::BuildInitialHull(const b3Array<b3Vec3>& vertices)
+{
+	if (vertices.Count() < 4)
+	{
+		B3_ASSERT(false);
+		return false;
+	}
+
+	u32 i1 = 0, i2 = 0;
+
+	{
+		// Find the points that maximizes the distance along the 
+		// canonical axes.
+		// Store tolerance for coplanarity checks.
+		u32 aabbMin[3], aabbMax[3];
+		m_tolerance = qhFindAABB(aabbMin, aabbMax, vertices);
+
+		// Find the longest segment.
+		float32 d0 = 0.0f;
+
+		for (u32 i = 0; i < 3; ++i)
+		{
+			b3Vec3 A = vertices[aabbMin[i]];
+			b3Vec3 B = vertices[aabbMax[i]];
+			
+			float32 d = b3DistanceSquared(A, B);
+
+			if (d > d0)
+			{
+				d0 = d;
+				i1 = aabbMin[i];
+				i2 = aabbMax[i];
+			}
+		}
+
+		// Coincident check.
+		if (d0 < B3_LINEAR_SLOP * B3_LINEAR_SLOP)
+		{
+			B3_ASSERT(false);
+			return false;
+		}
+	}
+
+	B3_ASSERT(i1 != i2);
+
+	b3Vec3 A = vertices[i1];
+	b3Vec3 B = vertices[i2];
+
+	u32 i3 = 0;
+
+	{
+		// Find the triangle which has the largest area.
+		float32 a0 = 0.0f;
+
+		for (u32 i = 0; i < vertices.Count(); ++i)
+		{
+			if (i == i1 || i == i2)
+			{
+				continue;
+			}
+
+			b3Vec3 C = vertices[i];
+
+			float32 a = b3AreaSquared(A, B, C);
+			
+			if (a > a0)
+			{
+				a0 = a;
+				i3 = i;
+			}
+		}
+
+		// Colinear check.
+		if (a0 < (2.0f * B3_LINEAR_SLOP) * (2.0f * B3_LINEAR_SLOP))
+		{
+			B3_ASSERT(false);
+			return false;
+		}
+	}
+
+	B3_ASSERT(i3 != i1 && i3 != i2);
+
+	b3Vec3 C = vertices[i3];
+
+	b3Vec3 N = b3Cross(B - A, C - A);
+	N.Normalize();
+
+	b3Plane plane(N, A);
+
+	u32 i4 = 0;
+
+	{
+		// Find the furthest point from the triangle plane.
+		float32 d0 = 0.0f;
+
+		for (u32 i = 0; i < vertices.Count(); ++i)
+		{
+			if (i == i1 || i == i2 || i == i3)
+			{
+				continue;
+			}
+
+			b3Vec3 D = vertices[i];
+			
+			float32 d = b3Abs(b3Distance(D, plane));
+
+			if (d > d0)
+			{
+				d0 = d;
+				i4 = i;
+			}
+		}
+
+		// Coplanar check.
+		if (d0 < m_tolerance)
+		{
+			B3_ASSERT(false);
+			return false;
+		}
+	}
+
+	B3_ASSERT(i4 != i1 && i4 != i2 && i4 != i3);
+
+	// Add okay simplex to the hull.
+	b3Vec3 D = vertices[i4];
+
+	qhVertex* v1 = AllocateVertex();
+	v1->position = A;
+
+	qhVertex* v2 = AllocateVertex();
+	v2->position = B;
+
+	qhVertex* v3 = AllocateVertex();
+	v3->position = C;
+
+	qhVertex* v4 = AllocateVertex();
+	v4->position = D;
+
+	qhFace* faces[4];
+
+	if (b3Distance(D, plane) < 0.0f)
+	{
+		faces[0] = AddTriangle(v1, v2, v3);
+		faces[1] = AddTriangle(v4, v2, v1);
+		faces[2] = AddTriangle(v4, v3, v2);
+		faces[3] = AddTriangle(v4, v1, v3);
+	}
+	else
+	{
+		// Ensure CCW order.
+		faces[0] = AddTriangle(v1, v3, v2);
+		faces[1] = AddTriangle(v4, v1, v2);
+		faces[2] = AddTriangle(v4, v2, v3);
+		faces[3] = AddTriangle(v4, v3, v1);
+	}
+
+	// Connectivity check.
+	bool ok = IsConsistent();
+	B3_ASSERT(ok);
+	if (!ok)
+	{
+		return false;
+	}
+
+	// Add remaining points to the hull.
+	// Assign closest face plane to each of them.
+	for (u32 i = 0; i < vertices.Count(); ++i)
+	{
+		if (i == i1 || i == i2 || i == i3 || i == i4)
+		{
+			continue;
+		}
+
+		b3Vec3 p = vertices[i];
+
+		// Discard internal points since they can't be in the hull.
+		float32 d0 = m_tolerance;
+		qhFace* f0 = nullptr;
+
+		for (u32 j = 0; j < 4; ++j)
+		{
+			qhFace* f = faces[j];
+			float32 d = b3Distance(p, f->plane);
+			if (d > d0)
+			{
+				d0 = d;
+				f0 = f;
+			}
+		}
+		
+		if (f0)
+		{
+			qhVertex* v = AllocateVertex();
+			v->position = p;
+			v->conflictFace = f0;
+			f0->conflictList.PushFront(v);
+		}
+	}
+
+	return true;
+}
+
+qhVertex* qhHull::NextVertex()
+{
+	// Find the point furthest from the current hull.
+	float32 d0 = m_tolerance;
+	qhVertex* v0 = nullptr;
+
+	qhFace* f = m_faceList.head;
+	while (f)
+	{
+		qhVertex* v = f->conflictList.head;
+		while (v)
+		{
+			float32 d = b3Distance(v->position, f->plane);
+			if (d > d0)
+			{
+				d0 = d;
+				v0 = v;
+			}
+
+			v = v->next;
+		}
+
+		f = f->next;
+	}
+
+	return v0;
+}
+
+void qhHull::AddVertex(qhVertex* eye)
+{
+	b3StackArray<qhHalfEdge*, 32> horizon;
+	BuildHorizon(horizon, eye);
+
+	b3StackArray<qhFace*, 32> newFaces;
+	AddNewFaces(newFaces, eye, horizon);
+	
+	MergeFaces(newFaces);
+}
+
+void qhHull::BuildHorizon(b3Array<qhHalfEdge*>& horizon, qhVertex* eye, qhHalfEdge* edge0, qhFace* face)
+{
+	// Mark face as visible/visited.
+	face->state = qhFace::e_visible;
+
+	qhHalfEdge* edge = edge0;
+
+	do
+	{
+		qhHalfEdge* adjEdge = edge->twin;
+		qhFace* adjFace = adjEdge->face;
+
+		if (adjFace->state == qhFace::e_invisible)
+		{
+			if (b3Distance(eye->position, adjFace->plane) > m_tolerance)
+			{
+				BuildHorizon(horizon, eye, adjEdge, adjFace);
+			}
+			else
+			{
+				horizon.PushBack(edge);
+			}
+		}
+
+		edge = edge->next;
+
+	} while (edge != edge0);
+}
+
+void qhHull::BuildHorizon(b3Array<qhHalfEdge*>& horizon, qhVertex* eye)
+{
+	// Clean visited flags
+	{
+		qhFace* f = m_faceList.head;
+		while (f)
+		{
+			f->state = qhFace::e_invisible;
+
+			qhHalfEdge* e = f->edge;
+			do
+			{
+				e = e->next;
+			} while (e != f->edge);
+			f = f->next;
+		}
+	}
+
+	// todo
+	// Iterative DFS.
+	// Ensure CCW order of horizon edges.
+	
+	// Build horizon.
+	BuildHorizon(horizon, eye, eye->conflictFace->edge, eye->conflictFace);
+}
+
+qhFace* qhHull::AddTriangle(qhVertex* v1, qhVertex* v2, qhVertex* v3)
+{
+	qhFace* face = AllocateFace();
+
+	qhHalfEdge* e1 = AllocateEdge();
+	qhHalfEdge* e2 = AllocateEdge();
+	qhHalfEdge* e3 = AllocateEdge();
+
+	e1->tail = v1;
+	e1->prev = e3;
+	e1->next = e2;
+	e1->twin = FindTwin(v2, v1);
+	if (e1->twin)
+	{
+		e1->twin->twin = e1;
+	}
+	e1->face = face;
+
+	e2->tail = v2;
+	e2->prev = e1;
+	e2->next = e3;
+	e2->twin = FindTwin(v3, v2);
+	if (e2->twin)
+	{
+		e2->twin->twin = e2;
+	}
+	e2->face = face;
+
+	e3->tail = v3;
+	e3->prev = e2;
+	e3->next = e1;
+	e3->twin = FindTwin(v1, v3);
+	if (e3->twin)
+	{
+		e3->twin->twin = e3;
+	}
+	e3->face = face;
+
+	face->edge = e1;
+	face->center = (v1->position + v2->position + v3->position) / 3.0f;
+	face->plane = b3Plane(v1->position, v2->position, v3->position);
+	face->state = qhFace::e_invisible;
+
+	m_faceList.PushFront(face);
+
+	return face;
+}
+
+qhHalfEdge* qhHull::AddAdjoiningTriangle(qhVertex* eye, qhHalfEdge* horizonEdge)
+{
+	B3_ASSERT(horizonEdge->face->state == qhFace::e_visible);
+
+	qhFace* face = AllocateFace();
+
+	qhVertex* v1 = eye;
+	qhVertex* v2 = horizonEdge->tail;
+	qhVertex* v3 = horizonEdge->twin->tail;
+
+	qhHalfEdge* e1 = AllocateEdge();
+	qhHalfEdge* e2 = AllocateEdge();
+	qhHalfEdge* e3 = AllocateEdge();
+	
+	e1->tail = v1;
+	e1->prev = e3;
+	e1->next = e2;
+	e1->twin = nullptr;
+	e1->face = face;
+
+	e2->tail = v2;
+	e2->prev = e1;
+	e2->next = e3;
+	e2->twin = horizonEdge->twin;
+	horizonEdge->twin->twin = e2;
+	e2->face = face;
+
+	e3->tail = v3;
+	e3->prev = e2;
+	e3->next = e1;
+	e3->twin = nullptr;
+	e3->face = face;
+
+	horizonEdge->twin = nullptr;
+
+	face->edge = e1;
+	face->center = (v1->position + v2->position + v3->position) / 3.0f;
+	face->plane = b3Plane(v1->position, v2->position, v3->position);
+	face->state = qhFace::e_invisible;
+
+	m_faceList.PushFront(face);
+
+	return e1;
+}
+
+void qhHull::AddNewFaces(b3Array<qhFace*>& newFaces, qhVertex* eye, const b3Array<qhHalfEdge*>& horizon)
+{
+	newFaces.Reserve(horizon.Count());
+
+	qhHalfEdge* beginEdge = nullptr;
+	qhHalfEdge* prevEdge = nullptr;
+
+	{
+		qhHalfEdge* edge = horizon[0];
+		qhHalfEdge* leftEdge = AddAdjoiningTriangle(eye, edge);
+		qhHalfEdge* rightEdge = leftEdge->prev;
+
+		prevEdge = rightEdge;
+
+		beginEdge = leftEdge;
+
+		newFaces.PushBack(leftEdge->face);
+	}
+
+	for (u32 i = 1; i < horizon.Count() - 1; ++i)
+	{
+		qhHalfEdge* edge = horizon[i];
+		qhHalfEdge* leftEdge = AddAdjoiningTriangle(eye, edge);
+		qhHalfEdge* rightEdge = leftEdge->prev;
+
+		leftEdge->twin = prevEdge;
+		prevEdge->twin = leftEdge;
+
+		prevEdge = rightEdge;
+
+		newFaces.PushBack(leftEdge->face);
+	}
+
+	{
+		qhHalfEdge* edge = horizon[horizon.Count() - 1];
+		qhHalfEdge* leftEdge = AddAdjoiningTriangle(eye, edge);
+		qhHalfEdge* rightEdge = leftEdge->prev;
+
+		leftEdge->twin = prevEdge;
+		prevEdge->twin = leftEdge;
+
+		rightEdge->twin = beginEdge;
+		beginEdge->twin = rightEdge;
+
+		newFaces.PushBack(leftEdge->face);
+	}
+
+	qhFace* f = m_faceList.head;
+	while (f)
+	{
+		if (f->state == qhFace::e_invisible)
+		{
+			f = f->next;
+			continue;
+		}
+
+		// Partition conflict vertices.
+		qhVertex* v = f->conflictList.head;
+		while (v)
+		{
+			b3Vec3 p = v->position;
+
+			// Use tolerance and discard internal points.
+			float32 max = m_tolerance;
+			qhFace* iMax = nullptr;
+
+			for (u32 i = 0; i < newFaces.Count(); ++i)
+			{
+				qhFace* newFace = newFaces[i];
+				float32 d = b3Distance(p, newFace->plane);
+				if (d > max)
+				{
+					max = d;
+					iMax = newFace;
+				}
+			}
+
+			if (iMax)
+			{
+				qhVertex* v0 = v;
+				v->conflictFace = nullptr;
+				v = f->conflictList.Remove(v);
+				iMax->conflictList.PushFront(v0);
+				v0->conflictFace = iMax;
+			}
+			else
+			{
+				qhVertex* v0 = v;
+				v->conflictFace = nullptr;
+				v = f->conflictList.Remove(v);
+				FreeVertex(v0);
+			}
+		}
+
+		// Remove face half-edges.
+		qhHalfEdge* e = f->edge;
+		do
+		{
+			qhHalfEdge* e0 = e;
+			e = e->next;
+			FreeEdge(e0);
+		} while (e != f->edge);
+
+		// Remove face.
+		qhFace* f0 = f;
+		f = m_faceList.Remove(f);
+		FreeFace(f0);
+	}
+}
+
+bool qhHull::MergeFace(qhFace* rightFace)
+{
+	qhHalfEdge* e = rightFace->edge;
+
+	do
+	{
+		qhFace* leftFace = e->twin->face;
+
+		float32 d1 = b3Distance(leftFace->center, rightFace->plane);
+		float32 d2 = b3Distance(rightFace->center, leftFace->plane);
+
+		if (d1 < -m_tolerance && d2 < -m_tolerance)
+		{
+			// convex
+			e = e->next;
+		}
+		else
+		{
+			// concave or coplanar
+			if (leftFace == rightFace)
+			{
+				e = e->next;
+				continue;
+			}
+
+			// Move left vertices into right
+			qhVertex* v = leftFace->conflictList.head;
+			while (v)
+			{
+				qhVertex* v0 = v;
+				v = leftFace->conflictList.Remove(v);
+				rightFace->conflictList.PushFront(v0);
+				v0->conflictFace = rightFace;
+			}
+			
+			// set right face to reference a non-deleted edge
+			B3_ASSERT(e->face == rightFace);
+			rightFace->edge = e->prev;
+			
+			// absorb face
+			qhHalfEdge* te = e->twin;
+			do
+			{
+				te->face = rightFace;
+				te = te->next;
+			} while (te != e->twin);
+
+			// link edges
+			e->prev->next = e->twin->next;
+			e->next->prev = e->twin->prev;
+			e->twin->prev->next = e->next;
+			e->twin->next->prev = e->prev;
+
+			FreeEdge(e->twin);
+			FreeEdge(e);
+			m_faceList.Remove(leftFace);
+			FreeFace(leftFace);
+			
+			rightFace->ComputeCenterAndPlane();
+
+			{
+				qhHalfEdge* he = rightFace->edge;
+				do
+				{
+					B3_ASSERT(he->face == rightFace);
+					B3_ASSERT(he->twin->twin == he);
+					he = he->next;
+				} while (he != rightFace->edge);
+			}
+
+			return true;
+		}
+
+	} while (e != rightFace->edge);
+
+	return false;
+}
+
+void qhHull::MergeFaces(b3Array<qhFace*>& newFaces)
+{
+	for (u32 i = 0; i < newFaces.Count(); ++i)
+	{
+		qhFace* f = newFaces[i];
+		
+		if (f->state == qhFace::e_deleted) 
+		{
+			continue;
+		}
+
+		// todo
+		// Fix topological and geometrical errors.
+
+		// Merge the faces while there is no face left 
+		// to merge.
+		while (MergeFace(f));
+	}
+}
+
+bool qhHull::IsConsistent() const
+{
+	u32 count = 0;
+
+	qhFace* f = m_faceList.head;
+	while (f)
+	{
+		B3_ASSERT(f->state != qhFace::e_deleted);
+		qhHalfEdge* e = f->edge;
+		do
+		{
+			++count;
+			//B3_ASSERT(e->face == f);
+			B3_ASSERT(e->twin->twin == e);
+			B3_ASSERT(count < 10000);
+			e = e->next;
+		} while (e != f->edge);
+
+		f = f->next;
+	}
+
+	return true;
+}
+
+void qhHull::Draw(b3Draw* b3Draw) const
+{
+	qhFace* face = m_faceList.head;
+	while (face)
+	{
+		b3Vec3 c = face->center;
+		b3Vec3 n = face->plane.normal;
+
+		qhVertex* v = face->conflictList.head;
+		while (v)
+		{
+			b3Draw->DrawPoint(v->position, b3Color(1.0f, 1.0f, 0.0f));
+			b3Draw->DrawSegment(c, v->position, b3Color(1.0f, 1.0f, 0.0f));
+			v = v->next;
+		}
+
+		b3Draw->DrawSegment(c, c + n, b3Color(1.0f, 1.0f, 1.0f));
+
+		b3StackArray<b3Vec3, 32> polygon;
+		qhHalfEdge* edge = face->edge;
+		do
+		{
+			polygon.PushBack(edge->tail->position);
+			edge = edge->next;
+		} while (edge != face->edge);
+
+		b3Draw->DrawSolidPolygon(polygon.Elements(), polygon.Count(), b3Color(0.0f, 0.0f, 1.0f, 1.0f));
+		
+		face = face->next;
+	}
+}

src/testbed/framework/debug_draw.cpp

@@ -0,0 +1,801 @@
+/*
+* 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 <glad\glad.h>
+#include <gl\GLU.h>
+#include <imgui\imgui.h>
+#include <testbed\framework\debug_draw.h> 
+
+extern Camera g_camera;
+
+Mat44 Camera::BuildProjectionMatrix() const
+{
+	// Tangent of the half cone angle along the y-axis
+	float32 t = tan(0.5f * m_fovy);
+	float32 sy = 1.0f / t;
+	
+	// Set the x-scale equals to the y-scale and 
+	// proportional to the aspect ratio
+	float32 aspect = m_width / m_height;
+	float32 sx = 1.0f / (aspect * t);
+
+	float32 invRange = 1.0f / (m_zNear - m_zFar);
+	float32 sz = invRange * (m_zNear + m_zFar);
+	float32 tz = invRange *  m_zNear * m_zFar;
+
+	Mat44 xf;
+	xf.x.Set(sx, 0.0f, 0.0f, 0.0f);
+	xf.y.Set(0.0f, sy, 0.0f, 0.0f);
+	xf.z.Set(0.0f, 0.0f, sz, -1.0f);
+	xf.w.Set(0.0f, 0.0f, tz, 0.0f);
+
+	return xf;
+}
+
+b3Transform Camera::BuildWorldTransform() const
+{
+	b3Transform xf;
+	xf.rotation = b3ConvertQuatToRot(m_q);
+	xf.position = (m_zoom * xf.rotation.z) - m_center;
+	return xf;
+}
+
+Mat44 Camera::BuildWorldMatrix() const
+{
+	b3Transform xf = BuildWorldTransform();
+	return GetMat44(xf);
+}
+
+b3Transform Camera::BuildViewTransform() const
+{
+	b3Transform xf;
+	xf.rotation = b3ConvertQuatToRot(m_q);
+	xf.position = (m_zoom * xf.rotation.z) - m_center;
+	return b3Inverse(xf);
+}
+
+Mat44 Camera::BuildViewMatrix() const
+{
+	b3Transform xf = BuildViewTransform();
+	return GetMat44(xf);
+}
+
+b3Vec2 Camera::ConvertWorldToScreen(const b3Vec3& pw) const
+{
+	Mat44 xf1 = BuildWorldMatrix();
+	Mat44 xf2 = BuildProjectionMatrix();
+	Mat44 viewProj = xf2 * xf1;
+
+	Vec4 ph(pw.x, pw.y, pw.z, 1.0f);
+	Vec4 ppj = viewProj * ph;
+
+	b3Vec3 pn;
+	pn.x = ppj.x / ppj.w;
+	pn.y = ppj.y / ppj.w;
+	pn.z = ppj.z / ppj.w;
+
+	b3Vec2 ps;
+	ps.x = 0.5f * m_width * pn.x + (0.5f * m_width);
+	ps.y = -0.5f * m_height * pn.y + (0.5f * m_height);
+	return ps;
+}
+
+Ray3 Camera::ConvertScreenToWorld(const b3Vec2& ps) const
+{
+	// Essential Math, page 250.
+	float32 t = tan(0.5f * m_fovy);
+	float32 aspect = m_width / m_height;
+	
+	b3Vec3 pv;
+	pv.x = 2.0f * aspect * ps.x / m_width - aspect;
+	pv.y = -2.0f * ps.y / m_height + 1.0f;
+	pv.z = -1.0f / t;
+
+	b3Transform xf = BuildWorldTransform();
+	b3Vec3 pw = xf * pv;
+		
+	Ray3 rw;
+	rw.direction = b3Normalize(pw - xf.position);
+	rw.origin = xf.position;
+	rw.fraction = m_zFar;
+	return rw;
+}
+
+#define BUFFER_OFFSET(i) ((char*)NULL + (i))
+
+static void AssertGL()
+{
+	GLenum errorCode = glGetError();
+	if (errorCode != GL_NO_ERROR)
+	{
+		fprintf(stderr, "OpenGL error = %d\n", errorCode);
+		assert(false);
+	}
+}
+
+struct DrawPoints
+{
+	DrawPoints()
+	{
+		m_count = 0;
+	}
+
+	~DrawPoints()
+	{
+	}
+
+	void Add(const b3Vec3& center, float32 radius, const b3Color& color)
+	{
+		if (m_count == e_quadCapacity)
+		{
+			Submit();
+		}
+
+		m_quads[m_count].center = center;
+		m_quads[m_count].radius = radius;
+		m_quads[m_count].color = color;
+		++m_count;
+	}
+	
+	void Submit()
+	{
+		// Build local quads
+		b3Vec3 kVertices[6];
+		kVertices[0].Set(-1.0f, 1.0f, 0.0f);
+		kVertices[1].Set(-1.0f, -1.0f, 0.0f);
+		kVertices[2].Set(1.0f, 1.0f, 0.0f);
+		kVertices[3].Set(1.0f, 1.0f, 0.0f);
+		kVertices[4].Set(-1.0f, -1.0f, 0.0f);
+		kVertices[5].Set(1.0f, -1.0f, 0.0f);
+
+		Mat44 xf2 = g_camera.BuildProjectionMatrix();
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glMultMatrixf(&xf2.x.x);
+
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		b3Transform xf1 = g_camera.BuildViewTransform();
+		for (u32 i = 0; i < m_count; ++i)
+		{
+			const Quad* c = m_quads + i;
+			b3Color color = c->color;
+
+			// Put the center of the quads
+			// into the reference frame of the camera
+			// so they are rendered in the xy plane
+			b3Vec3 v1 = c->radius * kVertices[0] + xf1 * c->center;
+			b3Vec3 v2 = c->radius * kVertices[1] + xf1 * c->center;
+			b3Vec3 v3 = c->radius * kVertices[2] + xf1 * c->center;
+			b3Vec3 v4 = c->radius * kVertices[3] + xf1 * c->center;
+			b3Vec3 v5 = c->radius * kVertices[4] + xf1 * c->center;
+			b3Vec3 v6 = c->radius * kVertices[5] + xf1 * c->center;
+
+			glBegin(GL_TRIANGLES);
+			glColor4f(color.r, color.g, color.b, color.a);
+			glVertex3f(v1.x, v1.y, v1.z);
+			glVertex3f(v2.x, v2.y, v2.z);
+			glVertex3f(v3.x, v3.y, v3.z);
+			glEnd();
+
+			glBegin(GL_TRIANGLES);
+			glColor4f(color.r, color.g, color.b, color.a);
+			glVertex3f(v4.x, v4.y, v4.z);
+			glVertex3f(v5.x, v5.y, v5.z);
+			glVertex3f(v6.x, v6.y, v6.z);
+			glEnd();
+		}
+
+		m_count = 0;
+	}
+	
+	struct Quad
+	{
+		b3Vec3 center;
+		float32 radius;
+		b3Color color;
+	};
+
+	enum
+	{
+		e_quadCapacity = 512,
+		e_vertexCapacity = 6 * e_quadCapacity
+	};
+
+	Quad m_quads[e_quadCapacity];
+	u32 m_count;
+};
+
+struct DrawLines
+{
+	DrawLines()
+	{
+		m_count = 0;
+	}
+
+	~DrawLines()
+	{
+	}
+
+	void Add(const b3Vec3& A, const b3Vec3& B, const b3Color& color)
+	{
+		if (m_count == e_lineCapacity)
+		{
+			Submit();
+		}
+
+		m_lines[m_count].p = A;
+		m_lines[m_count].q = B;
+		m_lines[m_count].c = color;
+		++m_count;
+	}
+
+	void Submit()
+	{
+		Mat44 xf2 = g_camera.BuildProjectionMatrix();
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glMultMatrixf(&xf2.x.x);
+
+		Mat44 xf1 = g_camera.BuildViewMatrix();
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		glMultMatrixf(&xf1.x.x);
+				
+		for (u32 i = 0; i < m_count; ++i)
+		{
+			b3Vec3 p = m_lines[i].p;
+			b3Vec3 q = m_lines[i].q;
+			b3Color c = m_lines[i].c;
+
+			glBegin(GL_LINES);
+			glColor4f(c.r, c.g, c.b, c.a);
+			glVertex3f(p.x, p.y, p.z);
+			glVertex3f(q.x, q.y, q.z);
+			glEnd();
+		}		
+
+		m_count = 0;
+	}
+
+	enum
+	{
+		e_lineCapacity = 512,
+		e_vertexCapacity = 2 * e_lineCapacity
+	};
+
+	struct Line
+	{
+		b3Vec3 p;
+		b3Vec3 q;
+		b3Color c;
+	};
+
+	Line m_lines[e_vertexCapacity];
+	u32 m_count;
+};
+
+struct DrawTriangles
+{
+	DrawTriangles()
+	{
+		m_count = 0;
+	}
+
+	~DrawTriangles()
+	{
+	}
+
+	void Add(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Color& color)
+	{
+		if (m_count == e_triangleCapacity)
+		{
+			Submit();
+		}
+
+		m_triangles[m_count].col = color;
+		m_triangles[m_count].a = A;
+		m_triangles[m_count].b = B;
+		m_triangles[m_count].c = C;
+
+		++m_count;
+	}
+
+	void Submit()
+	{
+		Mat44 xf2 = g_camera.BuildProjectionMatrix();
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glMultMatrixf(&xf2.x.x);
+
+		Mat44 xf1 = g_camera.BuildViewMatrix();
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		glMultMatrixf(&xf1.x.x);
+
+		for (u32 i = 0; i < m_count; ++i)
+		{
+			b3Vec3 a = m_triangles[i].a;
+			b3Vec3 b = m_triangles[i].b;
+			b3Vec3 c = m_triangles[i].c;
+			b3Color col = m_triangles[i].col;
+			
+			glBegin(GL_TRIANGLES);
+			glColor4f(col.r, col.g, col.b, col.a);
+			glVertex3f(a.x, a.y, a.z);
+			glVertex3f(b.x, b.y, b.z);
+			glVertex3f(c.x, c.y, c.z);
+			glEnd();
+		}
+
+		m_count = 0;
+	}
+
+	struct Triangle
+	{
+		b3Color col;
+		b3Vec3 a, b, c;
+	};
+
+	enum
+	{
+		e_triangleCapacity = 512,
+		e_vertexCapacity = 3 * e_triangleCapacity
+	};
+
+	Triangle m_triangles[e_triangleCapacity];
+	u32 m_count;
+};
+
+struct DrawShapes
+{
+	DrawShapes()
+	{
+		m_sphere = gluNewQuadric();
+		m_cylinder = gluNewQuadric();
+	}
+
+	~DrawShapes()
+	{
+		gluDeleteQuadric(m_sphere);
+		gluDeleteQuadric(m_cylinder);
+	}
+
+	void DrawSphere(const b3SphereShape* s, const b3Transform& xf)
+	{
+		float32 radius = s->m_radius;
+		Mat44 xf4 = GetMat44(xf);
+		
+		glMatrixMode(GL_MODELVIEW);
+		glPushMatrix();
+		glMultMatrixf(&xf4.x.x);
+		gluSphere(m_sphere, radius, 10, 10);
+		glPopMatrix();
+	}
+	
+	void DrawCapsule(const b3CapsuleShape* s, const b3Transform& xf)
+	{
+		b3Vec3 c1 = s->m_centers[0];
+		b3Vec3 c2 = s->m_centers[1];
+		float32 radius = s->m_radius;
+		float32 height = b3Length(c1 - c2);
+		b3Vec3 n1 = (1.0f / height) * (c1 - c2);
+		b3Vec3 n2 = -n1;
+
+		{
+			b3Transform xfc;
+			xfc.rotation = xf.rotation;
+			xfc.position = xf * c1;
+			
+			Mat44 m = GetMat44(xfc);
+
+			glMatrixMode(GL_MODELVIEW);
+			glPushMatrix();			
+			glMultMatrixf(&m.x.x);
+
+			GLdouble plane[4];
+			plane[0] = n1.x;
+			plane[1] = n1.y;
+			plane[2] = n1.z;
+			plane[3] = 0.05f;
+			glClipPlane(GL_CLIP_PLANE0, plane);
+
+			glEnable(GL_CLIP_PLANE0);
+			gluSphere(m_sphere, radius, 10, 10);
+			glDisable(GL_CLIP_PLANE0);
+			
+			glPopMatrix();
+		}
+
+		{
+			b3Transform xfc;
+			xfc.rotation = xf.rotation;
+			xfc.position = xf * c2;
+			
+			Mat44 m = GetMat44(xfc);
+			
+			glMatrixMode(GL_MODELVIEW);
+			glPushMatrix();
+			glMultMatrixf(&m.x.x);
+
+			GLdouble plane[4];
+			plane[0] = n2.x;
+			plane[1] = n2.y;
+			plane[2] = n2.z;
+			plane[3] = 0.05f;
+			glClipPlane(GL_CLIP_PLANE0, plane);
+
+			glEnable(GL_CLIP_PLANE0);
+			gluSphere(m_sphere, radius, 10, 10);
+			glDisable(GL_CLIP_PLANE0);
+
+			glPopMatrix();
+		}
+
+		{
+			Mat44 m = GetMat44(xf);
+
+			glMatrixMode(GL_MODELVIEW);
+			glPushMatrix();
+			glMultMatrixf(&m.x.x);
+			glTranslatef(0.0f, 0.5f * height, 0.0f);
+			glRotatef(90.0f, 1.0f, 0.0f, 0.0f);
+			gluCylinder(m_cylinder, 0.96f * radius, 0.96f * radius, height, 20, 10);
+			glPopMatrix();
+		}
+	}
+
+	void DrawHull(const b3HullShape* s, const b3Transform& xf)
+	{
+		const b3Hull* hull = s->m_hull;
+		for (u32 i = 0; i < hull->faceCount; ++i)
+		{
+			b3Vec3 n = xf.rotation * hull->planes[i].normal;
+			
+			glBegin(GL_POLYGON);
+			glNormal3f(n.x, n.y, n.z);
+
+			const b3Face* face = hull->GetFace(i);
+			const b3HalfEdge* begin = hull->GetEdge(face->edge);
+			const b3HalfEdge* edge = begin;
+			do
+			{
+				b3Vec3 v = xf * hull->GetVertex(edge->origin);
+				
+				glVertex3f(v.x, v.y, v.z);
+				
+				edge = hull->GetEdge(edge->next);
+			} while (edge != begin);
+			
+			glEnd();
+		}
+	}
+
+	void DrawMesh(const b3MeshShape* s, const b3Transform& xf)
+	{
+		const b3Mesh* mesh = s->m_mesh;
+		for (u32 i = 0; i < mesh->triangleCount; ++i)
+		{
+			const b3Triangle* triangle = mesh->triangles + i;
+
+			b3Vec3 v1 = xf * mesh->vertices[triangle->v1];
+			b3Vec3 v2 = xf * mesh->vertices[triangle->v2];
+			b3Vec3 v3 = xf * mesh->vertices[triangle->v3];
+			
+			b3Vec3 n = b3Cross(v2 - v1, v3 - v1);
+			n.Normalize();
+
+			glBegin(GL_TRIANGLES);
+			glNormal3f(n.x, n.y, n.z);
+
+			glVertex3f(v1.x, v1.y, v1.z);			
+			glVertex3f(v2.x, v2.y, v2.z);
+			glVertex3f(v3.x, v3.y, v3.z);
+			
+			glEnd();
+		}		
+	}
+
+	void DrawShape(const b3Shape* s, const b3Transform& xf)
+	{
+		switch (s->GetType())
+		{
+		case e_sphereShape:
+		{
+			DrawSphere((b3SphereShape*)s, xf);
+			break;
+		}
+		case e_capsuleShape:
+		{
+			DrawCapsule((b3CapsuleShape*)s, xf);
+			break;
+		}
+		case e_hullShape:
+		{
+			DrawHull((b3HullShape*)s, xf);
+			break;
+		}
+		case e_meshShape:
+		{
+			DrawMesh((b3MeshShape*)s, xf);
+			break;
+		}
+		default:
+		{
+			break;
+		}
+		}
+	}
+
+	void Draw(const b3World& world)
+	{
+		Mat44 xf1 = g_camera.BuildViewMatrix();
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		glMultMatrixf(&xf1.x.x);
+
+		Mat44 xf2 = g_camera.BuildProjectionMatrix();
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glMultMatrixf(&xf2.x.x);
+
+		glEnable(GL_LIGHTING);
+		glEnable(GL_LIGHT0);
+
+		float light_ambient[4] = { 0.5f, 0.5f, 0.5f, 1.0f };
+		glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
+		float light_diffuse[4] = { 0.5f, 0.5f, 0.5f, 1.0f };
+		glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
+		float light_position[4] = { 0.0f, 0.0f, 1.0f, 0.0f };
+		glLightfv(GL_LIGHT0, GL_POSITION, light_position);
+
+		b3Body* b = world.GetBodyList().m_head;
+		while (b)
+		{
+			b3Color fillColor;
+			if (b->IsAwake() == false)
+			{
+				fillColor = b3Color(0.5f, 0.25f, 0.25f, 1.0f);
+			}
+			else if (b->GetType() == e_staticBody)
+			{
+				fillColor = b3Color(0.5f, 0.5f, 0.5f, 1.0f);
+			}
+			else if (b->GetType() == e_dynamicBody)
+			{
+				fillColor = b3Color(1.0f, 0.5f, 0.5f, 1.0f);
+			}
+			else
+			{
+				fillColor = b3Color(0.5f, 0.5f, 1.0f, 1.0f);
+			}
+
+			glEnable(GL_COLOR_MATERIAL);
+			glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
+			glColor4fv(&fillColor.r);
+			
+			const b3Transform& xf = b->GetTransform();
+			b3Shape* s = b->GetShapeList().m_head;
+			while (s)
+			{
+				DrawShape(s, xf);	
+				s = s->m_next;
+			}
+
+			glDisable(GL_COLOR_MATERIAL);
+
+			b = b->GetNext();
+		}
+		
+		glDisable(GL_LIGHT0);
+		glDisable(GL_LIGHTING);
+	}
+
+	GLUquadricObj* m_sphere;
+	GLUquadricObj* m_cylinder;
+};
+
+DebugDraw::DebugDraw()
+{
+	m_points = new DrawPoints();
+	m_lines = new DrawLines();
+	m_triangles = new DrawTriangles();
+	m_shapes = new DrawShapes();
+}
+
+DebugDraw::~DebugDraw()
+{
+	delete m_points;
+	delete m_lines;
+	delete m_triangles;
+	delete m_shapes;
+}
+
+void DebugDraw::DrawPoint(const b3Vec3& p, const b3Color& color)
+{
+	m_points->Add(p, 0.05f, color);
+}
+
+void DebugDraw::DrawSegment(const b3Vec3& a, const b3Vec3& b, const b3Color& color)
+{
+	m_lines->Add(a, b, color);
+}
+
+void DebugDraw::DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color)
+{
+	b3Vec3 p1 = vertices[count - 1];
+	for (u32 i = 0; i < count; ++i)
+	{
+		b3Vec3 p2 = vertices[i];
+		m_lines->Add(p1, p2, color);
+		p1 = p2;
+	}
+}
+
+void DebugDraw::DrawSolidPolygon(const b3Vec3* vertices, u32 count, const b3Color& color)
+{
+	b3Vec3 p1 = vertices[0];
+	for (u32 i = 1; i < count - 1; ++i)
+	{
+		b3Vec3 p2 = vertices[i];
+		b3Vec3 p3 = vertices[i + 1];
+		m_triangles->Add(p1, p2, p3, color);
+	}
+
+	b3Color frameColor(0.0f, 0.0f, 0.0f, 1.0f);
+	DrawPolygon(vertices, count, frameColor);
+}
+
+void DebugDraw::DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+{
+	u32 kEdgeCount = 20;
+	float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
+	float32 cosInc = cos(kAngleInc);
+	float32 sinInc = sin(kAngleInc);
+	float32 tInc = 1.0f - cosInc;
+	
+	b3Vec3 n1 = b3Perp(normal);
+	b3Vec3 v1 = center + radius * n1;
+	for (u32 i = 0; i < kEdgeCount; ++i)
+	{
+		// Rodrigues' rotation formula
+		b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
+		b3Vec3 v2 = center + radius * n2;
+
+		m_lines->Add(v1, v2, color);
+
+		n1 = n2;
+		v1 = v2;
+	}
+}
+
+void DebugDraw::DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color)
+{
+	u32 kEdgeCount = 20;
+	float32 kAngleInc = 2.0f * B3_PI / float32(kEdgeCount);
+	float32 cosInc = cos(kAngleInc);
+	float32 sinInc = sin(kAngleInc);
+	float32 tInc = 1.0f - cosInc;
+
+	b3Vec3 n1 = b3Perp(normal);
+	b3Vec3 v1 = center + radius * n1;
+	for (u32 i = 0; i < kEdgeCount; ++i)
+	{
+		// Rodrigues' rotation formula
+		b3Vec3 n2 = cosInc * n1 + sinInc * b3Cross(normal, n1) + tInc * b3Dot(normal, n1) * normal;
+		b3Vec3 v2 = center + radius * n2;
+
+		m_triangles->Add(center, v1, v2, color);
+
+		n1 = n2;
+		v1 = v2;
+	}
+
+	b3Color frameColor(0.0f, 0.0f, 0.0f);
+	DrawCircle(normal, center, radius, frameColor);
+}
+
+void DebugDraw::DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color)
+{
+}
+
+void DebugDraw::DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color)
+{
+}
+
+void DebugDraw::DrawTransform(const b3Transform& xf)
+{
+	float32 lenght = 1.0f;
+
+	b3Color red(1.0f, 0.0f, 0.0f, 1.0f);
+	b3Color green(0.0f, 1.0f, 0.0f, 1.0f);
+	b3Color blue(0.0f, 0.0f, 1.0f, 1.0f);
+
+	b3Vec3 position = xf.position;
+	b3Mat33 rotation = xf.rotation;
+
+	b3Vec3 A = position + lenght * rotation.x;
+	b3Vec3 B = position + lenght * rotation.y;
+	b3Vec3 C = position + lenght * rotation.z;
+
+	DrawSegment(position, A, red);
+	DrawSegment(position, B, green);
+	DrawSegment(position, C, blue);
+}
+
+void DebugDraw::DrawAABB(const b3AABB3& aabb, const b3Color& color)
+{
+	b3Vec3 lower = aabb.m_lower;
+	b3Vec3 upper = aabb.m_upper;
+
+	b3Vec3 vs[8];
+	
+	// Face 1
+	vs[0] = lower;
+	vs[1] = b3Vec3(lower.x, upper.y, lower.z);
+	vs[2] = b3Vec3(upper.x, upper.y, lower.z);
+	vs[3] = b3Vec3(upper.x, lower.y, lower.z);
+	
+	// Face 2
+	vs[4] = upper;
+	vs[5] = b3Vec3(upper.x, lower.y, upper.z);
+	vs[6] = b3Vec3(lower.x, lower.y, upper.z);
+	vs[7] = b3Vec3(lower.x, upper.y, upper.z);
+
+	// Face 1 edges
+	DrawSegment(vs[0], vs[1], color);
+	DrawSegment(vs[1], vs[2], color);
+	DrawSegment(vs[2], vs[3], color);
+	DrawSegment(vs[3], vs[0], color);
+	
+	// Face 2 edges
+	DrawSegment(vs[4], vs[5], color);
+	DrawSegment(vs[5], vs[6], color);
+	DrawSegment(vs[6], vs[7], color);
+	DrawSegment(vs[7], vs[4], color);
+	
+	// Face 3 edges
+	DrawSegment(vs[2], vs[4], color);
+	DrawSegment(vs[5], vs[3], color);
+	
+	// Face 4 edges
+	DrawSegment(vs[6], vs[0], color);
+	DrawSegment(vs[1], vs[7], color);
+}
+
+void DebugDraw::DrawString(const char* text, const b3Color& color, ...)
+{
+	va_list arg;
+	va_start(arg, text);
+	ImGui::Begin("Log", NULL, ImVec2(0, 0), 0.0f, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
+	ImGui::TextColoredV(ImVec4(color.r, color.g, color.b, color.a), text, arg);
+	ImGui::End();
+	va_end(arg);
+}
+
+void DebugDraw::Submit(const b3World& world)
+{
+	m_shapes->Draw(world);
+}
+
+void DebugDraw::Submit()
+{
+	m_triangles->Submit();
+	m_lines->Submit();
+	m_points->Submit();
+}

src/testbed/framework/main.cpp

@@ -0,0 +1,471 @@
+/*
+* 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 <glad\glad.h>
+#include <glfw\glfw3.h>
+#include <imgui\imgui.h>
+#include <imgui\imgui_impl_glfw_gl3.h>
+#include <testbed\tests\test.h>
+
+GLFWwindow* g_window;
+Settings g_settings;
+Test* g_test;
+Camera g_camera;
+DebugDraw* g_debugDraw;
+bool g_leftDown;
+bool g_rightDown;
+bool g_altDown;
+b3Vec2 g_ps0;
+
+void WindowSize(int w, int h)
+{
+	g_camera.m_width = float32(w);
+	g_camera.m_height = float32(h);
+}
+
+void MouseMove(GLFWwindow* w, double x, double y)
+{
+	b3Vec2 p;
+	p.Set(float32(x), float32(y));
+
+	b3Vec2 dp = p - g_ps0;
+	g_ps0 = p;
+
+	Ray3 pw = g_camera.ConvertScreenToWorld(p);
+
+	float32 nx = b3Clamp(dp.x, -1.0f, 1.0f);
+	float32 ny = b3Clamp(dp.y, -1.0f, 1.0f);
+
+	if (g_altDown)
+	{
+		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;
+
+			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.Normalize();
+		}
+
+		if (g_rightDown)
+		{
+			b3Transform xf = g_camera.BuildWorldTransform();
+			g_camera.m_center += 0.2f * nx * xf.rotation.x;
+			g_camera.m_center += 0.2f * -ny * xf.rotation.y;
+		}
+	}
+	else
+	{
+		g_test->MouseMove(pw);
+	}
+}
+
+void MouseWheel(GLFWwindow* w, double dx, double dy)
+{
+	float32 n = b3Clamp(float32(dy), -1.0f, 1.0f);
+	if (g_altDown)
+	{
+		g_camera.m_zoom += 0.5f * -n;
+	}
+}
+
+void MouseButton(GLFWwindow* w, int button, int action, int mods)
+{
+	double x, y;
+	glfwGetCursorPos(w, &x, &y);
+	b3Vec2 p;
+	p.Set(float32(x), float32(y));
+
+	Ray3 pw = g_camera.ConvertScreenToWorld(p);
+
+	switch (action)
+	{
+	case GLFW_PRESS:
+	{
+		if (button == GLFW_MOUSE_BUTTON_LEFT)
+		{
+			g_leftDown = true;
+
+			if (g_altDown == false)
+			{
+				g_test->MouseLeftDown(pw);
+			}
+		}
+		
+		if (button == GLFW_MOUSE_BUTTON_RIGHT)
+		{
+			g_rightDown = true;
+		}
+
+		break;
+	}
+	case GLFW_RELEASE:
+	{
+		if (button == GLFW_MOUSE_BUTTON_LEFT)
+		{
+			g_leftDown = false;
+
+			if (g_altDown == false)
+			{
+				g_test->MouseLeftUp(pw);
+			}
+		}
+
+		if (button == GLFW_MOUSE_BUTTON_RIGHT)
+		{
+			g_rightDown = false;
+		}
+		break;
+	}
+	default:
+	{
+		break;
+	}
+	}
+}
+
+void KeyButton(GLFWwindow* w, int button, int scancode, int action, int mods)
+{
+	switch (action)
+	{
+	case GLFW_PRESS:
+	{
+		if (button == GLFW_KEY_LEFT_ALT)
+		{
+			g_altDown = true;
+			g_test->KeyDown(button);
+		}
+
+		if (g_altDown)
+		{
+			if (button == GLFW_KEY_DOWN)
+			{
+				g_camera.m_zoom += 0.05f;
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				g_camera.m_zoom -= 0.05f;
+			}
+
+			if (button == GLFW_KEY_R)
+			{
+				g_settings.lastTestID = -1;
+			}
+		}
+		else
+		{
+			g_test->KeyDown(button);
+		}
+		
+		break;
+	}
+	case GLFW_RELEASE:
+	{
+		if (button == GLFW_KEY_LEFT_ALT)
+		{
+			g_altDown = false;
+		}
+
+		if (g_altDown == false)
+		{
+			g_test->KeyUp(button);
+		}
+
+		break;
+	}
+	default:
+	{
+		break;
+	}
+	}
+}
+
+void Char(GLFWwindow* w, unsigned int codepoint)
+{
+	ImGui_ImplGlfwGL3_CharCallback(w, codepoint);
+}
+
+void CreateInterface()
+{
+	ImGui_ImplGlfwGL3_Init(g_window, false);
+	ImGuiIO& io = ImGui::GetIO();
+	io.Fonts[0].AddFontDefault();
+
+	ImGuiStyle& style = ImGui::GetStyle();
+	style.FrameRounding = style.GrabRounding = style.ScrollbarRounding = 2.0f;
+	style.FramePadding = ImVec2(4, 2);
+	style.DisplayWindowPadding = ImVec2(0, 0);
+	style.DisplaySafeAreaPadding = ImVec2(0, 0);
+}
+
+void DestroyInterface()
+{
+	ImGui_ImplGlfwGL3_Shutdown();
+}
+
+bool GetTestName(void*, int idx, const char** name)
+{
+	*name = g_tests[idx].name;
+	return true;
+}
+
+void Interface()
+{
+	ImGui::SetNextWindowPos(ImVec2(g_camera.m_width - 250.0f, 0.0f));
+	ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height));
+	ImGui::Begin("Controls", NULL, ImVec2(0.0f, 0.0f), 0.25f, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse);
+
+	ImGui::PushItemWidth(-1.0f);
+
+	ImGui::Text("Test");
+	if (ImGui::Combo("##Test", &g_settings.testID, GetTestName, NULL, e_testCount, e_testCount))
+	{
+		delete g_test;
+		g_test = g_tests[g_settings.testID].create();
+		g_settings.lastTestID = -1;
+	}
+
+	ImVec2 buttonSize = ImVec2(-1, 0);
+	if (ImGui::Button("Restart (R)", buttonSize))
+	{
+		g_settings.lastTestID = -1;
+	}
+	if (ImGui::Button("Previous", buttonSize))
+	{
+		g_settings.testID = b3Clamp(g_settings.testID - 1, 0, int(e_testCount) - 1);
+		g_settings.lastTestID = -1;
+	}
+	if (ImGui::Button("Next", buttonSize))
+	{
+		g_settings.testID = b3Clamp(g_settings.testID + 1, 0, int(e_testCount) - 1);
+		g_settings.lastTestID = -1;
+	}
+	if (ImGui::Button("Exit", buttonSize))
+	{
+		glfwSetWindowShouldClose(g_window, true);
+	}
+	
+	ImGui::Separator();
+
+	ImGui::Text("Step");
+	
+	ImGui::Text("Hertz");
+	ImGui::SliderFloat("##Hertz", &g_settings.hertz, 0.0f, 240.0f, "%.4f");
+	ImGui::Text("Velocity Iterations");
+	ImGui::SliderInt("##Velocity Iterations", &g_settings.velocityIterations, 0, 50);
+	ImGui::Text("Position Iterations");
+	ImGui::SliderInt("#Position Iterations", &g_settings.positionIterations, 0, 50);
+	ImGui::Checkbox("Warm Start", &g_settings.warmStart);
+	ImGui::Checkbox("Sleep", &g_settings.sleep);
+	//ImGui::Checkbox("Convex Cache", &g_settings.convexCache);
+
+	if (ImGui::Button("Play/Pause", buttonSize))
+	{
+		g_settings.pause = !g_settings.pause;
+	}
+	if (ImGui::Button("Single Step", buttonSize))
+	{
+		g_settings.pause = true;
+		g_settings.singleStep = true;
+	}
+
+	ImGui::PopItemWidth();
+
+	ImGui::Separator();
+
+	ImGui::Text("View");
+	ImGui::Checkbox("Grid", &g_settings.drawGrid);
+	ImGui::Checkbox("Polygons", &g_settings.drawShapes);
+	ImGui::Checkbox("Center of Masses", &g_settings.drawCenterOfMasses);
+	ImGui::Checkbox("Bounding Boxes", &g_settings.drawBounds);
+	ImGui::Checkbox("Joints", &g_settings.drawJoints);
+	ImGui::Checkbox("Contact Points", &g_settings.drawContactPoints);
+	ImGui::Checkbox("Contact Normals", &g_settings.drawContactNormals);
+	ImGui::Checkbox("Contact Tangents", &g_settings.drawContactTangents);
+	ImGui::Checkbox("Statistics", &g_settings.drawStats);
+	ImGui::Checkbox("Profile", &g_settings.drawProfile);
+	
+	ImGui::End();
+}
+
+void Step()
+{
+	if (g_settings.testID != g_settings.lastTestID)
+	{
+		delete g_test;
+		g_settings.lastTestID = g_settings.testID;
+		g_test = g_tests[g_settings.testID].create();
+		g_settings.pause = true;
+	}
+	g_test->Step();
+}
+
+void Run()
+{
+	glFrontFace(GL_CCW);
+	glCullFace(GL_BACK);
+	glEnable(GL_CULL_FACE);
+	glEnable(GL_DEPTH_TEST);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	glEnable(GL_BLEND);
+
+	glClearColor(0.3f, 0.3f, 0.3f, 1.0f);
+	glClearDepth(1.0f);
+
+	while (glfwWindowShouldClose(g_window) == 0)
+	{
+		int width, height;
+		glfwGetWindowSize(g_window, &width, &height);
+		g_camera.m_width = float32(width);
+		g_camera.m_height = float32(height);
+
+		glViewport(0, 0, width, height);
+		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+		
+		ImGui_ImplGlfwGL3_NewFrame();
+		
+		Step();
+
+		if (g_settings.drawShapes)
+		{
+			g_debugDraw->Submit(g_test->m_world);
+		}
+		
+		if (g_settings.drawGrid)
+		{
+			u32 n = 20;
+
+			b3Vec3 t;
+			t.x = -0.5f * float32(n);
+			t.y = 0.0f;
+			t.z = -0.5f * float32(n);
+
+			b3Color color(1.0f, 1.0f, 1.0f, 1.0f);
+			for (int i = 0; i < n; i += 1)
+			{
+				for (int j = 0; j < n; j += 1)
+				{
+					b3Vec3 vs[4];
+					vs[0] = b3Vec3((float)i, 0.0f, (float)j);
+					vs[1] = b3Vec3((float)i, 0.0f, (float)j + 1);
+					vs[2] = b3Vec3((float)i + 1, 0.0f, (float)j + 1);
+					vs[3] = b3Vec3((float)i + 1, 0.0f, (float)j);
+
+					for (u32 k = 0; k < 4; ++k)
+					{
+						vs[k] += t;
+					}
+
+					g_debugDraw->DrawPolygon(vs, 4, color);
+				}
+			}
+
+			b3Color color2(0.0f, 0.0f, 0.0f);
+
+			{
+				b3Vec3 p1(t.x, 0.005f, 0.0f);
+				b3Vec3 p2(-t.x, 0.005f, 0.0f);
+
+				g_debugDraw->DrawSegment(p1, p2, color2);
+			}
+
+			{
+				b3Vec3 p1(0.0f, 0.005f, t.x);
+				b3Vec3 p2(0.0f, 0.005f, -t.x);
+
+				g_debugDraw->DrawSegment(p1, p2, color2);
+			}
+		}
+
+		g_debugDraw->Submit();
+
+		Interface();
+
+		ImGui::Render();
+
+		glfwSwapBuffers(g_window);
+
+		glfwPollEvents();
+	}
+}
+
+int main(int argc, char** args)
+{	
+	// Create g_window
+	if (glfwInit() == 0)
+	{
+		fprintf(stderr, "Failed to initialize GLFW\n");
+		return -1;
+	}
+
+	extern b3Version b3_version;
+	char title[256];
+	sprintf_s(title, "Bounce Testbed Version %d.%d.%d", b3_version.major, b3_version.minor, b3_version.revision);
+
+	g_window = glfwCreateWindow(1024, 768, title, NULL, NULL);
+
+	g_leftDown = false;
+	g_rightDown = false;
+	g_altDown = false;
+	g_ps0.SetZero();
+
+	glfwMakeContextCurrent(g_window);	
+	glfwSwapInterval(1);
+	glfwSetCursorPosCallback(g_window, MouseMove);
+	glfwSetScrollCallback(g_window, MouseWheel);
+	glfwSetMouseButtonCallback(g_window, MouseButton);
+	glfwSetKeyCallback(g_window, KeyButton);
+	glfwSetCharCallback(g_window, Char);
+
+	if (gladLoadGL() == 0)
+	{
+		fprintf(stderr, "Error: %d\n", glad_glGetError());
+		exit(EXIT_FAILURE);
+	}
+
+	// Create UI
+	CreateInterface();
+
+	// Create renderer
+	g_debugDraw = new DebugDraw();
+
+	// Run the g_tests
+	g_test = nullptr;
+	Run();
+
+	if (g_test)
+	{
+		delete g_test;
+		g_test = nullptr;
+	}
+
+	// Destroy renderer
+	delete g_debugDraw;
+
+	// Destroy UI
+	DestroyInterface();
+
+	// Destroy g_window
+	glfwTerminate();
+}

src/testbed/framework/test.cpp

@@ -0,0 +1,441 @@
+/*
+* 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 <testbed\tests\test.h>
+
+extern u32 b3_allocCalls, b3_maxAllocCalls;
+extern u32 b3_gjkCalls, b3_gjkIters, b3_gjkMaxIters;
+extern u32 b3_convexCalls, b3_convexCacheHits;
+extern bool b3_enableConvexCache;
+
+extern Settings g_settings;
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+
+Test::Test()
+{
+	b3_allocCalls = 0;
+	b3_gjkCalls = 0;
+	b3_gjkIters = 0;
+	b3_gjkMaxIters = 0;
+	b3_convexCalls = 0;
+	b3_convexCacheHits = 0;
+	b3_enableConvexCache = g_settings.convexCache;
+
+	m_world.SetDebugDraw(g_debugDraw);
+	m_world.SetContactListener(this);
+
+	memset(&m_profile, 0, sizeof(b3Profile));
+	memset(&m_maxProfile, 0, sizeof(b3Profile));
+
+	g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.15f * B3_PI);
+	g_camera.m_q = g_camera.m_q * b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.15f * B3_PI);
+	g_camera.m_zoom = 50.0f;
+	g_camera.m_center.SetZero();
+	g_settings.drawGrid = false;
+
+	m_rayHit.m_shape = nullptr;
+	m_mouseJoint = nullptr;
+	m_groundBody = nullptr;
+
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(50.0f, 1.0f, 50.0f);
+		m_groundHull.SetTransform(xf);
+	}
+
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(1.0f, 1.0f, 1.0f);
+		m_boxHull.SetTransform(xf);
+	}
+
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(1.0f, 5.0f, 1.0f);
+		m_tallHull.SetTransform(xf);
+	}
+
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(2.0f, 4.0f, 0.5f);
+		m_doorHull.SetTransform(xf);
+	}
+	
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(25.0f, 0.5f, 25.0f);
+		m_rampHull.SetTransform(xf);
+	}
+
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(1.0f, 0.5f, 3.0f);
+		m_plankHull.SetTransform(xf);
+	}
+	
+	{
+		b3Transform xf;
+		xf.position.SetZero();
+		xf.rotation = b3Diagonal(4.05f, 2.0f * B3_LINEAR_SLOP, 4.05f);
+		m_thinHull.SetTransform(xf);
+	}
+
+	{
+		const u32 w = 100;
+		const u32 h = 100;
+
+		b3Vec3 t;
+		t.x = -0.5f * float32(w);
+		t.y = 0.0f;
+		t.z = -0.5f * float32(h);
+
+		b3Mesh* mesh = m_meshes + 0;
+
+		mesh->vertexCount = w * h;
+		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
+
+		for (u32 i = 0; i < w; ++i)
+		{
+			for (u32 j = 0; j < h; ++j)
+			{
+				u32 v1 = i * w + j;
+
+				b3Vec3 v;
+				v.x = float32(i);
+				v.y = 0.0f;
+				v.z = float32(j);
+
+				v += t;
+
+				mesh->vertices[v1] = v;
+			}
+		}
+
+		// 2 triangles per quad
+		mesh->triangleCount = 2 * (w - 1) * (h - 1);
+		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));
+
+		u32 triangleCount = 0;
+		for (u32 i = 0; i < w - 1; ++i)
+		{
+			for (u32 j = 0; j < h - 1; ++j)
+			{
+				u32 v1 = i * w + j;
+				u32 v2 = (i + 1) * w + j;
+				u32 v3 = (i + 1) * w + (j + 1);
+				u32 v4 = i * w + (j + 1);
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t1 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t1->v1 = v3;
+				t1->v2 = v2;
+				t1->v3 = v1;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t2 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t2->v1 = v1;
+				t2->v2 = v4;
+				t2->v3 = v3;
+			}
+		}
+
+		B3_ASSERT(triangleCount == mesh->triangleCount);
+
+		mesh->BuildTree();
+	}
+	
+	{
+		const u32 w = 100;
+		const u32 h = 100;
+
+		b3Vec3 t;
+		t.x = -0.5f * float32(w);
+		t.y = 0.0f;
+		t.z = -0.5f * float32(h);
+
+		b3Mesh* mesh = m_meshes + 1;
+		
+		mesh->vertexCount = w * h;
+		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
+		
+		for (u32 i = 0; i < w; ++i)
+		{
+			for (u32 j = 0; j < h; ++j)
+			{
+				u32 v1 = i * w + j;
+				
+				b3Vec3 v;
+				v.x = float32(i);
+				v.y = RandomFloat(0.0f, 0.5f);
+				v.z = float32(j);
+
+				v += t;
+
+				mesh->vertices[v1] = v;
+			}
+		}
+
+		// 2 triangles per quad
+		mesh->triangleCount = 2 * (w - 1) * (h - 1);
+		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));
+
+		u32 triangleCount = 0;
+		for (u32 i = 0; i < w - 1; ++i)
+		{
+			for (u32 j = 0; j < h - 1; ++j)
+			{
+				u32 v1 = i * w + j;
+				u32 v2 = (i + 1) * w + j;
+				u32 v3 = (i + 1) * w + (j + 1);
+				u32 v4 = i * w + (j + 1);
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t1 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t1->v1 = v3;
+				t1->v2 = v2;
+				t1->v3 = v1;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t2 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t2->v1 = v1;
+				t2->v2 = v4;
+				t2->v3 = v3;
+			}
+		}
+
+		B3_ASSERT(triangleCount == mesh->triangleCount);
+
+		mesh->BuildTree();
+	}
+}
+
+Test::~Test()
+{
+	for (u32 i = 0; i < e_maxMeshes; ++i)
+	{
+		b3Free(m_meshes[i].vertices);
+		b3Free(m_meshes[i].triangles);
+	}
+}
+
+void Test::BeginContact(b3Contact* contact)
+{
+}
+
+void Test::EndContact(b3Contact* contact)
+{
+
+}
+
+void Test::PreSolve(b3Contact* contact)
+{
+
+}
+
+void Test::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;
+		}
+	}
+
+	b3_allocCalls = 0;
+	b3_gjkCalls = 0;
+	b3_gjkIters = 0;
+	b3_gjkMaxIters = 0;
+	b3_convexCalls = 0;
+	b3_convexCacheHits = 0;
+	b3_enableConvexCache = g_settings.convexCache;
+
+	// Step
+	m_world.SetSleeping(g_settings.sleep);
+	m_world.SetWarmStart(g_settings.warmStart);
+	m_world.Step(dt, g_settings.velocityIterations, g_settings.positionIterations);
+
+	m_profile = m_world.GetProfile();
+
+	m_maxProfile.total = b3Max(m_maxProfile.total, m_profile.total);
+	m_maxProfile.collide.broadphase = b3Max(m_maxProfile.collide.broadphase, m_profile.collide.broadphase);
+	m_maxProfile.collide.narrowphase = b3Max(m_maxProfile.collide.narrowphase, m_profile.collide.narrowphase);
+	m_maxProfile.solver.solveVelocity = b3Max(m_maxProfile.solver.solveVelocity, m_profile.solver.solveVelocity);
+	m_maxProfile.solver.solvePosition = b3Max(m_maxProfile.solver.solvePosition, m_profile.solver.solvePosition);
+
+	// Draw Statistics
+	ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f));
+	//ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height));
+	ImGui::Begin("Log", NULL, ImVec2(0, 0), 0.0f, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
+
+	if (g_settings.drawStats)
+	{
+		ImGui::Text("Bodies %d", m_world.GetBodyList().m_count);
+		ImGui::Text("Joints %d", m_world.GetJointList().m_count);
+		ImGui::Text("Contacts %d", m_world.GetContactList().m_count);
+
+		float32 avgGjkIters = 0.0f;
+		if (b3_gjkCalls > 0)
+		{
+			avgGjkIters = float32(b3_gjkIters) / float32(b3_gjkCalls);
+		}
+
+		ImGui::Text("GJK Calls %d", b3_gjkCalls);
+		ImGui::Text("GJK Iterations %d (%d) (%f)", b3_gjkIters, b3_gjkMaxIters, avgGjkIters);
+		
+		float32 convexCacheHitRatio = 0.0f;
+		if (b3_convexCalls > 0)
+		{
+			convexCacheHitRatio = float32(b3_convexCacheHits) / float32(b3_convexCalls);
+		}
+
+		ImGui::Text("Convex Calls %d", b3_convexCalls);
+		ImGui::Text("Convex Cache Hits %d (%f)", b3_convexCacheHits, convexCacheHitRatio);
+		ImGui::Text("Frame Allocations %d (%d)", b3_allocCalls, b3_maxAllocCalls);
+	}
+
+	if (g_settings.drawProfile)
+	{
+		ImGui::Text("Step %.4f (%.4f) [ms]", m_profile.total, m_maxProfile.total);
+		ImGui::Text(" Insert Pairs %.4f (%.4f)", m_profile.collide.broadphase, m_maxProfile.collide.broadphase);
+		ImGui::Text(" Update Pairs %.4f (%.4f)", m_profile.collide.narrowphase, m_maxProfile.collide.narrowphase);
+		ImGui::Text(" Velocity Solver %.4f (%.4f)", m_profile.solver.solveVelocity, m_maxProfile.solver.solveVelocity);
+		ImGui::Text(" Position Solver %.4f (%.4f)", m_profile.solver.solvePosition, m_maxProfile.solver.solvePosition);
+	}
+	
+	ImGui::End();
+
+	u32 drawFlags = 0;
+	drawFlags += g_settings.drawBounds * b3Draw::e_aabbsFlag;
+	drawFlags += g_settings.drawShapes * b3Draw::e_shapesFlag;
+	drawFlags += g_settings.drawCenterOfMasses * b3Draw::e_centerOfMassesFlag;
+	drawFlags += g_settings.drawJoints * b3Draw::e_jointsFlag;
+	drawFlags += g_settings.drawContactPoints * b3Draw::e_contactPointsFlag;
+	drawFlags += g_settings.drawContactNormals * b3Draw::e_contactNormalsFlag;
+	drawFlags += g_settings.drawContactTangents * b3Draw::e_contactTangentsFlag;
+
+	g_debugDraw->SetFlags(drawFlags);
+
+	m_world.DebugDraw();
+}
+
+void Test::MouseMove(const Ray3& pw)
+{
+	if (m_mouseJoint)
+	{
+		float32 hitFraction = m_rayHit.m_fraction;
+		float32 w1 = 1.0f - hitFraction;
+		float32 w2 = hitFraction;
+
+		b3Vec3 worldPointA = w1 * pw.Start() + w2 * pw.End();
+		m_mouseJoint->SetWorldAnchorA(worldPointA);
+	}
+}
+
+void Test::MouseLeftDown(const Ray3& pw)
+{
+	// Clear the current hit
+	m_rayHit.m_shape = nullptr;
+	if (m_mouseJoint)
+	{
+		m_world.DestroyJoint(m_mouseJoint);
+		m_mouseJoint = nullptr;
+		m_world.DestroyBody(m_groundBody);
+	}
+
+	b3Vec3 p1 = pw.Start();
+	b3Vec3 p2 = pw.End();
+
+	// Perform the ray cast
+	RayCastListener listener;
+	m_world.CastRay(&listener, p1, p2);
+
+	int hitId = listener.FindClosestHit();
+
+	if (hitId >= 0)
+	{
+		// Hit
+		// Replace current hit
+		m_rayHit = listener.m_hits[hitId];
+
+		RayHit();
+	}
+}
+
+void Test::MouseLeftUp(const Ray3& pw)
+{
+	m_rayHit.m_shape = nullptr;
+	if (m_mouseJoint)
+	{
+		m_world.DestroyJoint(m_mouseJoint);
+		m_mouseJoint = nullptr;
+		m_world.DestroyBody(m_groundBody);
+	}
+}
+
+void Test::RayHit()
+{
+	b3BodyDef bdef;
+	m_groundBody = m_world.CreateBody(bdef);
+
+	b3Shape* shape = m_rayHit.m_shape;
+
+	b3Body* bodyA = m_groundBody;
+	b3Body* bodyB = shape->GetBody();
+
+	// Ray hit point in world space
+	b3Vec3 worldPointA = m_rayHit.m_point;
+
+	// xf from world space to the local space of the shape
+	b3Transform xf = shape->GetTransform();
+
+	// Ray hit point in world space
+	// lp = xf^-1 * wp
+	b3Vec3 localPointA = b3MulT(xf, worldPointA);
+
+	b3MouseJointDef def;
+	def.bodyA = bodyA;
+	def.bodyB = bodyB;
+	def.worldAnchorA = worldPointA;
+	def.localAnchorB = localPointA;
+	def.maxForce = 2000.0f * bodyB->GetMass();
+
+	m_mouseJoint = (b3MouseJoint*)m_world.CreateJoint(def);
+	bodyB->SetAwake(true);
+}

src/testbed/framework/test_entries.cpp

@@ -0,0 +1,80 @@
+/*
+* 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 <testbed\tests\test.h>
+#include <testbed\tests\quickhull_test.h>
+#include <testbed\tests\cluster_test.h>
+#include <testbed\tests\distance_test.h>
+#include <testbed\tests\capsule_distance.h>
+#include <testbed\tests\collide_test.h>
+#include <testbed\tests\capsule_collision.h>
+#include <testbed\tests\capsule_and_hull_collision.h>
+#include <testbed\tests\hull_collision.h>
+#include <testbed\tests\spring.h>
+#include <testbed\tests\newton_cradle.h>
+#include <testbed\tests\hinge_motor.h>
+#include <testbed\tests\hinge_chain.h>
+#include <testbed\tests\ragdoll.h>
+#include <testbed\tests\quadrics.h>
+#include <testbed\tests\mesh_contact_test.h>
+#include <testbed\tests\sphere_stack.h>
+#include <testbed\tests\capsule_stack.h>
+#include <testbed\tests\box_stack.h>
+#include <testbed\tests\shape_stack.h>
+#include <testbed\tests\jenga.h>
+#include <testbed\tests\thin.h>
+#include <testbed\tests\pyramid.h>
+#include <testbed\tests\pyramids.h>
+#include <testbed\tests\ray_cast.h>
+#include <testbed\tests\sensor_test.h>
+#include <testbed\tests\character_test.h>
+#include <testbed\tests\body_types.h>
+#include <testbed\tests\varying_friction.h>
+#include <testbed\tests\varying_restitution.h>
+
+TestEntry g_tests[e_testCount] =
+{
+	{ "Quickhull Test", &QuickhullTest::Create },
+	{ "Cluster Test", &Cluster::Create },
+	{ "Distance Test", &Distance::Create },
+	{ "Capsule Distance", &CapsuleDistance::Create },
+	{ "Capsule Collision", &CapsuleCollision::Create },
+	{ "Capsule and Hull Collision", &CapsuleAndHull::Create },
+	{ "Hull Collision", &HullAndHull::Create },
+	{ "Springs", &Spring::Create },
+	{ "Newton's Cradle", &NewtonCradle::Create },
+	{ "Hinge Motor", &HingeMotor::Create },
+	{ "Hinge Chain", &HingeChain::Create },
+	{ "Ragdoll", &Ragdoll::Create },
+	{ "Quadrics", &Quadric::Create },
+	{ "Mesh Contact Test", &MeshContactTest::Create },
+	{ "Sphere Stack", &SphereStack::Create },
+	{ "Capsule Stack", &CapsuleStack::Create },
+	{ "Box Stack", &BoxStack::Create },
+	{ "Shape Stack", &ShapeStack::Create },
+	{ "Jenga", &Jenga::Create },
+	{ "Thin Plates", &Thin::Create },
+	{ "Pyramid", &Pyramid::Create },
+	{ "Pyramid Rows", &Pyramids::Create },
+	{ "Ray Cast", &RayCast::Create },
+	{ "Sensor Test", &SensorTest::Create },
+	{ "Character Test", &Character::Create },
+	{ "Body Types", &BodyTypes::Create },
+	{ "Varying Friction", &VaryingFriction::Create },
+	{ "Varying Restitution", &VaryingRestitution::Create },
+};