first commit

2016-12-18 18:39:47 -02:00
commit 8f29bc7e21
232 changed files with 103257 additions and 0 deletions
--- a/include/bounce/collision/broad_phase.h
+++ b/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
--- a/include/bounce/collision/distance.h
+++ b/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
--- a/include/bounce/collision/gjk/gjk.h
+++ b/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
--- a/include/bounce/collision/gjk/gjk_cache.h
+++ b/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
--- a/include/bounce/collision/gjk/gjk_proxy.h
+++ b/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
--- a/include/bounce/collision/sat/sat.h
+++ b/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
--- a/include/bounce/collision/sat/sat_edge_and_hull.h
+++ b/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
--- a/include/bounce/collision/sat/sat_vertex_and_hull.h
+++ b/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
--- a/include/bounce/collision/shapes/aabb3.h
+++ b/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
--- a/include/bounce/collision/shapes/box_hull.h
+++ b/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
--- a/include/bounce/collision/shapes/capsule.h
+++ b/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
--- a/include/bounce/collision/shapes/hull.h
+++ b/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
--- a/include/bounce/collision/shapes/hull.inl
+++ b/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;
+}
--- a/include/bounce/collision/shapes/mesh.h
+++ b/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
--- a/include/bounce/collision/shapes/sphere.h
+++ b/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
--- a/include/bounce/collision/shapes/triangle_hull.h
+++ b/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
--- a/include/bounce/collision/trees/dynamic_tree.h
+++ b/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
--- a/include/bounce/collision/trees/static_tree.h
+++ b/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