first commit

include/bounce/bounce.h

@@ -0,0 +1,60 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOUNCE_H
+#define BOUNCE_H
+
+// Include this file header in your project to directly access Bounce objects.
+
+#include <bounce\common\settings.h>
+#include <bounce\common\math\math.h>
+#include <bounce\common\time.h>
+#include <bounce\common\draw.h>
+
+#include <bounce\collision\broad_phase.h>
+#include <bounce\collision\gjk\gjk.h>
+#include <bounce\collision\gjk\gjk_cache.h>
+#include <bounce\collision\sat\sat.h>
+
+#include <bounce\collision\shapes\sphere.h>
+#include <bounce\collision\shapes\capsule.h>
+#include <bounce\collision\shapes\hull.h>
+#include <bounce\collision\shapes\triangle_hull.h>
+#include <bounce\collision\shapes\box_hull.h>
+#include <bounce\collision\shapes\mesh.h>
+
+#include <bounce\dynamics\joints\mouse_joint.h>
+#include <bounce\dynamics\joints\spring_joint.h>
+#include <bounce\dynamics\joints\sphere_joint.h>
+#include <bounce\dynamics\joints\revolute_joint.h>
+#include <bounce\dynamics\joints\cone_joint.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\contacts\contact.h>
+#include <bounce\dynamics\contacts\convex_contact.h>
+#include <bounce\dynamics\contacts\mesh_contact.h>
+
+#include <bounce\dynamics\body.h>
+#include <bounce\dynamics\world.h>
+#include <bounce\dynamics\world_listeners.h>
+
+#endif

include/bounce/collision/broad_phase.h

@@ -0,0 +1,197 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_BROAD_PHASE_H
+#define B3_BROAD_PHASE_H
+
+#include <bounce\collision\trees\dynamic_tree.h>
+#include <algorithm>
+
+// A pair of broad-phase proxies.
+struct b3Pair
+{
+	i32 proxy1;
+	i32 proxy2;
+};
+
+// The broad-phase collision interface. 
+// It is used to perform ray, AABB, and overlapping-pair queries 
+// against AABBs.
+class b3BroadPhase 
+{
+public:
+	b3BroadPhase();
+	~b3BroadPhase();
+
+	// Create a broad-phase proxy and return a proxy.
+	i32 CreateProxy(const b3AABB3& aabb, void* userData);
+	
+	// Destroy an existing proxy.
+	void DestroyProxy(i32 proxyId);
+
+	// Update an existing proxy with a given AABB and a displacement.
+	// displacement = int[a, b](dv/dt) dt = F(b) - F(a) = x(b) - x(a) ~= v * dt
+	// Return true if the proxy has moved.
+	bool MoveProxy(i32 proxyId, const b3AABB3& aabb, const b3Vec3& displacement);
+
+	// Get the AABB of a given proxy.
+	const b3AABB3& GetAABB(i32 proxyId) const;
+
+	// Get the user data attached to a proxy.
+	void* GetUserData(i32 proxyId) const;
+
+	// Test if two proxy AABBs are overlapping.
+	bool TestOverlap(i32 proxy1, i32 proxy2) const;
+	
+	// Notify the client callback the AABBs that are overlapping with the passed AABB.
+	template<class T>
+	void QueryAABB(T* callback, const b3AABB3& aabb) const;
+	
+	// Notify the client callback the AABBs that are overlapping the 
+	// passed ray.
+	template<class T>
+	void QueryRay(T* callback, const b3RayCastInput& input) const;
+
+	// Notify the client callback the AABB pairs that are overlapping.
+	// The client must store the notified pairs.
+	template<class T>
+	void FindNewPairs(T* callback);
+
+	// Debug b3Draw the AABB proxies.
+	void Draw(b3Draw* b3Draw) const;
+private :
+	friend class b3DynamicTree;
+	
+	// Add a proxy to the list of moved proxies.
+	// Only moved proxies will be used as an AABB query reference object.
+	void BufferMove(i32 proxyId);
+
+	// The client callback used to add a overlapping pair
+	// to the overlapping pair buffer.
+	bool Report(i32 proxyId);
+	
+	// The dynamic tree.
+	b3DynamicTree m_tree;
+
+	// The current proxy being queried for 
+	// overlap witha another proxies. Is used to avoid a proxy overlap with itself.
+	i32 m_queryProxyId;
+
+	// Keep a buffer of the objects that have moved in a step.
+	i32* m_moveBuffer;
+	u32 m_moveBufferCount;
+	u32 m_moveBufferCapacity;
+
+	// The buffer holding the unique overlapping AABB pairs.
+	b3Pair* m_pairBuffer;
+	u32 m_pairBufferCapacity;
+	u32 m_pairBufferCount;
+};
+
+inline const b3AABB3& b3BroadPhase::GetAABB(i32 proxyId) const 
+{
+	return m_tree.GetAABB(proxyId);
+}
+
+inline void* b3BroadPhase::GetUserData(i32 proxyId) const 
+{
+	return m_tree.GetUserData(proxyId);
+}
+
+template<class T>
+inline void b3BroadPhase::QueryAABB(T* callback, const b3AABB3& aabb) const 
+{
+	return m_tree.QueryAABB(callback, aabb);
+}
+
+template<class T>
+inline void b3BroadPhase::QueryRay(T* callback, const b3RayCastInput& input) const 
+{
+	return m_tree.QueryRay(callback, input);
+}
+
+inline bool operator<(const b3Pair& pair1, const b3Pair& pair2) 
+{
+	if (pair1.proxy1 < pair2.proxy1) 
+	{
+		return true;
+	}
+
+	if (pair1.proxy1 == pair2.proxy1) 
+	{
+		return pair1.proxy2 < pair2.proxy2;
+	}
+
+	return false;
+}
+
+template<class T>
+inline void b3BroadPhase::FindNewPairs(T* callback) 
+{
+	// Reset the overlapping pairs buffer count for the current step.
+	m_pairBufferCount = 0;
+
+	// Notifying this class with QueryCallback(), gets the (duplicated) overlapping pair buffer.
+	for (u32 i = 0; i < m_moveBufferCount; ++i) 
+	{
+		// Keep the current queried proxy ID to avoid self overlapping.
+		m_queryProxyId = m_moveBuffer[i];
+		if (m_queryProxyId == NULL_NODE) 
+		{
+			continue;
+		}
+
+		const b3AABB3& aabb = m_tree.GetAABB(m_queryProxyId);
+		m_tree.QueryAABB(this, aabb);
+	}
+
+	// Reset the move buffer for the next step.
+	m_moveBufferCount = 0;
+
+	// Sort the (duplicated) overlapping pair buffer to prune duplicated pairs.
+	std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
+
+	// Skip duplicated overlapping pairs.
+	u32 index = 0;
+	while (index < m_pairBufferCount) 
+	{
+		const b3Pair* primaryPair = m_pairBuffer + index;
+
+		// Report an unique overlapping pair to the client.
+		callback->AddPair(m_tree.GetUserData(primaryPair->proxy1), m_tree.GetUserData(primaryPair->proxy2));
+
+		// Skip all duplicated pairs until an unique pair is found.
+		++index;
+		while (index < m_pairBufferCount) 
+		{
+			const b3Pair* secondaryPair = m_pairBuffer + index;
+			if (secondaryPair->proxy1 != primaryPair->proxy1 || secondaryPair->proxy2 != primaryPair->proxy2) 
+			{
+				break;
+			}
+			++index;
+		}
+	}
+}
+
+inline void b3BroadPhase::Draw(b3Draw* b3Draw) const
+{
+	m_tree.Draw(b3Draw);
+}
+
+#endif

include/bounce/collision/distance.h

@@ -0,0 +1,67 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_COLLISION_H
+#define B3_COLLISION_H
+
+#include <bounce\common\geometry.h>
+#include <bounce\collision\shapes\aabb3.h>
+#include <bounce\collision\shapes\capsule.h>
+
+// Input for a ray cast query.
+struct b3RayCastInput
+{
+	b3Vec3 p1; // first point on segment
+	b3Vec3 p2; // second point on segment
+	float32 maxFraction; // maximum intersection
+};
+
+// Output of ray cast query.
+struct b3RayCastOutput
+{
+	float32 fraction; // time of intersection
+	b3Vec3 normal; // surface normal of intersection
+};
+
+// Find the closest point for a point P to a normalized plane.
+b3Vec3 b3ClosestPointOnPlane(const b3Vec3& P, const b3Plane& plane);
+
+// Find the closest point for a point P to a segment AB.
+b3Vec3 b3ClosestPointOnSegment(const b3Vec3& P,
+	const b3Vec3& A, const b3Vec3& B);
+
+// Find the closest point for a point P to a triangle ABC.
+b3Vec3 b3ClosestPointOnTriangle(const b3Vec3& P,
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C);
+
+// Find the closest points of two lines.
+void b3ClosestPointsOnLines(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& E1,
+	const b3Vec3& P2, const b3Vec3& E2);
+
+// Find the closest points of two normalized lines.
+void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& N1,
+	const b3Vec3& P2, const b3Vec3& N2);
+
+// Find the closest points of two segments P1-Q1 to a segment P2-Q2.
+void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
+	const b3Vec3& P1, const b3Vec3& Q1,
+	const b3Vec3& P2, const b3Vec3& Q2);
+
+#endif

include/bounce/collision/gjk/gjk.h

@@ -0,0 +1,74 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GJK_H
+#define B3_GJK_H
+
+#include <bounce\common\geometry.h>
+
+class b3GJKProxy;
+struct b3SimplexCache;
+
+struct b3SimplexVertex
+{
+	b3Vec3 pointA; // support vertex on proxy A
+	b3Vec3 pointB; // support vertex on proxy B
+	b3Vec3 point; // minkowski vertex
+	float32 weight; // barycentric coordinate for point
+	u32 indexA; // support A index
+	u32 indexB; // support B index
+};
+
+struct b3Simplex
+{
+	b3SimplexVertex m_vertices[4];
+	u32 m_count;
+
+	b3Vec3 GetSearchDirection(const b3Vec3& Q) const;
+	b3Vec3 GetClosestPoint() const;
+	void GetClosestPoints(b3Vec3* pA, b3Vec3* pB) const;
+
+	void Solve2(const b3Vec3& Q);
+	void Solve3(const b3Vec3& Q);
+	void Solve4(const b3Vec3& Q);
+
+	// Cache
+	void ReadCache(const b3SimplexCache* cache,
+		const b3Transform& xfA, const b3GJKProxy& proxyA,
+		const b3Transform& xfB, const b3GJKProxy& proxyB);
+	void WriteCache(b3SimplexCache* cache) const;
+	float32 GetMetric() const;
+};
+
+// The output of the GJK algorithm.
+// It contains the closest points between two proxies 
+// and their euclidean distance.
+struct b3GJKOutput 
+{
+	b3Vec3 pointA; // closest point on proxy A
+	b3Vec3 pointB; // closest point on proxy B
+	float32 distance; // euclidean distance between the closest points
+	u32 iterations; // number of GJK iterations
+};
+
+// Find the closest points and distance between two proxies.
+// If the distance is zero then the proxies are overlapping.
+b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA, 
+	const b3Transform& xfB, const b3GJKProxy& proxyB);
+
+#endif

include/bounce/collision/gjk/gjk_cache.h

@@ -0,0 +1,66 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GJK_CACHE_H
+#define B3_GJK_CACHE_H
+
+#include <bounce\collision\gjk\gjk.h>
+
+// A simplex used to improve the performance 
+// of the GJK when called more than once. 
+// Make sure to set cache.count to zero before 
+// calling the GJK for the first time.
+struct b3SimplexCache
+{
+	float32 metric; // length or area or volume
+	u32 iterations; // number of GJK iterations
+	u16 count; // number of support vertices
+	u8 indexA[4]; // support vertices on proxy A
+	u8 indexB[4]; // support vertices on proxy B
+};
+
+// Find the closest points and distance between two proxies. 
+// Assumes a simplex is given for increasing the performance of 
+// the GJK when called more than once.
+b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
+				  const b3Transform& xfB, const b3GJKProxy& proxyB,
+				  bool applyRadius, b3SimplexCache* cache);
+
+struct b3GJKFeaturePair
+{
+	enum Type
+	{
+		e_unknown = 0,
+		e_vertex = 1,
+		e_edge = 2,
+		e_face = 3
+	};
+
+	Type typeA; // number of vertices on proxy A
+	Type typeB; // number of vertices on proxy B
+	u32 indexA[3]; // vertices on proxy A
+	u32 indexB[3]; // vertices on proxy B
+};
+
+// Get the vertices of the features that the closest points between two 
+// GJK proxies are contained on given a cached simplex.
+// The GJK must have been called using the pair of proxies and 
+// cache.count must be < 4, that is, the proxies must not be overlapping.
+b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache);
+
+#endif

include/bounce/collision/gjk/gjk_proxy.h

@@ -0,0 +1,82 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GJK_PROXY_H
+#define B3_GJK_PROXY_H
+
+#include <bounce\common\math\vec3.h>
+
+// A GJK proxy encapsulates any convex hull to be used by the GJK.
+class b3GJKProxy
+{
+public:
+	b3GJKProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) { }
+
+	// Get the number of vertices of this proxy.
+	u32 GetVertexCount() const;
+
+	// Read an indexed vertex from this proxy.
+	const b3Vec3& GetVertex(u32 index) const;
+
+	// Get the support vertex index in a given direction.
+	u32 GetSupportIndex(const b3Vec3& d) const;
+
+	// Convenience function.
+	// Get the support vertex in a given direction.
+	const b3Vec3& GetSupportVertex(const b3Vec3& d) const;
+
+	b3Vec3 m_buffer[3]; // for childs
+	const b3Vec3* m_vertices;
+	u32 m_count;
+	float32 m_radius;
+};
+
+inline u32 b3GJKProxy::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline const b3Vec3& b3GJKProxy::GetVertex(u32 index) const
+{
+	B3_ASSERT(0 <= index && index < m_count);
+	return m_vertices[index];
+}
+
+inline u32 b3GJKProxy::GetSupportIndex(const b3Vec3& d) const
+{
+	u32 maxIndex = 0;
+	float32 maxProjection = b3Dot(d, m_vertices[maxIndex]);
+	for (u32 i = 1; i < m_count; ++i)
+	{
+		float32 projection = b3Dot(d, m_vertices[i]);
+		if (projection > maxProjection)
+		{
+			maxIndex = i;
+			maxProjection = projection;
+		}
+	}
+	return maxIndex;
+}
+
+inline const b3Vec3& b3GJKProxy::GetSupportVertex(const b3Vec3& d) const
+{
+	u32 index = GetSupportIndex(d);
+	return m_vertices[index];
+}
+
+#endif

include/bounce/collision/sat/sat.h

@@ -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.
+*/
+
+#ifndef B3_SAT_H
+#define B3_SAT_H
+
+#include <bounce\common\geometry.h>
+
+struct b3Hull;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct b3FaceQuery
+{
+	u32 index;
+	float32 separation;
+};
+
+float32 b3Project(const b3Hull* hull, const b3Plane& plane);
+
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+struct b3EdgeQuery
+{
+	u32 indexA;
+	u32 indexB;
+	float32 separation;
+};
+
+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 b3Project(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C1);
+
+b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Hull* hullA,
+	const b3Transform& xfB, const b3Hull* hullB);
+
+#endif

include/bounce/collision/sat/sat_edge_and_hull.h

@@ -0,0 +1,40 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_EDGE_SAT_H
+#define B3_EDGE_SAT_H
+
+#include <bounce\collision\sat\sat.h>
+
+struct b3Capsule;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+float32 b3ProjectEdge(const b3Capsule* hull, const b3Plane& plane);
+
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Capsule* hullA,
+	const b3Transform& xfB, const b3Hull* hullB);
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+float32 b3ProjectEdge(const b3Vec3& P1, const b3Vec3& E1, const b3Vec3& P2, const b3Vec3& E2, const b3Vec3& C2);
+
+b3EdgeQuery b3QueryEdgeSeparation(const b3Transform& xfA, const b3Capsule* hullA, 
+	const b3Transform& xfB, const b3Hull* hullB);
+
+#endif

include/bounce/collision/sat/sat_vertex_and_hull.h

@@ -0,0 +1,33 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_VERTEX_SAT_H
+#define B3_VERTEX_SAT_H
+
+#include <bounce\collision\sat\sat.h>
+
+struct b3Sphere;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+
+float32 b3ProjectVertex(const b3Sphere* hull, const b3Plane& plane);
+
+b3FaceQuery b3QueryFaceSeparation(const b3Transform& xfA, const b3Sphere* hullA,
+	const b3Transform& xfB, const b3Hull* hullB);
+
+#endif

include/bounce/collision/shapes/aabb3.h

@@ -0,0 +1,239 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_AABB_3_H
+#define B3_AABB_3_H
+
+#include <bounce\common\math\transform.h>
+
+// A min-max representation of a three-dimensional AABB.
+struct b3AABB3 
+{
+	b3Vec3 GetSupportVertex(const b3Vec3& direction) const
+	{
+		b3Vec3 support;
+		support.x = direction.x < 0.0f ? m_lower.x : m_upper.x;
+		support.y = direction.y < 0.0f ? m_lower.y : m_upper.y;
+		support.z = direction.z < 0.0f ? m_lower.z : m_upper.z;
+		return support;
+	}
+
+	// Compute this AABB from a list of points.
+	void Compute(const b3Vec3* points, u32 count)
+	{
+		m_lower = m_upper = points[0];
+		for (u32 i = 1; i < count; ++i)
+		{
+			m_lower = b3Min(m_lower, points[i]);
+			m_upper = b3Max(m_upper, points[i]);
+		}
+	}
+
+	// Compute this AABB from a list of points and a transform.
+	void Compute(const b3Vec3* points, u32 count, const b3Transform& xf)
+	{
+		m_lower = m_upper = b3Mul(xf, points[0]);
+		for (u32 i = 1; i < count; ++i)
+		{
+			b3Vec3 v = b3Mul(xf, points[i]);
+			m_lower = b3Min(m_lower, v);
+			m_upper = b3Max(m_upper, v);
+		}
+	}
+
+	// Extend this AABB by a scalar.
+	void Extend(float32 s) 
+	{
+		b3Vec3 r(s, s, s);
+		m_lower -= r;
+		m_upper += r;
+	}
+	
+	// Extend this AABB by a radius vector.
+	void Extend(const b3Vec3& r)
+	{
+		m_lower -= r;
+		m_upper += r;
+	}
+
+	// Compute the centroid of this AABB.
+	b3Vec3 Centroid() const 
+	{
+		return  0.5f * (m_lower + m_upper);
+	}
+
+	// Compute the width of this AABB.
+	float32 Width() const 
+	{
+		return m_upper.x - m_lower.x;
+	}
+
+	// Compute the height of this AABB.
+	float32 Height() const 
+	{
+		return m_upper.y - m_lower.y;
+	}
+
+	// Compute the depth of this AABB.
+	float32 Depth() const 
+	{
+		return m_upper.z - m_lower.z;
+	}
+
+	// Compute the total of cubic units contained in this AABB.
+	float32 Volume() const 
+	{
+		return Width() * Height() * Depth();
+	}
+
+	// Compute the surface area of this AABB.
+	float32 SurfaceArea() const 
+	{
+		return 2.0f * (Width() * Depth() + Width() * Height() + Depth() * Height());
+	}
+
+	// Read the index of the longest axis of this AABB.
+	u32 GetLongestAxisIndex() const
+	{
+		b3Vec3 c = Centroid();
+		b3Vec3 r = m_upper - c;
+		float32 max = r[0];
+		u32 i = 0;
+		if (r[1] > max)
+		{
+			max = r[1];
+			i = 1;
+		}
+		if (r[2] > max)
+		{
+			max = r[2];
+			i = 2;
+		}
+		return i;
+	}
+
+	// Test if this AABB contains a point.
+	bool TestPoint(const b3Vec3& point) const
+	{
+		return	m_lower.x <= point.x && point.x <= m_upper.x &&
+				m_lower.y <= point.y && point.y <= m_upper.y &&
+				m_lower.z <= point.z && point.z <= m_upper.z;
+	}
+
+	// Test if this AABB contains another AABB.
+	bool Contains(const b3AABB3& aabb) const
+	{
+		return TestPoint(aabb.m_lower) && TestPoint(aabb.m_upper);
+	}
+
+	// Test if a ray intersect this AABB.
+	// Output the minimum fraction to derive the intersection point.
+	bool TestRay(const b3Vec3& p1, const b3Vec3& p2, float32 maxFraction, float32& minFraction) const 
+	{
+		// Solve segment to slab plane.
+		// S = p1 + w * d
+		// dot(S, n) = offset[i]
+		// Solution:
+		// dot(p1 + w * d, n) = offset[i]
+		// dot(p1, n) + w * dot(d, n) = offset[i]
+		// w * dot(d, n) = offset[i] - dot(p1, n)
+		// w = (offset[i] - dot(p1, n)) / dot(d, n)
+		b3Vec3 d = p2 - p1;
+		float32 lower = 0.0f;
+		float32 upper = maxFraction;		
+		for (u32 i = 0; i < 3; ++i) 
+		{
+			float32 numerators[2], denominators[2];
+			//numerators[0] = (-m_lower[i]) - (-p1[i]);
+			numerators[0] = p1[i] - m_lower[i];
+			numerators[1] = m_upper[i] - p1[i];
+			denominators[0] = -d[i];
+			denominators[1] = d[i];
+
+			// For each orthogonal plane...
+			for (u32 j = 0; j < 2; ++j) 
+			{
+				float32 numerator = numerators[j];
+				float32 denominator = denominators[j];
+
+				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 
+				{
+					if (denominator < 0.0f) 
+					{
+						// s enters this half-space.
+						if (numerator < lower * denominator) 
+						{
+							// Increase lower.
+							lower = numerator / denominator;
+						}
+					}
+					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 <= maxFraction);
+		minFraction = lower;
+		return true;
+	}
+
+	b3Vec3 m_lower; // lower vertex
+	b3Vec3 m_upper; // upper vertex
+};
+
+// Compute an AABB that encloses two AABBs.
+inline b3AABB3 b3Combine(const b3AABB3& a, const b3AABB3& b) 
+{
+	b3AABB3 aabb;
+	aabb.m_lower = b3Min(a.m_lower, b.m_lower);
+	aabb.m_upper = b3Max(a.m_upper, b.m_upper);
+	return aabb;
+}
+
+// Test if two AABBs are overlapping.
+inline bool b3TestOverlap(const b3AABB3& a, const b3AABB3& b) 
+{
+	return (a.m_lower.x <= b.m_upper.x) &&	(a.m_lower.y <= b.m_upper.y) &&	(a.m_lower.z <= b.m_upper.z) &&
+		(a.m_upper.x >= b.m_lower.x) && (a.m_upper.y >= b.m_lower.y) &&	(a.m_upper.z >= b.m_lower.z);
+}
+
+#endif

include/bounce/collision/shapes/box_hull.h

@@ -0,0 +1,165 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_BOX_HULL_H
+#define B3_BOX_HULL_H
+
+#include <bounce\collision\shapes\hull.h>
+
+struct b3BoxHull : public b3Hull
+{
+	b3Vec3 boxVertices[8];
+	b3HalfEdge boxEdges[24];
+	b3Face boxFaces[6];
+	b3Plane boxPlanes[6];
+
+	b3BoxHull() { }
+
+	// Set this box to the unit box.
+	void SetIdentity()
+	{
+		boxVertices[0] = b3Vec3(1.0f, 1.0f, -1.0f);
+		boxVertices[1] = b3Vec3(-1.0f, 1.0f, -1.0f);
+		boxVertices[2] = b3Vec3(-1.0f, -1.0f, -1.0f);
+		boxVertices[3] = b3Vec3(1.0f, -1.0f, -1.0f);
+		boxVertices[4] = b3Vec3(1.0f, 1.0f, 1.0f);
+		boxVertices[5] = b3Vec3(-1.0f, 1.0f, 1.0f);
+		boxVertices[6] = b3Vec3(-1.0f, -1.0f, 1.0f);
+		boxVertices[7] = b3Vec3(1.0f, -1.0f, 1.0f);
+
+		boxEdges[0] = b3MakeEdge(1, 1, 0, 2);
+		boxEdges[1] = b3MakeEdge(2, 0, 5, 21);
+		boxEdges[2] = b3MakeEdge(2, 3, 0, 4);
+		boxEdges[3] = b3MakeEdge(6, 2, 2, 18);
+		boxEdges[4] = b3MakeEdge(6, 5, 0, 6);
+		boxEdges[5] = b3MakeEdge(5, 4, 4, 17);
+		boxEdges[6] = b3MakeEdge(5, 7, 0, 0);
+		boxEdges[7] = b3MakeEdge(1, 6, 3, 22);
+		boxEdges[8] = b3MakeEdge(4, 9, 1, 10);
+		boxEdges[9] = b3MakeEdge(7, 8, 4, 23);
+		boxEdges[10] = b3MakeEdge(7, 11, 1, 12);
+		boxEdges[11] = b3MakeEdge(3, 10, 2, 16);
+		boxEdges[12] = b3MakeEdge(3, 13, 1, 14);
+		boxEdges[13] = b3MakeEdge(0, 12, 5, 19);
+		boxEdges[14] = b3MakeEdge(0, 15, 1, 8);
+		boxEdges[15] = b3MakeEdge(4, 14, 3, 20);
+		boxEdges[16] = b3MakeEdge(7, 17, 2, 3);
+		boxEdges[17] = b3MakeEdge(6, 16, 4, 9);
+		boxEdges[18] = b3MakeEdge(2, 19, 2, 11);
+		boxEdges[19] = b3MakeEdge(3, 18, 5, 1);
+		boxEdges[20] = b3MakeEdge(0, 21, 3, 7);
+		boxEdges[21] = b3MakeEdge(1, 20, 5, 13);
+		boxEdges[22] = b3MakeEdge(5, 23, 3, 15);
+		boxEdges[23] = b3MakeEdge(4, 22, 4, 5);
+
+		boxFaces[0].edge = 6;
+		boxFaces[1].edge = 14;
+		boxFaces[2].edge = 18;
+		boxFaces[3].edge = 15;
+		boxFaces[4].edge = 9;
+		boxFaces[5].edge = 21;
+
+		boxPlanes[0] = b3Plane(b3Vec3(-1.0f, 0.0f, 0.0f), boxVertices[1]);
+		boxPlanes[1] = b3Plane(b3Vec3(1.0f, 0.0f, 0.0f), boxVertices[0]);
+		boxPlanes[2] = b3Plane(b3Vec3(0.0f, -1.0f, 0.0f), boxVertices[2]);
+		boxPlanes[3] = b3Plane(b3Vec3(0.0f, 1.0f, 0.0f), boxVertices[1]);
+		boxPlanes[4] = b3Plane(b3Vec3(0.0f, 0.0f, 1.0f), boxVertices[4]);
+		boxPlanes[5] = b3Plane(b3Vec3(0.0f, 0.0f, -1.0f), boxVertices[0]);
+
+		centroid = b3Vec3(0.0f, 0.0f, 0.0f);
+		vertices = boxVertices;
+		vertexCount = 8;
+		edges = boxEdges;
+		edgeCount = 24;
+		faces = boxFaces;
+		planes = boxPlanes;
+		faceCount = 6;
+	}
+
+	// Set this box to the unit box and transform 
+	// it. The transform must not contain non-uniform 
+	// scaling!
+	void SetTransform(const b3Transform& T)
+	{
+		boxVertices[0] = b3Vec3(1.0f, 1.0f, -1.0f);
+		boxVertices[1] = b3Vec3(-1.0f, 1.0f, -1.0f);
+		boxVertices[2] = b3Vec3(-1.0f, -1.0f, -1.0f);
+		boxVertices[3] = b3Vec3(1.0f, -1.0f, -1.0f);
+		boxVertices[4] = b3Vec3(1.0f, 1.0f, 1.0f);
+		boxVertices[5] = b3Vec3(-1.0f, 1.0f, 1.0f);
+		boxVertices[6] = b3Vec3(-1.0f, -1.0f, 1.0f);
+		boxVertices[7] = b3Vec3(1.0f, -1.0f, 1.0f);
+
+		for (u32 i = 0; i < 8; ++i)
+		{
+			boxVertices[i] = T * boxVertices[i];
+		}
+
+		boxEdges[0] = b3MakeEdge(1, 1, 0, 2);
+		boxEdges[1] = b3MakeEdge(2, 0, 5, 21);
+		boxEdges[2] = b3MakeEdge(2, 3, 0, 4);
+		boxEdges[3] = b3MakeEdge(6, 2, 2, 18);
+		boxEdges[4] = b3MakeEdge(6, 5, 0, 6);
+		boxEdges[5] = b3MakeEdge(5, 4, 4, 17);
+		boxEdges[6] = b3MakeEdge(5, 7, 0, 0);
+		boxEdges[7] = b3MakeEdge(1, 6, 3, 22);
+		boxEdges[8] = b3MakeEdge(4, 9, 1, 10);
+		boxEdges[9] = b3MakeEdge(7, 8, 4, 23);
+		boxEdges[10] = b3MakeEdge(7, 11, 1, 12);
+		boxEdges[11] = b3MakeEdge(3, 10, 2, 16);
+		boxEdges[12] = b3MakeEdge(3, 13, 1, 14);
+		boxEdges[13] = b3MakeEdge(0, 12, 5, 19);
+		boxEdges[14] = b3MakeEdge(0, 15, 1, 8);
+		boxEdges[15] = b3MakeEdge(4, 14, 3, 20);
+		boxEdges[16] = b3MakeEdge(7, 17, 2, 3);
+		boxEdges[17] = b3MakeEdge(6, 16, 4, 9);
+		boxEdges[18] = b3MakeEdge(2, 19, 2, 11);
+		boxEdges[19] = b3MakeEdge(3, 18, 5, 1);
+		boxEdges[20] = b3MakeEdge(0, 21, 3, 7);
+		boxEdges[21] = b3MakeEdge(1, 20, 5, 13);
+		boxEdges[22] = b3MakeEdge(5, 23, 3, 15);
+		boxEdges[23] = b3MakeEdge(4, 22, 4, 5);
+
+		boxFaces[0].edge = 6;
+		boxFaces[1].edge = 14;
+		boxFaces[2].edge = 18;
+		boxFaces[3].edge = 15;
+		boxFaces[4].edge = 9;
+		boxFaces[5].edge = 21;
+
+		boxPlanes[0] = b3Plane(b3Vec3(-1.0f, 0.0f, 0.0f), boxVertices[1]);
+		boxPlanes[1] = b3Plane(b3Vec3(1.0f, 0.0f, 0.0f), boxVertices[0]);
+		boxPlanes[2] = b3Plane(b3Vec3(0.0f, -1.0f, 0.0f), boxVertices[2]);
+		boxPlanes[3] = b3Plane(b3Vec3(0.0f, 1.0f, 0.0f), boxVertices[1]);
+		boxPlanes[4] = b3Plane(b3Vec3(0.0f, 0.0f, 1.0f), boxVertices[4]);
+		boxPlanes[5] = b3Plane(b3Vec3(0.0f, 0.0f, -1.0f), boxVertices[0]);
+
+		centroid = b3Vec3(0.0f, 0.0f, 0.0f);
+		vertices = boxVertices;
+		vertexCount = 8;
+		edges = boxEdges;
+		edgeCount = 24;
+		faces = boxFaces;
+		planes = boxPlanes;
+		faceCount = 6;
+
+		centroid = T * centroid;
+	}
+};
+
+#endif

include/bounce/collision/shapes/capsule.h

@@ -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.
+*/
+
+#ifndef B3_CAPSULE_H
+#define B3_CAPSULE_H
+
+#include <bounce\common\math\vec3.h>
+
+struct b3Capsule
+{
+	b3Vec3 vertices[2];
+	float32 radius;
+
+	const b3Vec3& GetVertex(u32 index) const;
+
+	u32 GetSupportVertex(const b3Vec3& direction) const;
+};
+
+inline const b3Vec3& b3Capsule::GetVertex(u32 index) const
+{
+	return vertices[index];
+}
+
+inline u32 b3Capsule::GetSupportVertex(const b3Vec3& direction) const
+{
+	if (b3Dot(direction, vertices[0]) > b3Dot(direction, vertices[1]))
+	{
+		return 0;
+	}
+	return 1;
+}
+
+#endif

include/bounce/collision/shapes/hull.h

@@ -0,0 +1,65 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_HULL_H
+#define B3_HULL_H
+
+#include <bounce\common\geometry.h>
+
+struct b3Face
+{
+	u8 edge;
+};
+
+struct b3HalfEdge
+{
+	u8 origin;
+	u8 twin;
+	u8 face;
+	u8 next;
+};
+
+struct b3Hull
+{
+	b3Vec3 centroid;
+	u32 vertexCount;
+	b3Vec3* vertices;
+	u32 edgeCount;
+	b3HalfEdge* edges;
+	u32 faceCount;
+	b3Face* faces;
+	b3Plane* planes;
+
+	const b3Vec3& GetVertex(u32 index) const;
+	const b3HalfEdge* GetEdge(u32 index) const;
+	const b3Face* GetFace(u32 index) const;
+	const b3Plane& GetPlane(u32 index) const;
+
+	u32 GetSupportVertex(const b3Vec3& direction) const;
+	u32 GetSupportFace(const b3Vec3& direction) const;
+	
+	b3Plane GetEdgeSidePlane(u32 index) const;
+	u32 GetSize() const;
+	void Validate() const;
+	void Validate(const b3Face* face) const;
+	void Validate(const b3HalfEdge* edge) const;
+};
+
+#include <bounce\collision\shapes\hull.inl>
+
+#endif

include/bounce/collision/shapes/hull.inl

@@ -0,0 +1,89 @@
+inline b3HalfEdge b3MakeEdge(u32 origin, u32 twin, u32 face, u32 next)
+{
+	b3HalfEdge edge;
+	edge.origin = u8(origin);
+	edge.twin = u8(twin);
+	edge.face = u8(face);
+	edge.next = u8(next);
+	return edge;
+}
+
+inline const b3Vec3& b3Hull::GetVertex(u32 index) const
+{
+	return vertices[index];
+}
+
+inline const b3HalfEdge* b3Hull::GetEdge(u32 index) const
+{
+	return edges + index;
+}
+
+inline const b3Face* b3Hull::GetFace(u32 index) const
+{
+	return faces + index;
+}
+
+inline const b3Plane& b3Hull::GetPlane(u32 index) const
+{
+	return planes[index];
+}
+
+inline u32 b3Hull::GetSupportVertex(const b3Vec3& direction) const
+{
+	u32 maxIndex = 0;
+	float32 maxProjection = b3Dot(direction, vertices[maxIndex]);
+	for (u32 i = 1; i < vertexCount; ++i)
+	{
+		float32 projection = b3Dot(direction, vertices[i]);
+		if (projection > maxProjection)
+		{
+			maxIndex = i;
+			maxProjection = projection;
+		}
+	}
+	return maxIndex;
+}
+
+inline u32 b3Hull::GetSupportFace(const b3Vec3& direction) const
+{
+	u32 maxIndex = 0;
+	float32 maxProjection = b3Dot(direction, planes[maxIndex].normal);
+	for (u32 i = 1; i < faceCount; ++i)
+	{
+		float32 projection = b3Dot(direction, planes[i].normal);
+		if (projection > maxProjection)
+		{
+			maxIndex = i;
+			maxProjection = projection;
+		}
+	}
+	return maxIndex;
+}
+
+inline b3Plane b3Hull::GetEdgeSidePlane(u32 index) const
+{
+	const b3HalfEdge* edge = edges + index;
+	const b3HalfEdge* twin = edges + edge->twin;
+	const b3Plane* facePlane = planes + edge->face;
+
+	b3Vec3 P = vertices[edge->origin];
+	b3Vec3 Q = vertices[twin->origin];
+	b3Vec3 E = Q - P;
+	b3Vec3 D = b3Cross(E, facePlane->normal);
+
+	b3Plane plane;
+	plane.normal = b3Normalize(D);
+	plane.offset = b3Dot(plane.normal, P);
+	return plane;
+}
+
+inline u32 b3Hull::GetSize() const
+{
+	u32 size = 0;
+	size += sizeof(b3Hull);
+	size += vertexCount * sizeof(b3Vec3);
+	size += edgeCount * sizeof(b3HalfEdge);
+	size += faceCount * sizeof(b3Face);
+	size += faceCount * sizeof(b3Plane);
+	return size;
+}

include/bounce/collision/shapes/mesh.h

@@ -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.
+*/
+
+#ifndef B3_MESH_H
+#define B3_MESH_H
+
+#include <bounce\common\geometry.h>
+#include <bounce\collision\trees\static_tree.h>
+
+struct b3Mesh 
+{
+	u32 vertexCount;
+	b3Vec3* vertices;
+	u32 triangleCount;
+	b3Triangle* triangles;
+	b3StaticTree tree;
+
+	void BuildTree();
+	
+	const b3Vec3& GetVertex(u32 index) const;
+	const b3Triangle& GetTriangle(u32 index) const;
+	
+	void GetTriangleVertices(b3Vec3 out[3], u32 index) const;
+	b3Plane GetTrianglePlane(u32 index) const;
+	b3AABB3 GetTriangleAABB(u32 index) const;
+	u32 GetSize() const;
+};
+
+inline void b3Mesh::BuildTree()
+{
+	b3AABB3* aabbs = (b3AABB3*)b3Alloc(triangleCount * sizeof(b3AABB3));
+	u32* indices = (u32*)b3Alloc(triangleCount * sizeof(u32));
+	for (u32 i = 0; i < triangleCount; ++i)
+	{
+		aabbs[i] = GetTriangleAABB(i);
+		indices[i] = i;
+	}
+
+	tree.Build(indices, aabbs, triangleCount);
+
+	b3Free(indices);
+	b3Free(aabbs);
+}
+
+inline const b3Vec3& b3Mesh::GetVertex(u32 index) const
+{
+	return vertices[index];
+}
+
+inline const b3Triangle& b3Mesh::GetTriangle(u32 index) const
+{
+	return triangles[index];
+}
+
+inline void b3Mesh::GetTriangleVertices(b3Vec3 out[3], u32 index) const
+{
+	const b3Triangle* triangle = triangles + index;
+	u32 i1 = triangle->v1;
+	u32 i2 = triangle->v2;
+	u32 i3 = triangle->v3;
+	out[0] = vertices[i1];
+	out[1] = vertices[i2];
+	out[2] = vertices[i3];
+}
+
+inline b3Plane b3Mesh::GetTrianglePlane(u32 index) const
+{
+	b3Vec3 vs[3];
+	GetTriangleVertices(vs, index);
+	return b3Plane(vs[0], vs[1], vs[2]);
+}
+
+inline b3AABB3 b3Mesh::GetTriangleAABB(u32 index) const
+{
+	const b3Triangle* triangle = triangles + index;
+	
+	u32 i1 = triangle->v1;
+	u32 i2 = triangle->v2;
+	u32 i3 = triangle->v3;
+
+	b3AABB3 aabb;
+	aabb.m_lower = b3Min(b3Min(vertices[i1], vertices[i2]), vertices[i3]);
+	aabb.m_upper = b3Max(b3Max(vertices[i1], vertices[i2]), vertices[i3]);
+	
+	return aabb;
+}
+
+inline u32 b3Mesh::GetSize() const
+{
+	u32 memory = 0;
+	memory += sizeof(b3Mesh);
+	memory += sizeof(b3Vec3) * vertexCount;
+	memory += sizeof(b3Triangle) * triangleCount;
+	memory += sizeof(b3Plane) * triangleCount;
+	memory += sizeof(b3StaticTree);
+	return memory;
+}
+
+#endif

include/bounce/collision/shapes/sphere.h

@@ -0,0 +1,44 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SPHERE_H
+#define B3_SPHERE_H
+
+#include <bounce\common\math\vec3.h>
+
+struct b3Sphere
+{
+	b3Vec3 vertex;
+	float32 radius;
+	
+	const b3Vec3& GetVertex(u32 index) const;
+
+	u32 GetSupportVertex(const b3Vec3& direction) const;
+};
+
+inline const b3Vec3& b3Sphere::GetVertex(u32 index) const
+{
+	return vertex;
+}
+
+inline u32 b3Sphere::GetSupportVertex(const b3Vec3& direction) const
+{
+	return 0;
+}
+
+#endif

include/bounce/collision/shapes/triangle_hull.h

@@ -0,0 +1,76 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_TRIANGLE_HULL_H
+#define B3_TRIANGLE_HULL_H
+
+#include <bounce\collision\shapes\hull.h>
+
+struct b3TriangleHull : public b3Hull
+{
+	b3Vec3 triangleVertices[3];
+	b3HalfEdge triangleEdges[6];
+	b3Face triangleFaces[2];
+	b3Plane trianglePlanes[2];
+
+	b3TriangleHull() { }
+
+	b3TriangleHull(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C)
+	{
+		Set(A, B, C);
+	}
+
+	void Set(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C)
+	{
+		const float32 kInv3 = 1.0f / 3.0f;
+		
+		centroid = kInv3 * (A + B + C);
+
+		triangleVertices[0] = A;
+		triangleVertices[1] = B;
+		triangleVertices[2] = C;
+
+		// Each edge must be followed by its twin.
+		triangleEdges[0] = b3MakeEdge(0, 1, 0, 2); // Face 0 - Edge 0
+		triangleEdges[2] = b3MakeEdge(1, 3, 0, 4); // Face 0 - Edge 1
+		triangleEdges[4] = b3MakeEdge(2, 5, 0, 0); // Face 0 - Edge 2
+		
+		triangleEdges[1] = b3MakeEdge(1, 0, 1, 3); // Face 1 - Edge 0
+		triangleEdges[3] = b3MakeEdge(2, 2, 1, 5); // Face 1 - Edge 1
+		triangleEdges[5] = b3MakeEdge(0, 4, 1, 1); // Face 1 - Edge 2
+				
+		triangleFaces[0].edge = 0;
+		triangleFaces[1].edge = 1;
+		
+		b3Vec3 N = b3Cross(B - A, C - A);
+		N = b3Normalize(N);
+
+		trianglePlanes[0] = b3Plane(N, centroid);
+		trianglePlanes[1] = b3Plane(-N, centroid);
+		
+		vertices = triangleVertices;
+		vertexCount = 3;
+		edges = triangleEdges;
+		edgeCount = 6;
+		faces = triangleFaces;
+		planes = trianglePlanes;
+		faceCount = 2;
+	}
+};
+
+#endif

include/bounce/collision/trees/dynamic_tree.h

@@ -0,0 +1,240 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_DYNAMIC_TREE_H
+#define B3_DYNAMIC_TREE_H
+
+#include <bounce\common\draw.h>
+#include <bounce\common\template\stack.h>
+#include <bounce\collision\shapes\aabb3.h>
+#include <bounce\collision\distance.h>
+
+#define NULL_NODE (-1)
+
+// An AABB tree for dynamic AABBs.
+class b3DynamicTree 
+{
+public :
+	b3DynamicTree();
+	~b3DynamicTree();
+
+	// Insert a node to the tree and return its ID.
+	i32 InsertNode(const b3AABB3& aabb, void* userData);
+	
+	// Remove a node from the tree.
+	void RemoveNode(i32 proxyId);
+
+	// Update a node AABB.
+	void UpdateNode(i32 proxyId, const b3AABB3& aabb);
+
+	// Get the (fat) AABB of a given proxy.
+	const b3AABB3& GetAABB(i32 proxyId) const;
+
+	// Get the data associated with a given proxy.
+	void* GetUserData(i32 proxyId) const;
+
+	// Check if two aabbs of this tree are overlapping.
+	bool TestOverlap(i32 proxy1, i32 proxy2) const;
+
+	// Keep reporting the client callback the AABBs that are overlapping with
+	// the given AABB. The client callback must return true if the query 
+	// must be stopped or false to continue looking for more overlapping pairs.
+	template<class T> 
+	void QueryAABB(T* callback, const b3AABB3& aabb) const;
+
+	// Keep reporting the client callback all AABBs that are overlapping with
+	// the given ray. The client callback must return the new intersection fraction.
+	// If the fraction == 0 then the query is cancelled immediately.
+	template<class T>
+	void QueryRay(T* callback, const b3RayCastInput& input) const;
+
+	// Validate a given node of this tree.
+	void Validate(i32 node) const;
+
+	// Draw this tree.
+	void Draw(b3Draw* b3Draw) const;
+private :
+	struct b3Node 
+	{
+		bool IsLeaf() const 
+		{
+			return child1 == NULL_NODE; //or child 2 == NULL_NODE, or height == 0.
+		}
+
+		// The (enlarged) AABB of this node.
+		b3AABB3 aabb;
+
+		// The associated user data.
+		void* userData;
+
+		union 
+		{
+			i32 parent;
+			i32 next;
+		};
+
+		i32 child1;
+		i32 child2;
+
+		// leaf = 0, free node = -1
+		i32 height;
+	};
+	
+	// Insert a allocated (leaf) node into the tree.
+	void InsertLeaf(i32 node);
+	
+	// Remove a allocated node from the tree.
+	void RemoveLeaf(i32 node);
+
+	// Rebuild the tree hierarchy starting from the given node.
+	void WalkBackNodeAndCombineVolumes(i32 node);
+	
+	// Perform a basic surface area heuristic search to find the best
+	// node that can be merged with a given AABB.
+	i32 HeuristicSearch(const b3AABB3& leafAABB) const;
+
+	// Peel a node from the free list and insert into the node array. 
+	// Allocate a new node if necessary. The function returns the new node index.
+	i32 AllocateNode();
+
+	// Free a node (not destroy) from the node pool and add it to the free list.
+	void FreeNode(i32 node);
+
+	// Make a node available for the next allocation request.
+	void AddToFreeList(i32 node);
+
+	// The root of this tree.
+	i32 m_root;
+
+	// The nodes of this tree stored in an array.
+	b3Node* m_nodes;
+	i32 m_nodeCount;
+	i32 m_nodeCapacity;
+	i32 m_freeList;
+};
+
+inline bool b3DynamicTree::TestOverlap(i32 proxy1, i32 proxy2) const 
+{
+	B3_ASSERT(proxy1 < m_nodeCount);
+	B3_ASSERT(proxy2 < m_nodeCount);
+	return b3TestOverlap(m_nodes[proxy1].aabb, m_nodes[proxy2].aabb);
+}
+
+inline const b3AABB3& b3DynamicTree::GetAABB(i32 proxyId) const 
+{
+	B3_ASSERT(proxyId < m_nodeCount);
+	return m_nodes[proxyId].aabb;
+}
+
+inline void* b3DynamicTree::GetUserData(i32 proxyId) const 
+{
+	B3_ASSERT(proxyId < m_nodeCount);
+	return m_nodes[proxyId].userData;
+}
+
+template<class T>
+inline void b3DynamicTree::QueryAABB(T* callback, const b3AABB3& aabb) const 
+{
+	b3Stack<i32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.IsEmpty() == false) 
+	{
+		i32 nodeIndex = stack.Top();
+		stack.Pop();
+
+		if (nodeIndex == NULL_NODE) 
+		{
+			continue;
+		}
+
+		const b3Node* node = m_nodes + nodeIndex;
+
+		if (b3TestOverlap(node->aabb, aabb) == true) 
+		{
+			if (node->IsLeaf() == true) 
+			{
+				if (callback->Report(nodeIndex) == false) 
+				{
+					return;
+				}
+			}
+			else 
+			{
+				stack.Push(node->child1);
+				stack.Push(node->child2);
+			}
+		}
+	}
+}
+
+template<class T>
+inline void b3DynamicTree::QueryRay(T* callback, const b3RayCastInput& input) const 
+{
+	b3Vec3 p1 = input.p1;
+	b3Vec3 p2 = input.p2;
+	b3Vec3 d = p2 - p1;
+	float32 maxFraction = input.maxFraction;
+
+	// Ensure non-degeneracy.
+	B3_ASSERT(b3Dot(d, d) > B3_EPSILON * B3_EPSILON);
+
+	b3Stack<i32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.IsEmpty() == false) 
+	{
+		i32 nodeIndex = stack.Top();
+		
+		stack.Pop();
+
+		if (nodeIndex == NULL_NODE) 
+		{
+			continue;
+		}
+
+		const b3Node* node = m_nodes + nodeIndex;
+
+		float32 minFraction = 0.0f;
+		if (node->aabb.TestRay(p1, p2, maxFraction, minFraction) == true) 
+		{
+			if (node->IsLeaf() == true) 
+			{
+				b3RayCastInput subInput;
+				subInput.p1 = input.p1;
+				subInput.p2 = input.p2;
+				subInput.maxFraction = maxFraction;
+
+				float32 newFraction = callback->Report(subInput, nodeIndex);
+
+				if (newFraction == 0.0f)
+				{
+					// The client has stopped the query.
+					return;
+				}
+			}
+			else 
+			{
+				stack.Push(node->child1);
+				stack.Push(node->child2);
+			}
+		}
+	}
+}
+
+#endif

include/bounce/collision/trees/static_tree.h

@@ -0,0 +1,195 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_STATIC_TREE_H
+#define B3_STATIC_TREE_H
+
+#include <bounce\common\draw.h>
+#include <bounce\common\template\stack.h>
+#include <bounce\collision\shapes\aabb3.h>
+#include <bounce\collision\distance.h>
+
+#define NULL_NODE_S (0xFFFFFFFF)
+
+// An AABB tree for static AABBs.
+class b3StaticTree 
+{
+public:
+	b3StaticTree();
+	~b3StaticTree();
+
+	// Build the tree.
+	// Output a sorted index array.
+	void Build(u32* indices, const b3AABB3* aabbs, u32 count);
+
+	// Get the AABB of a given proxy.
+	const b3AABB3& GetAABB(u32 proxyId) const;
+
+	// Get the index associated of a given proxy.
+	u32 GetUserData(u32 proxyId) const;
+
+	// Report the client callback all AABBs that are overlapping with
+	// the given AABB. The client callback must return true if the query 
+	// must be stopped or false to continue looking for more overlapping pairs.
+	template<class T>
+	void QueryAABB(T* callback, const b3AABB3& aabb) const;
+
+	// Report the client callback all AABBs that are overlapping with
+	// the given ray. The client callback must return the new intersection fraction 
+	// (real). If the fraction == 0 then the query is cancelled immediatly.
+	template<class T>
+	void QueryRay(T* callback, const b3RayCastInput& input) const;
+
+	// Draw the hierarchy.
+	void Draw(b3Draw* b3Draw) const;
+private :
+	// A node in a static tree.
+	struct b3Node
+	{
+		b3AABB3 aabb;
+		u32 child1;
+		union
+		{
+			u32 child2;
+			u32 index;
+		};
+
+		// Check if a node is a leaf node
+		bool IsLeaf() const
+		{
+			return child1 == NULL_NODE_S;
+		}
+	};
+
+	u32 m_nodeCount;
+	b3Node* m_nodes;
+};
+
+inline const b3AABB3& b3StaticTree::GetAABB(u32 proxyId) const
+{
+	B3_ASSERT(proxyId < m_nodeCount);
+	return m_nodes[proxyId].aabb;
+}
+
+inline u32 b3StaticTree::GetUserData(u32 proxyId) const
+{
+	B3_ASSERT(proxyId < m_nodeCount);
+	B3_ASSERT(m_nodes[proxyId].IsLeaf());
+	return m_nodes[proxyId].index;
+}
+
+template<class T>
+inline void b3StaticTree::QueryAABB(T* callback, const b3AABB3& aabb) const
+{
+	if (m_nodeCount == 0) 
+	{
+		return;
+	}
+
+	u32 root = 0;
+
+	b3Stack<u32, 256> stack;
+	stack.Push(root);
+
+	while (stack.IsEmpty() == false) 
+	{
+		u32 nodeIndex = stack.Top();
+		
+		stack.Pop();
+
+		const b3Node* node = m_nodes + nodeIndex;
+
+		if (b3TestOverlap(node->aabb, aabb) == true) 
+		{
+			if (node->IsLeaf() == true) 
+			{
+				if (callback->Report(nodeIndex) == false) 
+				{
+					return;
+				}
+			}
+			else 
+			{
+				stack.Push(node->child1);
+				stack.Push(node->child2);
+			}
+		}
+	}
+}
+
+template<class T>
+inline void b3StaticTree::QueryRay(T* callback, const b3RayCastInput& input) const 
+{
+	if (m_nodeCount == 0)
+	{
+		return;
+	}
+
+	b3Vec3 p1 = input.p1;
+	b3Vec3 p2 = input.p2;
+	b3Vec3 d = p2 - p1;
+	float32 maxFraction = input.maxFraction;
+
+	// Ensure non-degeneracy.
+	B3_ASSERT(b3Dot(d, d) > B3_EPSILON * B3_EPSILON);
+
+	u32 root = 0;
+
+	b3Stack<u32, 256> stack;
+	stack.Push(root);
+
+	while (stack.IsEmpty() == false) 
+	{
+		i32 nodeIndex = stack.Top();	
+		stack.Pop();
+
+		if (nodeIndex == NULL_NODE_S)
+		{
+			continue;
+		}
+
+		const b3Node* node = m_nodes + nodeIndex;
+
+		float32 minFraction = 0.0f;
+		if (node->aabb.TestRay(p1, p2, maxFraction, minFraction) == true) 
+		{
+			if (node->IsLeaf() == true) 
+			{
+				b3RayCastInput subInput;
+				subInput.p1 = input.p1;
+				subInput.p2 = input.p2;
+				subInput.maxFraction = maxFraction;
+
+				float32 newFraction = callback->Report(subInput, nodeIndex);
+
+				if (newFraction == 0.0f) 
+				{
+					// The client has stopped the query.
+					return;
+				}
+			}
+			else 
+			{
+				stack.Push(node->child1);
+				stack.Push(node->child2);
+			}
+		}
+	}
+}
+
+#endif

include/bounce/common/draw.h

@@ -0,0 +1,108 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_DRAW_H
+#define B3_DRAW_H
+
+#include <bounce\common\math\math.h>
+#include <bounce\collision\shapes\aabb3.h>
+
+// Color channels used by the debug b3Draw interface.
+struct b3Color 
+{
+	b3Color() { }
+	
+	b3Color(float32 R, float32 G, float32 B, float32 A = 1.0f) : r(R),	g(G), b(B),	a(A) { }
+	
+	float32 r, g, b, a;
+};
+
+// Implement this interface and set to a world so it can b3Draw the physics entities.
+class b3Draw 
+{
+public :
+	// Bit flags to tell the world what needs to be b3Draw.
+	enum b3Flags
+	{
+		e_shapesFlag = 0x0001,
+		e_centerOfMassesFlag = 0x0002,
+		e_jointsFlag = 0x0004,
+		e_contactPointsFlag = 0x0008,
+		e_contactNormalsFlag = 0x0010,
+		e_contactTangentsFlag = 0x0020,
+		e_aabbsFlag = 0x0040,
+	};
+
+	b3Draw()
+	{
+		m_flags = 0;
+	}
+	
+	~b3Draw()
+	{
+
+	}
+
+	void SetFlags(u32 flags);
+	void AppendFlags(u32 flags);
+	
+	// Draw a point.
+	virtual void DrawPoint(const b3Vec3& point, const b3Color& color) = 0;
+
+	// Draw a line segment.
+	virtual void DrawSegment(const b3Vec3& a, const b3Vec3& b, const b3Color& color) = 0;
+	
+	// Draw a polygon with vertices ordered CCW.
+	virtual void DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color) = 0;
+	
+	// Draw a solid polygon with vertices ordered CCW.
+	virtual void DrawSolidPolygon(const b3Vec3* vertices, u32 count, const b3Color& color) = 0;
+
+	// Draw a circle with center, normal, and radius.
+	virtual void DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0;
+	
+	// Draw a solid circle with center, normal, and radius.
+	virtual void DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color) = 0;
+
+	// Draw a sphere with center and radius.
+	virtual void DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0;
+
+	// Draw a solid sphere with center and radius.
+	virtual void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color) = 0;
+	
+	// Draw a AABB.
+	virtual void DrawAABB(const b3AABB3& aabb, const b3Color& color) = 0;
+
+	// Draw a b3Transform.
+	virtual void DrawTransform(const b3Transform& xf) = 0;
+
+	// Debug b3Draw flags.
+	u32 m_flags;
+};
+
+inline void b3Draw::SetFlags(u32 flags)
+{
+	m_flags = flags;
+}
+
+inline void b3Draw::AppendFlags(u32 flags)
+{
+	m_flags |= flags;
+}
+
+#endif

include/bounce/common/geometry.h

@@ -0,0 +1,190 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GEOMETRY_H
+#define B3_GEOMETRY_H
+
+#include <bounce\common\math\math.h>
+#include <bounce\common\math\transform.h>
+
+// A triangle in indexed form.
+struct b3Triangle
+{
+	// Does nothing for performance.
+	b3Triangle() { }
+
+	// Set this triangle from three vertices.
+	b3Triangle(u32 _v1, u32 _v2, u32 _v3) 
+	{
+		v1 = _v1;
+		v2 = _v2;
+		v3 = _v3;
+	}
+	
+	// Set this triangle from three vertices.
+	void Set(u32 _v1, u32 _v2, u32 _v3)
+	{
+		v1 = _v1;
+		v2 = _v2;
+		v3 = _v3;
+	}
+
+	// Test if this triangle contains a given vertex.
+	bool TestVertex(u32 v) const
+	{
+		return v == v1 || v == v2 || v == v3;
+	}
+
+	// Test if this triangle contains two vertices.
+	bool TestEdge(u32 _v1, u32 _v2) const
+	{
+		return TestVertex(_v1) && TestVertex(_v2);
+	}
+
+	u32 v1, v2, v3;
+};
+
+// A plane in constant normal form.
+// dot(n, p) - d = 0.
+struct b3Plane
+{
+	// Does nothing for performance.
+	b3Plane() { }
+	
+	// Set this plane from a normal and a signed distance from its origin.
+	b3Plane(const b3Vec3& _normal, float32 _offset) 
+	{
+		normal = _normal;
+		offset = _offset;
+	}
+	
+	// Set this plane from a normal and a point on the plane.
+	b3Plane(const b3Vec3& _normal, const b3Vec3& _point)
+	{ 
+		normal = _normal;
+		offset = b3Dot(_normal, _point);
+	}
+
+	// Compute this plane from three non-colinear points.
+	b3Plane(const b3Vec3& A, const b3Vec3& B, const b3Vec3& C)
+	{
+		b3Vec3 N = b3Cross(B - A, C - A);
+		normal = b3Normalize(N);
+		offset = b3Dot(normal, A);
+	}
+
+	b3Vec3 normal;
+	float32 offset;
+};
+
+// Transform a plane by a given frame.
+inline b3Plane operator*(const b3Transform& T, const b3Plane& plane)
+{
+	b3Vec3 normal = b3Mul(T.rotation, plane.normal);
+	return b3Plane(normal, plane.offset + b3Dot(normal, T.position));
+}
+
+// Transform a plane by a given frame.
+inline b3Plane b3Mul(const b3Transform& T, const b3Plane& plane)
+{
+	b3Vec3 normal = b3Mul(T.rotation, plane.normal);
+	return b3Plane(normal, plane.offset + b3Dot(normal, T.position));
+}
+
+inline float32 b3Distance(const b3Vec3& P, const b3Plane& plane)
+{
+	return b3Dot(plane.normal, P) - plane.offset;
+}
+
+// Project a point onto a plane.
+// The plane must be normalized.
+inline b3Vec3 b3Project(const b3Vec3& P, const b3Plane& plane)
+{
+	float32 fraction = b3Distance(P, plane);
+	return P - fraction * plane.normal;
+}
+
+// Compute barycentric coordinates (u, v) for point Q to segment AB.
+// The last output value is the divisor.
+inline void b3Barycentric(float32 out[3], 
+	const b3Vec3& A, const b3Vec3& B, 
+	const b3Vec3& Q)
+{
+	b3Vec3 AB = B - A;
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	//float32 divisor = b3Dot(AB, AB);
+	out[0] = b3Dot(QB, AB);
+	out[1] = -b3Dot(QA, AB);
+	out[2] = out[0] + out[1];
+}
+
+// Compute barycentric coordinates (u, v, w) for point Q to triangle ABC.
+// The last output value is the divisor.
+inline void b3Barycentric(float32 out[4], 
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, 
+	const b3Vec3& Q)
+{
+	// RTCD, 140.
+	b3Vec3 AB = B - A;
+	b3Vec3 AC = C - A;
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+	
+	b3Vec3 QB_x_QC = b3Cross(QB, QC);
+	b3Vec3 QC_x_QA = b3Cross(QC, QA);
+	b3Vec3 QA_x_QB = b3Cross(QA, QB);
+	
+	b3Vec3 AB_x_AC = b3Cross(AB, AC);
+	//float32 divisor = b3Dot(AB_x_AC, AB_x_AC);
+
+	out[0] = b3Dot(QB_x_QC, AB_x_AC);
+	out[1] = b3Dot(QC_x_QA, AB_x_AC);
+	out[2] = b3Dot(QA_x_QB, AB_x_AC);
+	out[3] = out[0] + out[1] + out[2];
+}
+
+// Compute barycentric coordinates (u, v, w, x) for point Q to tetrahedron ABCD.
+// The last output value is the (positive) divisor.
+inline void b3Barycentric(float32 out[5], 
+	const b3Vec3& A, const b3Vec3& B, const b3Vec3& C, const b3Vec3& D, 
+	const b3Vec3& Q)
+{
+	// RTCD, 48, 49.
+	b3Vec3 AB = B - A;
+	b3Vec3 AC = C - A;
+	b3Vec3 AD = D - A;
+
+	b3Vec3 QA = A - Q;
+	b3Vec3 QB = B - Q;
+	b3Vec3 QC = C - Q;
+	b3Vec3 QD = D - Q;
+
+	float32 divisor = b3Det(AB, AC, AD);
+	float32 sign = b3Sign(divisor);
+
+	out[0] = sign * b3Det(QB, QC, QD);
+	out[1] = sign * b3Det(QA, QD, QC);
+	out[2] = sign * b3Det(QA, QB, QD);
+	out[3] = sign * b3Det(QA, QC, QB);
+	out[4] = sign * divisor;
+}
+
+#endif

include/bounce/common/math/mat.h

@@ -0,0 +1,85 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MAT_H
+#define B3_MAT_H
+
+#include <bounce\common\math\math.h>
+
+// A vector stored in column-major order.
+template<u32 n>
+struct b3Vec
+{
+	b3Vec() { }
+
+	const float32& operator[](u32 i) const
+	{
+		return e[i];
+	}
+	
+	float32& operator[](u32 i)
+	{
+		return e[i];
+	}
+	
+	void operator+=(const b3Vec<n>& v)
+	{
+		for (u32 i = 0; i < n; ++i)
+		{
+			e[i] += v[i];
+		}
+	}
+
+	float32 e[n];
+};
+
+template<u32 n>
+inline b3Vec<n> operator-(const b3Vec<n>& v)
+{
+	b3Vec<n> result;
+	for (u32 i = 0; i < n; ++i)
+	{
+		result[i] = -v[i];
+	}
+	return result;
+}
+
+// A matrix stored in column-major order.
+template<u32 n, u32 m>
+struct b3Mat
+{
+	b3Mat() { }
+
+	const float32& operator()(u32 i, u32 j) const
+	{
+		return e[i + n * j];
+	}
+
+	float32& operator()(u32 i, u32 j)
+	{
+		return e[i + n * j];
+	}
+
+	float32 e[n * m];
+};
+
+// Solve Ax = b.
+// Warning: Make sure to pass a copy of A to the function. It will be invalidated.
+bool b3Solve(float32* b, float32* A, u32 n);
+
+#endif

include/bounce/common/math/mat22.h

@@ -0,0 +1,59 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MAT_22_H
+#define B3_MAT_22_H
+
+#include <bounce\common\math\vec2.h>
+
+// A 2-by-2 matrix stored in column-major order.
+struct b3Mat22
+{
+	// Does nothing for performance.
+	b3Mat22() { }
+
+	// Set this matrix from two vector elements.
+	b3Mat22(const b3Vec2& _x, const b3Vec2& _y) : x(_x), y(_y) { }
+
+	// Solve Ax = b. 
+	// It doesn't compute the inverse. 
+	// Therefore, is more efficient.
+	// Returns the zero vector if the matrix is singular.
+	b3Vec2 Solve(const b3Vec2& b) const;
+
+	b3Vec2 x, y;
+};
+
+// Multiply a matrix times a vector.
+inline b3Vec2 operator*(const b3Mat22& A, const b3Vec2& v)
+{
+	return v.x * A.x + v.y * A.y;
+}
+
+// Multiply a matrix times a vector.
+inline b3Vec2 b3Mul(const b3Mat22& A, const b3Vec2& v)
+{
+	return v.x * A.x + v.y * A.y;
+}
+
+// Invert a matrix.
+// If the matrix determinant is zero this returns 
+// the zero matrix.
+inline b3Mat22 b3Inverse(const b3Mat22& A);
+
+#endif

include/bounce/common/math/mat33.h

@@ -0,0 +1,257 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MAT_33_H
+#define B3_MAT_33_H
+
+#include <bounce\common\math\vec3.h>
+
+// A 3-by-3 matrix stored in column-major order.
+struct b3Mat33 
+{
+	// Does nothing for performance.
+	b3Mat33() { }
+
+	// Set this matrix from three elements.
+	b3Mat33(const b3Vec3& _x, const b3Vec3& _y, const b3Vec3& _z) :	x(_x), y(_y), z(_z) { }
+
+	// Read an indexed column vector from this matrix.
+	const b3Vec3& operator[](u32 i) const 
+	{
+		return (&x)[i];
+	}
+	
+	// Write an indexed column vector to this matrix.
+	b3Vec3& operator[](u32 i) 
+	{
+		return (&x)[i];
+	}
+
+	// Add a matrix to this matrix.
+	void operator+=(const b3Mat33& B)
+	{
+		x += B.x;
+		y += B.y;
+		z += B.z;
+	}
+
+	// Set this matrix to the zero matrix.
+	void SetZero() 
+	{
+		x.SetZero();
+		y.SetZero();
+		z.SetZero();
+	}
+
+	// Set this matrix to the identity matrix.
+	void SetIdentity() 
+	{
+		x.Set(1.0f, 0.0f, 0.0f);
+		y.Set(0.0f, 1.0f, 0.0f);
+		z.Set(0.0f, 0.0f, 1.0f);
+	}
+
+	// Solve Ax = b. 
+	// It doesn't compute the inverse. 
+	// Therefore, is more efficient.
+	// Returns the zero vector if the matrix is singular.
+	b3Vec3 Solve(const b3Vec3& b) const;
+
+	float32 operator()(u32 i, u32 j) const
+	{
+		return (&x.x)[i + 3 * j];
+	}
+
+	b3Vec3 x, y, z;
+};
+
+// Add two matrices.
+inline b3Mat33 operator+(const b3Mat33& A, const b3Mat33& B) 
+{
+	return b3Mat33(A.x + B.x, A.y + B.y, A.z + B.z);
+}
+
+// Subtract two matrices.
+inline b3Mat33 operator-(const b3Mat33& A, const b3Mat33& B) 
+{
+	return b3Mat33(A.x - B.x, A.y - B.y, A.z - B.z);
+}
+
+// Multiply a scalar times a matrix.
+inline b3Mat33 operator*(float32 s, const b3Mat33& A) 
+{
+	return b3Mat33(s * A.x, s * A.y, s * A.z);
+}
+
+// Negate a matrix.
+inline b3Mat33 operator-(const b3Mat33& A)
+{
+	return -1.0f * A;
+}
+
+// Multiply a matrix times a vector. If the matrix 
+// represents a rotation this transforms the vector 
+// from one frame to another.
+inline b3Vec3 operator*(const b3Mat33& A, const b3Vec3& v) 
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z;
+}
+
+// Multiply two matrices.
+inline b3Mat33 operator*(const b3Mat33& A, const b3Mat33& B) 
+{
+	return b3Mat33(A * B.x, A * B.y, A * B.z);
+}
+
+// Multiply a matrix times a vector. If the matrix 
+// represents a rotation this transforms the vector 
+// from one frame to another.
+inline b3Vec3 b3Mul(const b3Mat33& A, const b3Vec3& v)
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z;
+}
+
+// Multiply two matrices.
+inline b3Mat33 b3Mul(const b3Mat33& A, const b3Mat33& B)
+{
+	return b3Mat33( b3Mul(A, B.x), b3Mul(A, B.y), b3Mul(A, B.z));
+}
+
+// Multiply the transpose of a matrix times a vector. If 
+// the matrix represents a rotation frame this transforms the 
+// vector from one frame to another (inverse b3Transform).
+inline b3Vec3 b3MulT(const b3Mat33& A, const b3Vec3& v) 
+{
+	return b3Vec3(b3Dot(A.x, v), b3Dot(A.y, v), b3Dot(A.z, v));
+}
+
+// Multiply the transpose of a matrix times another.
+inline b3Mat33 b3MulT(const b3Mat33& A, const b3Mat33& B) 
+{
+	return b3Mat33(
+		b3Vec3(b3Dot(A.x, B.x), b3Dot(A.y, B.x), b3Dot(A.z, B.x)),
+		b3Vec3(b3Dot(A.x, B.y), b3Dot(A.y, B.y), b3Dot(A.z, B.y)),
+		b3Vec3(b3Dot(A.x, B.z), b3Dot(A.y, B.z), b3Dot(A.z, B.z)));
+}
+
+// Transpose a matrix.
+inline b3Mat33 b3Transpose(const b3Mat33& A) 
+{
+	return b3Mat33(
+		b3Vec3(A.x.x, A.y.x, A.z.x),
+		b3Vec3(A.x.y, A.y.y, A.z.y),
+		b3Vec3(A.x.z, A.y.z, A.z.z)
+		);
+}
+
+// Uniform scale matrix.
+inline b3Mat33 b3Diagonal(float32 s) 
+{
+	return b3Mat33(
+		b3Vec3(s, 0.0f, 0.0f),
+		b3Vec3(0.0f, s, 0.0f),
+		b3Vec3(0.0f, 0.0f, s)
+		);
+}
+
+// Uniform or non-uniform scale matrix.
+inline b3Mat33 b3Diagonal(float32 x, float32 y, float32 z)
+{
+	return b3Mat33(
+		b3Vec3(x, 0.0f, 0.0f),
+		b3Vec3(0.0f, y, 0.0f),
+		b3Vec3(0.0f, 0.0f, z)
+		);
+}
+
+// Invert a matrix.
+// If the matrix is singular this 
+// returns the zero matrix.
+b3Mat33 b3Inverse(const b3Mat33& A);
+
+// Return a skew (anti-symmetric) matrix for a vector.
+inline b3Mat33 b3Skew(const b3Vec3& v) 
+{
+	return b3Mat33(
+		b3Vec3(0.0f, v.z, -v.y),
+		b3Vec3(-v.z, 0.0f, v.x),
+		b3Vec3(v.y, -v.x, 0.0f)
+		);
+}
+
+// Compute the dot product of two vectors.
+inline float32 b3Inner(const b3Vec3& a, const b3Vec3& b)
+{
+	return b3Dot(a, b);
+}
+
+// Compute the outer product of two vectors.
+// The result is a matrix A = a * b^T.
+inline b3Mat33 b3Outer(const b3Vec3& a, const b3Vec3& b) 
+{
+	return b3Mat33(b.x * a, b.y * a, b.z * a);
+}
+
+// Move an inertia tensor from the its current center
+// to another.
+inline b3Mat33 b3MoveToCOM(const b3Mat33& inertia, float32 mass, const b3Vec3& center)
+{
+	// Paralell Axis Theorem
+	// J = I + m * dot(r, r) * E - outer(r, r)
+	// where
+	// I - inertia about the center of mass
+	// m - mass
+	// E - identity 3x3
+	// r - displacement vector from the current com to the new com
+	// J - inertia tensor at the new center of rotation
+	float32 dd = b3Dot(center, center);
+	b3Mat33 A = b3Diagonal(mass * dd);
+	b3Mat33 B = b3Outer(center, center);
+	return inertia + A - B;
+}
+
+// Compute the inertia matrix of a body measured in 
+// inertial frame (variable over time) given the 
+// inertia matrix in body-fixed frame (constant) 
+// and a rotation matrix representing the orientation 
+// of the body frame relative to the inertial frame.
+inline b3Mat33 b3RotateToFrame(const b3Mat33& inertia, const b3Mat33& rotation)
+{
+	return rotation * inertia * b3Transpose(rotation);
+}
+
+// Compute an orthogonal basis given one of its vectors.
+// The vector must be normalized.
+inline b3Mat33 b3Basis(const b3Vec3& a)
+{
+	// Box2D
+	b3Mat33 A;
+	if (b3Abs(a.x) >= float32(0.57735027))
+	{
+		A.y.Set(a.y, -a.x, 0.0f);
+	}
+	else
+	{
+		A.y.Set(0.0f, a.z, -a.y);
+	}
+	A.y = b3Normalize(A.y);
+	A.z = b3Cross(a, A.y);
+	return A;
+}
+
+#endif

include/bounce/common/math/math.h

@@ -0,0 +1,96 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MATH_H
+#define B3_MATH_H
+
+#include <cmath>
+#include <bounce\common\settings.h>
+
+inline bool b3IsValid(float32 fx)
+{
+	u32 ix = *(u32*)(&fx);
+	return (ix & 0x7F800000) != 0x7F800000;
+}
+
+inline float32 b3Sqrt(float32 x) 
+{
+	return sqrtf(x);
+}
+
+template <class T>
+inline T b3Abs(T x) 
+{
+	return abs(x);
+}
+
+template <class T>
+inline T b3Min(T a, T b) 
+{
+	return a < b ? a : b;
+}
+
+template <class T>
+inline T b3Max(T a, T b) 
+{
+	return a > b ? a : b;
+}
+
+template <class T>
+inline T b3Clamp(T a, T low, T high) 
+{
+	return b3Max(low, b3Min(a, high));
+}
+
+template <class T>
+inline void b3Swap(T& a, T& b) 
+{
+	T tmp = a;
+	a = b;
+	b = tmp;
+}
+
+template <class T>
+inline T b3Sign(T x)
+{
+	return T(T(0) < x) - T(T(x) < T(0));
+}
+
+template <class T>
+inline u32 b3UniqueCount(T* V, u32 N)
+{
+	u32 count = 0;
+	for (u32 i = 0; i < N; ++i) 
+	{
+		u32 j;
+		for (j = 0; j < N; ++j)
+		{
+			if (V[i] == V[j])
+			{
+				break;
+			}
+		}		
+		if (i == j)
+		{
+			++count;
+		}
+	}
+	return count;
+}
+
+#endif

include/bounce/common/math/quat.h

@@ -0,0 +1,237 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_QUAT_H
+#define B3_QUAT_H
+
+#include <bounce\common\math\math.h>
+#include <bounce\common\math\mat33.h>
+
+// A quaternion represents an orientation with 4 real numbers.
+struct b3Quat 
+{
+	// Default constructor does nothing for performance.
+	b3Quat() { }
+
+	// Set this quaternion from four values.
+	b3Quat(float32 _x, float32 _y, float32 _z, float32 _w) : x(_x), y(_y), z(_z), w(_w) { }
+
+	// Set this quaternion from an axis and an angle 
+	// of rotation about the axis.
+	b3Quat(const b3Vec3& axis, float32 angle)
+	{
+		float32 theta = 0.5f * angle;
+		float32 s = sin(theta);
+		x = s * axis.x;
+		y = s * axis.y;
+		z = s * axis.z;
+		w = cos(theta);
+	}
+
+	// Add a quaternion to this quaternion.
+	void operator+=(const b3Quat& q)
+	{
+		x += q.x;
+		y += q.y;
+		z += q.z;
+		w += q.w;
+	}
+
+	// Subtract a quaternion from this quaternion.
+	void operator-=(const b3Quat& q)
+	{
+		x -= q.x;
+		y -= q.y;
+		z -= q.z;
+		w -= q.w;
+	}
+
+	// Set this quaternion to identity.
+	void SetIdentity() 
+	{
+		x = y = z = 0.0f; 
+		w = 1.0f;
+	}
+
+	// Set this quaternion from four values.
+	void Set(float32 _x, float32 _y, float32 _z, float32 _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+	
+	// Set this quaternion from an axis and an angle 
+	// of rotation about the axis.
+	void Set(const b3Vec3& axis, float32 angle)
+	{
+		float32 theta = 0.5f * angle;
+		float32 s = sin(theta);
+		x = s * axis.x;
+		y = s * axis.y;
+		z = s * axis.z;
+		w = cos(theta);
+	}
+
+	// Normalize this quaternion.
+	void Normalize()
+	{
+		float32 s = b3Sqrt(x * x + y * y + z * z + w * w);
+		if (s != 0.0f)
+		{
+			x /= s;
+			y /= s;
+			z /= s;
+			w /= s;
+		}
+	}
+
+	float32 x, y, z, w;
+};
+
+// Add two quaternions.
+inline b3Quat operator+(const b3Quat& a, const b3Quat& b)
+{
+	return b3Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+
+// Sobtract two quaternions.
+inline b3Quat operator-(const b3Quat& a, const b3Quat& b)
+{
+	return b3Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
+}
+
+// Multiply a quaternion by a scalar.
+inline b3Quat operator*(float32 s, const b3Quat& q)
+{
+	return b3Quat(s * q.x, s * q.y, s * q.z, s * q.w);
+}
+
+// Negate a quaternion.
+inline b3Quat operator-(const b3Quat& q)
+{
+	return b3Quat(-q.x, -q.y, -q.z, -q.w);
+}
+
+// Compute a quaternion-quaternion product.
+inline b3Quat operator*(const b3Quat& a, const b3Quat& b)
+{
+	return b3Quat(
+		a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
+		a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
+		a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
+		a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
+}
+
+// Compute the length of a quaternion.
+inline float32 b3Length(const b3Quat& q)
+{
+	return b3Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+}
+
+// Normalize a quarternion.
+inline b3Quat b3Normalize(const b3Quat& q)
+{
+	float32 s = b3Length(q);
+	if (s != 0.0f)
+	{
+		s = 1.0f / s;
+		return s * q;
+	}
+	return b3Quat(0.0f, 0.0f, 0.0f, 1.0f);
+}
+
+// Compute the dot poduct of two quaternions.
+inline float b3Dot(const b3Quat& a, const b3Quat& b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
+}
+
+// Compute the conjugate of a quaternion.
+inline b3Quat b3Conjugate(const b3Quat& q)
+{
+	return b3Quat(-q.x, -q.y, -q.z, q.w);
+}
+
+// Rotate a vector by an orientation quaternion.
+inline b3Vec3 b3Mul(const b3Quat& q, const b3Vec3& v)
+{
+	b3Vec3 qv(q.x, q.y, q.z);
+	float32 qs = q.w;
+	
+	b3Vec3 t = 2.0f * b3Cross(qv, v);
+	return v + qs * t + b3Cross(qv, t);
+}
+
+// Convert a quaternion to a 3-by-3 rotation matrix.
+inline b3Mat33 b3ConvertQuatToRot(const b3Quat& q)
+{
+	float32 x = q.x, y = q.y, z = q.z, w = q.w;
+	float32 x2 = x + x, y2 = y + y, z2 = z + z;
+	float32 xx = x * x2, xy = x * y2, xz = x * z2;
+	float32 yy = y * y2, yz = y * z2, zz = z * z2;
+	float32 wx = w * x2, wy = w * y2, wz = w * z2;
+
+	return b3Mat33(
+		b3Vec3(1.0f - (yy + zz),          xy + wz, xz - wy),
+		b3Vec3(         xy - wz, 1.0f - (xx + zz), yz + wx),
+		b3Vec3(         xz + wy,          yz - wx, 1.0f - (xx + yy)));
+}
+
+// Perform a linear interpolation between two quaternions.
+inline b3Quat b3Lerp(const b3Quat& a, const b3Quat& b, float32 fraction)
+{
+	B3_ASSERT(fraction >= 0.0f);
+	B3_ASSERT(fraction <= 1.0f);
+	float32 w1 = 1.0f - fraction;
+	float32 w2 = fraction;
+	return w1 * a + w2 * b;
+}
+
+// Perform a spherical interpolation between two quaternions.
+inline b3Quat b3Slerp(const b3Quat& a, const b3Quat& b, float32 fraction)
+{
+	B3_ASSERT(fraction >= 0.0f);
+	B3_ASSERT(fraction <= 1.0f);
+	float32 w1 = 1.0f - fraction;
+	float32 w2 = fraction;
+
+	float32 cosine = b3Dot(a, b);
+	b3Quat b2 = b;
+	if (cosine <= FLT_EPSILON * FLT_EPSILON)
+	{
+		b2 = -b;
+		cosine = -cosine;
+	}
+
+	if (cosine > 1.0f - FLT_EPSILON)
+	{
+		return w1 * a + w2 * b2;
+	}
+
+	float32 angle = acos(cosine);
+	float32 sine = sin(angle);
+	b3Quat q1 = sin(w1 * angle) * a;
+	b3Quat q2 = sin(w2 * angle) * b2;
+	float32 invSin = 1.0f / sine;
+
+	return invSin * (q1 + q2);
+}
+
+#endif

include/bounce/common/math/transform.h

@@ -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.
+*/
+
+#ifndef B3_TRANSFORM_H
+#define B3_TRANSFORM_H
+
+#include <bounce\common\math\mat33.h>
+#include <bounce\common\math\quat.h>
+
+// A b3Transform represents a rigid frame. 
+// It has a translation representing a position
+// and a rotation representing an orientation.
+struct b3Transform 
+{
+	// Default ctor does nothing for performance.
+	b3Transform() { }
+	
+	// Set this b3Transform from a translation vector and an orientation 
+	// quaternion.
+	b3Transform(const b3Vec3& p, const b3Quat& q)
+	{
+		position = p;
+		rotation = b3ConvertQuatToRot(q);
+	}
+	
+	// Set this b3Transform to the identity.
+	void SetIdentity() 
+	{
+		position.SetZero();
+		rotation.SetIdentity();
+	}
+
+	b3Vec3 position; // in fact a translation
+	b3Mat33 rotation;
+};
+
+// Motion proxy for TOI computation.
+struct b3Sweep
+{
+	b3Vec3 localCenter; // local center
+		
+	b3Vec3 worldCenter0; // last world center
+	b3Quat orientation0; // last orientation
+	float32 t0; // last fraction between [0, 1]
+
+	b3Vec3 worldCenter; // world center
+	b3Quat orientation; // world orientation
+
+	// Get this sweep b3Transform at a given time between [0, 1]
+	b3Transform GetTransform(float32 t) const;
+
+	// Advance to a new initial state.
+	void Advance(float32 t);
+};
+
+inline b3Transform b3Sweep::GetTransform(float32 t) const
+{
+	b3Vec3 c = (1.0f - t) * worldCenter0 + t * worldCenter;
+	b3Quat q = (1.0f - t) * orientation0 + t * orientation;
+	q.Normalize();
+
+	b3Transform xf;
+	xf.rotation = b3ConvertQuatToRot(q);
+	xf.position = c - b3Mul(q, localCenter);
+	return xf;
+}
+
+inline void b3Sweep::Advance(float32 t)
+{
+	B3_ASSERT(t < 1.0f);
+	float32 dt = (t - t0) / (1.0f / t0);
+	worldCenter += dt * (worldCenter - worldCenter0);
+	orientation += dt * (orientation - orientation0);
+	orientation.Normalize();
+	t0 = t;
+}
+
+// Multiply a b3Transform times a vector. If the b3Transform 
+// represents a frame this returns the vector in terms 
+// of the frame.
+inline b3Vec3 operator*(const b3Transform& T, const b3Vec3& v)
+{
+	return b3Mul(T.rotation, v) + T.position;
+}
+
+// Multiply a b3Transform times another b3Transform (composed b3Transform).
+// [A y][B x] = [AB Ax+y]
+// [0 1][0 1]   [0  1   ]
+inline b3Transform operator*(const b3Transform& A, const b3Transform& B)
+{
+	b3Transform C;
+	C.rotation = b3Mul(A.rotation, B.rotation);
+	C.position = b3Mul(A.rotation, B.position) + A.position;
+	return C;
+}
+
+// Multiply a b3Transform times a vector.
+inline b3Vec3 b3Mul(const b3Transform& T, const b3Vec3& v)
+{
+	return b3Mul(T.rotation, v) + T.position;
+}
+
+// Multiply a b3Transform times another b3Transform.
+// [A y][B x] = [AB Ax+y]
+// [0 1][0 1]   [0  1   ]
+inline b3Transform b3Mul(const b3Transform& A, const b3Transform& B)
+{
+	b3Transform C;
+	C.rotation = b3Mul(A.rotation, B.rotation);
+	C.position = b3Mul(A.rotation, B.position) + A.position;
+	return C;
+}
+
+// Multiply the transpose of one b3Transform (inverse 
+// b3Transform) times another b3Transform (composed b3Transform).
+//[A^-1  -A^-1*y][B x] = [A^-1*B A^-1(x-y)]
+//[0      1     ][0 1]   [0      1        ]
+inline b3Transform b3MulT(const b3Transform& A, const b3Transform& B) 
+{
+	b3Transform C;
+	C.rotation = b3MulT(A.rotation, B.rotation);
+	C.position = b3MulT(A.rotation, B.position - A.position);
+	return C;
+}
+
+// Multiply the transpose of a b3Transform times a vector.
+// If the b3Transform represents a frame then this transforms
+// the vector from one frame to another (inverse b3Transform).
+//[A^-1  -A^-1*y][x] = A^-1*x - A^-1*y = A^-1 * (x - y)
+//[0     1      ][1]   
+inline b3Vec3 b3MulT(const b3Transform& A, const b3Vec3& v)
+{
+	return b3MulT(A.rotation, v - A.position);
+}
+
+// Inverse b3Transform.
+inline b3Transform b3Inverse(const b3Transform& T)
+{
+	b3Transform B;
+	B.rotation = b3Transpose(T.rotation);
+	B.position = b3MulT(T.rotation, -T.position);
+	return B;
+}
+
+#endif

include/bounce/common/math/vec2.h

@@ -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.
+*/
+
+#ifndef B3_VEC2_H
+#define B3_VEC2_H
+
+#include <bounce\common\math\math.h>
+
+// A 2D column vector.
+struct b3Vec2
+{
+	// Does nothing for performance.
+	b3Vec2() { }
+
+	// Set this vector from two components.
+	b3Vec2(float32 _x, float32 _y) : x(_x), y(_y) { }
+
+	// Read an indexed component from this vector.
+	float32 operator[](u32 i) const { return (&x)[i]; }
+
+	// Write an indexed component to this vector.
+	float32& operator[](u32 i) { return (&x)[i]; }
+
+	// Add a vector to this vector.
+	void operator+=(const b3Vec2& v)
+	{
+		x += v.x;
+		y += v.y;
+	}
+
+	// Subtract a vector from this vector.
+	void operator-=(const b3Vec2& v)
+	{
+		x -= v.x;
+		y -= v.y;
+	}
+
+	// Scale this vector.
+	void operator*=(float32 s)
+	{
+		x *= s;
+		y *= s;
+	}
+
+	// Scale this vector.
+	void operator/=(float32 s)
+	{
+		x /= s;
+		y /= s;
+	}
+
+	// Set this vector to the zero vector.
+	void SetZero()
+	{
+		x = y = 0.0f;
+	}
+
+	// Set this vector from two components.
+	void Set(float32 _x, float32 _y)
+	{
+		x = _x;
+		y = _y;
+	}
+
+	// Normalize this vector.
+	void Normalize()
+	{
+		float32 s = b3Sqrt(x * x + y * y);
+		if (s > B3_EPSILON)
+		{
+			x /= s;
+			y /= s;
+		}
+	}
+
+	float32 x, y;
+};
+
+// Negate a vector.
+inline b3Vec2 operator-(const b3Vec2& v)
+{
+	return b3Vec2(-v.x, -v.y);
+}
+
+// Add two vectors.
+inline b3Vec2 operator+(const b3Vec2& a, const b3Vec2& b)
+{
+	return b3Vec2(a.x + b.x, a.y + b.y);
+}
+
+// Subtract two vectors.
+inline b3Vec2 operator-(const b3Vec2& a, const b3Vec2& b)
+{
+	return b3Vec2(a.x - b.x, a.y - b.y);
+}
+
+// Multiply a vector by a scalar.
+inline b3Vec2 operator*(float32 s, const b3Vec2& v)
+{
+	return b3Vec2(s * v.x, s * v.y);
+}
+
+// Compute the dot product of two vectors.
+inline float32 b3Dot(const b3Vec2& a, const b3Vec2& b)
+{
+	return a.x * b.x + a.y * b.y;
+}
+
+// Compute the length of a vector.
+inline float32 b3Length(const b3Vec2& v)
+{
+	return b3Sqrt(v.x * v.x + v.y * v.y);
+}
+
+// Normalize a vector.
+inline b3Vec2 b3Normalize(const b3Vec2& v)
+{
+	float32 length = b3Length(v);
+	if (length > B3_EPSILON)
+	{
+		float32 s = 1.0f / length;
+		return s * v;
+	}
+	return v;
+}
+
+// Compute the euclidean distance between two points.
+inline float32 b3Distance(const b3Vec2& a, const b3Vec2& b)
+{
+	return b3Length(a - b);
+}
+
+// Compute the determinant of two 2D vectors.
+inline float32 b3Det(const b3Vec2& a, const b3Vec2& b)
+{
+	return a.x * b.y - a.y * b.x;
+}
+
+#endif

include/bounce/common/math/vec3.h

@@ -0,0 +1,248 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_VEC_3_H
+#define B3_VEC_3_H
+
+#include <bounce\common\math\math.h>
+
+// A 3D column vector.
+struct b3Vec3 
+{
+	// Does nothing for performance.
+	b3Vec3() { }
+
+	// Set this vector from three components.
+	b3Vec3(float32 _x, float32 _y, float32 _z) : x(_x), y(_y), z(_z) { }
+
+	// Read an indexed component from this vector.
+	float32 operator[](u32 i) const
+	{
+		return (&x)[i];
+	}
+
+	// Write an indexed component to this vector.
+	float32& operator[](u32 i)
+	{
+		return (&x)[i];
+	}
+
+	// Add a vector to this vector.
+	void operator+=(const b3Vec3& b) 
+	{
+		x += b.x;
+		y += b.y;
+		z += b.z;
+	}
+
+	// Subtract this vector from another vector.
+	void operator-=(const b3Vec3& b) 
+	{
+		x -= b.x;
+		y -= b.y;
+		z -= b.z;
+	}
+
+	// Scale this vector.
+	void operator*=(float32 s) 
+	{
+		x *= s;
+		y *= s;
+		z *= s;
+	}
+	
+	// Scale this vector.
+	void operator/=(float32 s)
+	{
+		x /= s;
+		y /= s;
+		z /= s;
+	}
+
+	// Set this vector to the zero vector.
+	void SetZero() 
+	{
+		x = y = z = 0.0f;
+	}
+	
+	// Normalize this vector.
+	void Normalize()
+	{
+		float32 lenght = b3Sqrt(x * x + y * y + z * z);
+		if (lenght > B3_EPSILON)
+		{
+			x /= lenght;
+			y /= lenght;
+			z /= lenght;
+		}
+	}
+
+	// Set this vector from three coordinates.
+	void Set(float32 _x, float32 _y, float32 _z) 
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+	}
+
+	float32 x, y, z;
+};
+
+// Negate a vector.
+inline b3Vec3 operator-(const b3Vec3& v) 
+{
+	return b3Vec3(-v.x, -v.y, -v.z);
+}
+
+// Compute the sum of two vectors.
+inline b3Vec3 operator+(const b3Vec3& a, const b3Vec3& b) 
+{
+	return b3Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+
+// Compute the subtraction of two vectors.
+inline b3Vec3 operator-(const b3Vec3& a, const b3Vec3& b) 
+{
+	return b3Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+
+// Compute a scalar-vector product.
+inline b3Vec3 operator*(float32 s, const b3Vec3& v) 
+{
+	return b3Vec3(s * v.x, s * v.y, s * v.z);
+}
+
+// Inverse multiply a scalar-vector.
+inline b3Vec3 operator/(const b3Vec3& v, float32 s)
+{
+	return b3Vec3(v.x / s, v.y / s, v.z / s);
+}
+
+// Compute the dot-product of two vectors.
+inline float32 b3Dot(const b3Vec3& a, const b3Vec3& b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
+// Compute the cross-product of two vectors.
+inline b3Vec3 b3Cross(const b3Vec3& a, const b3Vec3& b)
+{
+	return b3Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+}
+
+// Compute the determinant of a matrix whose columns are three given vectors.
+// Useful property: det(a, b, c) = det(c, a, b) = det(b, c, a).
+inline float32 b3Det(const b3Vec3& a, const b3Vec3& b, const b3Vec3& c)
+{
+	return b3Dot(a, b3Cross(b, c));
+}
+
+// Compute the length of a vector.
+inline float32 b3Length(const b3Vec3& v) 
+{
+	return b3Sqrt(v.x * v.x + v.y * v.y + v.z * v.z);
+}
+
+// Compute the squared length of a vector.
+inline float32 b3LengthSquared(const b3Vec3& v)
+{
+	return v.x * v.x + v.y * v.y + v.z * v.z;
+}
+
+// Compute the normalized vector of a (non-zero!) vector.
+inline b3Vec3 b3Normalize(const b3Vec3& v)
+{
+	float32 length = b3Length(v);
+	if (length > B3_EPSILON)
+	{
+		float32 s = 1.0f / length;
+		return s * v;
+	}
+	return v;
+}
+
+// Compute the euclidean distance between two points.
+inline float32 b3Distance(const b3Vec3& a, const b3Vec3& b) 
+{
+	return b3Length(a - b);
+}
+
+// Compute the squared distance between two points.
+inline float32 b3DistanceSquared(const b3Vec3& a, const b3Vec3& b)
+{
+	b3Vec3 v = a - b;
+	return b3LengthSquared(v);
+}
+
+// Compute the triangle area.
+inline float32 b3Area(const b3Vec3& a, const b3Vec3& b, const b3Vec3& c)
+{
+	return b3Length(b3Cross(b - a, c - a));
+}
+
+// Compute the squared triangle area.
+inline float32 b3AreaSquared(const b3Vec3& a, const b3Vec3& b, const b3Vec3& c)
+{
+	return b3LengthSquared(b3Cross(b - a, c - a));
+}
+
+// Compute the tetrahedron volume.
+inline float32 b3Volume(const b3Vec3& a, const b3Vec3& b, const b3Vec3& c, const b3Vec3& d)
+{
+	float32 volume = b3Det(b - a, c - a, d - a);
+	// Force a positive volume.
+	float32 sign = b3Sign(volume);
+	const float32 inv6 = 1.0f / 6.0f;
+	return sign * inv6 * volume;
+}
+
+// Compute the squared tetrahedron volume.
+inline float32 b3VolumeSquared(const b3Vec3& a, const b3Vec3& b, const b3Vec3& c, const b3Vec3& d)
+{
+	float32 volume = b3Volume(a, b, c, d);
+	return volume * volume;
+}
+
+// Compute the minimum vector between two vector (per-element).
+inline b3Vec3 b3Min(const b3Vec3& a, const b3Vec3& b) 
+{
+	return b3Vec3(b3Min(a.x, b.x), b3Min(a.y, b.y), b3Min(a.z, b.z));
+}
+
+// Compute the maximum vector between two vector (per-element).
+inline b3Vec3 b3Max(const b3Vec3& a, const b3Vec3& b)
+{
+	return b3Vec3(b3Max(a.x, b.x), b3Max(a.y, b.y), b3Max(a.z, b.z));
+}
+
+// Find a perpendicular vector to a vector.
+inline b3Vec3 b3Perp(const b3Vec3& v)
+{
+	// Box2D
+	// Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
+	// Then 3*s*s = 1, s = sqrt(1/3) = 0.57735. This means that at least one component of a
+	// unit vector must be greater or equal to 0.57735.
+	if (b3Abs(v.x) >= float32(0.57735027))
+	{
+		return b3Vec3(v.y, -v.x, 0.0f);
+	}
+
+	return b3Vec3(0.0f, v.z, -v.y);
+}
+
+#endif

include/bounce/common/memory/block_pool.h

@@ -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.
+*/
+
+#ifndef B3_BLOCK_POOL_H
+#define B3_BLOCK_POOL_H
+
+#include <bounce\common\settings.h>
+
+// Number of blocks per chunk.
+const u32 b3_blockCount = 32;
+
+// A pool of memory blocks.
+class b3BlockPool
+{
+public:
+	b3BlockPool(u32 blockSize);
+	~b3BlockPool();
+
+	void* Allocate();
+	void Free(void* p);
+private:
+	struct b3Block
+	{
+		b3Block* next;
+	};
+
+	struct b3Chunk
+	{
+		b3Block* freeBlocks;
+		b3Chunk* next;
+	};
+
+	u32 m_blockSize;
+	u32 m_chunkSize;
+
+	b3Chunk* m_chunks;
+	u32 m_chunkCount;
+};
+
+#endif

include/bounce/common/memory/stack_allocator.h

@@ -0,0 +1,53 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_STACK_ALLOCATOR_H
+#define B3_STACK_ALLOCATOR_H
+
+#include <bounce\common\settings.h>
+
+// Allocate 10 MiB from the stack. 
+// Increase as you want.
+const u32 b3_maxStackSize = B3_MiB(10);
+
+// An stack allocator.
+class b3StackAllocator 
+{
+public :
+	b3StackAllocator();
+	~b3StackAllocator();
+
+	void* Allocate(u32 size);
+	void Free(void* p);
+private :
+	struct b3Block 
+	{
+		u32 size;
+		u8* data;
+		bool parent;
+	};
+	
+	u32 m_blockCapacity;
+	b3Block* m_blocks;
+	u32 m_blockCount;
+
+	u32 m_allocatedSize; // marker
+	u8 m_memory[b3_maxStackSize];
+};
+
+#endif

include/bounce/common/settings.h

@@ -0,0 +1,152 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SETTINGS_H
+#define B3_SETTINGS_H
+
+#include <assert.h>
+#include <cstring>
+#include <new>
+#include <float.h>
+
+typedef signed int i32;
+typedef signed short i16;
+typedef signed char	i8;
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+typedef unsigned long long u64;
+typedef double float64;
+typedef float float32;
+
+// You can modify the following parameters as long
+// as you know what you're doing.
+
+#define B3_PI (3.14159265359f)
+#define	B3_MAX_FLOAT (FLT_MAX)
+#define	B3_EPSILON (FLT_EPSILON)
+
+// Collision
+
+// Maximum number of vertices, edges, and faces a 
+// polyhedron can have. Don't increase this value.
+#define B3_MAX_HULL_FEATURES (256)
+
+// How much an AABB in the broad-phase should be extended by 
+// to disallow unecessary proxy updates.
+// A larger value increases performance when there are 
+// no objects closer to the AABB because no contacts are 
+// even created.
+#define B3_AABB_EXTENSION (0.2f)
+
+// This is used to extend AABBs in the broad-phase. 
+// Is used to predict the future position based on the current displacement.
+// This is a dimensionless multiplier.
+#define B3_AABB_MULTIPLIER (2.0f)
+
+// Collision and constraint tolerance.
+#define B3_LINEAR_SLOP (0.005f)
+
+// Collision and constraint tolerance.
+#define B3_ANGULAR_SLOP (2.0f / 180.0f * B3_PI)
+
+// The radius of the hull shape skin.
+#define B3_HULL_RADIUS (2.0f * B3_LINEAR_SLOP)
+
+// Twice the radius of the hull shape skin.
+#define B3_HULL_RADIUS_SUM (2.0f * B3_HULL_RADIUS)
+
+// Dynamics
+
+// The maximum number of manifolds that can be build 
+// for all contacts. 
+#define B3_MAX_MANIFOLDS (3)
+
+// If this is equal to 4 then the contact generator
+// will keep the hull-hull manifold clipped points up to 4 such that 
+// still creates a stable manifold to the solver. More points 
+// usually means better torque balance but can decrease 
+// the performance of the solver significantly. 
+// Therefore, keep this to 4 for greater performance.
+#define B3_MAX_MANIFOLD_POINTS (4)
+
+// Maximum translation per step to prevent numerical instability 
+// due to large linear velocity.
+#define B3_MAX_TRANSLATION (2.0f)
+#define B3_MAX_TRANSLATION_SQUARED (B3_MAX_TRANSLATION * B3_MAX_TRANSLATION)
+
+// Maximum rotation per step to prevent numerical instability due to 
+// large angular velocity.
+#define B3_MAX_ROTATION (0.5f * B3_PI)
+#define B3_MAX_ROTATION_SQUARED (B3_MAX_ROTATION * B3_MAX_ROTATION)
+
+// The maximum linear position correction used when solving constraints. This helps to
+// prevent overshoot.
+#define B3_MAX_LINEAR_CORRECTION (0.2f)
+
+// The maximum angular position correction used when solving constraints. This helps to
+// prevent overshoot.
+#define B3_MAX_ANGULAR_CORRECTION (8.0f / 180.0f * B3_PI)
+
+// This controls how faster overlaps should be resolved per step.
+// This is less than and would be close to 1, so that the all overlap is resolved per step.
+// However values very close to 1 may lead to overshoot.
+#define B3_BAUMGARTE (0.1f)
+
+// If the relative velocity of a contact point is below 
+// the threshold then restitution is not applied.
+#define B3_VELOCITY_THRESHOLD (1.0f)
+
+// Sleep
+
+#define B3_TIME_TO_SLEEP (0.2f )
+#define B3_SLEEP_LINEAR_TOL (0.05f)
+#define B3_SLEEP_ANGULAR_TOL (2.0f / 180.0f * B3_PI)
+
+// Memory
+
+#define B3_NOT_USED(x) ((void)(x))
+#define B3_ASSERT(c) assert(c)
+#define B3_STATIC_ASSERT(c) static_assert(c)
+
+#define B3_KiB(n) (1024 * n)
+#define B3_MiB(n) (1024 * B3_KiB(n))
+#define B3_GiB(n) (1024 * B3_MiB(n))
+
+// You should implement this function to use your own memory allocator.
+void* b3Alloc(u32 size);
+
+// You must implement this function if you have implemented b3Alloc.
+void b3Free(void* block);
+
+// You should implement this function to visualize log messages coming 
+// from this software.
+void b3Log(const char* string, ...);
+
+// The current version this software.
+struct b3Version
+{
+	u32 major; //significant changes 
+	u32 minor; //minor features
+	u32 revision; //patches
+};
+
+// The current version of Bounce.
+extern b3Version b3_version;
+
+#endif

include/bounce/common/template/array.h

@@ -0,0 +1,243 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_ARRAY_POD_H
+#define B3_ARRAY_POD_H
+
+#include <bounce\common\settings.h>
+
+// An array for bytes (POD).
+template <typename T>
+class b3Array
+{
+public:
+	const T& operator[](u32 i) const
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements[i];
+	}
+
+	T& operator[](u32 i)
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements[i];
+	}
+
+	const T* Get(u32 i) const
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements + i;
+	}
+
+	T* Get(u32 i)
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements + i;
+	}
+
+	const T* Elements() const
+	{
+		return m_elements;
+	}
+
+	T* Elements()
+	{
+		return m_elements;
+	}
+
+	void PushBack(const T& ele)
+	{
+		if (m_count == m_capacity)
+		{
+			T* oldElements = m_elements;
+			m_capacity *= 2;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+			memcpy(m_elements, oldElements, m_count * sizeof(T));
+			if (oldElements != m_localElements)
+			{
+				b3Free(oldElements);
+			}
+		}
+		B3_ASSERT(m_count < m_capacity);
+		m_elements[m_count] = ele;
+		++m_count;
+	}
+
+	void PopBack()
+	{
+		B3_ASSERT(m_count > 0);
+		--m_count;
+	}
+
+	const T& Back() const
+	{
+		B3_ASSERT(m_count > 0);
+		return m_elements[m_count - 1];
+	}
+
+	T& Back()
+	{
+		B3_ASSERT(m_count > 0);
+		return m_elements[m_count - 1];
+	}
+
+	u32 Count() const
+	{
+		return m_count;
+	}
+
+	bool IsEmpty() const
+	{
+		return m_count == 0;
+	}
+
+	void Remove(u32 index)
+	{
+		B3_ASSERT(m_count > 0);
+		B3_ASSERT(index < m_count);
+		--m_count;
+		// Swap current element with its next.
+		for (u32 i = index; i < m_count; ++i)
+		{
+			m_elements[i] = m_elements[i + 1];
+		}
+	}
+	
+	void Reserve(u32 size)
+	{
+		if (m_capacity < size)
+		{
+			T* oldElements = m_elements;
+			m_capacity = 2 * size;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+			memcpy(m_elements, oldElements, m_count * sizeof(T));
+			if (oldElements != m_localElements)
+			{
+				b3Free(oldElements);
+			}
+		}
+
+		B3_ASSERT(m_capacity >= size);
+	}
+
+	void Resize(u32 size)
+	{
+		if (m_capacity < size)
+		{
+			T* oldElements = m_elements;
+			m_capacity = 2 * size;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+			memcpy(m_elements, oldElements, m_count * sizeof(T));
+			if (oldElements != m_localElements)
+			{
+				b3Free(oldElements);
+			}
+		}
+		B3_ASSERT(m_capacity >= size);
+		m_count = size;
+	}
+
+	void Swap(const b3Array<T>& other)
+	{
+		if (m_elements == other.m_elements)
+		{
+			return;
+		}
+
+		// Ensure sufficient capacity for copy.
+		if (m_capacity < other.m_count)
+		{
+			if (m_elements != m_localElements)
+			{
+				b3Free(m_elements);
+			}
+			m_capacity = other.m_capacity;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+		}
+		
+		// Copy.
+		B3_ASSERT(m_capacity >= other.m_count);
+		m_count = other.m_count;
+		memcpy(m_elements, other.m_elements, other.m_count * sizeof(T));
+	}
+
+	void operator=(const b3Array<T>& other)
+	{
+		Swap(other);
+	}
+protected:
+	b3Array(T* elements, u32 N)
+	{
+		B3_ASSERT(N > 0);
+		m_localElements = elements;
+		m_capacity = N;
+		m_elements = m_localElements;
+		m_count = 0;
+	}
+
+	b3Array(const b3Array<T>& other)
+	{
+		Swap(other);
+	}
+
+	~b3Array()
+	{
+		if (m_elements != m_localElements)
+		{
+			b3Free(m_elements);
+		}
+	}
+
+	u32 m_capacity;
+	T* m_elements;
+	u32 m_count;
+	
+	T* m_localElements;
+};
+
+template <typename T, u32 N>
+class b3StackArray : public b3Array<T>
+{
+public :
+	b3StackArray<T, N>() : b3Array<T>(m_stackElements, N)
+	{
+	}
+
+	b3StackArray<T, N>(const b3StackArray<T, N>& other) : b3Array<T>(other)
+	{
+	}
+	
+	b3StackArray<T, N>(const b3Array<T>& other) : b3Array<T>(other)
+	{
+	}
+
+	void operator=(const b3StackArray<T, N>& other)
+	{
+		Swap(other);
+	}
+	
+	void operator=(const b3Array<T>& other)
+	{
+		Swap(other);
+	}
+
+protected:
+	T m_stackElements[N];
+};
+
+#endif

include/bounce/common/template/list.h

@@ -0,0 +1,100 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_LIST_H
+#define B3_LIST_H
+
+#include <bounce\common\settings.h>
+
+// A singly-linked list.
+template<class T>
+class b3List1
+{
+public:
+	b3List1()
+	{
+		m_head = nullptr;
+		m_count = 0;
+	}
+	
+	~b3List1() { }
+
+	void PushFront(T* link)
+	{
+		link->m_next = m_head;
+		m_head = link;
+		++m_count;
+	}
+
+	void Remove(T* link)
+	{
+		m_head = link->m_next;
+		--m_count;
+	}
+
+	T* m_head;
+	u32 m_count;
+};
+
+// A doubly-linked list.
+template<class T>
+class b3List2
+{
+public:
+	b3List2()
+	{
+		m_head = nullptr;
+		m_count = 0;
+	}
+
+	~b3List2() { }
+	
+	void PushFront(T* link)
+	{
+		link->m_prev = nullptr;
+		link->m_next = m_head;
+		if (m_head)
+		{
+			m_head->m_prev = link;
+		}
+		m_head = link;
+		++m_count;
+	}
+
+	void Remove(T* link)
+	{
+		if (link->m_prev)
+		{
+			link->m_prev->m_next = link->m_next;
+		}
+		if (link->m_next)
+		{
+			link->m_next->m_prev = link->m_prev;
+		}
+		if (link == m_head)
+		{
+			m_head = link->m_next;
+		}
+		--m_count;
+	}
+
+	T* m_head;
+	u32 m_count;
+};
+
+#endif

include/bounce/common/template/object_array.h

@@ -0,0 +1,283 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OBJECT_ARRAY_H
+#define B3_OBJECT_ARRAY_H
+
+#include <bounce\common\settings.h>
+
+// An array for objects.
+template <typename T>
+class b3ObjectArray
+{
+public:
+	const T& operator[](u32 i) const
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements[i];
+	}
+
+	T& operator[](u32 i)
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements[i];
+	}
+
+	const T* Get(u32 i) const
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements + i;
+	}
+
+	T* Get(u32 i)
+	{
+		B3_ASSERT(i < m_count);
+		return m_elements + i;
+	}
+
+	const T* Elements() const
+	{
+		return m_elements;
+	}
+
+	T* Elements()
+	{
+		return m_elements;
+	}
+
+	void PushBack(const T& ele)
+	{
+		if (m_count == m_capacity)
+		{
+			// There is no capacity for one more element.
+			T* oldElements = m_elements;
+			m_capacity *= 2;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+			
+			for (u32 i = 0; i < m_count; ++i)
+			{
+				T* old = oldElements + i;
+				T* e = m_elements + i;
+				new (e) T(*old);
+				old->~T();
+			}
+
+			if (oldElements != m_localElements)
+			{
+				b3Free(oldElements);
+			}
+		}
+
+		B3_ASSERT(m_count < m_capacity);
+		T* e = m_elements + m_count;
+		new(e) T(ele);
+		++m_count;
+	}
+
+	void PopBack()
+	{
+		B3_ASSERT(m_count > 0);
+		--m_count;
+	}
+
+	const T& Back() const
+	{
+		B3_ASSERT(m_count > 0);
+		return m_elements[m_count - 1];
+	}
+
+	T& Back()
+	{
+		B3_ASSERT(m_count > 0);
+		return m_elements[m_count - 1];
+	}
+
+	u32 Count() const
+	{
+		return m_count;
+	}
+
+	bool Empty() const
+	{
+		return m_count == 0;
+	}
+
+	void Resize(u32 size)
+	{
+		if (m_capacity < size)
+		{
+			// There is no capacity for the requested size.
+			T* oldElements = m_elements;
+			m_capacity = 2 * size;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+
+			// Construct and copy objects.
+			for (u32 i = 0; i < m_count; ++i)
+			{
+				T* old = oldElements + i;
+				T* e = m_elements + i;
+				new (e) T(*old);
+				old->~T();
+			}
+
+			// Construct objects up to the requested size.
+			for (u32 i = m_count; i < size; ++i)
+			{
+				T* e = m_elements + i;
+				new (e) T();
+			}
+
+			if (oldElements != m_localElements)
+			{
+				b3Free(oldElements);
+			}
+
+			m_count = size;
+			return;
+		}
+
+		B3_ASSERT(m_capacity >= size);
+		if (size < m_count)
+		{
+			// Destroy objects beyond the requested size.
+			for (u32 i = size; i < m_count; ++i)
+			{
+				T* e = m_elements + i;
+				e->~T();
+			}
+		}
+		else
+		{
+			// Construct objects up to the requested size.
+			for (u32 i = m_count; i < size; ++i)
+			{
+				T* e = m_elements + i;
+				new (e) T();
+			}
+		}
+
+		m_count = size;
+	}
+
+	void Swap(const b3ObjectArray<T>& other)
+	{
+		if (m_elements == other.m_elements)
+		{
+			return;
+		}
+
+		// Destroy all objects.
+		for (u32 i = 0; i < m_count; ++i)
+		{
+			T* e = m_elements + i;
+			e->~T();
+		}
+
+		// Ensure sufficient capacity for a copy.
+		if (m_capacity < other.m_count)
+		{
+			if (m_elements != m_localElements)
+			{
+				b3Free(m_elements);
+			}
+			m_capacity = 2 * other.m_count;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+		}
+
+		// Copy.
+		B3_ASSERT(m_capacity >= other.m_count);
+		for (u32 i = 0; i < other.m_count; ++i)
+		{
+			T* e2 = other.m_elements + i;
+			T* e1 = m_elements + i;
+			new (e1) T(*e2);
+		}
+		m_count = other.m_count;
+	}
+protected:
+	b3ObjectArray(T* elements, u32 N)
+	{
+		B3_ASSERT(N > 0);
+		m_localElements = elements;
+		m_capacity = N;
+		m_elements = m_localElements;
+		m_count = 0;
+	}
+
+	b3ObjectArray(const b3ObjectArray<T>& other)
+	{
+		Swap(other);
+	}
+
+	~b3ObjectArray()
+	{
+		if (m_elements != m_localElements)
+		{
+			for (u32 i = 0; i < m_count; ++i)
+			{
+				T* e = m_elements + i;
+				e->~T();
+			}
+			b3Free(m_elements);
+		}
+	}
+
+	void operator=(const b3ObjectArray<T>& other)
+	{
+		Swap(other);
+	}
+
+	u32 m_capacity;
+	T* m_elements;
+	u32 m_count;
+	T* m_localElements;
+};
+
+template <typename T, u32 N>
+class b3StackObjectArray : public b3ObjectArray<T>
+{
+public:
+	b3StackObjectArray<T, N>() : b3ObjectArray<T>(m_stackElements, N)
+	{
+	}
+
+	b3StackObjectArray<T, N>(const b3StackObjectArray<T, N>& other) : b3ObjectArray<T>(other)
+	{
+	}
+
+	b3StackObjectArray<T, N>(const b3ObjectArray<T>& other) : b3ObjectArray<T>(other)
+	{
+	}
+
+	void operator=(const b3StackObjectArray<T, N>& other)
+	{
+		Swap(other);
+	}
+
+	void operator=(const b3ObjectArray<T>& other)
+	{
+		Swap(other);
+	}
+
+protected:
+	//@todo
+	// u8 m_bytes[N * sizeof(T)];
+	T m_stackElements[N];
+};
+
+#endif

include/bounce/common/template/stack.h

@@ -0,0 +1,97 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_STACK_H
+#define B3_STACK_H
+
+#include <bounce\common\settings.h>
+
+// A growable stack for plain-old-data (POD).
+template <typename T, u32 N>
+class b3Stack
+{
+public:
+	b3Stack()
+	{
+		m_capacity = N;
+		m_elements = m_stackElements;
+		m_count = 0;
+	}
+
+	~b3Stack()
+	{
+		if (m_elements != m_stackElements)
+		{
+			b3Free(m_elements);
+		}
+		m_elements = nullptr;
+	}
+
+	const T& Top() const
+	{
+		B3_ASSERT(m_count);
+		return m_elements[m_count - 1];
+	}
+
+	T& Top()
+	{
+		B3_ASSERT(m_count);
+		return m_elements[m_count - 1];
+	}
+
+	void Push(const T& ele)
+	{
+		if (m_count == m_capacity)
+		{
+			T* oldElements = m_elements;
+			m_capacity *= 2;
+			m_elements = (T*)b3Alloc(m_capacity * sizeof(T));
+			memcpy(m_elements, oldElements, m_count * sizeof(T));
+			if (oldElements != m_stackElements)
+			{
+				b3Free(oldElements);
+			}
+		}
+		B3_ASSERT(m_count < m_capacity);
+		m_elements[m_count] = ele;
+		++m_count;
+	}
+
+	void Pop()
+	{
+		B3_ASSERT(m_count);
+		--m_count;
+	}
+
+	u32 Count() const
+	{
+		return m_count;
+	}
+
+	bool IsEmpty() const
+	{
+		return m_count == 0;
+	}
+private:
+	u32 m_capacity;
+	T* m_elements;
+	u32 m_count;
+	T m_stackElements[N];
+};
+
+#endif

include/bounce/common/time.h

@@ -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.
+*/
+
+#ifndef B3_TIME_H
+#define B3_TIME_H
+
+#include <bounce\common\settings.h>
+
+// A timer class that accumulates time.
+// Usefull for measuring elapsed times of 
+// sections of code.
+class b3Time 
+{
+public :
+	b3Time();
+	
+	// Get the accumulated time in miliseconds
+	// from this timer.
+	float64 GetCurMilis() const;
+
+	// Get the elapsed time since this timer was updated.
+	float64 GetElapsedMilis() const;
+
+	// Add the elapsed time since this function
+	// was called to this timer.
+	void Update();
+
+	// Add a given ammout of time to this timer.
+	void UpdateBy(float64 dt);
+private:
+	static float64 m_invFrequency;
+	
+	u64 m_lastRealTime;
+	float64 m_lastTime;
+	float64 m_curTime;
+};
+
+inline float64 b3Time::GetCurMilis() const 
+{ 
+	return m_curTime;
+}
+
+inline float64 b3Time::GetElapsedMilis() const 
+{ 
+	return m_curTime - m_lastTime;
+}
+
+#endif

include/bounce/dynamics/body.h

@@ -0,0 +1,625 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_BODY_H
+#define B3_BODY_H
+
+#include <bounce\common\math\vec3.h>
+#include <bounce\common\math\mat33.h>
+#include <bounce\common\math\quat.h>
+#include <bounce\common\math\transform.h>
+#include <bounce\common\template\list.h>
+
+class b3World;
+class b3Shape;
+
+struct b3ShapeDef;
+struct b3JointEdge;
+
+// Static bodies have zero mass and velocity, and therefore they can't move.
+// Kinematic bodies are't moved by external and internal forces but can be moved by contact forces.
+// Dynamic bodies have non-zero mass and can move due to internal and external forces.
+enum b3BodyType 
+{
+	e_staticBody,
+	e_kinematicBody,
+	e_dynamicBody
+};
+
+// Pass this definition to the world to create a new rigid body.
+struct b3BodyDef 
+{
+	b3BodyDef() 
+	{
+		type = e_staticBody;
+		awake = true;
+		fixedRotationX = false;
+		fixedRotationY = false;
+		fixedRotationZ = false;
+		userData = nullptr;
+		position.SetZero();
+		orientation.SetIdentity();
+		linearVelocity.SetZero();
+		angularVelocity.SetZero();
+		gravityScale = 1.0f;
+	}
+
+	b3BodyType type;
+	bool awake;
+	bool fixedRotationX;
+	bool fixedRotationY;
+	bool fixedRotationZ;
+	void* userData;
+	b3Vec3 position;
+	b3Quat orientation;
+	b3Vec3 linearVelocity;
+	b3Vec3 angularVelocity;
+	float32 gravityScale;
+};
+
+class b3Body
+{
+public:
+	// A world manages the body construction.
+	b3Body(const b3BodyDef& def, b3World* world);
+	
+	// A world manages the body destruction.
+	~b3Body() { }
+
+	// Create a new shape for the body given the shape definition and return a pointer to its clone.
+	// The shape passed to the definition it will be cloned and is not recommended modifying 
+	// it inside simulation callbacks. 
+	// Therefore you can create shapes on the stack memory.
+	b3Shape* CreateShape(const b3ShapeDef& def);
+	
+	// Destroy a given shape from the body.
+	void DestroyShape(b3Shape* shape);
+
+	// Destroy all shapes associated with the body.
+	void DestroyShapes();
+
+	// Destroy all contacts associated with the body.
+	void DestroyContacts();
+
+	// Destroy all joints connected to the body.
+	void DestroyJoints();
+
+	// Get the shapes associated with the body.
+	const b3List1<b3Shape>& GetShapeList() const;
+	b3List1<b3Shape>& GetShapeList();
+
+	// Get the type of the body.
+	b3BodyType GetType() const;
+
+	// Set the type of the body. 
+	// This will reset the current body inertial properties.
+	void SetType(b3BodyType type);
+
+	// Get the world the body belongs to.
+	const b3World* GetWorld() const;
+	b3World* GetWorld();
+
+	// Get the body world b3Transform.
+	const b3Transform& GetTransform() const;
+	
+	// Set the body world b3Transform from a position, axis of rotation and an angle 
+	// of rotation about the axis.
+	// However, manipulating a body b3Transform during the simulation may cause non-physical behaviour.
+	void SetTransform(const b3Vec3& position, const b3Vec3& axis, float32 angle);
+	
+	// Get the gravity scale of the body. One is used by default.
+	float32 GetGravityScale() const;
+	
+	// Set the gravity scale of the body.
+	void SetGravityScale(float32 scale);
+
+	// See if the body is awake.
+	bool IsAwake() const;
+
+	// Set the awake status of the body.
+	void SetAwake(bool flag);
+
+	// Get the user data associated with the body.
+	// The user data is usually a game entity.
+	void* GetUserData() const;
+
+	// Set the user data to the body.
+	void SetUserData(void* _userData);
+	
+	// Get the body sweep.
+	const b3Sweep& GetSweep() const;
+
+	// Apply a force to a specific point of the body. 
+	// If the point isn't the center of mass then a non-zero torque is applied.
+	// The body must be dynamic.
+	void ApplyForce(const b3Vec3& force, const b3Vec3& point, bool wake);
+	
+	// Apply a force to the body center of mass. Generally this is a external force. 
+	// The body must be dynamic.
+	void ApplyForceToCenter(const b3Vec3& force, bool wake);
+	
+	// Apply a torque (angular momentum change) to the body.
+	// The body must be dynamic.
+	void ApplyTorque(const b3Vec3& torque, bool wake);
+
+	// Apply a linear impulse (linear velocity change) to a specific point (particle) of the body. 
+	// If the point isn't the center of mass then a non-zero angular impulse is applied.
+	// The body must be dynamic.
+	void ApplyLinearImpulse(const b3Vec3& impulse, const b3Vec3& point, bool wake);
+	
+	// Apply a angular impulse (angular velocity change) to the body.
+	// The body must be dynamic.
+	void ApplyAngularImpulse(const b3Vec3& impulse, bool wake);
+
+	// Get the linear velocity of the body.
+	b3Vec3 GetLinearVelocity() const;
+
+	// Set the linear velocity of the body. If is a non-zero velocity then the body awakes.
+	// The body must be dynamic or kinematic.
+	void SetLinearVelocity(const b3Vec3& linearVelocity);
+	
+	// Get the angular velocity of the body.
+	b3Vec3 GetAngularVelocity() const;
+
+	// Set the angular velocity of the body. If is a non-zero velocity then the body awakes.
+	// The body must be dynamic or kinematic.
+	void SetAngularVelocity(const b3Vec3& angularVelocity);
+	
+	// Get the mass of the body. Typically in kg/m^3.
+	float32 GetMass() const;
+
+	// Get the rotational inertia of the body about the center of mass. Typically in kg/m^3.
+	const b3Mat33& GetInertia() const;
+	
+	// Get the total kinetic energy of the body.
+	float32 GetKineticEnergy() const;
+
+	// Transform a vector to the local space of this body.
+	b3Vec3 GetLocalVector(const b3Vec3& vector) const;
+
+	// Transform a vector to the world space.
+	b3Vec3 GetWorldVector(const b3Vec3& localVector) const;
+
+	// Transform a point to the local space of this body.
+	b3Vec3 GetLocalPoint(const b3Vec3& point) const;
+
+	// Transform a point to the world space.
+	b3Vec3 GetWorldPoint(const b3Vec3& localPoint) const;
+
+	// Transform a frame to the local space of this body.
+	b3Transform GetLocalFrame(const b3Transform& frame) const;
+
+	// Transform a frame to the world space.
+	b3Transform GetWorldFrame(const b3Transform& localFrame) const;
+
+	// Get the next body in the world body list.
+	const b3Body* GetNext() const;
+	b3Body* GetNext();
+
+	// Dump this body to a file.
+	void Dump() const;
+private:
+	friend class b3World;
+	friend class b3Island;
+
+	friend class b3Contact;
+	friend class b3ConvexContact;
+	friend class b3MeshContact;
+	friend class b3ContactManager;
+	friend class b3ContactSolver;
+	
+	friend class b3Joint;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+	friend class b3MouseJoint;
+	friend class b3SpringJoint;
+	friend class b3RevoluteJoint;
+	friend class b3SphereJoint;
+	friend class b3ConeJoint;
+
+	friend class b3List2<b3Body>;
+
+	enum b3BodyFlags 
+	{
+		e_awakeFlag = 0x0001,
+		e_islandFlag = 0x0002,
+		e_fixedRotationX = 0x0004,
+		e_fixedRotationY = 0x0008,
+		e_fixedRotationZ = 0x0010,
+	};
+	
+	// Recalculate the mass of the body based on the shapes associated
+	// with it.
+	void ResetMass();
+
+	// Synchronize this body b3Transform with its world 
+	// center of mass and orientation.
+	void SynchronizeTransform();
+
+	// Compute the body world inertial tensor.
+	// Compute the shapes world AABBs.
+	// Usually this is called after the body b3Transform is synchronized.	
+	void SynchronizeShapes();
+
+	// Check if this body should collide with another.
+	bool ShouldCollide(const b3Body* other) const;
+
+	b3BodyType m_type;
+	i32 m_islandID;
+	u32 m_flags;
+	float32 m_sleepTime;
+
+	// The shapes attached to this body.
+	b3List1<b3Shape> m_shapeList;
+	
+	// Bidirectional joint edges for this body
+	b3List2<b3JointEdge> m_jointEdges;
+
+	// User associated data (usually an entity).
+	void* m_userData;
+
+	// Body mass.
+	float32 m_mass;
+	// Inverse body mass.
+	float32 m_invMass;
+	
+	// Inertia about the body local center of mass.
+	b3Mat33 m_I;	
+	// Inverse inertia about the body local center of mass.
+	b3Mat33 m_invI;	
+	// Inverse inertia about the body world center of mass.
+	b3Mat33 m_worldInvI;
+	
+	float32 m_gravityScale;
+	b3Vec3 m_force;
+	b3Vec3 m_torque;
+	b3Vec3 m_linearVelocity;
+	b3Vec3 m_angularVelocity;
+	
+	b3Sweep m_sweep;
+	b3Transform m_xf;
+		
+	// The parent world of this body.
+	b3World* m_world;
+	
+	// Links to the world body list.
+	b3Body* m_prev;
+	b3Body* m_next;
+};
+
+inline const b3Body* b3Body::GetNext() const
+{
+	return m_next;
+}
+
+inline b3Body* b3Body::GetNext()
+{
+	return m_next;
+}
+
+inline const b3World* b3Body::GetWorld() const 
+{
+	return m_world;
+}
+
+inline b3World* b3Body::GetWorld() 
+{
+	return m_world;
+}
+
+inline b3BodyType b3Body::GetType() const 
+{ 
+	return m_type; 
+}
+
+inline void b3Body::SetType(b3BodyType type) 
+{
+	if (m_type == type) 
+	{
+		return;
+	}
+
+	m_type = type;
+
+	ResetMass();
+
+	if (m_type == e_staticBody) 
+	{
+		m_linearVelocity.SetZero();
+		m_angularVelocity.SetZero();
+		SynchronizeShapes();
+	}
+
+	SetAwake(true);
+
+	m_force.SetZero();
+	m_torque.SetZero();
+
+	DestroyContacts();
+}
+
+inline void* b3Body::GetUserData() const 
+{ 
+	return m_userData; 
+}
+
+inline void b3Body::SetUserData(void* userData) 
+{ 
+	m_userData = userData; 
+}
+
+inline const b3List1<b3Shape>& b3Body::GetShapeList() const
+{
+	return m_shapeList;
+}
+
+inline b3List1<b3Shape>& b3Body::GetShapeList()
+{
+	return m_shapeList;
+}
+
+inline const b3Transform& b3Body::GetTransform() const
+{
+	return m_xf;
+}
+
+inline void b3Body::SetTransform(const b3Vec3& position, const b3Vec3& axis, float32 angle) 
+{
+	b3Quat q = b3Quat(axis, angle);
+	
+	m_xf.position = position;
+	m_xf.rotation = b3ConvertQuatToRot(q);
+
+	m_sweep.worldCenter = b3Mul(m_xf, m_sweep.localCenter);
+	m_sweep.orientation = q;
+
+	m_sweep.worldCenter0 = m_sweep.worldCenter;
+	m_sweep.orientation0 = m_sweep.orientation;
+
+	SynchronizeShapes();
+}
+
+inline b3Vec3 b3Body::GetLocalVector(const b3Vec3& vector) const
+{
+	return b3MulT(m_xf.rotation, vector);
+}
+
+inline b3Vec3 b3Body::GetWorldVector(const b3Vec3& localVector) const
+{
+	return b3Mul(m_xf.rotation, localVector);
+}
+
+inline b3Vec3 b3Body::GetLocalPoint(const b3Vec3& point) const
+{
+	return b3MulT(m_xf, point);
+}
+
+inline b3Vec3 b3Body::GetWorldPoint(const b3Vec3& point) const
+{
+	return b3Mul(m_xf, point);
+}
+
+inline b3Transform b3Body::GetLocalFrame(const b3Transform& xf) const
+{
+	return b3MulT(m_xf, xf);
+}
+
+inline b3Transform b3Body::GetWorldFrame(const b3Transform& xf) const
+{
+	return b3Mul(m_xf, xf);
+}
+
+inline const b3Sweep& b3Body::GetSweep() const
+{
+	return m_sweep;
+}
+
+inline void b3Body::SetAwake(bool flag) 
+{
+	if (flag) 
+	{
+		if (!IsAwake()) 
+		{
+			m_flags |= e_awakeFlag;
+			m_sleepTime = 0.0f;
+		}
+	}
+	else 
+	{
+		m_flags &= ~e_awakeFlag;
+		m_sleepTime = 0.0f;
+		m_force.SetZero();
+		m_torque.SetZero();
+		m_linearVelocity.SetZero();
+		m_angularVelocity.SetZero();		
+	}
+}
+
+inline bool b3Body::IsAwake() const 
+{
+	return (m_flags & e_awakeFlag) != 0;
+}
+
+inline float32 b3Body::GetGravityScale() const
+{ 
+	return m_gravityScale; 
+}
+
+inline void b3Body::SetGravityScale(float32 scale)
+{
+	if (m_type != e_staticBody) 
+	{
+		m_gravityScale = scale;
+	}
+}
+
+inline b3Vec3 b3Body::GetLinearVelocity() const
+{
+	return m_linearVelocity;
+}
+
+inline void b3Body::SetLinearVelocity(const b3Vec3& linearVelocity)
+{
+	if (m_type == e_staticBody) 
+	{
+		return;
+	}
+	
+	if (b3Dot(linearVelocity, linearVelocity) > 0.0f) 
+	{
+		SetAwake(true);
+	}
+
+	m_linearVelocity = linearVelocity;
+}
+
+inline b3Vec3 b3Body::GetAngularVelocity() const
+{
+	return m_angularVelocity;
+}
+
+inline void b3Body::SetAngularVelocity(const b3Vec3& angularVelocity) 
+{
+	if (m_type == e_staticBody) 
+	{
+		return;
+	}
+
+	if (b3Dot(angularVelocity, angularVelocity) > 0.0f) 
+	{
+		SetAwake(true);
+	}
+
+	m_angularVelocity = angularVelocity;
+}
+
+inline float32 b3Body::GetMass() const
+{
+	return m_mass;
+}
+
+inline const b3Mat33& b3Body::GetInertia() const
+{
+	return m_I;
+}
+
+inline float32 b3Body::GetKineticEnergy() const
+{
+	b3Vec3 P = m_mass * m_linearVelocity;
+	float32 linearEnergy = b3Dot(P, m_linearVelocity);
+	
+	b3Mat33 I = b3RotateToFrame(m_I, m_xf.rotation);
+	b3Vec3 L = I * m_angularVelocity;
+	float32 angularEnergy = b3Dot(L, m_angularVelocity);
+	
+	return 0.5f * (linearEnergy + angularEnergy);
+}
+
+inline void b3Body::ApplyForce(const b3Vec3& force, const b3Vec3& point, bool wake) 
+{
+	if (m_type != e_dynamicBody) 
+	{
+		return;
+	}
+
+	if (wake && !IsAwake()) 
+	{
+		SetAwake(true);
+	}
+
+	if (IsAwake()) 
+	{
+		m_force += force;
+		m_torque += b3Cross(point - m_sweep.worldCenter, force);
+	}
+}
+
+inline void b3Body::ApplyForceToCenter(const b3Vec3& force, bool wake) 
+{
+	if (m_type != e_dynamicBody) 
+	{
+		return;
+	}
+
+	if (wake && !IsAwake()) 
+	{
+		SetAwake(true);
+	}
+
+	if (IsAwake()) 
+	{
+		m_force += force;
+	}
+}
+
+inline void b3Body::ApplyTorque(const b3Vec3& torque, bool wake) 
+{
+	if (m_type != e_dynamicBody) 
+	{
+		return;
+	}
+
+	if (wake && !IsAwake()) 
+	{
+		SetAwake(true);
+	}
+
+	if (IsAwake()) 
+	{
+		m_torque += torque;
+	}
+}
+
+inline void b3Body::ApplyLinearImpulse(const b3Vec3& impulse, const b3Vec3& worldPoint, bool wake) 
+{
+	if (m_type != e_dynamicBody) 
+	{
+		return;
+	}
+
+	if (wake && !IsAwake()) 
+	{
+		SetAwake(true);
+	}
+
+	if (IsAwake()) 
+	{
+		m_linearVelocity += m_invMass * impulse;
+		m_angularVelocity += b3Mul(m_worldInvI, b3Cross(worldPoint - m_sweep.worldCenter, impulse));
+	}
+}
+
+inline void b3Body::ApplyAngularImpulse(const b3Vec3& impulse, bool wake) 
+{
+	if (m_type != e_dynamicBody) 
+	{
+		return;
+	}
+
+	if (wake && !IsAwake()) 
+	{
+		SetAwake(true);
+	}
+
+	if (IsAwake()) 
+	{
+		m_angularVelocity += b3Mul(m_worldInvI, impulse);
+	}
+}
+
+#endif

include/bounce/dynamics/contact_manager.h

@@ -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.
+*/
+
+#ifndef B3_CONTACT_MANAGER_H
+#define B3_CONTACT_MANAGER_H
+
+#include <bounce\common\memory\block_pool.h>
+#include <bounce\common\template\list.h>
+#include <bounce\collision\broad_phase.h>
+
+class b3Shape;
+class b3Contact;
+class b3ContactFilter;
+class b3ContactListener;
+struct b3MeshContactLink;
+
+// This is used to avoid b3World pollution.
+class b3ContactManager 
+{
+public:
+	b3ContactManager();
+
+	// The broad-phase callback.
+	void AddPair(void* proxyDataA, void* proxyDataB);
+
+	void SynchronizeShapes();
+
+	void FindNewContacts();
+	
+	void UpdateContacts();
+
+	b3Contact* Create(b3Shape* shapeA, b3Shape* shapeB);
+
+	void Destroy(b3Contact* c);
+
+	b3BlockPool m_convexBlocks;
+	b3BlockPool m_meshBlocks;
+	
+	b3BroadPhase m_broadPhase;	
+	b3List2<b3Contact> m_contactList;
+	b3List2<b3MeshContactLink> m_meshContactList;
+	b3ContactFilter* m_contactFilter;
+	b3ContactListener* m_contactListener;
+};
+
+#endif

include/bounce/dynamics/contacts/collide/clip.h

@@ -0,0 +1,113 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CLIP_H
+#define B3_CLIP_H
+
+#include <bounce\common\template\array.h>
+#include <bounce\common\geometry.h>
+
+#define B3_NULL_EDGE (0xFF)
+
+// A combination of features used to uniquely identify a vertex on a feature.
+struct b3FeaturePair
+{
+	u8 inEdgeA; // incoming edge on hull A
+	u8 inEdgeB; // incoming edge on hull B
+	u8 outEdgeA; // outgoing edge on hull A
+	u8 outEdgeB; // outgoing edge on hull B
+};
+
+inline b3FeaturePair b3MakePair(u32 inEdgeA, u32 inEdgeB, u32 outEdgeA, u32 outEdgeB)
+{
+	b3FeaturePair out;
+	out.inEdgeA = u8(inEdgeA);
+	out.inEdgeB = u8(inEdgeB);
+	out.outEdgeA = u8(outEdgeA);
+	out.outEdgeB = u8(outEdgeB);
+	return out;
+}
+
+// Make a 32-bit key for a feature pair.
+inline u32 b3MakeKey(const b3FeaturePair& featurePair)
+{
+	union b3FeaturePairKey
+	{
+		b3FeaturePair pair;
+		u32 key;
+	};
+
+	b3FeaturePairKey key;
+	key.pair = featurePair;
+	return key.key;
+}
+
+// A clip vertex.
+struct b3ClipVertex
+{
+	b3Vec3 position;
+	b3FeaturePair pair; // the features that built the clip point
+};
+
+// A clip polygon.
+typedef b3Array<b3ClipVertex> b3ClipPolygon;
+
+// A clip plane.
+struct b3ClipPlane
+{
+	b3Plane plane;
+	u32 id;
+};
+
+struct b3Hull;
+struct b3Capsule;
+
+// Build a clip edge for an edge.
+void b3BuildEdge(b3ClipVertex vOut[2],
+	const b3Capsule* hull);
+
+// Build a clip polygon given an index to the polygon face.
+void b3BuildPolygon(b3ClipPolygon& pOut,
+	const b3Transform& xf, u32 index, const b3Hull* hull);
+
+// Clip a segment by a plane. 
+// Output a segment whose points are behind or on the input plane.
+// Return the number of output points.
+u32 b3ClipEdgeToPlane(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3ClipPlane& plane);
+
+// Clip a polygon by a plane.  
+// Output a polygon whose points are behind or on the input plane.
+void b3ClipPolygonToPlane(b3ClipPolygon& pOut,
+	const b3ClipPolygon& pIn, const b3ClipPlane& plane);
+
+// Clip a segment by a hull face (side planes).
+// Return the number of output points.
+u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3Capsule* hull);
+
+// Clip a segment by a hull face (side planes).
+// Return the number of output points.
+u32 b3ClipEdgeToFace(b3ClipVertex vOut[2],
+	const b3ClipVertex vIn[2], const b3Transform& xf, u32 index, const b3Hull* hull);
+
+// Clip a polygon by a hull face (side planes).
+void b3ClipPolygonToFace(b3ClipPolygon& pOut,
+	const b3ClipPolygon& pIn, const b3Transform& xf, u32 index, const b3Hull* hull);
+
+#endif

include/bounce/dynamics/contacts/collide/collide.h

@@ -0,0 +1,139 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_COLLIDE_H
+#define B3_COLLIDE_H
+
+#include <bounce\collision\gjk\gjk_proxy.h>
+#include <bounce\collision\gjk\gjk_cache.h>
+#include <bounce\collision\sat\sat.h>
+#include <bounce\collision\sat\sat_edge_and_hull.h>
+#include <bounce\collision\sat\sat_vertex_and_hull.h>
+
+class b3Shape;
+class b3SphereShape;
+class b3CapsuleShape;
+class b3HullShape;
+class b3MeshShape;
+
+struct b3Manifold;
+
+enum b3SATCacheType
+{
+	e_separation,
+	e_overlap,
+	e_empty,
+};
+
+struct b3SATFeaturePair
+{
+	enum Type
+	{
+		e_edgeA, // an edge on hull A and an edge on hull B
+		e_faceA, // a face on hull A and a vertex/edge/face on hull B
+		e_faceB, // a face on hull B and a vertex/edge/face on hull A
+	};
+
+	b3SATCacheType state; // sat result
+	Type type; // feature pair type
+	u32 indexA; // feature index on hull A
+	u32 indexB; // feature index on hull B
+};
+
+struct b3FeatureCache
+{
+	// Read the current state of the cache.
+	// Return e_unkown if neither a separation or penetration was detected.
+	b3SATCacheType ReadState(const b3Transform& xfA, const b3Hull* hullA,
+		const b3Transform& xfB, const b3Hull* hullB);
+
+	b3SATCacheType ReadEdge(const b3Transform& xfA, const b3Hull* hullA,
+		const b3Transform& xfB, const b3Hull* hullB);
+
+	b3SATCacheType ReadFace(const b3Transform& xfA, const b3Hull* hullA,
+		const b3Transform& xfB, const b3Hull* hullB);
+
+	// We could increase the cache size (e.g. a feature pair of the last two frames).
+	b3SATFeaturePair m_featurePair;
+};
+
+// A convex cache contains information used to exploit temporal 
+// coherence of the contact generation algorithms between two shapes.
+struct b3ConvexCache
+{
+	b3SimplexCache simplexCache; // last step simplex from the GJK
+	b3FeatureCache featureCache; // last step result of the SAT
+};
+
+// Used for computing the distance between two generic shapes.
+class b3ShapeGJKProxy : public b3GJKProxy
+{
+public:
+	b3ShapeGJKProxy() { }
+	
+	b3ShapeGJKProxy(const b3Shape* shape, u32 index)
+	{
+		Set(shape, index);
+	}
+
+	void Set(const b3Shape* shape, u32 index);
+};
+
+// Test if two generic shapes are overlapping.
+bool b3TestOverlap(const b3Transform& xfA, u32 indexA, const b3Shape* shapeA,
+	const b3Transform& xfB, u32 indexB, const b3Shape* shapeB,
+	b3ConvexCache* cache);
+
+// Compute a manifold for two generic shapes except when one of them is a mesh.
+void b3CollideShapeAndShape(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3Shape* shapeA,
+	const b3Transform& xfB, const b3Shape* shapeB,
+	b3ConvexCache* cache);
+
+// Compute a manifold for two spheres.
+void b3CollideSphereAndSphere(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3SphereShape* shapeA, 
+	const b3Transform& xfB, const b3SphereShape* shapeB);
+
+// Compute a manifold for a sphere and a hull.
+void b3CollideSphereAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3SphereShape* shapeA, 
+	const b3Transform& xfB, const b3HullShape* shapeB);
+
+// Compute a manifold for a sphere and a capsule.
+void b3CollideSphereAndCapsule(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3SphereShape* shapeA, 
+	const b3Transform& xfB, const b3CapsuleShape* shapeB);
+
+// Compute a manifold for two capsules.
+void b3CollideCapsuleAndCapsule(b3Manifold& manifold, 
+	const b3Transform& xfA, const b3CapsuleShape* shapeA, 
+	const b3Transform& xfB, const b3CapsuleShape* shapeB);
+
+// Compute a manifold for a capsule and a hull.
+void b3CollideCapsuleAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3CapsuleShape* shapeA, 
+	const b3Transform& xfB, const b3HullShape* shapeB);
+
+// Compute a manifold for two hulls. 
+void b3CollideHullAndHull(b3Manifold& manifold,
+	const b3Transform& xfA, const b3HullShape* shapeA, 
+	const b3Transform& xfB, const b3HullShape* shapeB,
+	b3ConvexCache* cache);
+
+#endif

include/bounce/dynamics/contacts/contact.h

@@ -0,0 +1,183 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT_H
+#define B3_CONTACT_H
+
+#include <bounce\common\math\math.h>
+#include <bounce\common\template\list.h>
+#include <bounce\common\template\array.h>
+
+struct b3Manifold;
+struct b3WorldManifold;
+
+class b3Shape;
+class b3Body;
+class b3Contact;
+class b3ContactListener;
+
+// A contact edge for the contact graph, 
+// where a shape is a vertex and a contact 
+// an edge.
+struct b3ContactEdge
+{
+	b3Shape* other;
+	b3Contact* contact;
+	// Links to the contact edge list.
+	b3ContactEdge* m_prev;
+	b3ContactEdge* m_next;
+};
+
+// This goes inside a contact.
+// It holds two shapes that are overlapping.
+struct b3OverlappingPair
+{
+	// To the shape A edge list.
+	b3Shape* shapeA;
+	b3ContactEdge edgeA;
+	// To the shape B edge list.
+	b3Shape* shapeB;
+	b3ContactEdge edgeB;
+};
+
+// todo
+struct b3TOIEvent
+{
+	//float32 t;
+};
+
+enum b3ContactType
+{
+	e_convexContact,
+	e_meshContact,
+	e_maxContact
+};
+
+class b3Contact
+{
+public:
+	// Get the contact type.
+	b3ContactType GetType() const;
+
+	// Get the shape A in this contact.
+	const b3Shape* GetShapeA() const;
+	b3Shape* GetShapeA();
+
+	// Get the shape B in this contact.
+	const b3Shape* GetShapeB() const;
+	b3Shape* GetShapeB();
+
+	// Get a contact manifold from this contact.
+	const b3Manifold* GetManifold(u32 index) const;
+	b3Manifold* GetManifold(u32 index);
+
+	// Get a world contact manifold from this contact.
+	void GetWorldManifold(b3WorldManifold* out, u32 index) const;
+	
+	// Are the shapes in this contact overlapping?
+	bool IsOverlapping() const;
+
+	// Get the next contact in the world contact list.
+	const b3Contact* GetNext() const;
+	b3Contact* GetNext();
+protected:
+	friend class b3World;
+	friend class b3Island;
+	friend class b3Shape;
+	friend class b3ContactManager;
+	friend class b3ContactSolver;
+	friend class b3List2<b3Contact>;
+
+	enum b3ContactFlags 
+	{
+		e_overlapFlag = 0x0001,
+		e_islandFlag = 0x0002,
+	};
+
+	b3Contact() { }
+	virtual ~b3Contact() { }
+
+	// Update the contact state.
+	void Update(b3ContactListener* listener);
+
+	// Test if the shapes in this contact are overlapping.
+	virtual bool TestOverlap() = 0;
+
+	// Build new contact points.
+	virtual void Collide() = 0;
+
+	b3ContactType m_type;
+	u32 m_flags;
+	b3OverlappingPair m_pair;
+
+	// Collision event from discrete collision to 
+	// discrete physics.
+	u32 m_manifoldCapacity;
+	b3Manifold* m_manifolds;
+	u32 m_manifoldCount;
+
+	// Time of impact event from continuous collision
+	// to continuous physics.
+	//b3TOIEvent m_toi;
+
+	// Links to the world contact list.
+	b3Contact* m_prev;
+	b3Contact* m_next;
+};
+
+inline b3ContactType b3Contact::GetType() const
+{
+	return m_type;
+}
+
+inline b3Shape* b3Contact::GetShapeA() 
+{
+	return m_pair.shapeA;
+}
+
+inline const b3Shape* b3Contact::GetShapeA() const 
+{
+	return m_pair.shapeA;
+}
+
+inline b3Shape* b3Contact::GetShapeB() 
+{
+	return m_pair.shapeB;
+}
+
+inline const b3Shape* b3Contact::GetShapeB() const 
+{
+	return m_pair.shapeB;
+}
+
+inline bool b3Contact::IsOverlapping() const 
+{
+	return (m_flags & e_overlapFlag) != 0;
+}
+
+inline const b3Contact* b3Contact::GetNext() const
+{
+	return m_next;
+}
+
+inline b3Contact* b3Contact::GetNext()
+{
+	return m_next;
+}
+
+#endif

include/bounce/dynamics/contacts/contact_cluster.h

@@ -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.
+*/
+
+#ifndef B3_CONTACT_CLUSTER_H
+#define B3_CONTACT_CLUSTER_H
+
+#include <bounce\common\geometry.h>
+#include <bounce\common\template\array.h>
+#include <bounce\dynamics\contacts\manifold.h>
+
+#define B3_NULL_CLUSTER (-1)
+
+// Used for contact cluster reduction.
+struct b3ClusterVertex
+{
+	b3Vec3 position; // point on the cluster plane
+	u32 clipIndex; // where did this vertex came from (hint: local point)
+};
+
+// Used for contact cluster reduction.
+typedef b3Array<b3ClusterVertex> b3ClusterPolygon;
+
+// A observation represents a contact normal.	
+struct b3Observation
+{
+	u32 manifold;
+	u32 manifoldPoint;
+	b3Vec3 point;
+	i32 cluster; // normal
+};
+
+// A group of contact points with a similar contact normal.
+struct b3Cluster
+{
+	b3Vec3 centroid;
+};
+
+// Initialize a set of clusters.
+void b3InitializeClusters(b3Array<b3Cluster>& outClusters, 
+const b3Array<b3Observation>& inObservations);
+
+// Run the cluster algorithm.
+void b3Clusterize(b3Array<b3Cluster>& outClusters, b3Array<b3Observation>& outObservations,
+	const b3Array<b3Cluster>& inClusters, const b3Array<b3Observation>& inObservations);
+
+u32 b3Clusterize(b3Manifold outManifolds[3], const b3Manifold* inManifolds, u32 numIn,
+	const b3Transform& xfA, float32 radiusA, const b3Transform& xfB, float32 radiusB);
+
+// Reduce a set of contact points to a quad (approximate convex polygon).
+// All points must lie in a common plane and an initial point must be given.
+void b3ReducePolygon(b3ClusterPolygon& pOut, const b3ClusterPolygon& pIn, 
+	u32 startIndex);
+
+#endif

include/bounce/dynamics/contacts/contact_solver.h

@@ -0,0 +1,150 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT_SOLVER_H
+#define B3_CONTACT_SOLVER_H
+
+#include <bounce\common\math\vec2.h>
+#include <bounce\common\math\mat22.h>
+#include <bounce\dynamics\time_step.h>
+#include <bounce\dynamics\contacts\manifold.h>
+
+class b3StackAllocator;
+class b3Contact;
+struct b3Position;
+struct b3Velocity;
+
+struct b3PositionConstraintPoint
+{
+	b3Vec3 localNormalA;
+	b3Vec3 localPointA;
+	b3Vec3 localPointB;
+};
+
+struct b3PositionConstraintManifold
+{
+	b3PositionConstraintPoint* points;
+	u32 pointCount;
+};
+
+struct b3ContactPositionConstraint 
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+	float32 radiusA;
+	b3Vec3 localCenterA;
+	u32 indexB;
+	b3Vec3 localCenterB;
+	float32 invMassB;
+	b3Mat33 invIB;
+	float32 radiusB;
+	b3PositionConstraintManifold* manifolds;
+	u32 manifoldCount;
+};
+
+struct b3VelocityConstraintPoint 
+{
+	b3Vec3 rA;
+	b3Vec3 rB;
+	b3Vec3 normal;
+	float32 normalMass;
+	float32 normalImpulse;
+	float32 velocityBias;
+};
+
+struct b3VelocityConstraintManifold
+{
+	b3Vec3 center;
+	b3Vec3 normal;
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Vec3 rA;
+	b3Vec3 rB;
+	b3Mat22 tangentMass;
+	b3Vec2 tangentImpulse;	
+	float32 motorImpulse;
+	float32 motorMass;
+	//float32 maxTangentImpulse;
+	//float32 area;
+
+	b3VelocityConstraintPoint* points;
+	u32 pointCount;
+};
+
+// The idea is to allow anything to bounce off an inelastic surface.
+inline float32 b3MixRestitution(float32 e1, float32 e2)
+{
+	return b3Max(e1, e2);
+}
+
+// The idea is to drive the restitution to zero. 
+inline float32 b3MixFriction(float32 u1, float32 u2)
+{
+	return b3Sqrt(u1 * u2);
+}
+
+struct b3ContactVelocityConstraint 
+{
+	u32 indexA;
+	float32 invMassA;
+	b3Mat33 invIA;
+	float32 invMassB;
+	u32 indexB;
+	b3Mat33 invIB;
+	float32 friction;
+	float32 restitution;
+	b3VelocityConstraintManifold* manifolds;
+	u32 manifoldCount;
+};
+
+struct b3ContactSolverDef 
+{
+	b3Position* positions;
+	b3Velocity* velocities;
+	b3Contact** contacts;
+	u32 count;
+	b3StackAllocator* allocator;
+	float32 dt;
+};
+
+class b3ContactSolver 
+{
+public:
+	b3ContactSolver(const b3ContactSolverDef* def);
+	~b3ContactSolver();
+
+	void InitializeConstraints();
+	void WarmStart();
+	
+	void SolveVelocityConstraints();
+	void StoreImpulses();
+
+	bool SolvePositionConstraints();
+protected:
+	b3Position* m_positions;
+	b3Velocity* m_velocities;
+	b3Contact** m_contacts;
+	b3ContactPositionConstraint* m_positionConstraints;
+	b3ContactVelocityConstraint* m_velocityConstraints;
+	u32 m_count;
+	float32 m_dt, m_invDt;
+	b3StackAllocator* m_allocator;
+};
+
+#endif

include/bounce/dynamics/contacts/convex_contact.h

@@ -0,0 +1,45 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONVEX_CONTACT_H
+#define B3_CONVEX_CONTACT_H
+
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\contacts\collide\collide.h>
+
+class b3ConvexContact : public b3Contact 
+{
+public:
+private:
+	friend class b3ContactManager;
+
+	b3ConvexContact(b3Shape* shapeA, b3Shape* shapeB);
+	~b3ConvexContact() { }
+
+	bool TestOverlap();
+
+	void Collide();
+	
+	void SynchronizeShapes();
+
+	b3Manifold m_stackManifold;
+	b3ConvexCache m_cache;
+};
+
+#endif

include/bounce/dynamics/contacts/manifold.h

@@ -0,0 +1,91 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MANIFOLD_H
+#define B3_MANIFOLD_H
+
+#include <bounce\common\math\vec2.h>
+#include <bounce\common\geometry.h>
+
+#define B3_NULL_TRIANGLE (0xFFFFFFFF)
+
+// A contact manifold point.
+struct b3ManifoldPoint
+{
+	u32 triangleKey; // triangle identifier
+	u32 key; // point identifier
+	b3Vec3 localNormal; // local normal on the first shape
+	b3Vec3 localPoint; // local point on the first shape	
+	b3Vec3 localPoint2; // local point on the other shape
+	float32 normalImpulse; // normal impulse
+	u8 persisting; // indicates that the point is persisting
+};
+
+// A manifold is a group of contact points with similar contact normal.
+struct b3Manifold
+{
+	// Choose arbitrary impulses for warm starting.
+	void GuessImpulses();
+	
+	// Initialize impulses for warm starting.
+	void FindImpulses(const b3Manifold& old);
+	
+	b3ManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // manifold points
+	u32 pointCount; // number of manifold points
+
+	b3Vec3 center;
+	b3Vec3 normal;
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+	b3Vec2 tangentImpulse;
+	float32 motorImpulse;
+};
+
+// A world manifold point.
+struct b3WorldManifoldPoint
+{
+	// Initialize this manifold from a local manifold point and two transforms.
+	// The radii should come from the shapes that generated the manifold.
+	void Initialize(const b3ManifoldPoint* point,
+		const b3Transform& xfA, float32 radiusA,
+		const b3Transform& xfB, float32 radiusB);
+
+	b3Vec3 normal;
+	b3Vec3 point;
+	float32 separation;
+};
+
+// A contact manifold is a group of contact points with similar normal.
+struct b3WorldManifold
+{
+	// Initialize this world manifold from a local manifold and two transforms.
+	// The radii should come from the shapes that generated the manifold.
+	void Initialize(const b3Manifold* manifold,
+		const b3Transform& xfA, float32 radiusA,
+		const b3Transform& xfB, float32 radiusB);
+
+	b3Vec3 center;
+	b3Vec3 normal;
+	b3Vec3 tangent1;
+	b3Vec3 tangent2;
+
+	b3WorldManifoldPoint points[B3_MAX_MANIFOLD_POINTS]; // contact points
+	u32 pointCount; // number of contact points
+};
+
+#endif

include/bounce/dynamics/contacts/mesh_contact.h

@@ -0,0 +1,84 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MESH_CONTACT_H
+#define B3_MESH_CONTACT_H
+
+#include <bounce\collision\shapes\aabb3.h>
+#include <bounce\dynamics\contacts\contact.h>
+#include <bounce\dynamics\contacts\manifold.h>
+#include <bounce\dynamics\contacts\collide\collide.h>
+
+struct b3TriangleCache
+{
+	u32 index;
+	b3ConvexCache cache;
+};
+
+class b3MeshContact;
+
+struct b3MeshContactLink
+{
+	b3MeshContact* m_c;
+	b3MeshContactLink* m_prev;
+	b3MeshContactLink* m_next;
+};
+
+class b3MeshContact : public b3Contact
+{
+public:
+private:
+	friend class b3ContactManager;
+	friend class b3List2<b3MeshContact>;
+	friend class b3StaticTree;
+
+	b3MeshContact(b3Shape* shapeA, b3Shape* shapeB);
+	~b3MeshContact();
+
+	bool TestOverlap();
+
+	void Collide();
+	
+	void SynchronizeShapes();
+
+	bool MoveAABB(const b3AABB3& aabb, const b3Vec3& displacement);
+
+	void FindNewPairs();
+
+	bool Report(u32 proxyId);
+
+	// Did the AABB move significantly?
+	bool m_aabbMoved;
+
+	// The first shape AABB in the frame of the other shape.
+	b3AABB3 m_aabbA; 
+	
+	// Child shapes potentially overlapping with 
+	// the first shape.
+	u32 m_triangleCapacity;
+	b3TriangleCache* m_triangles;
+	u32 m_triangleCount;
+
+	// Contact manifolds.
+	b3Manifold m_stackManifolds[B3_MAX_MANIFOLDS];
+
+	// Links to the world mesh contact list.
+	b3MeshContactLink m_link;
+};
+
+#endif

include/bounce/dynamics/island.h

@@ -0,0 +1,93 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_ISLAND_H
+#define B3_ISLAND_H
+
+#include <bounce\common\math\vec3.h>
+
+class b3StackAllocator;
+class b3Contact;
+class b3Joint;
+class b3Body;
+struct b3Velocity;
+struct b3Position;
+struct b3Profile;
+
+struct b3IslandBody
+{
+	b3Body* b;
+	u32 id;
+	bool visited;
+};
+
+struct b3IslandJoint
+{
+	b3Joint* j;
+	u32 id;
+	bool visited;
+};
+
+struct b3IslandContact
+{
+	b3Contact* c;
+	u32 id;
+	bool visited;
+};
+
+class b3Island 
+{
+public :
+	b3Island(b3StackAllocator* stack, u32 bodyCapacity, u32 contactCapacity, u32 jointCapacity);
+	~b3Island();
+
+	void Clear();
+	
+	void Add(b3Body* body);
+	void Add(b3Contact* contact);
+	void Add(b3Joint* joint);
+	
+	void Solve(b3Profile* profile, const b3Vec3& gravity, float32 dt, u32 velocityIterations, u32 positionIterations, u32 flags);
+private :
+	enum b3IslandFlags
+	{
+		e_warmStartBit = 0x0001,
+		e_sleepBit = 0x0002
+	};
+
+	friend class b3World;
+
+	b3StackAllocator* m_allocator;
+	
+	b3Body** m_bodies;
+	u32 m_bodyCapacity;
+	u32 m_bodyCount;
+
+	b3Contact** m_contacts;
+	u32 m_contactCapacity;
+	u32 m_contactCount;
+
+	b3Joint** m_joints;
+	u32 m_jointCapacity;
+	u32 m_jointCount;
+	
+	b3Position* m_positions;
+	b3Velocity* m_velocities;
+};
+
+#endif

include/bounce/dynamics/joint_manager.h

@@ -0,0 +1,39 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_JOINT_MAN_H
+#define B3_JOINT_MAN_H
+
+#include <bounce\common\template\list.h>
+
+struct b3JointDef;
+class b3Joint;
+
+// This is used to avoid b3World pollution.
+class b3JointManager
+{
+public:
+	b3JointManager();
+	
+	b3Joint* Create(const b3JointDef* def);
+	void Destroy(b3Joint* j);
+
+	b3List2<b3Joint> m_jointList;
+};
+
+#endif

include/bounce/dynamics/joints/cone_joint.h

@@ -0,0 +1,125 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONE_JOINT_H
+#define B3_CONE_JOINT_H
+
+#include <bounce\dynamics\joints\joint.h>
+
+struct b3ConeJointDef : public b3JointDef
+{
+	b3ConeJointDef()
+	{
+		type = e_coneJoint;
+		localFrameA.SetIdentity();
+		localFrameB.SetIdentity();
+		enableLimit = false;
+		coneAngle = 0.0f;
+	}
+
+	// Initialize this definition given an axis, anchor point, and cone angle limit in radians.
+	void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& axis, const b3Vec3& anchor, float32 angle);
+
+	// The joint frame in the frame of body A.
+	b3Transform localFrameA;
+	
+	// The joint frame in the frame of body B.
+	b3Transform localFrameB;
+
+	// Enable the joint limit.
+	bool enableLimit;
+
+	// The cone angle limit in radians.
+	float32 coneAngle;
+};
+
+class b3ConeJoint : public b3Joint
+{
+public:
+	// Get the joint frame in the frame of body A.
+	const b3Transform& GetFrameA() const;
+
+	// Set the joint frame in the frame of body A.
+	void SetFrameA(const b3Transform& xf);
+
+	// Get the joint frame in the frame of body B.
+	const b3Transform& GetFrameB() const;
+
+	// Set the joint frame in the frame of body B.
+	void SetFrameB(const b3Transform& xf);
+
+	// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	// Set the joint limit enabled.
+	void SetEnableLimit(bool bit);
+
+	// Get the lower cone angle limit.
+	float32 GetLowerLimit() const;
+
+	// Set the lower cone angle limit.
+	void SetLimit(float32 lowerAngle);
+
+	// Draw this joint.
+	void Draw(b3Draw* b3Draw) const;
+private:
+	friend class b3Joint;
+	friend class b3Body;
+	friend class b3World;
+	friend class b3Island;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	b3ConeJoint(const b3ConeJointDef* def);
+
+	virtual void InitializeConstraints(const b3SolverData* data);
+	virtual void WarmStart(const b3SolverData* data);
+	virtual void SolveVelocityConstraints(const b3SolverData* data);
+	virtual bool SolvePositionConstraints(const b3SolverData* data);
+
+	// Solver shared
+	b3Transform m_localFrameA;
+	b3Transform m_localFrameB;
+
+	bool m_enableLimit;
+	float32 m_coneAngle;
+	
+	// Solver temp
+	u32 m_indexA;
+	u32 m_indexB;
+	float32 m_mA;
+	float32 m_mB;
+	b3Mat33 m_iA;
+	b3Mat33 m_iB;	
+	b3Vec3 m_localCenterA;
+	b3Vec3 m_localCenterB;
+
+	// Point-to-point
+	b3Vec3 m_rA;
+	b3Vec3 m_rB;
+	b3Mat33 m_mass;
+	b3Vec3 m_impulse;
+
+	// Limit
+	b3Vec3 m_limitAxis;
+	float32 m_limitMass;
+	float32 m_limitImpulse;
+	b3LimitState m_limitState;
+};
+
+#endif

include/bounce/dynamics/joints/joint.h

@@ -0,0 +1,239 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_JOINT_H
+#define B3_JOINT_H
+
+#include <bounce\common\math\transform.h>
+#include <bounce\common\template\list.h>
+#include <bounce\dynamics\time_step.h>
+
+class b3Draw;
+class b3Body;
+class b3Joint;
+struct b3SolverData;
+
+enum b3JointType
+{
+	e_unknownJoint,
+	e_mouseJoint,
+	e_springJoint,
+	e_revoluteJoint,
+	e_sphereJoint,
+	e_coneJoint,
+	e_maxJoints,
+};
+
+struct b3JointDef
+{
+	b3JointDef()
+	{
+		type = e_unknownJoint;
+		bodyA = nullptr;
+		bodyB = nullptr;
+		userData = nullptr;
+		collideLinked = false;
+	}
+
+	b3JointType type;
+	b3Body* bodyA;
+	b3Body* bodyB;
+	void* userData;
+	bool collideLinked;
+};
+
+struct b3JointEdge
+{
+	b3Body* other;
+	b3Joint* joint;
+	// Links to the joint edge list.
+	b3JointEdge* m_prev;
+	b3JointEdge* m_next;
+};
+
+// This goes inside a joint.
+// It holds two bodies that are linked.
+struct b3LinkedPair
+{
+	// To the body A joint edge list
+	b3Body* bodyA;
+	b3JointEdge edgeA;
+
+	// To the body B joint edge list
+	b3Body* bodyB;
+	b3JointEdge edgeB;
+};
+
+// @note All the data members of the joint structure they will be initialized
+// by the world.
+class b3Joint
+{
+public :
+	// Get the joint type.
+	b3JointType GetType() const;
+
+	// Get the first body connected to the joint.
+	const b3Body* GetBodyA() const;
+	b3Body* GetBodyA();
+
+	// Set the body to be connected to the joint as the first body.
+	virtual void SetBodyA(b3Body* bodyA);
+
+	// Get the second body connected to the joint.
+	const b3Body* GetBodyB() const;
+	b3Body* GetBodyB();
+
+	// Set the body to be connected to the joint as the second body.
+	virtual void SetBodyB(b3Body* bodyB);
+
+	// Get the user specific data associated with the joint.
+	void* GetUserData();
+	const void* GetUserData() const;
+
+	// Set the user data to be associated with the joint.
+	void SetUserData(void* data);
+
+	// Tell the world if the bodies linked by this joint 
+	// should collide with each other.
+	void SetCollideLinked(bool flag);
+	
+	// Check if the bodies linked by this joint 
+	// should collide with each other.
+	bool CollideLinked() const;
+
+	// Dump this joint to the log file.
+	virtual void Dump() const
+	{
+		b3Log("Dump not implemented for this joint type.\n");
+	}
+
+	// Get the next joint in the world joint list.
+	b3Joint* GetNext();
+	const b3Joint* GetNext() const;
+
+	// Create joint.
+	static b3Joint* Create(const b3JointDef* def);
+
+	// Destroy joint.
+	static void Destroy(b3Joint* j);
+protected :
+	friend class b3Body;
+	friend class b3World;
+	friend class b3Island;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	friend class b3List2<b3Joint>;
+
+	b3Joint() {	}
+	virtual ~b3Joint() { }
+	
+	virtual void InitializeConstraints(const b3SolverData* data) = 0;
+	virtual void WarmStart(const b3SolverData* data) = 0;
+	virtual void SolveVelocityConstraints(const b3SolverData* data) = 0;
+	virtual bool SolvePositionConstraints(const b3SolverData* data) = 0;
+
+	enum b3JointFlags 
+	{
+		e_islandFlag = 0x0001,
+		e_activeFlag = 0x0002
+	};
+	
+	b3JointType m_type;
+	u32 m_flags;
+	b3LinkedPair m_pair;	
+	
+	void* m_userData;
+	bool m_collideLinked;
+
+	// Links to the world joint list.
+	b3Joint* m_prev;
+	b3Joint* m_next;
+};
+
+inline b3JointType b3Joint::GetType() const 
+{ 
+	return m_type; 
+}
+
+inline const b3Body* b3Joint::GetBodyA() const
+{
+	return m_pair.bodyA;
+}
+
+inline b3Body* b3Joint::GetBodyA()
+{
+	return m_pair.bodyA;
+}
+
+inline void b3Joint::SetBodyA(b3Body* bodyA) 
+{
+	m_pair.bodyA = bodyA;
+}
+
+inline b3Body* b3Joint::GetBodyB() 
+{
+	return m_pair.bodyB;
+}
+
+inline const b3Body* b3Joint::GetBodyB() const 
+{
+	return m_pair.bodyB;
+}
+
+inline void b3Joint::SetBodyB(b3Body* bodyB) 
+{
+	m_pair.bodyB = bodyB;
+}
+
+inline void b3Joint::SetUserData(void* data) 
+{
+	m_userData = data;
+}
+
+inline void* b3Joint::GetUserData() 
+{
+	return m_userData;
+}
+
+inline const void* b3Joint::GetUserData() const 
+{
+	return m_userData;
+}
+
+inline void b3Joint::SetCollideLinked(bool bit)
+{
+	m_collideLinked = bit;
+}
+
+inline bool b3Joint::CollideLinked() const
+{
+	return m_collideLinked;
+}
+
+inline b3Joint* b3Joint::GetNext()
+{
+	return m_next;
+}
+
+inline const b3Joint* b3Joint::GetNext() const
+{
+	return m_next;
+}
+
+#endif

include/bounce/dynamics/joints/joint_solver.h

@@ -0,0 +1,71 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_JOINT_SOLVER_H
+#define B3_JOINT_SOLVER_H
+
+#include <bounce\dynamics\time_step.h>
+
+class b3Joint;
+
+// A 1x12 Jacobian row.
+struct b3Jacobian
+{
+	b3Vec3 linearA;
+	b3Vec3 angularA;
+	b3Vec3 linearB;
+	b3Vec3 angularB;
+};
+
+// 1x3 times 3x3.
+inline b3Vec3 b3Mul(const b3Vec3& v, const b3Mat33& A)
+{
+	return b3Transpose(A) * v;
+}
+
+// 1x3 times 3x1.
+inline float32 b3Mul(const b3Vec3& v, const b3Vec3& w)
+{
+	return b3Dot(v, w);
+}
+
+struct b3JointSolverDef 
+{
+	float32 dt;
+	u32 count;
+	b3Joint** joints;
+	b3Position* positions;
+	b3Velocity* velocities;
+};
+
+class b3JointSolver 
+{
+public :
+	b3JointSolver(const b3JointSolverDef* def);
+
+	void InitializeConstraints();
+	void WarmStart();
+	void SolveVelocityConstraints();	
+	bool SolvePositionConstraints();
+private :
+	b3SolverData m_solverData;
+	b3Joint** m_joints;
+	u32 m_count;
+};
+
+#endif

include/bounce/dynamics/joints/mouse_joint.h

@@ -0,0 +1,108 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MOUSE_JOINT_H
+#define B3_MOUSE_JOINT_H
+
+#include <bounce\dynamics\joints\joint.h>
+
+struct b3MouseJointDef : public b3JointDef 
+{
+	b3MouseJointDef() 
+	{
+		type = e_mouseJoint;
+		worldAnchorA.SetZero();
+		localAnchorB.SetZero();
+		maxForce = 0.0f;
+	}
+	
+	b3Vec3 worldAnchorA;
+	b3Vec3 localAnchorB;
+	float32 maxForce; 
+};
+
+class b3MouseJoint : public b3Joint 
+{
+public : 	
+	// Get the world space anchor point on the first body (usually the mouse world space position).
+	b3Vec3 GetWorldAnchorA() const;
+
+	// Set the world space anchor position on the first body.
+	void SetWorldAnchorA(const b3Vec3& v);
+
+	// Get the world space anchor point on the first body (usually the mouse world space position).
+	b3Vec3 GetWorldAnchorB() const;
+
+	// Get the local space anchor point on the second body (usually the ray cast intersection).
+	const b3Vec3& GetLocalAnchorB() const;
+
+	// Set the mouse position on the space of the second body (usually the ray cast intersection).
+	void SetLocalAnchorB(const b3Vec3& v);
+
+	// Implement b3Joint.
+	void Draw(b3Draw* b3Draw) const;
+private:
+	friend class b3Joint;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	b3MouseJoint(const b3MouseJointDef* def);
+
+	virtual void InitializeConstraints(const b3SolverData* data);
+	virtual void WarmStart(const b3SolverData* data);
+	virtual void SolveVelocityConstraints(const b3SolverData* data);
+	virtual bool SolvePositionConstraints(const b3SolverData* data);
+
+	// The two anchor points on each body.
+	// The first body has infinite mass. Therefore,
+	// we store the world space anchor point.
+	b3Vec3 m_worldAnchorA;
+	b3Vec3 m_localAnchorB;
+	float32 m_maxForce; // maximum reaction force in Newtons
+
+	// Constraint data for the solver.
+	u32 m_indexB;
+	float32 m_mB;
+	b3Mat33 m_iB;
+	b3Mat33 m_mass;
+	b3Vec3 m_rB;
+	b3Vec3 m_impulse;
+	b3Vec3 m_C;
+};
+
+inline b3Vec3 b3MouseJoint::GetWorldAnchorA() const 
+{
+	return m_worldAnchorA;
+}
+
+inline void b3MouseJoint::SetWorldAnchorA(const b3Vec3& v) 
+{
+	m_worldAnchorA = v;
+}
+
+inline const b3Vec3& b3MouseJoint::GetLocalAnchorB() const 
+{
+	return m_localAnchorB;
+}
+
+inline void b3MouseJoint::SetLocalAnchorB(const b3Vec3& v) 
+{
+	m_localAnchorB = v;
+}
+
+#endif

include/bounce/dynamics/joints/revolute_joint.h

@@ -0,0 +1,178 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_REVOLUTE_JOINT_H
+#define B3_REVOLUTE_JOINT_H
+
+#include <bounce\dynamics\joints\joint.h>
+#include <bounce\common\math\mat.h>
+
+struct b3RevoluteJointDef : public b3JointDef 
+{
+	b3RevoluteJointDef() 
+	{
+		type = e_revoluteJoint;
+		localFrameA.SetIdentity();
+		localFrameB.SetIdentity();
+		enableLimit = false;
+		lowerAngle = 0.0f;
+		upperAngle = 0.0f;
+		enableMotor = false;
+		motorSpeed = 0.0f;
+		maxMotorTorque = 0.0f;
+	}
+
+	// Initialize this definition given an axis, anchor point, and the lower and upper angle limits in radians.
+	void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& axis, const b3Vec3& anchor, float32 lowerAngle, float32 upperAngle);
+
+	// The joint frame relative to the frame of body A.
+	b3Transform localFrameA;
+	
+	// The joint frame relative to the frame of body B.
+	b3Transform localFrameB;
+
+	// Enable the joint limit.
+	bool enableLimit;
+
+	// The lower angle limit in radians.
+	float32 lowerAngle;
+	
+	// The upper angle limit in radians.
+	float32 upperAngle;
+
+	// Enable the joint motor.
+	bool enableMotor;
+
+	// The desired motor speed in radians per second.
+	float32 motorSpeed;
+	
+	// The maximum motor torque in Newton per meter.
+	float32 maxMotorTorque;
+};
+
+// A revolute joint constrains two bodies to share a common point while they
+// are free to rotate about the point and a given axis. 
+// The relative rotation about the shared axis
+// is the joint angle. You can limit the relative rotation with
+// a lower and upper angle limit. Also, you can use a motor
+// to drive the relative rotation about the shared axis. 
+// A maximum motor torque is provided so that infinite forces are not generated.
+class b3RevoluteJoint : public b3Joint
+{
+public:
+	// Get the joint frame relative to the frame of body A.
+	const b3Transform& GetFrameA() const;	
+
+	// Set the joint frame relative to the frame of body A.
+	void SetFrameA(const b3Transform& xf);
+
+	// Get the joint frame relative to the frame of body B.
+	const b3Transform& GetFrameB() const;
+	
+	// Set the joint frame relative to the frame of body B.
+	void SetFrameB(const b3Transform& xf);
+
+	// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	// Set the joint limit enabled.
+	void SetEnableLimit(bool bit);
+
+	// Get the lower angle limit.
+	float32 GetLowerLimit() const;
+
+	// Get the upper limit.
+	float32 GetUpperLimit() const;
+
+	// Set the angle limits.
+	void SetLimits(float32 lowerAngle, float32 upperAngle);
+
+	// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	// Set the joint motor enabled.
+	void SetEnableMotor(bool bit);
+
+	// Get the desired motor speed (radians per second).
+	float32 GetMotorSpeed() const;
+
+	// Set the desired motor speed (radians per second).
+	void SetMotorSpeed(float32 speed);
+
+	// Get the maximum motor torque (Newton per meter).
+	float32 GetMaxMotorTorque() const;
+
+	// Set the maximum motor torque (Newton per meter).
+	void SetMaxMotorTorque(float32 torque);
+
+	// Draw this joint.
+	void Draw(b3Draw* b3Draw) const;
+private:
+	friend class b3Joint;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	b3RevoluteJoint(const b3RevoluteJointDef* def);
+	
+	virtual void InitializeConstraints(const b3SolverData* data);
+	virtual void WarmStart(const b3SolverData* data);
+	virtual void SolveVelocityConstraints(const b3SolverData* data);
+	virtual bool SolvePositionConstraints(const b3SolverData* data);
+
+	// Solver shared
+	b3Transform m_localFrameA;
+	b3Transform m_localFrameB;
+	
+	bool m_enableMotor;
+	float32 m_motorSpeed;
+	float32 m_maxMotorTorque;
+	
+	bool m_enableLimit;
+	float32 m_lowerAngle;
+	float32 m_upperAngle;
+
+	// Solver temp
+	u32 m_indexA;
+	u32 m_indexB;
+	float32 m_mA;
+	float32 m_mB;
+	b3Mat33 m_iA;
+	b3Mat33 m_iB;
+	b3Vec3 m_localCenterA;
+	b3Vec3 m_localCenterB;
+	
+	// Motor
+	// The limit axis is the same as the motor axis
+	float32 m_motorMass;
+	float32 m_motorImpulse;
+
+	// Limit
+	b3Vec3 m_limitAxis; // axis of rotation for limit contraint
+	float32 m_limitImpulse;
+	b3LimitState m_limitState; // constraint state
+
+	// Point-to-point + axes-to-axes
+	b3Vec3 m_rA;
+	b3Vec3 m_rB;
+	b3Vec3 m_nA;
+	b3Vec3 m_nB;
+	b3Mat<5, 5> m_mass;
+	b3Vec<5> m_impulse;
+};
+
+#endif

include/bounce/dynamics/joints/sphere_joint.h

@@ -0,0 +1,112 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SPHERE_JOINT_H
+#define B3_SPHERE_JOINT_H
+
+#include <bounce\dynamics\joints\joint.h>
+
+struct b3SphereJointDef : public b3JointDef
+{
+	b3SphereJointDef()
+	{
+		type = b3JointType::e_sphereJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+	}
+
+	// Initialize this definition.
+	void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& anchor);
+
+	b3Vec3 localAnchorA;
+	b3Vec3 localAnchorB;
+};
+
+// A ball-in-socket joint.
+class b3SphereJoint : public b3Joint
+{
+public:
+	// Get the local anchor point on body A.
+	const b3Vec3& GetLocalAnchorA() const;
+	
+	// Set the local anchor point on body A.
+	void SetLocalAnchorA(const b3Vec3& point);
+
+	// Get the local anchor point on body B.
+	const b3Vec3& GetLocalAnchorB() const;
+	
+	// Set the local anchor point on body B.
+	void SetLocalAnchorB(const b3Vec3& point);
+
+	// Implement b3Joint
+	void Draw(b3Draw* b3Draw) const;
+private:
+	friend class b3Joint;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	b3SphereJoint(const b3SphereJointDef* def);
+
+	virtual void InitializeConstraints(const b3SolverData* data);
+	virtual void WarmStart(const b3SolverData* data);
+	virtual void SolveVelocityConstraints(const b3SolverData* data);
+	virtual bool SolvePositionConstraints(const b3SolverData* data);
+
+	// The local joint frames on each body.
+	b3Vec3 m_localAnchorA;
+	b3Vec3 m_localAnchorB;
+
+	// Temporary data copied from the joint solver
+	// to reduce cache misses.
+	u32 m_indexA;
+	u32 m_indexB;
+	float32 m_mA;
+	float32 m_mB;
+	b3Mat33 m_iA;
+	b3Mat33 m_iB;
+
+	// Constraint data.
+	b3Vec3 m_localCenterA;
+	b3Vec3 m_localCenterB;
+	b3Vec3 m_rA;
+	b3Vec3 m_rB;
+	b3Mat33 m_mass;
+	b3Vec3 m_impulse;
+};
+
+inline const b3Vec3& b3SphereJoint::GetLocalAnchorA() const
+{
+	return m_localAnchorA;
+}
+
+inline void b3SphereJoint::SetLocalAnchorA(const b3Vec3& point)
+{
+	m_localAnchorA = point;
+}
+
+inline const b3Vec3& b3SphereJoint::GetLocalAnchorB() const
+{
+	return m_localAnchorB;
+}
+
+inline void b3SphereJoint::SetLocalAnchorB(const b3Vec3& point)
+{
+	m_localAnchorB = point;
+}
+
+#endif

include/bounce/dynamics/joints/spring_joint.h

@@ -0,0 +1,116 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SPRING_JOINT_H
+#define B3_SPRING_JOINT_H
+
+#include <bounce\dynamics\joints\joint.h>
+
+struct b3SpringJointDef : public b3JointDef 
+{
+	b3SpringJointDef()
+	{
+		type = e_springJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		length = 0.0f;
+		frequencyHz = 0.0f;
+		dampingRatio = 0.0f;
+	}
+
+	// Initialize this definition from bodies and world anchors.
+	void Initialize(b3Body* bodyA, b3Body* bodyB, const b3Vec3& anchorA, const b3Vec3& anchorB);
+
+	// The joint anchor point relative to the frame of body A
+	b3Vec3 localAnchorA;
+
+	// The joint anchor point relative to the frame of body B
+	b3Vec3 localAnchorB;
+	
+	// The spring rest length
+	float32 length;
+	
+	// The mass-spring-damper frequency in units of hertz
+	// 0 = disable spring softness
+	float32 frequencyHz;
+	
+	// The damping ration in the interval [0, 1].
+	// 0 = undamped spring
+	// 1 = critical damping
+	float32 dampingRatio;
+};
+
+class b3SpringJoint : public b3Joint 
+{
+public :	
+	// Get the local anchor point in the local space of the first body.
+	const b3Vec3& GetLocalAnchorA() const;
+
+	// Get the local anchor point in the local space of the second body.
+	const b3Vec3& GetLocalAnchorB() const;
+
+	void Draw(b3Draw* b3Draw) const;
+private:
+	friend class b3Joint;
+	friend class b3JointManager;
+	friend class b3JointSolver;
+
+	b3SpringJoint(const b3SpringJointDef* def);
+
+	void InitializeConstraints(const b3SolverData* data);
+	void WarmStart(const b3SolverData* data);
+	void SolveVelocityConstraints(const b3SolverData* data);
+	bool SolvePositionConstraints(const b3SolverData* data);
+
+	// Solver shared
+	b3Vec3 m_localAnchorA;
+	b3Vec3 m_localAnchorB;
+	float32 m_length;
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+
+	// Solver temp
+	u32 m_indexA;
+	u32 m_indexB;
+	float32 m_mA;
+	float32 m_mB;
+	b3Mat33 m_iA;
+	b3Mat33 m_iB;
+	b3Vec3 m_localCenterA;
+	b3Vec3 m_localCenterB;
+
+	float32 m_bias;
+	float32 m_gamma;
+	b3Vec3 m_n;
+	b3Vec3 m_rA;
+	b3Vec3 m_rB;
+	float32 m_mass;
+	float32 m_impulse;
+};
+
+inline const b3Vec3& b3SpringJoint::GetLocalAnchorA() const
+{
+	return m_localAnchorA;
+}
+
+inline const b3Vec3& b3SpringJoint::GetLocalAnchorB() const
+{
+	return m_localAnchorB;
+}
+
+#endif

include/bounce/dynamics/shapes/capsule_shape.h

@@ -0,0 +1,44 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CAPSULE_SHAPE_H
+#define B3_CAPSULE_SHAPE_H
+
+#include <bounce\dynamics\shapes\shape.h>
+
+// A capsule defined along the up-axis.
+class b3CapsuleShape : public b3Shape 
+{
+public:
+	b3CapsuleShape();
+	~b3CapsuleShape();
+
+	void Swap(const b3CapsuleShape& other);
+
+	void ComputeMass(b3MassData* data, float32 density) const;
+
+	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;
+
+	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+
+	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
+
+	b3Vec3 m_centers[2];
+};
+
+#endif

include/bounce/dynamics/shapes/hull_shape.h

@@ -0,0 +1,45 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_HULL_SHAPE_H
+#define B3_HULL_SHAPE_H
+
+#include <bounce\dynamics\shapes\shape.h>
+
+struct b3Hull;
+
+class b3HullShape : public b3Shape 
+{
+public :
+	b3HullShape();
+	~b3HullShape();
+
+	void Swap(const b3HullShape& other);
+
+	void ComputeMass(b3MassData* data, float32 density) const;	
+
+	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;	
+
+	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+
+	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
+
+	const b3Hull* m_hull;
+};
+
+#endif

include/bounce/dynamics/shapes/mesh_shape.h

@@ -0,0 +1,49 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_MESH_SHAPE_H
+#define B3_MESH_SHAPE_H
+
+#include <bounce\dynamics\shapes\shape.h>
+
+struct b3Mesh;
+
+class b3MeshShape : public b3Shape 
+{
+public:
+	b3MeshShape();
+	~b3MeshShape();
+
+	void Swap(const b3MeshShape& other);
+
+	void ComputeMass(b3MassData* data, float32 density) const;
+
+	void ComputeAABB(b3AABB3* output, const b3Transform& xf) const;
+
+	void ComputeAABB(b3AABB3* output, const b3Transform& xf, u32 index) const;
+
+	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+
+	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
+
+	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf, u32 index) const;
+
+	const b3Mesh* m_mesh;
+};
+
+#endif

include/bounce/dynamics/shapes/shape.h

@@ -0,0 +1,218 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SHAPE_H
+#define B3_SHAPE_H
+
+#include <bounce\common\math\transform.h>
+#include <bounce\common\template\list.h>
+#include <bounce\collision\distance.h>
+
+struct b3ContactEdge;
+
+class b3Body;
+class b3Shape;
+
+enum b3ShapeType
+{
+	e_sphereShape,
+	e_capsuleShape,
+	e_hullShape,
+	e_meshShape,
+	e_maxShapes
+};
+
+struct b3ShapeDef 
+{
+	b3ShapeDef() 
+	{
+		shape = nullptr;
+		userData = nullptr;
+		isSensor = false;
+		density = 0.0f;
+		friction = 0.3f;
+		restitution = 0.0f;
+	}
+
+	const b3Shape* shape;
+	void* userData;
+	bool isSensor;
+	float32 density;
+	float32 restitution;
+	float32 friction;
+};
+
+struct b3MassData 
+{
+	float32 mass;
+	b3Vec3 center;
+	b3Mat33 I;
+};
+
+class b3Shape
+{
+public:
+	// A shape is created and initialized by an user and a body.
+	b3Shape() { }
+	virtual ~b3Shape() { }
+
+	// Get the shape type.
+	b3ShapeType GetType() const;
+
+	// Get the parent body of this shape.
+	const b3Body* GetBody() const;
+	b3Body* GetBody();
+
+	// Get the frame of the shape relative to the world.
+	b3Transform GetTransform() const;
+
+	// Calculate the mass data for this shape given the shape density, that is,
+	// the mass per unit volume.
+	virtual void ComputeMass(b3MassData* data, float32 density) const = 0;
+
+	// Compute the shape world AABB.
+	virtual void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const = 0;
+
+	// Test if a point is contained inside this shape.
+	virtual bool TestPoint(const b3Vec3& point, const b3Transform& xf) const = 0;
+
+	// Compute the ray intersection point, normal, and fraction.
+	virtual bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const = 0;
+	
+	bool IsSensor() const;
+	void SetSensor(bool flag);
+
+	float32 GetDensity() const;
+	void SetDensity(float32 density);
+
+	float32 GetRestitution() const;
+	void SetRestitution(float32 restitution);
+
+	float32 GetFriction() const;
+	void SetFriction(float32 friction);
+
+	void* GetUserData() const;
+	void SetUserData(void* data);
+
+	// Destroy the contacts associated with this shape.
+	void DestroyContacts();
+
+	// Dump this shape to the log file.
+	void Dump(i32 bodyIndex) const;
+
+	// Get the next shape in the body shape list.
+	const b3Shape* GetNext() const;
+	b3Shape* GetNext();
+
+	// Create a shape.
+	static b3Shape* Create(const b3ShapeDef& def);
+	
+	// Destroy a shape.
+	static void Destroy(b3Shape* shape);
+
+	b3ShapeType m_type;
+	bool m_isSensor;
+	void* m_userData;
+	i32 m_broadPhaseID;
+
+	float32 m_radius;
+	float32 m_density;
+	float32 m_restitution;
+	float32 m_friction;
+	
+	// Contact edges for this shape.
+	b3List2<b3ContactEdge> m_contactEdges;
+	
+	// The parent body of this shape.
+	b3Body* m_body;
+	
+	// Links to the body shape list.
+	b3Shape* m_next;
+};
+
+inline b3ShapeType b3Shape::GetType() const 
+{ 
+	return m_type; 
+}
+
+inline float32 b3Shape::GetDensity() const 
+{ 
+	return m_density; 
+}
+
+inline void b3Shape::SetDensity(float32 density) 
+{ 
+	m_density = density; 
+}
+
+inline float32 b3Shape::GetRestitution() const 
+{ 
+	return m_restitution; 
+}
+
+inline void b3Shape::SetRestitution(float32 restitution) 
+{ 
+	m_restitution = restitution; 
+}
+
+inline float32 b3Shape::GetFriction() const 
+{
+	return m_friction;
+}
+
+inline void b3Shape::SetFriction(float32 friction) 
+{
+	m_friction = friction;
+}
+
+inline bool b3Shape::IsSensor() const 
+{
+	return m_isSensor;
+}
+
+inline void* b3Shape::GetUserData() const 
+{ 
+	return m_userData; 
+}
+
+inline void b3Shape::SetUserData(void* data) 
+{ 
+	m_userData = data; 
+}
+
+inline const b3Body* b3Shape::GetBody() const 
+{
+	return m_body;
+}
+
+inline b3Body* b3Shape::GetBody() 
+{
+	return m_body;
+}
+
+inline const b3Shape* b3Shape::GetNext() const
+{
+	return m_next;
+}
+
+inline b3Shape* b3Shape::GetNext()
+{
+	return m_next;
+}
+
+#endif

include/bounce/dynamics/shapes/sphere_shape.h

@@ -0,0 +1,43 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SPHERE_SHAPE_H
+#define B3_SPHERE_SHAPE_H
+
+#include <bounce\dynamics\shapes\shape.h>
+
+class b3SphereShape : public b3Shape 
+{
+public :
+	b3SphereShape();
+	~b3SphereShape();
+
+	void Swap(const b3SphereShape& other);
+
+	void ComputeMass(b3MassData* data, float32 density) const;
+
+	void ComputeAABB(b3AABB3* aabb, const b3Transform& xf) const;
+
+	bool TestPoint(const b3Vec3& point, const b3Transform& xf) const;
+	
+	bool RayCast(b3RayCastOutput* output, const b3RayCastInput& input, const b3Transform& xf) const;
+
+	b3Vec3 m_center;
+};
+
+#endif

include/bounce/dynamics/time_step.h

@@ -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.
+*/
+
+#ifndef B3_TIME_STEP_H
+#define B3_TIME_STEP_H
+
+#include <bounce\common\math\vec3.h>
+#include <bounce\common\math\mat33.h>
+#include <bounce\common\math\quat.h>
+
+struct b3Position
+{
+	b3Vec3 x;
+	b3Quat q;
+};
+
+struct b3Velocity
+{
+	b3Vec3 v;
+	b3Vec3 w;
+};
+
+struct b3SolverData
+{
+	b3Position* positions;
+	b3Velocity* velocities;
+	float32 dt;
+	float32 invdt;
+};
+
+enum b3LimitState
+{
+	e_inactiveLimit,
+	e_atLowerLimit,
+	e_atUpperLimit,
+	e_equalLimits
+};
+
+// Compute the time derivative of an orientation given
+// the angular velocity of the rotating frame represented by the orientation.
+inline b3Quat b3Derivative(const b3Quat& orientation, const b3Vec3& velocity)
+{
+	b3Quat xf(0.5f * velocity.x, 0.5f * velocity.y, 0.5f * velocity.z, 0.0f);
+	return xf * orientation;
+}
+
+// Integrate an orientation over a time step given
+// the current orientation, angular velocity of the rotating frame
+// represented by the orientation, and the time step dt.
+inline b3Quat b3Integrate(const b3Quat& orientation, const b3Vec3& velocity, float32 dt)
+{
+	// Integrate from [t0, t0 + h] using the explicit Euler method
+	b3Quat qdot = b3Derivative(orientation, velocity);
+	b3Quat integral = dt * qdot;
+	return orientation + integral;
+}
+
+// Compute the time derivative of an orientation given
+// the angular velocity of the rotating frame represented by the orientation.
+inline b3Mat33 b3Derivative(const b3Mat33& orientation, const b3Vec3& velocity)
+{
+	// Rate of change of a basis in a rotating frame:
+	// xdot = cross(w, ex)
+	// ydot = cross(w, ey)
+	// zdot = cross(w, ez)
+	// This should yield in:
+	// qdot = skew(velocity) * q
+	b3Mat33 xf = b3Skew(velocity);
+	return xf * orientation;
+}
+
+// Integrate an orientation over a time step given
+// the current orientation, angular velocity of the rotating frame
+// represented by the orientation, and the time step dt.
+inline b3Mat33 b3Integrate(const b3Mat33& orientation, const b3Vec3& velocity, float32 dt)
+{
+	b3Mat33 qdot = b3Derivative(orientation, velocity);
+	b3Mat33 integral = dt * qdot;
+	return orientation + integral;
+}
+
+// These structures contain the elapsed time it took 
+// to run particular method.
+struct b3CollideProfile
+{
+	float64 broadphase;
+	float64 narrowphase;
+};
+
+struct b3SolverProfile
+{
+	float64 initializeContacts;
+	float64 initializeJoints;
+	float64 solveVelocity;
+	float64 solvePosition;
+};
+
+struct b3Profile
+{
+	float64 total;
+	b3CollideProfile collide;
+	b3SolverProfile solver;
+};
+
+#endif

include/bounce/dynamics/world.h

@@ -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.
+*/
+
+#ifndef B3_WORLD_H
+#define B3_WORLD_H
+
+#include <bounce\common\memory\stack_allocator.h>
+#include <bounce\common\memory\block_pool.h>
+#include <bounce\common\template\list.h>
+#include <bounce\dynamics\time_step.h>
+#include <bounce\dynamics\joint_manager.h>
+#include <bounce\dynamics\contact_manager.h>
+
+struct b3BodyDef;
+class b3Body;
+class b3QueryListener;
+class b3RayCastListener;
+class b3ContactListener;
+class b3ContactFilter;
+class b3Draw;
+
+// Use a physics world to create/destroy rigid bodies, execute ray cast and AABB queries.
+class b3World
+{
+public:
+	b3World();
+	~b3World();
+
+	// Set the debug b3Draw interface.
+	// The user must implement this interface to tell the world to 
+	// b3Draw the physics entities.
+	void SetDebugDraw(b3Draw* b3Draw);
+
+	b3Draw* GetDebugDraw();
+
+	// The filter passed can tell the world to disallow the contact creation between 
+	// two shapes.
+	void SetContactFilter(b3ContactFilter* filter);
+
+	// The listener passed will be notified when two body shapes begin/stays/ends
+	// touching with each other.
+	void SetContactListener(b3ContactListener* listener);
+	
+	// Enable body sleeping. This improves performance.
+	void SetSleeping(bool flag);
+
+	// Enable warm-starting for the constraint solvers. This improves stability significantly.
+	void SetWarmStart(bool flag);
+
+	// Set the world gravity force.
+	void SetGravity(const b3Vec3& gravity);
+
+	// Create a new rigid body.
+	b3Body* CreateBody(const b3BodyDef& def);
+	
+	// Destroy an existing rigid body.
+	void DestroyBody(b3Body* body);
+	
+	// Create a new joint.
+	b3Joint* CreateJoint(const b3JointDef& def);
+
+	// Remove a joint from the world and deallocate it from the memory.
+	void DestroyJoint(b3Joint* joint);
+
+	// Call the function below to simulate a physics step.
+	// The function parameters are the ammount of time to simulate, 
+	// and the number of contact solver iterations.
+	void Step(float32 dt, u32 velocityIterations, u32 positionIterations);
+	
+	// Perform a ray cast with the world.
+	// The given ray cast listener will be notified when a ray intersects a shape 
+	// in the world. The ray cast output is the intercepted shape, the intersection 
+	// point in world space, the face normal on the shape associated with the point, 
+	// and the intersection fraction.
+	void CastRay(b3RayCastListener* listener, const b3Vec3& p1, const b3Vec3& p2) const;
+
+	// Perform a AABB cast with the world.
+	// The query listener will be notified when two shape AABBs are overlapping.
+	// If the listener returns false then the query is stopped immediately.
+	// Otherwise, it continues searching for new overlapping shape AABBs.
+	void CastAABB(b3QueryListener* listener, const b3AABB3& aabb) const;
+
+	// Get the list of bodies in this world.
+	const b3List2<b3Body>& GetBodyList() const;
+	b3List2<b3Body>& GetBodyList();
+
+	// Get the list of joints in this world.
+	const b3List2<b3Joint>& GetJointList() const;
+	b3List2<b3Joint>& GetJointList();
+
+	// Get the list of contacts in this world.
+	const b3List2<b3Contact>& GetContactList() const;
+	b3List2<b3Contact>& GetContactList();
+
+	// Get the time spent to finish executing each simulation module of the last physics step.
+	const b3Profile& GetProfile() const;
+
+	// Tell the world to b3Draw the entities that belong to this world.
+	void DebugDraw() const;
+	void DrawShape(const b3Transform& xf, const b3Shape* shape) const;
+	void DrawJoint(const b3Joint* joint) const;
+	void DrawContact(const b3Contact* contact) const;
+private :
+	enum b3Flags 
+	{
+		e_shapeAddedFlag = 0x0001,
+		e_clearForcesFlag = 0x0002,
+	};
+
+	friend class b3Body;
+	friend class b3Shape;
+	friend class b3Contact;
+	friend class b3ConvexContact;
+	friend class b3MeshContact;
+	friend class b3Joint;
+
+	void Solve(float32 dt, u32 velocityIterations, u32 positionIterations);
+
+	bool m_sleeping;
+	bool m_warmStarting;
+	u32 m_flags;
+	b3Vec3 m_gravity;
+	b3Draw* m_debugDraw;
+	b3Profile m_profile;
+
+	b3StackAllocator m_stackAllocator;
+	b3BlockPool m_bodyBlocks;
+
+	// List of bodies
+	b3List2<b3Body> m_bodyList;
+	
+	// List of joints
+	b3JointManager m_jointMan;
+	
+	// List of contacts
+	b3ContactManager m_contactMan;
+};
+
+inline b3Draw* b3World::GetDebugDraw()
+{
+	return m_debugDraw;
+}
+
+inline void b3World::SetContactListener(b3ContactListener* listener)
+{
+	m_contactMan.m_contactListener = listener;
+}
+
+inline void b3World::SetContactFilter(b3ContactFilter* filter)
+{
+	m_contactMan.m_contactFilter = filter;
+}
+
+inline void b3World::SetGravity(const b3Vec3& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline void b3World::SetWarmStart(bool flag)
+{
+	m_warmStarting = flag;
+}
+
+inline const b3Profile& b3World::GetProfile() const 
+{
+	return m_profile;
+}
+
+inline const b3List2<b3Body>& b3World::GetBodyList() const
+{
+	return m_bodyList;
+}
+
+inline b3List2<b3Body>& b3World::GetBodyList()
+{
+	return m_bodyList;
+}
+
+inline const b3List2<b3Joint>& b3World::GetJointList() const
+{
+	return m_jointMan.m_jointList;
+}
+
+inline b3List2<b3Joint>& b3World::GetJointList()
+{
+	return m_jointMan.m_jointList;
+}
+
+inline const b3List2<b3Contact>& b3World::GetContactList() const
+{
+	return m_contactMan.m_contactList;
+}
+
+inline b3List2<b3Contact>& b3World::GetContactList()
+{
+	return m_contactMan.m_contactList;
+}
+
+#endif

include/bounce/dynamics/world_listeners.h

@@ -0,0 +1,79 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_WORLD_LISTENERS_H
+#define B3_WORLD_LISTENERS_H
+
+#include <bounce\common\math\math.h>
+
+class b3Shape;
+class b3Contact;
+
+class b3QueryListener 
+{
+public:
+	virtual ~b3QueryListener() {}
+
+	// Report to the contact listener that a shape is overlapping 
+	// the queried AABB.
+	virtual bool ReportShape(b3Shape* shape) = 0;
+};
+
+class b3RayCastListener 
+{
+public:	
+	// The user must return the new ray cast fraction.
+	// If fraction equals zero then the ray cast query will be canceled immediately.
+	virtual ~b3RayCastListener() { }
+
+	// Report that a shape was hit by the ray to this contact listener.
+	// The reported information are the shape hit by the ray,
+	// the intersection point on the shape, the surface normal associated with the point, and the 
+	// intersection fraction for the ray.
+	virtual float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction) = 0;
+};
+
+class b3ContactListener 
+{
+public:
+	// @warning You cannot create/destroy Bounce objects inside these callbacks.
+	// Inherit from this class and set it in the world to listen for collision events.	
+	// Call the functions below to inspect when a shape start/end colliding with another shape.
+	
+	// A contact has begun.
+	virtual void BeginContact(b3Contact* contact) = 0;
+
+	// A contact has ended.
+	virtual void EndContact(b3Contact* contact) = 0;
+	
+	// The contact will be solved after this notification.
+	virtual void PreSolve(b3Contact* contact) = 0;
+};
+
+// By implementing this interface the contact filter will 
+// be notified before a contact between two shapes is created and updated.
+class b3ContactFilter
+{
+public:
+	virtual ~b3ContactFilter() { }
+
+	// Should the two shapes collide?
+	virtual bool ShouldCollide(b3Shape* shapeA, b3Shape* shapeB) = 0;
+};
+
+#endif

include/bounce/quickhull/qh_hull.h

@@ -0,0 +1,174 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef QH_HULL_H
+#define QH_HULL_H
+
+#include <bounce\common\geometry.h>
+#include <bounce\common\template\array.h>
+
+template<class T>
+struct qhList
+{
+	void PushFront(T* link);
+	T* Remove(T* link);
+
+	T* head;
+	u32 count;
+};
+
+// Half-edge data structure definition used by qhHull.
+struct qhHalfEdge;
+struct qhVertex;
+
+struct qhFace
+{
+	enum State
+	{
+		e_invisible,
+		e_visible,
+		e_unknown,
+		e_deleted
+	};
+
+	qhFace* freeNext;
+
+	qhFace* prev;
+	qhFace* next;
+
+	qhHalfEdge* edge;
+
+	qhList<qhVertex> conflictList;
+	
+	State state;
+	b3Vec3 center;
+	b3Plane plane;
+
+	u32 VertexCount() const;
+	u32 EdgeCount() const; 
+	qhHalfEdge* FindTwin(const qhVertex* tail, const qhVertex* head) const;
+	void ComputeCenterAndPlane();
+};
+
+struct qhHalfEdge
+{
+	qhHalfEdge* freeNext;
+
+	qhVertex* tail;
+
+	qhHalfEdge* prev;
+	qhHalfEdge* next;
+	qhHalfEdge* twin;
+
+	qhFace* face;
+};
+
+struct qhVertex
+{
+	qhVertex* freeNext;
+
+	qhVertex* prev;
+	qhVertex* next;
+
+	b3Vec3 position;
+
+	qhFace* conflictFace;
+};
+
+// todo
+// Snapshots of the algorithm for debug drawing.
+struct qhDraw
+{
+	//DrawIteration* iter; // current iteration
+	//b3Array<DrawIteration> iterations;
+};
+
+class b3Draw;
+
+// Given a number of points return the required memory size in bytes for constructing the 
+// convex hull of those points. Use this function before allocating the memory buffer passed 
+// as argument to Construct.
+u32 qhGetMemorySize(u32 V);
+
+// A convex hull builder. Given a list of points constructs its convex hull. 
+// The output convex hull might contain polygonal faces and not only triangles. 
+// Coplanar face merging is necessary for stable physics simulation.
+class qhHull
+{
+public:
+	qhHull();
+	~qhHull();
+
+	// Entry point of qhHull.
+	// Construct this hull given a memory buffer and a list of points.
+	// Use qhGetMemorySize to see how many free bytes should be available in the buffer.
+	void Construct(void* memory, const b3Array<b3Vec3>& vertices);
+
+	// Output of qhHull.
+	// todo 
+	// Output a cleaner data structure. Maybe similar to b3Hull but storing larger hulls?
+	qhList<qhFace> m_faceList; // convex hull
+	u32 m_iteration; // number of quickhull iterations
+
+	bool IsConsistent() const;
+	
+	void Draw(b3Draw* draw) const;
+private:
+	bool BuildInitialHull(const b3Array<b3Vec3>& vertices);
+
+	qhVertex* NextVertex();
+	
+	void AddVertex(qhVertex* v);
+	
+	void BuildHorizon(b3Array<qhHalfEdge*>& horizon, qhVertex* eye);
+	
+	void BuildHorizon(b3Array<qhHalfEdge*>& horizon, qhVertex* eye, qhHalfEdge* e0, qhFace* f);
+	
+	qhFace* AddTriangle(qhVertex* v1, qhVertex* v2, qhVertex* v3);
+	
+	qhHalfEdge* AddAdjoiningTriangle(qhVertex* v, qhHalfEdge* he);
+	
+	void AddNewFaces(b3Array<qhFace*>& newFaces, qhVertex* eye, const b3Array<qhHalfEdge*>& horizon);
+
+	bool MergeFace(qhFace* face);
+	
+	void MergeFaces(b3Array<qhFace*>& newFaces);
+
+	qhHalfEdge* FindTwin(const qhVertex* tail, const qhVertex* head) const;
+
+	// Coplanarity tolerance
+	float32 m_tolerance;
+	
+	// Memory
+	qhVertex* AllocateVertex();
+	void FreeVertex(qhVertex* p);
+	
+	qhHalfEdge* AllocateEdge();
+	void FreeEdge(qhHalfEdge* p);
+	
+	qhFace* AllocateFace();
+	void FreeFace(qhFace* p);
+		
+	qhVertex* m_freeVertices;
+	qhHalfEdge* m_freeEdges;
+	qhFace* m_freeFaces;
+};
+
+#include <bounce/quickhull/qh_hull.inl>
+
+#endif

include/bounce/quickhull/qh_hull.inl

@@ -0,0 +1,190 @@
+// qhHull.h
+
+template<class T>
+inline void qhList<T>::PushFront(T* link)
+{
+	link->prev = nullptr;
+	link->next = head;
+	if (head)
+	{
+		head->prev = link;
+	}
+	head = link;
+	++count;
+}
+
+template<class T>
+inline T* qhList<T>::Remove(T* link)
+{
+	T* next = link->next;
+
+	if (link->prev)
+	{
+		link->prev->next = link->next;
+	}
+	if (link->next)
+	{
+		link->next->prev = link->prev;
+	}
+	if (link == head)
+	{
+		head = link->next;
+	}
+
+	link->prev = nullptr;
+	link->next = nullptr;
+
+	--count;
+	return next;
+}
+
+inline u32 qhFace::VertexCount() const
+{
+	u32 count = 0;
+	qhHalfEdge* e = edge;
+	do
+	{
+		++count;
+		e = e->next;
+	} while (e != edge);
+	return count;
+}
+
+inline u32 qhFace::EdgeCount() const
+{
+	u32 count = 0;
+	qhHalfEdge* e = edge;
+	do
+	{
+		++count;
+		e = e->next;
+	} while (e != edge);
+	return count;
+}
+
+inline qhHalfEdge* qhFace::FindTwin(const qhVertex* tail, const qhVertex* head) const
+{
+	qhHalfEdge* e = edge;
+	do
+	{
+		qhVertex* tail2 = e->tail;
+		qhVertex* head2 = e->next->tail;
+
+		if (tail2 == tail && head2 == head)
+		{
+			return e;
+		}
+
+		e = e->next;
+	} while (e != edge);
+
+	return nullptr;
+}
+
+inline b3Vec3 b3Newell(const b3Vec3& a, const b3Vec3& b)
+{
+	return b3Vec3((a.y - b.y) * (a.z + b.z), (a.z - b.z) * (a.x + b.x), (a.x - b.x) * (a.y + b.y));
+}
+
+inline void qhFace::ComputeCenterAndPlane()
+{
+	b3Vec3 c;
+	c.SetZero();
+
+	b3Vec3 n;
+	n.SetZero();
+
+	u32 count = 0;
+	qhHalfEdge* e = edge;
+	do
+	{
+		b3Vec3 v1 = e->tail->position;
+		b3Vec3 v2 = e->next->tail->position;
+
+		n += b3Newell(v1, v2);
+		c += v1;
+
+		++count;
+		e = e->next;
+	} while (e != edge);
+
+	B3_ASSERT(count > 0);
+	c /= float32(count);
+	n.Normalize();
+	plane.normal = n;
+	plane.offset = b3Dot(n, c);
+	center = c;
+}
+
+inline qhHalfEdge* qhHull::FindTwin(const qhVertex* tail, const qhVertex* head) const
+{
+	qhFace* face = m_faceList.head;
+	while (face)
+	{
+		qhHalfEdge* e = face->FindTwin(tail, head);
+		if (e)
+		{
+			return e;
+		}
+		face = face->next;
+	}
+	return nullptr;
+}
+
+inline u32 qhGetMemorySize(u32 V)
+{
+	u32 E = 3 * V - 6;
+	u32 HE = 2 * E;
+	u32 F = 2 * V - 4;
+
+	// V - E + F = 2
+	
+	HE *= 2;
+	F *= 2;
+
+	u32 size = 0;
+	size += V * sizeof(qhVertex);
+	size += HE * sizeof(qhHalfEdge);
+	size += F * sizeof(qhFace);
+	return size;
+}
+
+inline qhVertex* qhHull::AllocateVertex()
+{
+	qhVertex* v = m_freeVertices;
+	m_freeVertices = v->freeNext;
+	return v;
+}
+
+inline void qhHull::FreeVertex(qhVertex* v)
+{
+	v->freeNext = m_freeVertices;
+	m_freeVertices = v;
+}
+
+inline qhHalfEdge* qhHull::AllocateEdge()
+{
+	qhHalfEdge* e = m_freeEdges;
+	m_freeEdges = e->freeNext;
+	return e;
+}
+
+inline void qhHull::FreeEdge(qhHalfEdge* e)
+{
+	e->freeNext = m_freeEdges;
+	m_freeEdges = e;
+}
+
+inline qhFace* qhHull::AllocateFace()
+{
+	qhFace* f = m_freeFaces;
+	m_freeFaces = f->freeNext;
+	return f;
+}
+
+inline void qhHull::FreeFace(qhFace* f)
+{
+	f->state = qhFace::e_deleted;
+	f->freeNext = m_freeFaces;
+	m_freeFaces = f;
+}

include/testbed/framework/debug_draw.h

@@ -0,0 +1,100 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef DEBUG_DRAW_H
+#define DEBUG_DRAW_H
+
+#include <bounce\bounce.h>
+#include "mat44.h"
+
+struct DrawPoints;
+struct DrawLines;
+struct DrawTriangles;
+struct DrawShapes;
+
+class Camera
+{
+public:
+	Camera()
+	{
+		m_center.Set(0.0f, 5.0f, 0.0f);
+		m_q.SetIdentity();
+		m_width = 1024.0f;
+		m_height = 768.0f;
+		m_zNear = 1.0f;
+		m_zFar = 500.0f;
+		m_fovy = 0.25f * B3_PI;
+		m_zoom = 10.0f;
+	}
+
+	Mat44 BuildProjectionMatrix() const;
+	Mat44 BuildViewMatrix() const;
+	b3Transform BuildViewTransform() const;
+	Mat44 BuildWorldMatrix() const;
+	b3Transform BuildWorldTransform() const;
+	
+	b3Vec2 ConvertWorldToScreen(const b3Vec3& pw) const;
+	Ray3 ConvertScreenToWorld(const b3Vec2& ps) const;
+
+	float32 m_zoom;
+	b3Vec3 m_center;
+	b3Quat m_q;
+	float32 m_width, m_height;
+	float32 m_fovy;
+	float32 m_zNear;
+	float32 m_zFar;
+};
+
+class DebugDraw : public b3Draw
+{
+public:
+	DebugDraw();
+	~DebugDraw();
+
+	void DrawPoint(const b3Vec3& point, const b3Color& color);
+
+	void DrawSegment(const b3Vec3& a, const b3Vec3& b, const b3Color& color);
+
+	void DrawPolygon(const b3Vec3* vertices, u32 count, const b3Color& color);
+	
+	void DrawSolidPolygon(const b3Vec3* vertices, u32 count, const b3Color& color);
+
+	void DrawCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+
+	void DrawSolidCircle(const b3Vec3& normal, const b3Vec3& center, float32 radius, const b3Color& color);
+
+	void DrawSphere(const b3Vec3& center, float32 radius, const b3Color& color);
+	
+	void DrawSolidSphere(const b3Vec3& center, float32 radius, const b3Color& color);
+
+	void DrawAABB(const b3AABB3& aabb, const b3Color& color);
+
+	void DrawTransform(const b3Transform& xf);
+
+	void DrawString(const char* string, const b3Color& color, ...);
+
+	void Submit();
+	void Submit(const b3World& world);
+private:
+	DrawPoints* m_points;
+	DrawLines* m_lines;
+	DrawTriangles* m_triangles;
+	DrawShapes* m_shapes;
+};
+
+#endif

include/testbed/framework/mat44.h

@@ -0,0 +1,131 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MAT44_H
+#define MAT44_H
+
+#include <bounce\bounce.h>
+
+struct Vec4
+{
+	Vec4() { }
+	Vec4(float32 _x, float32 _y, float32 _z, float32 _w) : x(_x), y(_y), z(_z), w(_w) { }
+
+	void SetZero()
+	{
+		x = y = z = w = 0.0f;
+	}
+
+	void Set(float32 _x, float32 _y, float32 _z, float32 _w)
+	{
+		x = _x;
+		y = _y;
+		z = _z;
+		w = _w;
+	}
+
+	float32 x, y, z, w;
+};
+
+inline Vec4 operator+(const Vec4& a, const Vec4& b)
+{
+	return Vec4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
+}
+
+inline Vec4 operator*(float32 s, const Vec4& v)
+{
+	return Vec4(s * v.x, s * v.y, s * v.z, s * v.w);
+}
+
+struct Mat44
+{	
+	Mat44() { }
+	Mat44(const Vec4& _x, const Vec4& _y, const Vec4& _z, const Vec4& _w) : x(_x), y(_y), z(_z), w(_w) { }
+	
+	void SetIdentity()
+	{
+		x.Set(1.0f, 0.0f, 0.0f, 0.0f);
+		y.Set(0.0f, 1.0f, 0.0f, 0.0f);
+		z.Set(0.0f, 0.0f, 1.0f, 0.0f);
+		w.Set(0.0f, 0.0f, 0.0f, 1.0f);
+	}
+
+	Vec4 x, y, z, w;
+};
+
+inline Vec4 operator*(const Mat44& A, const Vec4& v)
+{
+	return v.x * A.x + v.y * A.y + v.z * A.z + v.w * A.w;
+}
+
+inline b3Vec3 operator*(const Mat44& A, const b3Vec3& v)
+{
+	Vec4 q = v.x * A.x + v.y * A.y + v.z * A.z + A.w;
+	return b3Vec3(q.x, q.y, q.z);
+}
+
+inline Mat44 operator*(const Mat44& A, const Mat44& B)
+{
+	return Mat44(A * B.x, A * B.y, A * B.z, A * B.w);
+}
+
+inline Mat44 GetMat44(const b3Transform& T)
+{
+	return Mat44(
+		Vec4(T.rotation.x.x, T.rotation.x.y, T.rotation.x.z, 0.0f),
+		Vec4(T.rotation.y.x, T.rotation.y.y, T.rotation.y.z, 0.0f),
+		Vec4(T.rotation.z.x, T.rotation.z.y, T.rotation.z.z, 0.0f),
+		Vec4(T.position.x, T.position.y, T.position.z, 1.0f));
+}
+
+inline b3Transform GetTransform(const Mat44& T)
+{
+	b3Transform xf;
+	xf.rotation.x.Set(T.x.x, T.x.y, T.x.z);
+	xf.rotation.y.Set(T.y.x, T.y.y, T.y.z);
+	xf.rotation.z.Set(T.z.x, T.z.y, T.z.z);
+	xf.position.Set(T.w.x, T.w.y, T.w.z);
+	return xf;
+}
+
+inline float32 RandomFloat(float32 a, float32 b)
+{
+	float32 x = float32(rand()) / float32(RAND_MAX);
+	float32 diff = b - a;
+	float32 r = x * diff;
+	return a + r;
+}
+
+struct Ray3
+{
+	b3Vec3 Start() const
+	{
+		return origin;
+	}
+
+	b3Vec3 End() const
+	{
+		return origin + fraction * direction;
+	}
+
+	b3Vec3 direction;
+	b3Vec3 origin;
+	float32 fraction;
+};
+
+#endif

include/testbed/tests/body_types.h

@@ -0,0 +1,245 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BODY_TYPES_H
+#define BODY_TYPES_H
+
+class BodyTypes : public Test
+{
+public:
+	BodyTypes()
+	{
+		g_camera.m_zoom = 50.0f;
+
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, 3.5f, 0.0f);
+			bd.linearVelocity.Set(0.0f, 0.0f, 0.0f);
+			bd.angularVelocity.Set(0.0f, B3_PI, 0.0f);
+
+			m_body = m_world.CreateBody(bd);
+
+			b3CapsuleShape cap;
+			cap.m_centers[0].Set(0.0f, 2.0f, 0.0f);
+			cap.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cap.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = ∩
+			sd.density = 1.5f;
+			sd.friction = 0.7f;
+
+			m_body->CreateShape(sd);
+		}
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		b3Color color(1.0f, 1.0f, 1.0f);
+		g_debugDraw->DrawString("S - Static", color);
+		g_debugDraw->DrawString("D - Dynamic", color);
+		g_debugDraw->DrawString("K - Kinematic", color);
+		g_debugDraw->DrawString("Space - Throw Bomb", color);
+		g_debugDraw->DrawString("Arrows - Apply Force/Velocity/Position", color);
+	}
+
+	void KeyDown(int button) 
+	{
+		if (button == GLFW_KEY_S)
+		{
+			m_body->SetType(e_staticBody);
+		}
+		
+		if (button == GLFW_KEY_K)
+		{
+			m_body->SetType(e_kinematicBody);
+		}
+
+		if (button == GLFW_KEY_D)
+		{
+			m_body->SetType(e_dynamicBody);
+		}
+
+		if (button == GLFW_KEY_SPACE)
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(RandomFloat(-20.0f, 20.0f), RandomFloat(10.0f, 20.0f), RandomFloat(-20.0f, 20.0f));
+
+			b3Vec3 n = m_body->GetTransform().position - bd.position;
+			n.Normalize();
+
+			bd.linearVelocity = 100.0f * n;
+
+			b3Body* enemy = m_world.CreateBody(bd);
+
+			b3SphereShape shape;
+			shape.m_center.Set(0.0f, 0.0f, 0.0f);
+			shape.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+			sd.density = 1.0f;
+			sd.friction = 1.0f;
+
+			enemy->CreateShape(sd);
+		}
+
+		if (m_body->GetType() == e_staticBody)
+		{
+			if (button == GLFW_KEY_LEFT)
+			{
+				b3Vec3 p = m_body->GetSweep().worldCenter;
+				b3Quat q = m_body->GetSweep().orientation;
+				
+				p.x -= 1.0f;
+
+				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+			}
+
+			if (button == GLFW_KEY_RIGHT)
+			{
+				b3Vec3 p = m_body->GetSweep().worldCenter;
+				b3Quat q = m_body->GetSweep().orientation;
+
+				p.x += 1.0f;
+
+				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				b3Vec3 p = m_body->GetSweep().worldCenter;
+				b3Quat q = m_body->GetSweep().orientation;
+
+				p.z += 1.0f;
+
+				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+			}
+
+			if (button == GLFW_KEY_DOWN)
+			{
+				b3Vec3 p = m_body->GetSweep().worldCenter;
+				b3Quat q = m_body->GetSweep().orientation;
+
+				p.z -= 1.0f;
+
+				m_body->SetTransform(p, b3Vec3(q.x, q.y, q.z), q.w);
+			}
+		}
+
+		if (m_body->GetType() == e_kinematicBody)
+		{
+			if (button == GLFW_KEY_LEFT)
+			{
+				b3Vec3 v = m_body->GetLinearVelocity();
+				b3Vec3 w = m_body->GetAngularVelocity();
+
+				v.x -= 5.0f;
+				w.y -= 0.25f * B3_PI;
+
+				m_body->SetLinearVelocity(v);
+				m_body->SetAngularVelocity(w);
+			}
+
+			if (button == GLFW_KEY_RIGHT)
+			{
+				b3Vec3 v = m_body->GetLinearVelocity();
+				b3Vec3 w = m_body->GetAngularVelocity();
+
+				v.x += 5.0f;
+				w.y += 0.25f * B3_PI;
+
+				m_body->SetLinearVelocity(v);
+				m_body->SetAngularVelocity(w);
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				b3Vec3 v = m_body->GetLinearVelocity();
+				b3Vec3 w = m_body->GetAngularVelocity();
+
+				v.z -= 5.0f;
+				w.y -= 0.25f * B3_PI;
+
+				m_body->SetLinearVelocity(v);
+				m_body->SetAngularVelocity(w);
+			}
+
+			if (button == GLFW_KEY_DOWN)
+			{
+				b3Vec3 v = m_body->GetLinearVelocity();
+				b3Vec3 w = m_body->GetAngularVelocity();
+
+				v.z += 5.0f;
+				w.y += 0.25f * B3_PI;
+
+				m_body->SetLinearVelocity(v);
+				m_body->SetAngularVelocity(w);
+			}
+		}
+
+		if (m_body->GetType() == e_dynamicBody)
+		{
+			if (button == GLFW_KEY_LEFT)
+			{
+				m_body->ApplyForceToCenter(b3Vec3(-100.0f, 0.0f, 0.0f), true);
+			}
+
+			if (button == GLFW_KEY_RIGHT)
+			{
+				m_body->ApplyForceToCenter(b3Vec3(100.0f, 0.0f, 0.0f), true);
+			}
+
+			if (button == GLFW_KEY_UP)
+			{
+				m_body->ApplyForceToCenter(b3Vec3(0.0f, 0.0f, -100.0f), true);
+			}
+
+			if (button == GLFW_KEY_DOWN)
+			{
+				m_body->ApplyForceToCenter(b3Vec3(0.0f, 0.0f, 100.0f), true);
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new BodyTypes();
+	}
+
+	b3Body* m_body;
+};
+
+#endif

include/testbed/tests/box_stack.h

@@ -0,0 +1,97 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BOX_STACK_H
+#define BOX_STACK_H
+
+class BoxStack : public Test
+{
+public:
+	enum
+	{
+		e_rowCount = 1,
+		e_columnCount = 5,
+		e_depthCount = 1
+	};
+
+	BoxStack()
+	{
+		g_camera.m_center.Set(2.5f, -2.0f, 5.5f);
+		g_camera.m_zoom = 40.0f;
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+			sdef.friction = 1.0f;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}		
+
+		b3Vec3 stackOrigin;
+		stackOrigin.Set(0.0f, 4.05f, 0.0f);
+
+		b3Vec3 boxScale;
+		boxScale.Set(2.05f, 2.05f, 2.05f);
+
+		for (u32 i = 0; i < e_rowCount; ++i)
+		{
+			for (u32 j = 0; j < e_columnCount; ++j)
+			{
+				for (u32 k = 0; k < e_depthCount; ++k)
+				{
+					b3BodyDef bdef;
+					bdef.type = b3BodyType::e_dynamicBody;
+
+					bdef.position.x = float32(i) * boxScale.x;
+					bdef.position.y = 1.5f * float32(j) * boxScale.y;
+					bdef.position.z = float32(k) * boxScale.z;
+					bdef.position += stackOrigin;
+
+					b3Body* body = m_world.CreateBody(bdef);
+
+					b3HullShape hs;
+					hs.m_hull = &m_boxHull;
+
+					b3ShapeDef sdef;
+					sdef.density = 0.5f;
+					sdef.friction = 0.3f;
+					sdef.shape = &hs;
+					sdef.userData = nullptr;
+
+					b3Shape* shape = body->CreateShape(sdef);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new BoxStack();
+	}
+};
+
+#endif

include/testbed/tests/capsule_and_hull_collision.h

@@ -0,0 +1,60 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CAPSULE_HULL_H
+#define CAPSULE_HULL_H
+
+class CapsuleAndHull : public Collide
+{
+public:
+	CapsuleAndHull()
+	{
+		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
+		m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.55f * B3_PI));
+		
+		m_sA.m_centers[0].Set(0.0f, -1.0f, 0.0f);
+		m_sA.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+		m_sA.m_radius = 2.0f;
+
+		m_xfB.position.Set(0.f, 0.0f, 0.0f);
+		m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.0f * B3_PI));
+
+		b3Transform xf;
+		xf.SetIdentity();
+		xf.rotation = b3Diagonal(4.0f, 1.0f, 4.0f);
+
+		m_box.SetTransform(xf);
+
+		m_sB.m_hull = &m_box;
+
+		m_shapeA = &m_sA;
+		m_shapeB = &m_sB;
+		m_cache.count = 0;
+	}
+
+	static Test* Create()
+	{
+		return new CapsuleAndHull();
+	}
+
+	b3CapsuleShape m_sA;
+	b3HullShape m_sB;
+	b3BoxHull m_box;
+};
+
+#endif

include/testbed/tests/capsule_collision.h

@@ -0,0 +1,57 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CAPSULE_COLLISION_H
+#define CAPSULE_COLLISION_H
+
+class CapsuleCollision : public Collide
+{
+public:
+	CapsuleCollision()
+	{
+		m_xfA.SetIdentity();
+		m_xfA.position.Set(0.0f, 0.0f, 0.0f);
+		//m_xfA.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.25f * B3_PI));
+		m_xfA.rotation.SetIdentity();
+		m_sA.m_centers[0].Set(0.0f, -5.0f, 0.0f);
+		m_sA.m_centers[1].Set(0.0f, 5.0f, 0.0f);
+		m_sA.m_radius = 1.0f;
+
+		m_xfB.SetIdentity();
+		m_xfB.position.Set(0.f, 0.0f, 0.0f);
+		//m_xfB.rotation = b3ConvertQuatToRot(b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.251f * B3_PI));
+		m_xfB.rotation.SetIdentity();
+		m_sB.m_centers[0].Set(0.0f, -1.0f, 0.0f);
+		m_sB.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+		m_sB.m_radius = 1.0f;
+
+		m_cache.count = 0;
+		m_shapeA = &m_sA;
+		m_shapeB = &m_sB;
+	}
+
+	static Test* Create()
+	{
+		return new CapsuleCollision();
+	}
+
+	b3CapsuleShape m_sA;
+	b3CapsuleShape m_sB;
+};
+
+#endif

include/testbed/tests/capsule_distance.h

@@ -0,0 +1,128 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CAPSULE_DISTANCE_H
+#define CAPSULE_DISTANCE_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+
+class CapsuleDistance : public Test
+{
+public:
+	CapsuleDistance()
+	{
+		g_camera.m_zoom = 25.0f;
+
+		m_xfA.SetIdentity();
+		m_xfA.position.Set(-5.0f, 0.0f, 0.0f);
+		m_xfA.rotation.SetIdentity();
+		m_shapeA.m_centers[0].Set(0.0f, -2.0f, 0.0f);
+		m_shapeA.m_centers[1].Set(0.0f, 2.0f, 0.0f);
+		m_shapeA.m_radius = 1.0f;
+
+		m_xfB.SetIdentity();
+		m_xfB.position.Set(5.0f, 0.0f, 0.0f);
+		m_xfB.rotation.SetIdentity();
+		m_shapeB.m_centers[0].Set(0.0f, -2.0f, 0.0f);
+		m_shapeB.m_centers[1].Set(0.0f, 2.0f, 0.0f);
+		m_shapeB.m_radius = 1.0f;
+	}
+
+	void Step()
+	{
+		b3Capsule edgeA;
+		edgeA.vertices[0] = m_xfA * m_shapeA.m_centers[0];
+		edgeA.vertices[1] = m_xfA * m_shapeA.m_centers[1];
+		edgeA.radius = m_shapeA.m_radius;
+
+		b3Capsule edgeB;
+		edgeB.vertices[0] = m_xfB * m_shapeB.m_centers[0];
+		edgeB.vertices[1] = m_xfB * m_shapeB.m_centers[1];
+		edgeB.radius = m_shapeB.m_radius;
+
+		b3Vec3 pointA, pointB;
+		b3ClosestPointsOnSegments(&pointA, &pointB, edgeA.vertices[0], edgeA.vertices[1], edgeB.vertices[1], edgeB.vertices[0]);
+		
+		if (b3Distance(pointA, pointB) > 0.0f)
+		{
+			g_debugDraw->DrawPoint(pointA, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawPoint(pointB, b3Color(1.0f, 0.0f, 0.0f));
+			
+			g_debugDraw->DrawSegment(pointA, pointB, b3Color(1.0f, 1.0f, 0.0f));
+		}
+
+		g_debugDraw->DrawTransform(m_xfA);
+		g_debugDraw->DrawTransform(m_xfB);
+
+		m_world.DrawShape(m_xfA, &m_shapeA);
+		m_world.DrawShape(m_xfB, &m_shapeB);
+	}
+
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_xfB.position.x -= 0.05f;
+		}
+
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_xfB.position.x += 0.05f;
+		}
+
+		if (key == GLFW_KEY_UP)
+		{
+			m_xfB.position.y += 0.05f;
+		}
+
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_xfB.position.y -= 0.05f;
+		}
+
+		if (key == GLFW_KEY_X)
+		{
+			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfx = b3ConvertQuatToRot(qx);
+
+			m_xfB.rotation = m_xfB.rotation * xfx;
+		}
+
+		if (key == GLFW_KEY_Y)
+		{
+			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfy = b3ConvertQuatToRot(qy);
+
+			m_xfB.rotation = m_xfB.rotation * xfy;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new CapsuleDistance();
+	}
+
+	b3CapsuleShape m_shapeA;
+	b3Transform m_xfA;
+	
+	b3CapsuleShape m_shapeB;
+	b3Transform m_xfB;
+};
+
+#endif

include/testbed/tests/capsule_stack.h

@@ -0,0 +1,75 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CAPSULE_STACK_H
+#define CAPSULE_STACK_H
+
+class CapsuleStack : public Test
+{
+public:
+	CapsuleStack()
+	{		
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_staticBody;
+			
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.density = 0.0f;
+			sd.friction = 1.0f;
+			sd.restitution = 0.0f;
+			
+			b3Shape* shape = body->CreateShape(sd);
+		}
+				
+		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		{
+			b3BodyDef bdef;
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bdef.position.Set(0.0f, y, 0.0f);
+			bdef.linearVelocity.Set(0.0f, -1.0f, 0.0f);
+			bdef.type = b3BodyType::e_dynamicBody;
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3CapsuleShape capsule;
+			capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
+			capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+			capsule.m_radius = 1.0f;
+
+			b3ShapeDef sdef;
+			sdef.shape = &capsule;
+			sdef.density = 1.0f;
+			sdef.friction = 0.3f;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new CapsuleStack();
+	}
+};
+
+#endif

include/testbed/tests/character_test.h

@@ -0,0 +1,102 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CHARACTER_H
+#define CHARACTER_H
+
+class Character : public Test
+{
+public:
+	Character()
+	{
+		{
+			b3BodyDef bdef;
+			b3Body* ground = m_world.CreateBody(bdef);
+
+			b3MeshShape ms;
+			ms.m_mesh = m_meshes + e_gridMesh;
+
+			b3ShapeDef sd;
+			sd.shape = &ms;
+			
+			ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_dynamicBody;
+			bdef.fixedRotationY = true;
+			bdef.position.Set(0.0f, 5.0f, 0.0f);
+			
+			m_character = m_world.CreateBody(bdef);
+
+			b3CapsuleShape cap;
+			cap.m_centers[0].Set(0.0f, 1.0f, 0.0f);
+			cap.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			cap.m_radius = 1.0f;
+
+			b3ShapeDef sdef;
+			sdef.shape = ∩
+			sdef.density = 1.0f;
+			sdef.friction = 0.5f;
+
+			m_character->CreateShape(sdef);
+		}
+	}
+
+	void RayHit()
+	{
+		if (m_rayHit.m_shape)
+		{
+			if (m_rayHit.m_shape->GetBody() != m_character)
+			{
+				Test::RayHit();
+			}
+		}
+	}
+
+	void Step()
+	{
+		if (m_rayHit.m_shape)
+		{
+			if (m_rayHit.m_shape->GetBody() != m_character)
+			{
+				b3Vec3 point = m_rayHit.m_point;
+				b3Vec3 normal = m_rayHit.m_normal;
+
+				const b3Transform& xf = m_character->GetTransform();
+				b3Vec3 n = point - xf.position;
+				n.Normalize();
+				
+				m_character->ApplyForceToCenter(100.0f * n, true);
+				g_debugDraw->DrawSolidCircle(normal, point + (0.05f * normal), 5.0f, b3Color(0.5f, 0.5f, 1.0f, 0.5f));
+			}
+		}
+
+		Test::Step();
+	}
+
+	static Test* Create()
+	{
+		return new Character();
+	}
+
+	b3Body* m_character;
+};
+
+#endif

include/testbed/tests/cluster_test.h

@@ -0,0 +1,89 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CLUSTER_H
+#define CLUSTER_H
+
+#include <bounce\dynamics\contacts\contact_cluster.h>
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+
+class Cluster : public Test
+{
+public:
+	Cluster()
+	{
+		g_camera.m_zoom = 10.0f;
+
+		// Initialize observations
+		b3StackArray<b3Observation, 256> tempObservations;
+		tempObservations.Resize(90);
+		for (u32 i = 0; i < tempObservations.Count(); ++i)
+		{
+			float32 x = RandomFloat(-1.0f, 1.0f);
+			float32 y = RandomFloat(-1.0f, 1.0f);
+			float32 z = RandomFloat(-1.0f, 1.0f);
+			
+			tempObservations[i].point.Set(x, y, z);
+			tempObservations[i].point = b3Normalize(tempObservations[i].point);
+			tempObservations[i].cluster = 0xFFFFFFFF;
+		}
+		
+		// Initialize clusters
+		b3StackArray<b3Cluster, 3> tempClusters;
+		b3InitializeClusters(tempClusters, tempObservations);
+
+		// Clusterize
+		b3Clusterize(m_clusters, m_observs, tempClusters, tempObservations);
+
+		for (u32 i = 0; i < m_clusters.Count(); ++i)
+		{
+			m_colors[i] = b3Color(RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f), RandomFloat(0.0f, 1.0f));
+		}
+	}
+
+	void Step()
+	{
+		for (u32 i = 0; i < m_clusters.Count(); ++i)
+		{
+			g_debugDraw->DrawSegment(b3Vec3(0, 0, 0), m_clusters[i].centroid, b3Color(1, 1, 1));
+			g_debugDraw->DrawPoint(m_clusters[i].centroid, m_colors[i]);
+
+			for (u32 j = 0; j < m_observs.Count(); ++j)
+			{
+				b3Observation obs = m_observs[j];
+				if (obs.cluster == i)
+				{
+					g_debugDraw->DrawPoint(obs.point, m_colors[i]);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Cluster();
+	}
+
+	b3StackArray<b3Observation, 256> m_observs;
+	b3StackArray<b3Cluster, 3> m_clusters;
+	b3Color m_colors[3];
+};
+
+#endif

include/testbed/tests/collide_test.h

@@ -0,0 +1,118 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef COLLIDE_H
+#define COLLIDE_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class Collide : public Test
+{
+public:
+	Collide()
+	{
+		g_camera.m_zoom = 25.0f;
+	}
+
+	void Step()
+	{
+		b3ConvexCache cache;
+		cache.simplexCache.count = 0;
+		cache.featureCache.m_featurePair.state = b3SATCacheType::e_empty;
+
+		b3Manifold manifold;
+		manifold.GuessImpulses();
+
+		b3CollideShapeAndShape(manifold, m_xfA, m_shapeA, m_xfB, m_shapeB, &cache);
+
+		b3WorldManifold wm;
+		wm.Initialize(&manifold, m_xfA, m_shapeA->m_radius, m_xfB, m_shapeB->m_radius);
+		
+		for (u32 i = 0; i < wm.pointCount; ++i)
+		{
+			b3WorldManifoldPoint* wmp = wm.points + i;
+			b3Vec3 pw = wmp->point;
+			b3Vec2 ps = g_camera.ConvertWorldToScreen(pw);
+			
+			g_debugDraw->DrawPoint(pw, b3Color(0.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawSegment(pw, pw + wmp->normal, b3Color(1.0f, 1.0f, 1.0f));
+		}
+
+		if (wm.pointCount > 0)
+		{
+			g_debugDraw->DrawPoint(wm.center, b3Color(1.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawSegment(wm.center, wm.center + wm.normal, b3Color(1.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent1, b3Color(1.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawSegment(wm.center, wm.center + wm.tangent2, b3Color(1.0f, 1.0f, 0.0f));
+		}
+		
+		m_world.DrawShape(m_xfA, m_shapeA);
+		m_world.DrawShape(m_xfB, m_shapeB);
+	}
+
+	virtual void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_xfB.position.x -= 0.05f;
+		}
+
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_xfB.position.x += 0.05f;
+		}
+		
+		if (key == GLFW_KEY_UP)
+		{
+			m_xfB.position.y += 0.05f;
+		}
+		
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_xfB.position.y -= 0.05f;
+		}
+
+		if (key == GLFW_KEY_X)
+		{
+			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfx = b3ConvertQuatToRot(qx);
+
+			m_xfB.rotation = m_xfB.rotation * xfx;
+		}
+
+		if (key == GLFW_KEY_Y)
+		{
+			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfy = b3ConvertQuatToRot(qy);
+
+			m_xfB.rotation = m_xfB.rotation * xfy;
+		}
+	}
+
+	b3Shape* m_shapeA;
+	b3Transform m_xfA;
+	
+	b3Shape* m_shapeB;
+	b3Transform m_xfB;
+	
+	b3SimplexCache m_cache;
+};
+
+#endif

include/testbed/tests/distance_test.h

@@ -0,0 +1,136 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef DISTANCE_H
+#define DISTANCE_H
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class Distance : public Test
+{
+public:
+	Distance()
+	{	
+		g_camera.m_zoom = 25.0f;
+		
+		m_xfA.SetIdentity();
+		m_xfA.position.Set(-5.0f, 0.0f, 0.0f);
+		m_xfA.rotation.SetIdentity();
+		m_shapeA.m_centers[0].Set(0.0f, -2.0f, 0.0f);
+		m_shapeA.m_centers[1].Set(0.0f, 2.0f, 0.0f);
+		m_shapeA.m_radius = 1.0f;
+
+		m_xfB.SetIdentity();
+		m_xfB.position.Set(5.0f, 0.0f, 0.0f);
+		m_xfB.rotation.SetIdentity();
+		m_shapeB.m_hull = &m_boxHull;
+
+		m_proxyA.Set(&m_shapeA, 0);
+		m_proxyB.Set(&m_shapeB, 0);
+		m_cache.count = 0;
+	}
+
+	void Step()
+	{
+		b3GJKOutput out = b3GJK(m_xfA, m_proxyA, m_xfB, m_proxyB, false, &m_cache);
+		if (0 < m_cache.count && m_cache.count < 4)
+		{
+			b3GJKFeaturePair featurePair = b3GetFeaturePair(m_cache);
+
+			for (u32 i = 0; i < (u32)featurePair.typeA; ++i)
+			{
+				u32 index = featurePair.indexA[i];
+				g_debugDraw->DrawPoint(m_xfA * m_proxyA.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
+			}
+
+			for (u32 i = 0; i < (u32)featurePair.typeB; ++i)
+			{
+				u32 index = featurePair.indexB[i];
+				g_debugDraw->DrawPoint(m_xfB * m_proxyB.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
+			}
+		}
+		
+		g_debugDraw->DrawPoint(out.pointA, b3Color(0.0f, 1.0f, 0.0f));
+		g_debugDraw->DrawPoint(out.pointB, b3Color(0.0f, 1.0f, 0.0f));
+		g_debugDraw->DrawSegment(out.pointA, out.pointB, b3Color(1.0f, 1.0f, 1.0f));
+
+		g_debugDraw->DrawTransform(m_xfA);
+		g_debugDraw->DrawTransform(m_xfB);
+		
+		m_world.DrawShape(m_xfA, &m_shapeA);
+		m_world.DrawShape(m_xfB, &m_shapeB);
+	}
+
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_LEFT)
+		{
+			m_xfB.position.x -= 0.05f;
+		}
+		
+		if (key == GLFW_KEY_RIGHT)
+		{
+			m_xfB.position.x += 0.05f;
+		}
+		
+		if (key == GLFW_KEY_UP)
+		{
+			m_xfB.position.y += 0.05f;
+		}
+
+		if (key == GLFW_KEY_DOWN)
+		{
+			m_xfB.position.y -= 0.05f;
+		}
+
+		if (key == GLFW_KEY_X)
+		{
+			b3Quat qx(b3Vec3(1.0f, 0.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfx = b3ConvertQuatToRot(qx);
+
+			m_xfB.rotation = m_xfB.rotation * xfx;
+		}
+		
+		if (key == GLFW_KEY_Y)
+		{
+			b3Quat qy(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
+			b3Mat33 xfy = b3ConvertQuatToRot(qy);
+
+			m_xfB.rotation = m_xfB.rotation * xfy;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Distance();
+	}
+
+	b3CapsuleShape m_shapeA;
+	b3Transform m_xfA;
+	b3ShapeGJKProxy m_proxyA;
+
+	b3HullShape m_shapeB;
+	b3Transform m_xfB;
+	b3ShapeGJKProxy m_proxyB;
+	
+	b3SimplexCache m_cache;
+};
+
+#endif

include/testbed/tests/hinge_chain.h

@@ -0,0 +1,90 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HINGE_CHAIN_H
+#define HINGE_CHAIN_H
+
+class HingeChain : public Test
+{
+public:
+	HingeChain()
+	{
+		g_camera.m_zoom = 100.0f;
+		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_center.SetZero();
+
+		float32 x = -50.0f;
+		float32 y = 0.0f;
+
+		b3Body* lastHinge;
+		{
+			b3BodyDef bd;
+			bd.position.Set(x, y, 0.0f);
+			lastHinge = m_world.CreateBody(bd);
+
+			b3HullShape hull;
+			hull.m_hull = &m_doorHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hull;
+			
+			lastHinge->CreateShape(sdef);
+		}
+		
+		x += 4.25f;
+
+		for (u32 i = 0; i < 20; ++i) 
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(x, y, 0.0f);
+			b3Body* hinge = m_world.CreateBody(bd);
+
+			b3HullShape hull;
+			hull.m_hull = &m_doorHull;
+			
+			b3ShapeDef sdef;
+			sdef.shape = &hull;
+			sdef.density = 1.0f;
+
+			hinge->CreateShape(sdef);
+			
+			{
+				b3Vec3 hingeAxis(0.0f, 1.0f, 0.0f);
+				b3Vec3 hingeAnchor(x - 2.25f, y, 0.0f);
+
+				b3RevoluteJointDef jd;
+				jd.Initialize(lastHinge, hinge, hingeAxis, hingeAnchor, 0.0f, 0.5f * B3_PI);
+				jd.collideLinked = true;
+
+				b3RevoluteJoint* rj = (b3RevoluteJoint*)m_world.CreateJoint(jd);
+			} 
+			
+			x += 4.25f;
+			lastHinge = hinge;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new HingeChain();
+	}
+};
+
+#endif

include/testbed/tests/hinge_motor.h

@@ -0,0 +1,127 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HINGE_MOTOR_H
+#define HINGE_MOTOR_H
+
+class HingeMotor : public Test
+{
+public:
+	HingeMotor()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape shape;
+			shape.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+			
+			ground->CreateShape(sd);
+		}
+
+		b3Body* hinge, *door;
+
+		{
+			b3BodyDef bd;
+			bd.position.Set(-2.0f, 5.05f, 0.0f);
+			hinge = m_world.CreateBody(bd);
+
+			b3CapsuleShape shape;
+			shape.m_centers[0].Set(0.0f, -3.5f, 0.0f);
+			shape.m_centers[1].Set(0.0f, 3.5f, 0.0f);
+			shape.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+			sd.density = 1.0f;
+
+			hinge->CreateShape(sd);
+			m_hinge = hinge;
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(1.0f, 5.05f, 0.0f);
+
+			door = m_world.CreateBody(bd);
+
+			b3HullShape hull;
+			hull.m_hull = &m_doorHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hull;
+			sdef.density = 2.0f;
+			
+			door->CreateShape(sdef);
+		}
+		
+		{
+			b3Vec3 hingeAxis(0.0f, 1.0f, 0.0f);
+			b3Vec3 hingeAnchor(-2.0f, 5.0f, 0.0f);
+			
+			b3RevoluteJointDef jd;
+			jd.Initialize(hinge, door, hingeAxis, hingeAnchor, 0.0f, 0.5f * B3_PI);
+			jd.motorSpeed = B3_PI;
+			jd.maxMotorTorque = 10000.0f;
+			
+			m_rj = (b3RevoluteJoint*)m_world.CreateJoint(jd);
+		}
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_M)
+		{
+			m_rj->SetEnableMotor(!m_rj->IsMotorEnabled());
+		}
+
+		if (button == GLFW_KEY_L)
+		{
+			m_rj->SetEnableLimit(!m_rj->IsLimitEnabled());
+		}
+		
+		if (button == GLFW_KEY_D)
+		{
+			m_hinge->SetType(e_dynamicBody);
+		}
+
+		if (button == GLFW_KEY_S)
+		{
+			m_hinge->SetType(e_staticBody);
+		}
+
+		if (button == GLFW_KEY_K)
+		{
+			m_hinge->SetType(e_kinematicBody);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new HingeMotor();
+	}
+
+	b3Body* m_hinge;
+	b3RevoluteJoint* m_rj;
+};
+
+#endif

include/testbed/tests/hull_collision.h

@@ -0,0 +1,58 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef HULL_HULL_H
+#define HULL_HULL_H
+
+class HullAndHull : public Collide
+{
+public:
+	HullAndHull()
+	{
+		b3Transform xf;
+		xf.rotation = b3Diagonal(1.0f, 2.0f, 1.0f);
+		xf.position.SetZero();
+
+		m_box.SetTransform(xf);
+
+		b3Quat qA(0.0f, 1.0f, 0.0f, 0.025f * B3_PI);
+		m_xfA.SetIdentity();
+		m_xfA.position.Set(0.0186814368f, 1.96078217f, 0.0253920462f);
+		m_xfA.rotation = b3ConvertQuatToRot(qA);
+		m_sA.m_hull = &m_box;
+		
+		m_xfB.SetIdentity();
+		m_xfB.position.Set(0.f, 0.0f, 0.0f);
+		m_sB.m_hull = &m_box;
+
+		m_cache.count = 0;
+		m_shapeA = &m_sA;
+		m_shapeB = &m_sB;
+	}
+
+	static Test* Create()
+	{
+		return new HullAndHull();
+	}
+
+	b3BoxHull m_box;
+	b3HullShape m_sA;
+	b3HullShape m_sB;
+};
+
+#endif

include/testbed/tests/jenga.h

@@ -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.
+*/
+
+#ifndef JENGA_H
+#define JENGA_H
+
+class Jenga : public Test
+{
+public:
+	enum
+	{
+		e_layerCount = 20,
+		e_depthCount = 3,
+	};
+
+	Jenga()
+	{
+		g_camera.m_center.Set(2.0f, -2.0f, 0.0f);
+		g_camera.m_zoom = 60.0f;
+
+		{
+			b3BodyDef bd;
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			body->CreateShape(sd);
+		}
+
+		b3Vec3 boxScale;
+		boxScale.Set(1.0f, 0.5f, 3.0f);
+
+		float32 y = 2.0f;
+
+		for (u32 i = 0; i < e_layerCount / 2; ++i)
+		{
+			for (u32 j = 0; j < e_depthCount; ++j)
+			{
+				b3BodyDef bd;
+				bd.type = b3BodyType::e_dynamicBody;
+
+				bd.position.x = 2.0f * float32(j) * boxScale.x;
+				bd.position.y = y;
+				bd.position.z = 0.0f;
+				
+				b3Body* body = m_world.CreateBody(bd);
+
+				b3HullShape hs;
+				hs.m_hull = &m_plankHull;
+
+				b3ShapeDef sd;
+				sd.shape = &hs;
+				sd.density = 0.1f;
+				sd.friction = 0.1f;
+
+				body->CreateShape(sd);
+			}
+
+			y += 2.05f * boxScale.y;
+
+			for (u32 j = 0; j < e_depthCount; ++j)
+			{
+				b3BodyDef bd;
+				bd.type = b3BodyType::e_dynamicBody;
+
+				bd.orientation.Set(b3Vec3(0.0f, 1.0f, 0.0f), 0.5f * B3_PI);
+
+				bd.position.x = 2.0f * boxScale.x;
+				bd.position.y = y;
+				bd.position.z = -2.0f * boxScale.x + 2.0f * float32(j) * boxScale.x;
+				
+				b3Body* body = m_world.CreateBody(bd);
+
+				b3HullShape hs;
+				hs.m_hull = &m_plankHull;
+
+				b3ShapeDef sd;
+				sd.shape = &hs;
+				sd.density = 0.1f;
+				sd.friction = 0.1f;
+
+				body->CreateShape(sd);
+			}
+
+			y += 2.05f * boxScale.y;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Jenga();
+	}
+};
+
+#endif

include/testbed/tests/mesh_contact_test.h

@@ -0,0 +1,176 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef MESH_TEST_H
+#define MESH_TEST_H
+
+class MeshContactTest : public Test
+{
+public:
+	MeshContactTest()
+	{
+		{
+			b3BodyDef bd;
+			m_ground = m_world.CreateBody(bd);
+
+			b3MeshShape ms;
+			ms.m_mesh = m_meshes + e_gridMesh;
+
+			b3ShapeDef sd;
+			sd.shape = &ms;
+
+			m_ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, 5.0f, 0.0f);
+
+			m_body = m_world.CreateBody(bd);
+			
+			{
+				b3SphereShape sphere;
+				sphere.m_center.SetZero();
+				sphere.m_radius = 1.0f;
+
+				b3ShapeDef sd;
+				sd.shape = &sphere;
+				sd.density = 1.0f;
+				sd.friction = 0.5f;
+
+				m_body->CreateShape(sd);
+			}
+		}
+	}
+
+	void KeyDown(int key)
+	{
+		if (key == GLFW_KEY_S || key == GLFW_KEY_C || key == GLFW_KEY_H)
+		{
+			if (m_body)
+			{
+				m_world.DestroyBody(m_body);
+			}
+
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, 5.0f, 0.0f);
+
+			m_body = m_world.CreateBody(bd);
+
+			if (key == GLFW_KEY_S)
+			{
+				b3SphereShape sphere;
+				sphere.m_center.SetZero();
+				sphere.m_radius = 1.0f;
+
+				b3ShapeDef sd;
+				sd.shape = &sphere;
+				sd.density = 1.0f;
+				sd.friction = 0.5f;
+
+				m_body->CreateShape(sd);
+			}
+
+			if (key == GLFW_KEY_C)
+			{
+				b3CapsuleShape capsule;
+				capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
+				capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+				capsule.m_radius = 1.0f;
+
+				b3ShapeDef sd;
+				sd.shape = &capsule;
+				sd.density = 1.0f;
+				sd.friction = 0.5f;
+
+				m_body->CreateShape(sd);
+			}
+
+			if (key == GLFW_KEY_H)
+			{
+				b3HullShape hull;
+				hull.m_hull = &m_boxHull;
+
+				b3ShapeDef sd;
+				sd.shape = &hull;
+				sd.density = 1.0f;
+				sd.friction = 0.5f;
+
+				m_body->CreateShape(sd);
+			}
+		}
+
+		if (key == GLFW_KEY_G || key == GLFW_KEY_T)
+		{
+			if (m_ground)
+			{
+				m_world.DestroyBody(m_ground);
+			}
+
+			b3BodyDef bd;
+			m_ground = m_world.CreateBody(bd);
+
+			if (key == GLFW_KEY_G)
+			{
+				b3MeshShape ms;
+				ms.m_mesh = m_meshes + e_gridMesh;
+
+				b3ShapeDef sd;
+				sd.shape = &ms;
+
+				m_ground->CreateShape(sd);
+			}
+
+			if (key == GLFW_KEY_T)
+			{
+				b3MeshShape ms;
+				ms.m_mesh = m_meshes + e_terrainMesh;
+
+				b3ShapeDef sd;
+				sd.shape = &ms;
+
+				m_ground->CreateShape(sd);
+			}
+		}
+	}
+
+	void Step()
+	{
+		Test::Step();
+
+		b3Color color(1.0f, 1.0f, 1.0f);
+		
+		g_debugDraw->DrawString("S - Sphere", color);
+		g_debugDraw->DrawString("C - Capsule", color);
+		g_debugDraw->DrawString("H - Hull", color);
+		g_debugDraw->DrawString("G - Grid", color);
+		g_debugDraw->DrawString("T - Terrain", color);
+	}
+
+	static Test* Create()
+	{
+		return new MeshContactTest();
+	}
+
+	b3Body* m_ground;
+	b3Body* m_body;
+};
+
+#endif

include/testbed/tests/newton_cradle.h

@@ -0,0 +1,124 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef NEWTON_CRADLE_H
+#define NEWTON_CRADLE_H
+
+class NewtonCradle : public Test
+{
+public:
+	NewtonCradle()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			
+			ground->CreateShape(sd);
+		}
+
+		b3CapsuleShape edge;
+		edge.m_centers[0].Set(0.0f, -10.0f, 0.0f);
+		edge.m_centers[1].Set(0.0f, 10.0f, 0.0f);
+		edge.m_radius = 0.5f;
+
+		b3Body* frame1, *frame2;
+
+		{
+			b3BodyDef bd;
+			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.position.Set(0.0f, 10.0f, -5.0f);
+
+			frame1 = m_world.CreateBody(bd);
+
+			b3ShapeDef sd;
+			sd.shape = &edge;
+
+			frame1->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			bd.position.Set(0.0f, 10.0f, 5.0f);
+
+			frame2 = m_world.CreateBody(bd);
+
+			b3ShapeDef sd;
+			sd.shape = &edge;
+
+			frame2->CreateShape(sd);
+		}
+
+		b3Vec3 center;
+		center.Set(-5.0f, 4.0f, 0.0f);
+
+		b3SphereShape vertex;
+		vertex.m_center.SetZero();
+		vertex.m_radius = 1.0f;
+
+		u32 count = 6;
+		for (u32 i = 0; i < count; ++i)
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position = center;
+			if (i == count - 1)
+			{
+				bd.linearVelocity.x = 5.0f;
+			}
+
+			b3Body* ball = m_world.CreateBody(bd);
+
+			b3ShapeDef sd;
+			sd.shape = &vertex;
+			sd.density = 1.0f;
+			sd.friction = 0.8f;
+			
+			ball->CreateShape(sd);
+			
+			b3Vec3 c1;
+			c1.x = center.x;
+			c1.y = 10.0f;
+			c1.z = 0.0f;
+
+			b3SphereJointDef jd1;
+			jd1.bodyA = frame1;
+			jd1.collideLinked = true;
+			jd1.bodyB = ball;
+			jd1.localAnchorA = b3MulT(frame1->GetTransform(), c1);
+			jd1.localAnchorB = b3MulT(ball->GetTransform(), c1);
+			
+			m_world.CreateJoint(jd1);
+
+			center.x += 2.0f * vertex.m_radius;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new NewtonCradle();
+	}
+};
+
+#endif

include/testbed/tests/pyramid.h

@@ -0,0 +1,104 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PYRAMID_H
+#define PYRAMID_H
+
+class Pyramid : public Test
+{
+public:
+	enum
+	{
+		e_count = 10,
+	};
+
+	Pyramid()
+	{
+		g_camera.m_zoom = 100.0f;
+
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		b3Vec3 boxSize;
+		boxSize.Set(2.0f, 2.0f, 2.0f);
+
+		// shift to ground center
+		b3Vec3 translation;
+		translation.x = -0.5f * float32(e_count) * boxSize.x;
+		translation.y = 1.5f * boxSize.y;
+		translation.z = -0.5f * float32(e_count) * boxSize.z;
+
+		u32 count = e_count;
+		for (u32 i = 0; i < e_count; ++i)
+		{
+			u32 j0 = 0, k0 = 0;
+			for (u32 j = j0; j < count; ++j)
+			{
+				for (u32 k = k0; k < count; ++k)
+				{
+					b3BodyDef bd;
+					bd.type = b3BodyType::e_dynamicBody;
+					bd.position.x = 1.05f * float32(j) * boxSize.x;
+					bd.position.y = 0.0f;
+					bd.position.z = 1.05f * float32(k) * boxSize.z;
+					bd.position += translation;
+
+					b3Body* body = m_world.CreateBody(bd);
+
+					b3HullShape hs;
+					hs.m_hull = &m_boxHull;
+
+					b3ShapeDef sd;
+					sd.shape = &hs;
+					sd.density = 0.5f;
+					sd.friction = 0.5f;
+
+					body->CreateShape(sd);
+				}
+			}
+
+			// reduce dimension
+			++j0;
+			++k0;
+			--count;
+			
+			// increment column
+			translation.y += 1.5f * boxSize.y;
+			// track offset
+			translation.x += 0.5f * boxSize.x;
+			translation.z += 0.5f * boxSize.z;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Pyramid();
+	}
+};
+
+#endif

include/testbed/tests/pyramids.h

@@ -0,0 +1,104 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef PYRAMIDS_H
+#define PYRAMIDS_H
+
+class Pyramids : public Test
+{
+public:
+	enum
+	{
+		e_count = 10,
+		e_depthCount = 10,
+	};
+
+	Pyramids()
+	{
+		g_camera.m_zoom = 100.0f;
+
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		b3Vec3 boxSize;
+		boxSize.Set(2.0f, 2.0f, 2.0f);
+
+		// shift to ground center
+		b3Vec3 translation;
+		translation.x = -0.5f * float32(e_count - 1) * 4.0f * boxSize.x;
+		translation.y = 1.5f * boxSize.y;
+		translation.z = -0.5f * float32(e_depthCount) * boxSize.z;
+		
+		for (u32 i = 0; i < e_count; ++i)
+		{
+			// reset 
+			translation.y = 1.5f * boxSize.y;
+			translation.z = -0.5f * float32(e_depthCount) * boxSize.z;
+
+			for (u32 j = 0; j < e_depthCount; ++j)
+			{
+				for (u32 k = j; k < e_depthCount; ++k)
+				{
+					b3BodyDef bd;
+					bd.type = e_dynamicBody;
+					bd.position.x = 0.0f;
+					bd.position.y = 0.0f;
+					bd.position.z = 1.05f * float32(k) * boxSize.z;
+					bd.position += translation;
+
+					b3Body* body = m_world.CreateBody(bd);
+
+					b3HullShape hs;
+					hs.m_hull = &m_boxHull;
+
+					b3ShapeDef sd;
+					sd.shape = &hs;
+					sd.density = 0.5f;
+					sd.friction = 0.5f;
+
+					body->CreateShape(sd);
+				}
+
+				// increment column
+				translation.y += 1.5f * boxSize.y;
+				// track offset
+				translation.z -= 0.5f * boxSize.z;
+			}
+
+			// increment row
+			translation.x += 4.0f * boxSize.x;
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Pyramids();
+	}
+};
+
+#endif

include/testbed/tests/quadrics.h

@@ -0,0 +1,127 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef QUADRIC_H
+#define QUADRIC_H
+
+#include <testbed\tests\quickhull_test.h>
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+class Quadric : public Test
+{
+public:
+	Quadric()
+	{
+		g_camera.m_zoom = 20.0f;
+		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_center.SetZero();
+
+		{
+			qhHull hull;
+
+			b3StackArray<b3Vec3, 32> points;
+			ConstructCone(points);
+
+			u32 size = qhGetMemorySize(points.Count());
+			void* p = b3Alloc(size);
+			hull.Construct(p, points);
+			m_coneHull = ConvertHull(hull);
+			b3Free(p);
+		}
+		
+		{
+			qhHull hull;
+
+			b3StackArray<b3Vec3, 32> points;
+			ConstructCylinder(points);
+
+			u32 size = qhGetMemorySize(points.Count());
+			void* p = b3Alloc(size);
+			hull.Construct(p, points);
+			m_cylinderHull = ConvertHull(hull);
+			b3Free(p);
+		}
+
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(2.0f, 5.0f, 0.0f);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hull;
+			hull.m_hull = &m_coneHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.2f;
+			sdef.friction = 0.3f;
+			sdef.shape = &hull;
+
+			body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(-2.0f, 5.0f, 0.0f);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hull;
+			hull.m_hull = &m_cylinderHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.2f;
+			sdef.friction = 0.3f;
+			sdef.shape = &hull;
+
+			body->CreateShape(sdef);
+		}
+	}
+
+	~Quadric()
+	{
+	}
+
+	static Test* Create()
+	{
+		return new Quadric();
+	}
+
+	b3Hull m_coneHull;
+	b3Hull m_cylinderHull;
+};
+
+#endif

include/testbed/tests/quickhull_test.h

@@ -0,0 +1,380 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef QHULL_H
+#define QHULL_H
+
+#include <bounce\quickhull\qh_hull.h>
+
+extern DebugDraw* g_debugDraw;
+extern Camera g_camera;
+extern Settings g_settings;
+
+struct Pair
+{
+	void* key;
+	u8 value;
+};
+
+struct Map
+{
+	void Add(const Pair& pair)
+	{
+		m_pairs.PushBack(pair);
+	}
+
+	Pair* Find(void* key)
+	{
+		for (u32 i = 0; i < m_pairs.Count(); ++i)
+		{
+			Pair* pair = m_pairs.Get(i);
+			if (pair->key == key)
+			{
+				return pair;
+			}
+		}
+		return nullptr;
+	}
+
+	b3StackArray<Pair, 256> m_pairs;
+};
+
+#define NULL_FEATURE 0xFF
+
+inline b3Hull ConvertHull(const qhHull& hull)
+{
+	u8 V = 0;
+	u8 E = 0;
+	u8 F = 0;
+
+	qhFace* face = hull.m_faceList.head;
+	while (face)
+	{
+		qhHalfEdge* e = face->edge;
+		do
+		{
+			++E;
+			++V;
+			e = e->next;
+		} while (e != face->edge);
+
+		++F;
+		face = face->next;
+	}
+
+	u8 vertexCount = 0;
+	b3Vec3* vertices = (b3Vec3*)b3Alloc(V * sizeof(b3Vec3));
+	u8 edgeCount = 0;
+	b3HalfEdge* edges = (b3HalfEdge*)b3Alloc(E * sizeof(b3HalfEdge));
+	u8 faceCount = 0;
+	b3Face* faces = (b3Face*)b3Alloc(F * sizeof(b3Face));
+	b3Plane* planes = (b3Plane*)b3Alloc(F * sizeof(b3Plane));
+
+	Map vertexMap;
+	Map edgeMap;
+
+	face = hull.m_faceList.head;
+	while (face)
+	{
+		B3_ASSERT(faceCount < F);
+		u8 iface = faceCount;
+		b3Face* f = faces + faceCount;
+		b3Plane* plane = planes + faceCount;
+		++faceCount;
+
+		*plane = face->plane;
+
+		b3StackArray<u8, 32> faceEdges;
+
+		qhHalfEdge* edge = face->edge;
+		do
+		{
+			qhHalfEdge* twin = edge->twin;
+			qhVertex* v1 = edge->tail;
+			qhVertex* v2 = twin->tail;
+
+			Pair* mte = edgeMap.Find(edge);
+			Pair* mv1 = vertexMap.Find(v1);
+			Pair* mv2 = vertexMap.Find(v2);
+
+			u8 iv1;
+			if (mv1)
+			{
+				iv1 = mv1->value;
+			}
+			else
+			{
+				B3_ASSERT(vertexCount < V);
+				iv1 = vertexCount;
+				vertices[iv1] = v1->position;
+				vertexMap.Add({ v1, iv1 });
+				++vertexCount;
+			}
+
+			u8 iv2;
+			if (mv2)
+			{
+				iv2 = mv2->value;
+			}
+			else
+			{
+				B3_ASSERT(vertexCount < V);
+				iv2 = vertexCount;
+				vertices[iv2] = v2->position;
+				vertexMap.Add({ v2, iv2 });
+				++vertexCount;
+			}
+
+			if (mte)
+			{
+				u8 ie2 = mte->value;
+				b3HalfEdge* e2 = edges + ie2;
+				B3_ASSERT(e2->face == NULL_FEATURE);
+				e2->face = iface;
+				faceEdges.PushBack(ie2);
+			}
+			else
+			{
+				B3_ASSERT(edgeCount < E);
+				u8 ie1 = edgeCount;
+				b3HalfEdge* e1 = edges + edgeCount;
+				++edgeCount;
+
+				B3_ASSERT(edgeCount < E);
+				u8 ie2 = edgeCount;
+				b3HalfEdge* e2 = edges + edgeCount;
+				++edgeCount;
+
+				e1->face = iface;
+				e1->origin = iv1;
+				e1->twin = ie2;
+
+				e2->face = NULL_FEATURE;
+				e2->origin = iv2;
+				e2->twin = ie1;
+
+				faceEdges.PushBack(ie1);
+
+				edgeMap.Add({ edge, ie1 } );
+				edgeMap.Add({ twin, ie2 } );
+			}
+
+			edge = edge->next;
+		} while (edge != face->edge);
+
+		f->edge = faceEdges[0];
+		for (u32 i = 0; i < faceEdges.Count(); ++i)
+		{
+			u32 j = i < faceEdges.Count() - 1 ? i + 1 : 0;
+			edges[faceEdges[i]].next = faceEdges[j];
+		}
+
+		face = face->next;
+	}
+
+	b3Hull out;
+	out.vertexCount = vertexCount;
+	out.vertices = vertices;
+	out.edgeCount = edgeCount;
+	out.edges = edges;
+	out.faceCount = faceCount;
+	out.faces = faces;
+	out.planes = planes;
+	out.centroid.SetZero();
+
+	for (u32 i = 0; i < vertexCount; ++i)
+	{
+		out.centroid += vertices[i];
+	}
+	out.centroid /= float32(vertexCount);
+
+	out.Validate();
+	return out;
+}
+
+inline void ConstructCylinder(b3Array<b3Vec3>& points, float32 radius = 1.0f, float32 height = 1.0f)
+{
+	{
+		b3Vec3 center(0.0f, 0.0f, 0.0f);
+
+		b3Vec3 normal;
+		normal.Set(0.0f, 1.0f, 0.0f);
+
+		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;
+
+			points.PushBack(v1);
+			points.PushBack(v2);
+
+			n1 = n2;
+			v1 = v2;
+		}
+	}
+
+	{
+		b3Vec3 center(0.0f, height, 0.0f);
+
+		b3Vec3 normal;
+		normal.Set(0.0f, 1.0f, 0.0f);
+
+		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;
+
+			points.PushBack(v1);
+			points.PushBack(v2);
+
+			n1 = n2;
+			v1 = v2;
+		}
+	}
+}
+
+inline void ConstructCone(b3Array<b3Vec3>& points, float32 radius = 1.0f, float32 height = 1.0f)
+{
+	{
+		b3Vec3 center(0.0f, 0.0f, 0.0f);
+
+		b3Vec3 normal;
+		normal.Set(0.0f, 1.0f, 0.0f);
+
+		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;
+
+			points.PushBack(v1);
+			points.PushBack(v2);
+
+			n1 = n2;
+			v1 = v2;
+		}
+	}
+
+	b3Vec3 c(0.0f, height, 0.0f);
+	points.PushBack(c);
+}
+
+class QuickhullTest : public Test
+{
+public:
+	QuickhullTest()
+	{
+		g_camera.m_zoom = 15.0f;
+		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_center.SetZero();
+
+		b3BoxHull box;
+		box.SetIdentity();
+
+		b3StackArray<b3Vec3, 256> tetra;
+		b3Vec3 v1(-1.0f, 0.0f, 0.0f);
+		b3Vec3 v2(1.0f, 0.0f, 0.0f);
+		b3Vec3 v3(0.0f, 0.0f, -1.0f);
+		b3Vec3 v4 = 0.5f * (v1 + v2 + v3);
+		v4.y += 2.0f;
+
+		tetra.PushBack(v1);
+		tetra.PushBack(v2);
+		tetra.PushBack(v3);
+		tetra.PushBack(v4);
+
+		// Minkowski sum of box and tetrahedron
+		b3StackArray<b3Vec3, 256> points;
+		for (u32 i = 0; i < box.vertexCount; ++i)
+		{
+			for (u32 j = 0; j < tetra.Count(); ++j)
+			{
+				b3Vec3 p = box.vertices[i] - tetra[j];
+				points.PushBack(p);
+			}
+		}
+
+		u32 size = qhGetMemorySize(points.Count());
+		m_memory = b3Alloc(size);
+		m_qhull.Construct(m_memory, points);
+	}
+
+	~QuickhullTest()
+	{
+		b3Free(m_memory);
+	}
+
+	void Step()
+	{
+		m_qhull.Draw(g_debugDraw);
+	}
+
+	void KeyDown(int button)
+	{
+		if (button == GLFW_KEY_LEFT)
+		{
+			//m_index = b3Max(m_index - 1, 0);
+		}
+
+		if (button == GLFW_KEY_RIGHT)
+		{
+			//m_index = b3Min(m_index + 1, i32(horizon.Count()) - 1);
+		}
+
+		if (button == GLFW_KEY_I)
+		{
+		}
+	}
+
+	static Test* Create()
+	{
+		return new QuickhullTest();
+	}
+
+	void* m_memory;
+	qhHull m_qhull;
+};
+
+#endif

include/testbed/tests/ragdoll.h

@@ -0,0 +1,223 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RAGDOLL_H
+#define RAGDOLL_H
+
+class Ragdoll : public Test
+{
+public:
+	Ragdoll()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			ground->CreateShape(sd);
+		}
+		
+		b3Body* head;
+		b3Body* hip;
+		b3Body* lArm;
+		b3Body* rArm;
+		b3Body* lLeg;
+		b3Body* rLeg;
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(0.0f, 10.0f, 0.0f);
+			hip = m_world.CreateBody(bd);
+
+			hip->ApplyForceToCenter(b3Vec3(0.0f, 0.0f, -5000.0f), true);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 0.5f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -0.5f, 0.0f);
+			cs.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 1.0f;
+			hip->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(0.0f, 12.25f, 0.0f);
+			bd.angularVelocity.Set(0.0f, 0.0f, 0.005f);
+			head = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 0.15f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -0.15f, 0.0f);
+			cs.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 5.0f;
+			head->CreateShape(sd);
+		}
+
+		// Link head to chest
+		{
+			b3ConeJointDef cd;
+			cd.bodyA = hip;
+			cd.bodyB = head;
+			cd.collideLinked = false;
+			cd.enableLimit = true;
+			cd.Initialize(hip, head, b3Vec3(0.0f, 1.0f, 0.0f), b3Vec3(0.0f, 11.55f, 0.0f), 0.25f * B3_PI);
+			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-2.5f, 11.0f, 0.0f);
+			bd.orientation.Set(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			lArm = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			cs.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 0.25f;
+			
+			lArm->CreateShape(sd);
+		}
+		
+		// Link left arm to chest
+		{
+			b3ConeJointDef cd;
+			cd.bodyA = hip;
+			cd.bodyB = lArm;
+			cd.collideLinked = false;
+			cd.enableLimit = true;
+			cd.Initialize(hip, lArm, b3Vec3(-1.0f, 0.0f, 0.0f), b3Vec3(-1.0f, 11.0f, 0.0f), B3_PI);
+			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(2.5f, 11.0f, 0.0f);
+			bd.orientation.Set(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+			rArm = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			cs.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 0.25f;
+
+			rArm->CreateShape(sd);
+		}
+		
+		// Link right arm to chest
+		{
+			b3ConeJointDef cd;
+			cd.bodyA = hip;
+			cd.bodyB = rArm;
+			cd.collideLinked = false;
+			cd.enableLimit = true;
+			cd.Initialize(hip, rArm, b3Vec3(1.0f, 0.0f, 0.0f), b3Vec3(1.0f, 11.0f, 0.0f), B3_PI);
+			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(-0.5f, 6.0f, 0.0f);
+			lLeg = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 2.0f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cs.m_radius = 0.45f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 0.25f;
+
+			lLeg->CreateShape(sd);
+		}
+
+		// Link left leg to chest
+		{
+			b3ConeJointDef cd;
+			cd.bodyA = hip;
+			cd.bodyB = lLeg;
+			cd.collideLinked = false;
+			cd.enableLimit = true;
+			cd.Initialize(hip, lLeg, b3Vec3(0.0f, -1.0f, 0.0f), b3Vec3(-0.5f, 8.5f, 0.0f), 0.25f * B3_PI);
+			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = e_dynamicBody;
+			bd.position.Set(0.5f, 6.0f, 0.0f);
+			rLeg = m_world.CreateBody(bd);
+
+			b3CapsuleShape cs;
+			cs.m_centers[0].Set(0.0f, 2.0f, 0.0f);
+			cs.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cs.m_radius = 0.45f;
+
+			b3ShapeDef sd;
+			sd.shape = &cs;
+			sd.density = 0.25f;
+
+			rLeg->CreateShape(sd);
+		}
+
+		// Link right leg to chest
+		{
+			b3ConeJointDef cd;
+			cd.bodyA = hip;
+			cd.bodyB = rLeg;
+			cd.collideLinked = false;
+			cd.enableLimit = true;
+			cd.Initialize(hip, rLeg, b3Vec3(0.0f, -1.0f, 0.0f), b3Vec3(0.5f, 8.5f, 0.0f), 0.25f * B3_PI);
+			b3ConeJoint* cj = (b3ConeJoint*)m_world.CreateJoint(cd);
+		}
+	}
+	
+	void KeyDown(int button)
+	{
+	}
+
+	static Test* Create()
+	{
+		return new Ragdoll();
+	}
+};
+
+#endif

include/testbed/tests/ray_cast.h

@@ -0,0 +1,271 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RAY_CAST_H
+#define RAY_CAST_H
+
+extern Settings g_settings;
+
+class RayCast : public Test
+{
+public:
+	RayCast()
+	{
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(0.0f, 2.0f, 10.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.25f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(-10.0f, 6.0f, -10.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.25f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_tallHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(10.0f, 2.0f, 0.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.20f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(-10.0f, 2.0f, 14.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), 0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(-14.0f, 2.0f, 5.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), -0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(20.0f, 2.0f, 5.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), -0.05f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+		
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(12.0f, 2.0f, 5.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), -0.35f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3SphereShape hs;
+			hs.m_radius = 2.5f;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+			bdef.position.Set(0.0f, 1.0f, -12.0f);
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3CapsuleShape hs;
+			hs.m_centers[0].Set(0.0f, 1.0f, 0.0f);
+			hs.m_centers[1].Set(0.0f, -1.0f, 0.0f);
+			hs.m_radius = 3.0f;
+
+			b3ShapeDef sdef;
+			sdef.density = 0.0f;
+			sdef.friction = 0.0f;
+			sdef.shape = &hs;
+			sdef.userData = nullptr;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		m_p1.Set(0.0f, 2.0f, 0.0f);
+		m_p2.Set(50.0f, 2.0f, 0.0f);
+
+		m_p12.Set(0.0f, 2.0f, 0.0f);
+		m_p22.Set(-50.0f, 2.0f, 0.0f);
+	}
+
+	void CastRay(const b3Vec3 p1, const b3Vec3 p2) const
+	{
+		// Perform the ray cast
+		RayCastListener listener;
+		m_world.CastRay(&listener, p1, p2);
+
+		int hitId = listener.FindClosestHit();
+
+		if (hitId >= 0)
+		{
+			// Hit
+			// Replace current hit
+			RayCastHit rayHit = listener.m_hits[hitId];
+
+			b3Shape* shape = rayHit.m_shape;
+
+			b3Body* bodyA = m_groundBody;
+			b3Body* bodyB = shape->GetBody();
+
+			// Ray hit point in world space
+			b3Vec3 worldPointA = rayHit.m_point;
+
+			// xf from world space to the local space of the shape
+			b3Transform xf = shape->GetTransform();
+
+			// lp = xf^-1 * wp
+			b3Vec3 localPointA = b3MulT(xf, worldPointA);
+
+			extern DebugDraw* g_debugDraw;
+
+			g_debugDraw->DrawSegment(p1, worldPointA, b3Color(0.0f, 1.0f, 0.0f));
+			g_debugDraw->DrawPoint(worldPointA, b3Color(1.0f, 0.0f, 0.0f));
+			g_debugDraw->DrawSegment(worldPointA, worldPointA + rayHit.m_normal, b3Color(1.0f, 1.0f, 1.0f));
+		}
+		else
+		{
+			g_debugDraw->DrawSegment(p1, p2, b3Color(0.0f, 1.0f, 0.0f));
+		}
+	}
+
+	void Step()
+	{
+		float32 dt = g_settings.hertz > 0.0f ? 1.0f / g_settings.hertz : 0.0f;
+		b3Quat q(b3Vec3(0.0f, 1.0f, 0.0f), dt * 0.05f * B3_PI);
+		
+		m_p1 = b3Mul(q, m_p1);
+		m_p2 = b3Mul(q, m_p2);
+
+		m_p12 = b3Mul(q, m_p12);
+		m_p22 = b3Mul(q, m_p22);
+
+		CastRay(m_p1, m_p2);
+		CastRay(m_p12, m_p22);
+
+		Test::Step();
+	}
+
+	static Test* Create()
+	{
+		return new RayCast();
+	}
+
+	b3Vec3 m_p1, m_p2;
+	b3Vec3 m_p12, m_p22;
+};
+
+#endif

include/testbed/tests/sensor_test.h

@@ -0,0 +1,164 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SENSOR_TEST_H
+#define SENSOR_TEST_H
+
+class SensorTest : public Test
+{
+public:
+	SensorTest()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+
+			ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.position.Set(0.0f, 6.0f, 0.0f);
+			
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_tallHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.isSensor = true;
+			m_sensor = body->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, 4.0f, 10.0f);
+			bd.linearVelocity.Set(0.0f, 0.0f, -5.0f);
+
+			m_character = m_world.CreateBody(bd);
+
+			b3CapsuleShape cap;
+			cap.m_centers[0].Set(0.0f, 2.0f, 0.0f);
+			cap.m_centers[1].Set(0.0f, -2.0f, 0.0f);
+			cap.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = ∩
+			sd.density = 1.5f;
+			sd.friction = 0.7f;
+
+			m_character->CreateShape(sd);
+		}
+
+		m_attack = false;
+	}
+
+	void BeginContact(b3Contact* c)
+	{
+		b3Shape* sA = c->GetShapeA();
+		b3Body* bA = sA->GetBody();
+		b3Shape* sB = c->GetShapeB();
+		b3Body* bB = sB->GetBody();
+
+		if (sA == m_sensor)
+		{
+			if (bB == m_character)
+			{
+				m_attack = true;
+			}
+		}
+
+		if (sB == m_sensor)
+		{
+			if (bA == m_character)
+			{
+				m_attack = true;
+			}
+		}
+
+	}
+
+	void EndContact(b3Contact* c)
+	{
+		b3Shape* sA = c->GetShapeA();
+		b3Body* bA = sA->GetBody();
+		b3Shape* sB = c->GetShapeB();
+		b3Body* bB = sB->GetBody();
+
+		if (sA == m_sensor)
+		{
+		}
+
+		if (sB == m_sensor)
+		{
+		}
+	}
+
+	void Step()
+	{
+		if (m_attack)
+		{
+			b3Body* sensorBody = m_sensor->GetBody();
+
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(RandomFloat(-20.0f, 20.0f), RandomFloat(10.0f, 20.0f), RandomFloat(-20.0f, 20.0f));
+
+			b3Vec3 n = m_character->GetTransform().position - bd.position;
+			n.Normalize();
+
+			bd.linearVelocity = 60.0f * n;
+
+			b3Body* enemy = m_world.CreateBody(bd);
+
+			b3SphereShape shape;
+			shape.m_center.Set(0.0f, 0.0f, 0.0f);
+			shape.m_radius = 0.5f;
+
+			b3ShapeDef sd;
+			sd.shape = &shape;
+			sd.density = 1.0f;
+			sd.friction = 1.0f;
+
+			enemy->CreateShape(sd);
+			
+			m_attack = false;
+		}
+
+		Test::Step();
+	}
+
+	static Test* Create()
+	{
+		return new SensorTest();
+	}
+	
+	b3Body* m_character;
+	b3Shape* m_sensor;
+	bool m_attack;
+};
+
+#endif

include/testbed/tests/shape_stack.h

@@ -0,0 +1,108 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SHAPE_STACK_H
+#define SHAPE_STACK_H
+
+class ShapeStack : public Test
+{
+public:
+	ShapeStack()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3MeshShape ms;
+			ms.m_mesh = m_meshes + e_gridMesh;
+			
+			b3ShapeDef sd;
+			sd.shape = &ms;
+			
+			ground->CreateShape(sd);
+		}
+
+
+		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(-10.0f, y, 0.0f);
+
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape sphere;
+			sphere.m_center.SetZero();
+			sphere.m_radius = 1.0f;
+
+			b3ShapeDef sdef;
+			sdef.shape = &sphere;
+			sdef.density = 1.0f;
+			sdef.friction = 0.3f;
+			
+			body->CreateShape(sdef);
+		}
+
+		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, y, 0.0f);
+			bd.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
+
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3CapsuleShape capsule;
+			capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
+			capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
+			capsule.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &capsule;
+			sd.density = 1.0f;
+			sd.friction = 0.3f;
+			
+			body->CreateShape(sd);
+		}
+		
+		for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(10.0f, y, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hull;
+			hull.m_hull = &m_boxHull;
+			
+			b3ShapeDef sd;
+			sd.shape = &hull;
+			sd.density = 1.0f;
+			sd.friction = 0.3f;
+			
+			body->CreateShape(sd);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new ShapeStack();
+	}
+};
+
+#endif

include/testbed/tests/sphere_stack.h

@@ -0,0 +1,93 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPHERE_STACK_H
+#define SPHERE_STACK_H
+
+class SphereStack : public Test
+{
+public:
+	enum
+	{
+		e_rowCount = 1,
+		e_columnCount = 5,
+		e_depthCount = 1
+	};
+
+	SphereStack()
+	{
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_staticBody;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+			
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			sd.density = 0.0f;
+			sd.friction = 1.0f;
+			sd.restitution = 0.0f;
+			
+			b3Shape* groundShape = ground->CreateShape(sd);
+		}
+
+		b3Vec3 stackOrigin;
+		stackOrigin.Set(0.0f, 5.0f, 0.0f);
+		float32 radius = 1.0f;
+		float32 diameter = 2.0f * radius;
+
+		for (u32 i = 0; i < e_rowCount; ++i)
+		{
+			for (u32 j = 0; j < e_columnCount; ++j)
+			{
+				for (u32 k = 0; k < e_depthCount; ++k)
+				{
+					b3BodyDef bdef;
+					bdef.type = b3BodyType::e_dynamicBody;
+					bdef.position.x = float32(i) * diameter;
+					bdef.position.y = float32(j) * diameter;
+					bdef.position.z = float32(k) * diameter;
+					bdef.position += stackOrigin;
+					bdef.linearVelocity.Set(0.0f, -50.0f, 0.0f);
+
+					b3Body* body = m_world.CreateBody(bdef);
+
+					b3SphereShape sphere;
+					sphere.m_center.SetZero();
+					sphere.m_radius = radius;
+
+					b3ShapeDef sdef;
+					sdef.shape = &sphere;
+					sdef.density = 1.0f;
+					sdef.friction = 0.3f;
+
+					b3Shape* shape = body->CreateShape(sdef);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new SphereStack();
+	}
+};
+
+#endif

include/testbed/tests/spring.h

@@ -0,0 +1,191 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef SPRING_H
+#define SPRING_H
+
+class Spring : public Test
+{
+public:
+	Spring()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			
+			ground->CreateShape(sd);
+		}
+		
+		// Car frame shape
+		{
+			b3Transform xf;
+			xf.SetIdentity();
+			xf.rotation = b3Diagonal(2.0f, 0.5f, 5.0f);
+
+			m_frameHull.SetTransform(xf);
+		}
+
+		b3HullShape box;
+		box.m_hull = &m_frameHull;
+
+		// Wheel shape
+		b3SphereShape sphere;
+		sphere.m_center.SetZero();
+		sphere.m_radius = 1.0f;
+
+		// Car frame
+		b3Body* frame;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(0.0f, 10.0f, 0.0f);
+			
+			frame = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 0.3f;
+			sdef.shape = &box;
+			
+			frame->CreateShape(sdef);
+		}
+		
+		b3Body* wheelLF;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(-1.0f, 7.0f, -4.5f);
+			bdef.fixedRotationY = true;
+
+			wheelLF = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.shape = &sphere;
+			sdef.density = 1.0f;
+			sdef.friction = 1.0f;
+			
+			wheelLF->CreateShape(sdef);
+		}
+
+		{
+			b3SpringJointDef def;
+			def.Initialize(frame, wheelLF, b3Vec3(-1.0f, 9.0f, -4.5), b3Vec3(-1.0f, 9.0f, -4.5f));
+			def.collideLinked = true;
+			def.dampingRatio = 0.5f;
+			def.frequencyHz = 4.0f;
+			
+			m_world.CreateJoint(def);
+		}
+
+		b3Body* wheelRF;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(1.0f, 7.0, -4.5f);
+			bdef.fixedRotationY = true;
+
+			wheelRF = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 1.0f;
+			sdef.shape = &sphere;
+			
+			wheelRF->CreateShape(sdef);
+		}
+		
+		{
+			b3SpringJointDef def;
+			def.Initialize(frame, wheelRF, b3Vec3(1.0f, 9.0, -4.5), b3Vec3(1.0f, 9.0, -4.5f));
+			def.collideLinked = true;
+			def.dampingRatio = 0.5f;
+			def.frequencyHz = 4.0f;
+
+			m_world.CreateJoint(def);
+		}
+		
+		b3Body* wheelLB;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(-1.0f, 7.0f, 4.5f);
+			bdef.fixedRotationY = true;
+
+			wheelLB = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.shape = &sphere;
+			sdef.density = 1.0f;
+			sdef.friction = 1.0f;
+
+			wheelLB->CreateShape(sdef);
+		}
+
+		{
+			b3SpringJointDef def;
+			def.Initialize(frame, wheelLB, b3Vec3(-1.0f, 9.0f, 4.5f), b3Vec3(-1.0f, 9.0f, 4.5f));
+			def.collideLinked = true;
+			def.dampingRatio = 0.8f;
+			def.frequencyHz = 4.0f;
+
+			m_world.CreateJoint(def);
+		}
+
+		b3Body* wheelRB;
+		{
+			b3BodyDef bdef;
+			bdef.type = e_dynamicBody;
+			bdef.position.Set(1.0f, 7.0f, 4.5f);
+			bdef.fixedRotationY = true;
+
+			wheelRB = m_world.CreateBody(bdef);
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 1.0f;
+			sdef.shape = &sphere;
+
+			wheelRB->CreateShape(sdef);
+		}
+
+		{
+			b3SpringJointDef def;
+			def.Initialize(frame, wheelRB, b3Vec3(1.0f, 9.0f, 4.5f), b3Vec3(1.0f, 9.0f, 4.5f));
+			def.collideLinked = true;
+			def.frequencyHz = 4.0f;
+			def.dampingRatio = 0.8f;
+
+			m_world.CreateJoint(def);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Spring();
+	}
+
+	b3BoxHull m_frameHull;
+};
+
+#endif

include/testbed/tests/test.h

@@ -0,0 +1,218 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef TEST_H
+#define TEST_H
+
+#include <glfw\glfw3.h>
+#include <imgui\imgui.h>
+#include "..\framework\debug_draw.h"
+#include <bounce\bounce.h>
+
+struct Settings
+{
+	Settings()
+	{
+		hertz = 60.0f;
+		velocityIterations = 8;
+		positionIterations = 2;
+		sleep = false;
+		warmStart = true;
+		convexCache = true;
+		drawCenterOfMasses = false;
+		drawShapes = true;
+		drawBounds = false;
+		drawJoints = true;
+		drawContactPoints = true;
+		drawContactNormals = false;
+		drawContactTangents = false;
+		drawStats = true;
+		drawProfile = true;
+		drawGrid = false;;
+		pause = false;
+		singleStep = false;
+		lastTestID = -1;
+		testID = 0;
+	}
+
+	int lastTestID;
+	int testID;
+
+	float32 hertz;
+	int velocityIterations;
+	int positionIterations;
+	bool sleep;
+	bool warmStart;
+	bool convexCache;
+	bool drawCenterOfMasses;
+	bool drawBounds;
+	bool drawShapes;
+	bool drawSolidShapes;
+	bool drawJoints;
+	bool drawContactPoints;
+	bool drawContactNormals;
+	bool drawContactTangents;
+	bool drawStats;
+	bool drawProfile;
+	bool drawGrid;
+	bool pause;
+	bool singleStep;
+};
+
+class Test;
+
+struct TestEntry
+{
+	typedef Test* (*TestCreate)();
+	const char* name;
+	TestCreate create;
+};
+
+enum TestType
+{
+	// Collision
+	e_QHull,
+	e_Cluster,
+	e_Distance,
+	e_CapsuleDistance,
+	e_CapsuleAndCapsule,
+	e_CapsuleAndHull,
+	e_HullAndHull,
+	// Dynamics
+	// Joints
+	e_NewtonCradle,
+	e_Vehicle,
+	e_Door,
+	e_HingeChain,
+	e_Ragdoll,
+	// Contacts
+	e_Quadric,
+	e_MeshContact,
+	// World
+	e_SphereStack,
+	e_CapsuleStack,
+	e_BoxStack,
+	e_ShapeStack,
+	e_Jenga,
+	e_Thin,
+	e_Pyramid,
+	e_Pyramids,
+	// World Queries
+	e_RayCast,
+	e_SensorTest,
+	e_Character,
+	e_BodyTypes,
+	e_VaryingFriction,
+	e_VaryingRestitution,
+	e_testCount
+};
+
+extern TestEntry g_tests[e_testCount];
+
+struct RayCastHit
+{
+	b3Shape* m_shape;
+	b3Vec3 m_point;
+	b3Vec3 m_normal;
+	float32 m_fraction;
+};
+
+class RayCastListener : public b3RayCastListener
+{
+public:
+	float32 ReportShape(b3Shape* shape, const b3Vec3& point, const b3Vec3& normal, float32 fraction)
+	{
+		RayCastHit hit;
+		hit.m_shape = shape;
+		hit.m_point = point;
+		hit.m_normal = normal;
+		hit.m_fraction = fraction;
+
+		m_hits.PushBack(hit);
+
+		// Continue.
+		return 1.0f;
+	}
+
+	int FindClosestHit() const
+	{
+		float32 minFraction = FLT_MAX;
+		int minIndex = -1;
+		for (u32 i = 0; i < m_hits.Count(); ++i)
+		{
+			const RayCastHit* hit = m_hits.Get(i);
+			if (hit->m_fraction < minFraction)
+			{
+				minFraction = hit->m_fraction;
+				minIndex = i;
+			}
+		}
+		return minIndex;
+	}
+
+	b3StackArray<RayCastHit, 256> m_hits;
+};
+
+class Test : public b3ContactListener
+{
+public:
+	enum Meshes
+	{
+		e_gridMesh,
+		e_terrainMesh,
+		e_maxMeshes,
+	};
+
+	Test();
+	virtual ~Test();
+
+	virtual void BeginContact(b3Contact* contact);
+	virtual void EndContact(b3Contact* contact);
+	virtual void PreSolve(b3Contact* contact);
+
+	virtual void Step();
+	virtual void RayHit();
+
+	virtual void MouseMove(const Ray3& pw);
+	virtual void MouseLeftDown(const Ray3& pw);
+	virtual void MouseLeftUp(const Ray3& pw);
+	virtual void KeyDown(int button) { }
+	virtual void KeyUp(int button) { }
+
+	b3World m_world;
+	b3Profile m_profile;
+	b3Profile m_maxProfile;
+
+	RayCastHit m_rayHit;
+	b3Body* m_groundBody;
+	b3BoxHull m_groundHull;
+	b3BoxHull m_boxHull;
+	b3BoxHull m_tallHull;
+	b3BoxHull m_doorHull;
+	b3BoxHull m_rampHull;
+	b3BoxHull m_plankHull;
+	b3BoxHull m_thinHull;
+
+	b3Hull* m_qhull;
+
+	b3Mesh m_meshes[e_maxMeshes];
+
+	b3MouseJoint* m_mouseJoint;
+};
+
+#endif

include/testbed/tests/thin.h

@@ -0,0 +1,95 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef THIN_H
+#define THIN_H
+
+class Thin : public Test
+{
+public:
+	enum
+	{
+		e_rowCount = 1,
+		e_columnCount = 10,
+		e_depthCount = 1
+	};
+
+	Thin()
+	{
+		g_camera.m_center.Set(2.5f, -2.0f, 5.5f);
+		g_camera.m_zoom = 40.0f;
+
+		{
+			b3BodyDef bdef;
+			bdef.type = b3BodyType::e_staticBody;
+
+			b3Body* body = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 1.0f;
+
+			b3Shape* shape = body->CreateShape(sdef);
+		}
+
+		b3Vec3 stackOrigin;
+		stackOrigin.Set(0.0f, 4.05f, 0.0f);
+
+		b3Vec3 boxScale;
+		boxScale.Set(4.05f, 2.05f, 4.05f);
+
+		for (u32 i = 0; i < e_rowCount; ++i)
+		{
+			for (u32 j = 0; j < e_columnCount; ++j)
+			{
+				for (u32 k = 0; k < e_depthCount; ++k)
+				{
+					b3BodyDef bdef;
+					bdef.type = b3BodyType::e_dynamicBody;
+
+					bdef.position.x = float32(i) * boxScale.x;
+					bdef.position.y = 1.5f * float32(j) * boxScale.y;
+					bdef.position.z = float32(k) * boxScale.z;
+					bdef.position += stackOrigin;
+
+					b3Body* body = m_world.CreateBody(bdef);
+
+					b3HullShape hs;
+					hs.m_hull = &m_thinHull;
+
+					b3ShapeDef sdef;
+					sdef.shape = &hs;
+					sdef.density = 0.5f;
+					sdef.friction = 0.2f;
+					
+					body->CreateShape(sdef);
+				}
+			}
+		}
+	}
+
+	static Test* Create()
+	{
+		return new Thin();
+	}
+};
+
+#endif

include/testbed/tests/varying_friction.h

@@ -0,0 +1,165 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef VARYING_FRICTION_H
+#define VARYING_FRICTION_H
+
+class VaryingFriction : public Test
+{
+public:
+	VaryingFriction()
+	{
+		g_camera.m_zoom = 200.0f;
+		g_camera.m_q = b3Quat(b3Vec3(1.0f, 0.0f, 0.0f), -0.1f * B3_PI);
+		g_camera.m_q = b3Quat(b3Vec3(0.0f, 1.0f, 0.0f), -0.1f * B3_PI) * g_camera.m_q;
+
+		{
+			b3BodyDef bdef;	
+			b3Body* ground = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			ground->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(-20.0f, 20.0f, 0.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), -0.1f * B3_PI);
+
+			b3Body* ramp = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_rampHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 0.4f;
+			ramp->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(20.0f, 30.0f, 0.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.1f * B3_PI);
+
+			b3Body* ramp = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_rampHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 0.3f;
+			ramp->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(-20.0f, 40.0f, 0.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), -0.1f * B3_PI);
+
+			b3Body* ramp = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_rampHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 0.2f;
+			ramp->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bdef;
+			bdef.position.Set(20.0f, 50.0f, 0.0f);
+			bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.1f * B3_PI);
+
+			b3Body* ramp = m_world.CreateBody(bdef);
+
+			b3HullShape hs;
+			hs.m_hull = &m_rampHull;
+
+			b3ShapeDef sdef;
+			sdef.shape = &hs;
+			sdef.friction = 0.1f;
+			ramp->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(40.0f, 70.0f, -10.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 0.2f;
+			sdef.shape = &hs;
+
+			body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(40.0f, 70.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 0.5f;
+			sdef.shape = &hs;
+
+			body->CreateShape(sdef);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(40.0f, 70.0f, 10.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_boxHull;
+
+			b3ShapeDef sdef;
+			sdef.density = 1.0f;
+			sdef.friction = 0.8f;
+			sdef.shape = &hs;
+
+			body->CreateShape(sdef);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new VaryingFriction();
+	}
+};
+
+#endif

include/testbed/tests/varying_restitution.h

@@ -0,0 +1,137 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef VARYING_RESTITUTION_H
+#define VARYING_RESTITUTION_H
+
+class VaryingRestitution : public Test
+{
+public:
+	VaryingRestitution()
+	{
+		{
+			b3BodyDef bd;
+			b3Body* ground = m_world.CreateBody(bd);
+
+			b3HullShape hs;
+			hs.m_hull = &m_groundHull;
+
+			b3ShapeDef sd;
+			sd.shape = &hs;
+			
+			ground->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(-10.0f, 10.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape ball;
+			ball.m_center.SetZero();
+			ball.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &ball;
+			sd.density = 1.0f;
+			sd.restitution = 0.2f;
+			
+			body->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(-5.0f, 10.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape ball;
+			ball.m_center.SetZero();
+			ball.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &ball;
+			sd.density = 1.0f;
+			sd.restitution = 0.4f;
+
+			body->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(0.0f, 10.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape ball;
+			ball.m_center.SetZero();
+			ball.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &ball;
+			sd.density = 1.0f;
+			sd.restitution = 0.6f;
+
+			body->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(5.0f, 10.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape ball;
+			ball.m_center.SetZero();
+			ball.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &ball;
+			sd.density = 1.0f;
+			sd.restitution = 0.8f;
+
+			body->CreateShape(sd);
+		}
+
+		{
+			b3BodyDef bd;
+			bd.type = b3BodyType::e_dynamicBody;
+			bd.position.Set(10.0f, 10.0f, 0.0f);
+			b3Body* body = m_world.CreateBody(bd);
+
+			b3SphereShape ball;
+			ball.m_center.SetZero();
+			ball.m_radius = 1.0f;
+
+			b3ShapeDef sd;
+			sd.shape = &ball;
+			sd.density = 1.0f;
+			sd.restitution = 1.0f;
+
+			body->CreateShape(sd);
+		}
+	}
+
+	static Test* Create()
+	{
+		return new VaryingRestitution();
+	}
+};
+
+#endif