+box2d

22 files changed, 5847 insertions, 0 deletions

diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.cpp b/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.cpp
new file mode 100644
index 0000000..a709585
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.cpp
@@ -0,0 +1,198 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/Shapes/b2ChainShape.h"
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include <new>
+#include <string.h>
+
+b2ChainShape::~b2ChainShape()
+{
+	Clear();
+}
+
+void b2ChainShape::Clear()
+{
+	b2Free(m_vertices);
+	m_vertices = nullptr;
+	m_count = 0;
+}
+
+void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(m_vertices == nullptr && m_count == 0);
+	b2Assert(count >= 3);
+	if (count < 3)
+	{
+		return;
+	}
+
+	for (int32 i = 1; i < count; ++i)
+	{
+		b2Vec2 v1 = vertices[i-1];
+		b2Vec2 v2 = vertices[i];
+		// If the code crashes here, it means your vertices are too close together.
+		b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
+	}
+
+	m_count = count + 1;
+	m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+	memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
+	m_vertices[count] = m_vertices[0];
+	m_prevVertex = m_vertices[m_count - 2];
+	m_nextVertex = m_vertices[1];
+	m_hasPrevVertex = true;
+	m_hasNextVertex = true;
+}
+
+void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(m_vertices == nullptr && m_count == 0);
+	b2Assert(count >= 2);
+	for (int32 i = 1; i < count; ++i)
+	{
+		// If the code crashes here, it means your vertices are too close together.
+		b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop);
+	}
+
+	m_count = count;
+	m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
+	memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
+
+	m_hasPrevVertex = false;
+	m_hasNextVertex = false;
+
+	m_prevVertex.SetZero();
+	m_nextVertex.SetZero();
+}
+
+void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
+{
+	m_prevVertex = prevVertex;
+	m_hasPrevVertex = true;
+}
+
+void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
+{
+	m_nextVertex = nextVertex;
+	m_hasNextVertex = true;
+}
+
+b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainShape));
+	b2ChainShape* clone = new (mem) b2ChainShape;
+	clone->CreateChain(m_vertices, m_count);
+	clone->m_prevVertex = m_prevVertex;
+	clone->m_nextVertex = m_nextVertex;
+	clone->m_hasPrevVertex = m_hasPrevVertex;
+	clone->m_hasNextVertex = m_hasNextVertex;
+	return clone;
+}
+
+int32 b2ChainShape::GetChildCount() const
+{
+	// edge count = vertex count - 1
+	return m_count - 1;
+}
+
+void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
+{
+	b2Assert(0 <= index && index < m_count - 1);
+	edge->m_type = b2Shape::e_edge;
+	edge->m_radius = m_radius;
+
+	edge->m_vertex1 = m_vertices[index + 0];
+	edge->m_vertex2 = m_vertices[index + 1];
+
+	if (index > 0)
+	{
+		edge->m_vertex0 = m_vertices[index - 1];
+		edge->m_hasVertex0 = true;
+	}
+	else
+	{
+		edge->m_vertex0 = m_prevVertex;
+		edge->m_hasVertex0 = m_hasPrevVertex;
+	}
+
+	if (index < m_count - 2)
+	{
+		edge->m_vertex3 = m_vertices[index + 2];
+		edge->m_hasVertex3 = true;
+	}
+	else
+	{
+		edge->m_vertex3 = m_nextVertex;
+		edge->m_hasVertex3 = m_hasNextVertex;
+	}
+}
+
+bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	B2_NOT_USED(xf);
+	B2_NOT_USED(p);
+	return false;
+}
+
+bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& xf, int32 childIndex) const
+{
+	b2Assert(childIndex < m_count);
+
+	b2EdgeShape edgeShape;
+
+	int32 i1 = childIndex;
+	int32 i2 = childIndex + 1;
+	if (i2 == m_count)
+	{
+		i2 = 0;
+	}
+
+	edgeShape.m_vertex1 = m_vertices[i1];
+	edgeShape.m_vertex2 = m_vertices[i2];
+
+	return edgeShape.RayCast(output, input, xf, 0);
+}
+
+void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+	b2Assert(childIndex < m_count);
+
+	int32 i1 = childIndex;
+	int32 i2 = childIndex + 1;
+	if (i2 == m_count)
+	{
+		i2 = 0;
+	}
+
+	b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
+	b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
+
+	aabb->lowerBound = b2Min(v1, v2);
+	aabb->upperBound = b2Max(v1, v2);
+}
+
+void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	B2_NOT_USED(density);
+
+	massData->mass = 0.0f;
+	massData->center.SetZero();
+	massData->I = 0.0f;
+}
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.h b/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.h
new file mode 100644
index 0000000..7c8c1a8
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2ChainShape.h
@@ -0,0 +1,105 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+*
+* 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 B2_CHAIN_SHAPE_H
+#define B2_CHAIN_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+class b2EdgeShape;
+
+/// A chain shape is a free form sequence of line segments.
+/// The chain has two-sided collision, so you can use inside and outside collision.
+/// Therefore, you may use any winding order.
+/// Since there may be many vertices, they are allocated using b2Alloc.
+/// Connectivity information is used to create smooth collisions.
+/// WARNING: The chain will not collide properly if there are self-intersections.
+class b2ChainShape : public b2Shape
+{
+public:
+	b2ChainShape();
+
+	/// The destructor frees the vertices using b2Free.
+	~b2ChainShape();
+
+	/// Clear all data.
+	void Clear();
+
+	/// Create a loop. This automatically adjusts connectivity.
+	/// @param vertices an array of vertices, these are copied
+	/// @param count the vertex count
+	void CreateLoop(const b2Vec2* vertices, int32 count);
+
+	/// Create a chain with isolated end vertices.
+	/// @param vertices an array of vertices, these are copied
+	/// @param count the vertex count
+	void CreateChain(const b2Vec2* vertices, int32 count);
+
+	/// Establish connectivity to a vertex that precedes the first vertex.
+	/// Don't call this for loops.
+	void SetPrevVertex(const b2Vec2& prevVertex);
+
+	/// Establish connectivity to a vertex that follows the last vertex.
+	/// Don't call this for loops.
+	void SetNextVertex(const b2Vec2& nextVertex);
+
+	/// Implement b2Shape. Vertices are cloned using b2Alloc.
+	b2Shape* Clone(b2BlockAllocator* allocator) const override;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const override;
+
+	/// Get a child edge.
+	void GetChildEdge(b2EdgeShape* edge, int32 index) const;
+
+	/// This always return false.
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+					const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
+
+	/// Chains have zero mass.
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const override;
+
+	/// The vertices. Owned by this class.
+	b2Vec2* m_vertices;
+
+	/// The vertex count.
+	int32 m_count;
+
+	b2Vec2 m_prevVertex, m_nextVertex;
+	bool m_hasPrevVertex, m_hasNextVertex;
+};
+
+inline b2ChainShape::b2ChainShape()
+{
+	m_type = e_chain;
+	m_radius = b2_polygonRadius;
+	m_vertices = nullptr;
+	m_count = 0;
+	m_hasPrevVertex = false;
+	m_hasNextVertex = false;
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.cpp b/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.cpp
new file mode 100644
index 0000000..fa24dc8
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.cpp
@@ -0,0 +1,99 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include <new>
+
+b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2CircleShape));
+	b2CircleShape* clone = new (mem) b2CircleShape;
+	*clone = *this;
+	return clone;
+}
+
+int32 b2CircleShape::GetChildCount() const
+{
+	return 1;
+}
+
+bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
+{
+	b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
+	b2Vec2 d = p - center;
+	return b2Dot(d, d) <= m_radius * m_radius;
+}
+
+// Collision Detection in Interactive 3D Environments by Gino van den Bergen
+// From Section 3.1.2
+// x = s + a * r
+// norm(x) = radius
+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& transform, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
+	b2Vec2 s = input.p1 - position;
+	float32 b = b2Dot(s, s) - m_radius * m_radius;
+
+	// Solve quadratic equation.
+	b2Vec2 r = input.p2 - input.p1;
+	float32 c =  b2Dot(s, r);
+	float32 rr = b2Dot(r, r);
+	float32 sigma = c * c - rr * b;
+
+	// Check for negative discriminant and short segment.
+	if (sigma < 0.0f || rr < b2_epsilon)
+	{
+		return false;
+	}
+
+	// Find the point of intersection of the line with the circle.
+	float32 a = -(c + b2Sqrt(sigma));
+
+	// Is the intersection point on the segment?
+	if (0.0f <= a && a <= input.maxFraction * rr)
+	{
+		a /= rr;
+		output->fraction = a;
+		output->normal = s + a * r;
+		output->normal.Normalize();
+		return true;
+	}
+
+	return false;
+}
+
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
+	aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
+	aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
+}
+
+void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	massData->mass = density * b2_pi * m_radius * m_radius;
+	massData->center = m_p;
+
+	// inertia about the local origin
+	massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
+}
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.h b/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.h
new file mode 100644
index 0000000..d2c646e
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2CircleShape.h
@@ -0,0 +1,60 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_CIRCLE_SHAPE_H
+#define B2_CIRCLE_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+/// A circle shape.
+class b2CircleShape : public b2Shape
+{
+public:
+	b2CircleShape();
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const override;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const override;
+
+	/// Implement b2Shape.
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+				const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const override;
+
+	/// Position
+	b2Vec2 m_p;
+};
+
+inline b2CircleShape::b2CircleShape()
+{
+	m_type = e_circle;
+	m_radius = 0.0f;
+	m_p.SetZero();
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.cpp b/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.cpp
new file mode 100644
index 0000000..7b8dd57
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.cpp
@@ -0,0 +1,138 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include <new>
+
+void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
+{
+	m_vertex1 = v1;
+	m_vertex2 = v2;
+	m_hasVertex0 = false;
+	m_hasVertex3 = false;
+}
+
+b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeShape));
+	b2EdgeShape* clone = new (mem) b2EdgeShape;
+	*clone = *this;
+	return clone;
+}
+
+int32 b2EdgeShape::GetChildCount() const
+{
+	return 1;
+}
+
+bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	B2_NOT_USED(xf);
+	B2_NOT_USED(p);
+	return false;
+}
+
+// p = p1 + t * d
+// v = v1 + s * e
+// p1 + t * d = v1 + s * e
+// s * e - t * d = p1 - v1
+bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	// Put the ray into the edge's frame of reference.
+	b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+	b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
+	b2Vec2 d = p2 - p1;
+
+	b2Vec2 v1 = m_vertex1;
+	b2Vec2 v2 = m_vertex2;
+	b2Vec2 e = v2 - v1;
+	b2Vec2 normal(e.y, -e.x);
+	normal.Normalize();
+
+	// q = p1 + t * d
+	// dot(normal, q - v1) = 0
+	// dot(normal, p1 - v1) + t * dot(normal, d) = 0
+	float32 numerator = b2Dot(normal, v1 - p1);
+	float32 denominator = b2Dot(normal, d);
+
+	if (denominator == 0.0f)
+	{
+		return false;
+	}
+
+	float32 t = numerator / denominator;
+	if (t < 0.0f || input.maxFraction < t)
+	{
+		return false;
+	}
+
+	b2Vec2 q = p1 + t * d;
+
+	// q = v1 + s * r
+	// s = dot(q - v1, r) / dot(r, r)
+	b2Vec2 r = v2 - v1;
+	float32 rr = b2Dot(r, r);
+	if (rr == 0.0f)
+	{
+		return false;
+	}
+
+	float32 s = b2Dot(q - v1, r) / rr;
+	if (s < 0.0f || 1.0f < s)
+	{
+		return false;
+	}
+
+	output->fraction = t;
+	if (numerator > 0.0f)
+	{
+		output->normal = -b2Mul(xf.q, normal);
+	}
+	else
+	{
+		output->normal = b2Mul(xf.q, normal);
+	}
+	return true;
+}
+
+void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 v1 = b2Mul(xf, m_vertex1);
+	b2Vec2 v2 = b2Mul(xf, m_vertex2);
+
+	b2Vec2 lower = b2Min(v1, v2);
+	b2Vec2 upper = b2Max(v1, v2);
+
+	b2Vec2 r(m_radius, m_radius);
+	aabb->lowerBound = lower - r;
+	aabb->upperBound = upper + r;
+}
+
+void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	B2_NOT_USED(density);
+
+	massData->mass = 0.0f;
+	massData->center = 0.5f * (m_vertex1 + m_vertex2);
+	massData->I = 0.0f;
+}
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.h b/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.h
new file mode 100644
index 0000000..63b1a56
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2EdgeShape.h
@@ -0,0 +1,74 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+*
+* 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 B2_EDGE_SHAPE_H
+#define B2_EDGE_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+/// A line segment (edge) shape. These can be connected in chains or loops
+/// to other edge shapes. The connectivity information is used to ensure
+/// correct contact normals.
+class b2EdgeShape : public b2Shape
+{
+public:
+	b2EdgeShape();
+
+	/// Set this as an isolated edge.
+	void Set(const b2Vec2& v1, const b2Vec2& v2);
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const override;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const override;
+
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+				const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const override;
+	
+	/// These are the edge vertices
+	b2Vec2 m_vertex1, m_vertex2;
+
+	/// Optional adjacent vertices. These are used for smooth collision.
+	b2Vec2 m_vertex0, m_vertex3;
+	bool m_hasVertex0, m_hasVertex3;
+};
+
+inline b2EdgeShape::b2EdgeShape()
+{
+	m_type = e_edge;
+	m_radius = b2_polygonRadius;
+	m_vertex0.x = 0.0f;
+	m_vertex0.y = 0.0f;
+	m_vertex3.x = 0.0f;
+	m_vertex3.y = 0.0f;
+	m_hasVertex0 = false;
+	m_hasVertex3 = false;
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.cpp b/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.cpp
new file mode 100644
index 0000000..3c8c47d
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.cpp
@@ -0,0 +1,468 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+#include <new>
+
+b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonShape));
+	b2PolygonShape* clone = new (mem) b2PolygonShape;
+	*clone = *this;
+	return clone;
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
+{
+	m_count = 4;
+	m_vertices[0].Set(-hx, -hy);
+	m_vertices[1].Set( hx, -hy);
+	m_vertices[2].Set( hx,  hy);
+	m_vertices[3].Set(-hx,  hy);
+	m_normals[0].Set(0.0f, -1.0f);
+	m_normals[1].Set(1.0f, 0.0f);
+	m_normals[2].Set(0.0f, 1.0f);
+	m_normals[3].Set(-1.0f, 0.0f);
+	m_centroid.SetZero();
+}
+
+void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+{
+	m_count = 4;
+	m_vertices[0].Set(-hx, -hy);
+	m_vertices[1].Set( hx, -hy);
+	m_vertices[2].Set( hx,  hy);
+	m_vertices[3].Set(-hx,  hy);
+	m_normals[0].Set(0.0f, -1.0f);
+	m_normals[1].Set(1.0f, 0.0f);
+	m_normals[2].Set(0.0f, 1.0f);
+	m_normals[3].Set(-1.0f, 0.0f);
+	m_centroid = center;
+
+	b2Transform xf;
+	xf.p = center;
+	xf.q.Set(angle);
+
+	// Transform vertices and normals.
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		m_vertices[i] = b2Mul(xf, m_vertices[i]);
+		m_normals[i] = b2Mul(xf.q, m_normals[i]);
+	}
+}
+
+int32 b2PolygonShape::GetChildCount() const
+{
+	return 1;
+}
+
+static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
+{
+	b2Assert(count >= 3);
+
+	b2Vec2 c; c.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+
+	// pRef is the reference point for forming triangles.
+	// It's location doesn't change the result (except for rounding error).
+	b2Vec2 pRef(0.0f, 0.0f);
+#if 0
+	// This code would put the reference point inside the polygon.
+	for (int32 i = 0; i < count; ++i)
+	{
+		pRef += vs[i];
+	}
+	pRef *= 1.0f / count;
+#endif
+
+	const float32 inv3 = 1.0f / 3.0f;
+
+	for (int32 i = 0; i < count; ++i)
+	{
+		// Triangle vertices.
+		b2Vec2 p1 = pRef;
+		b2Vec2 p2 = vs[i];
+		b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
+
+		b2Vec2 e1 = p2 - p1;
+		b2Vec2 e2 = p3 - p1;
+
+		float32 D = b2Cross(e1, e2);
+
+		float32 triangleArea = 0.5f * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		c += triangleArea * inv3 * (p1 + p2 + p3);
+	}
+
+	// Centroid
+	b2Assert(area > b2_epsilon);
+	c *= 1.0f / area;
+	return c;
+}
+
+void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(3 <= count && count <= b2_maxPolygonVertices);
+	if (count < 3)
+	{
+		SetAsBox(1.0f, 1.0f);
+		return;
+	}
+	
+	int32 n = b2Min(count, b2_maxPolygonVertices);
+
+	// Perform welding and copy vertices into local buffer.
+	b2Vec2 ps[b2_maxPolygonVertices];
+	int32 tempCount = 0;
+	for (int32 i = 0; i < n; ++i)
+	{
+		b2Vec2 v = vertices[i];
+
+		bool unique = true;
+		for (int32 j = 0; j < tempCount; ++j)
+		{
+			if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop)))
+			{
+				unique = false;
+				break;
+			}
+		}
+
+		if (unique)
+		{
+			ps[tempCount++] = v;
+		}
+	}
+
+	n = tempCount;
+	if (n < 3)
+	{
+		// Polygon is degenerate.
+		b2Assert(false);
+		SetAsBox(1.0f, 1.0f);
+		return;
+	}
+
+	// Create the convex hull using the Gift wrapping algorithm
+	// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
+
+	// Find the right most point on the hull
+	int32 i0 = 0;
+	float32 x0 = ps[0].x;
+	for (int32 i = 1; i < n; ++i)
+	{
+		float32 x = ps[i].x;
+		if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
+		{
+			i0 = i;
+			x0 = x;
+		}
+	}
+
+	int32 hull[b2_maxPolygonVertices];
+	int32 m = 0;
+	int32 ih = i0;
+
+	for (;;)
+	{
+		b2Assert(m < b2_maxPolygonVertices);
+		hull[m] = ih;
+
+		int32 ie = 0;
+		for (int32 j = 1; j < n; ++j)
+		{
+			if (ie == ih)
+			{
+				ie = j;
+				continue;
+			}
+
+			b2Vec2 r = ps[ie] - ps[hull[m]];
+			b2Vec2 v = ps[j] - ps[hull[m]];
+			float32 c = b2Cross(r, v);
+			if (c < 0.0f)
+			{
+				ie = j;
+			}
+
+			// Collinearity check
+			if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
+			{
+				ie = j;
+			}
+		}
+
+		++m;
+		ih = ie;
+
+		if (ie == i0)
+		{
+			break;
+		}
+	}
+	
+	if (m < 3)
+	{
+		// Polygon is degenerate.
+		b2Assert(false);
+		SetAsBox(1.0f, 1.0f);
+		return;
+	}
+
+	m_count = m;
+
+	// Copy vertices.
+	for (int32 i = 0; i < m; ++i)
+	{
+		m_vertices[i] = ps[hull[i]];
+	}
+
+	// Compute normals. Ensure the edges have non-zero length.
+	for (int32 i = 0; i < m; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i + 1 < m ? i + 1 : 0;
+		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+		b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
+		m_normals[i] = b2Cross(edge, 1.0f);
+		m_normals[i].Normalize();
+	}
+
+	// Compute the polygon centroid.
+	m_centroid = ComputeCentroid(m_vertices, m);
+}
+
+bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+		if (dot > 0.0f)
+		{
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+								const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	// Put the ray into the polygon's frame of reference.
+	b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+	b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
+	b2Vec2 d = p2 - p1;
+
+	float32 lower = 0.0f, upper = input.maxFraction;
+
+	int32 index = -1;
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		// p = p1 + a * d
+		// dot(normal, p - v) = 0
+		// dot(normal, p1 - v) + a * dot(normal, d) = 0
+		float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+		float32 denominator = b2Dot(m_normals[i], d);
+
+		if (denominator == 0.0f)
+		{	
+			if (numerator < 0.0f)
+			{
+				return false;
+			}
+		}
+		else
+		{
+			// Note: we want this predicate without division:
+			// lower < numerator / denominator, where denominator < 0
+			// Since denominator < 0, we have to flip the inequality:
+			// lower < numerator / denominator <==> denominator * lower > numerator.
+			if (denominator < 0.0f && numerator < lower * denominator)
+			{
+				// Increase lower.
+				// The segment enters this half-space.
+				lower = numerator / denominator;
+				index = i;
+			}
+			else if (denominator > 0.0f && numerator < upper * denominator)
+			{
+				// Decrease upper.
+				// The segment exits this half-space.
+				upper = numerator / denominator;
+			}
+		}
+
+		// The use of epsilon here causes the assert on lower to trip
+		// in some cases. Apparently the use of epsilon was to make edge
+		// shapes work, but now those are handled separately.
+		//if (upper < lower - b2_epsilon)
+		if (upper < lower)
+		{
+			return false;
+		}
+	}
+
+	b2Assert(0.0f <= lower && lower <= input.maxFraction);
+
+	if (index >= 0)
+	{
+		output->fraction = lower;
+		output->normal = b2Mul(xf.q, m_normals[index]);
+		return true;
+	}
+
+	return false;
+}
+
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
+	b2Vec2 upper = lower;
+
+	for (int32 i = 1; i < m_count; ++i)
+	{
+		b2Vec2 v = b2Mul(xf, m_vertices[i]);
+		lower = b2Min(lower, v);
+		upper = b2Max(upper, v);
+	}
+
+	b2Vec2 r(m_radius, m_radius);
+	aabb->lowerBound = lower - r;
+	aabb->upperBound = upper + r;
+}
+
+void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	// Polygon mass, centroid, and inertia.
+	// Let rho be the polygon density in mass per unit area.
+	// Then:
+	// mass = rho * int(dA)
+	// centroid.x = (1/mass) * rho * int(x * dA)
+	// centroid.y = (1/mass) * rho * int(y * dA)
+	// I = rho * int((x*x + y*y) * dA)
+	//
+	// We can compute these integrals by summing all the integrals
+	// for each triangle of the polygon. To evaluate the integral
+	// for a single triangle, we make a change of variables to
+	// the (u,v) coordinates of the triangle:
+	// x = x0 + e1x * u + e2x * v
+	// y = y0 + e1y * u + e2y * v
+	// where 0 <= u && 0 <= v && u + v <= 1.
+	//
+	// We integrate u from [0,1-v] and then v from [0,1].
+	// We also need to use the Jacobian of the transformation:
+	// D = cross(e1, e2)
+	//
+	// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
+	//
+	// The rest of the derivation is handled by computer algebra.
+
+	b2Assert(m_count >= 3);
+
+	b2Vec2 center; center.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+	float32 I = 0.0f;
+
+	// s is the reference point for forming triangles.
+	// It's location doesn't change the result (except for rounding error).
+	b2Vec2 s(0.0f, 0.0f);
+
+	// This code would put the reference point inside the polygon.
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		s += m_vertices[i];
+	}
+	s *= 1.0f / m_count;
+
+	const float32 k_inv3 = 1.0f / 3.0f;
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		// Triangle vertices.
+		b2Vec2 e1 = m_vertices[i] - s;
+		b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
+
+		float32 D = b2Cross(e1, e2);
+
+		float32 triangleArea = 0.5f * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		center += triangleArea * k_inv3 * (e1 + e2);
+
+		float32 ex1 = e1.x, ey1 = e1.y;
+		float32 ex2 = e2.x, ey2 = e2.y;
+
+		float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
+		float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
+
+		I += (0.25f * k_inv3 * D) * (intx2 + inty2);
+	}
+
+	// Total mass
+	massData->mass = density * area;
+
+	// Center of mass
+	b2Assert(area > b2_epsilon);
+	center *= 1.0f / area;
+	massData->center = center + s;
+
+	// Inertia tensor relative to the local origin (point s).
+	massData->I = density * I;
+	
+	// Shift to center of mass then to original body origin.
+	massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
+}
+
+bool b2PolygonShape::Validate() const
+{
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
+		b2Vec2 p = m_vertices[i1];
+		b2Vec2 e = m_vertices[i2] - p;
+
+		for (int32 j = 0; j < m_count; ++j)
+		{
+			if (j == i1 || j == i2)
+			{
+				continue;
+			}
+
+			b2Vec2 v = m_vertices[j] - p;
+			float32 c = b2Cross(e, v);
+			if (c < 0.0f)
+			{
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.h b/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.h
new file mode 100644
index 0000000..26c5e61
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2PolygonShape.h
@@ -0,0 +1,89 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_POLYGON_SHAPE_H
+#define B2_POLYGON_SHAPE_H
+
+#include "Box2D/Collision/Shapes/b2Shape.h"
+
+/// A convex polygon. It is assumed that the interior of the polygon is to
+/// the left of each edge.
+/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
+/// In most cases you should not need many vertices for a convex polygon.
+class b2PolygonShape : public b2Shape
+{
+public:
+	b2PolygonShape();
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const override;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const override;
+
+	/// Create a convex hull from the given array of local points.
+	/// The count must be in the range [3, b2_maxPolygonVertices].
+	/// @warning the points may be re-ordered, even if they form a convex polygon
+	/// @warning collinear points are handled but not removed. Collinear points
+	/// may lead to poor stacking behavior.
+	void Set(const b2Vec2* points, int32 count);
+
+	/// Build vertices to represent an axis-aligned box centered on the local origin.
+	/// @param hx the half-width.
+	/// @param hy the half-height.
+	void SetAsBox(float32 hx, float32 hy);
+
+	/// Build vertices to represent an oriented box.
+	/// @param hx the half-width.
+	/// @param hy the half-height.
+	/// @param center the center of the box in local coordinates.
+	/// @param angle the rotation of the box in local coordinates.
+	void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
+
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+					const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const override;
+
+	/// Validate convexity. This is a very time consuming operation.
+	/// @returns true if valid
+	bool Validate() const;
+
+	b2Vec2 m_centroid;
+	b2Vec2 m_vertices[b2_maxPolygonVertices];
+	b2Vec2 m_normals[b2_maxPolygonVertices];
+	int32 m_count;
+};
+
+inline b2PolygonShape::b2PolygonShape()
+{
+	m_type = e_polygon;
+	m_radius = b2_polygonRadius;
+	m_count = 0;
+	m_centroid.SetZero();
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/Shapes/b2Shape.h b/src/3rdparty/Box2D/Collision/Shapes/b2Shape.h
new file mode 100644
index 0000000..653e362
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/Shapes/b2Shape.h
@@ -0,0 +1,104 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_SHAPE_H
+#define B2_SHAPE_H
+
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Collision.h"
+
+/// This holds the mass data computed for a shape.
+struct b2MassData
+{
+	/// The mass of the shape, usually in kilograms.
+	float32 mass;
+
+	/// The position of the shape's centroid relative to the shape's origin.
+	b2Vec2 center;
+
+	/// The rotational inertia of the shape about the local origin.
+	float32 I;
+};
+
+/// A shape is used for collision detection. You can create a shape however you like.
+/// Shapes used for simulation in b2World are created automatically when a b2Fixture
+/// is created. Shapes may encapsulate a one or more child shapes.
+class b2Shape
+{
+public:
+	
+	enum Type
+	{
+		e_circle = 0,
+		e_edge = 1,
+		e_polygon = 2,
+		e_chain = 3,
+		e_typeCount = 4
+	};
+
+	virtual ~b2Shape() {}
+
+	/// Clone the concrete shape using the provided allocator.
+	virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
+
+	/// Get the type of this shape. You can use this to down cast to the concrete shape.
+	/// @return the shape type.
+	Type GetType() const;
+
+	/// Get the number of child primitives.
+	virtual int32 GetChildCount() const = 0;
+
+	/// Test a point for containment in this shape. This only works for convex shapes.
+	/// @param xf the shape world transform.
+	/// @param p a point in world coordinates.
+	virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
+
+	/// Cast a ray against a child shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	/// @param transform the transform to be applied to the shape.
+	/// @param childIndex the child shape index
+	virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+						const b2Transform& transform, int32 childIndex) const = 0;
+
+	/// Given a transform, compute the associated axis aligned bounding box for a child shape.
+	/// @param aabb returns the axis aligned box.
+	/// @param xf the world transform of the shape.
+	/// @param childIndex the child shape
+	virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
+
+	/// Compute the mass properties of this shape using its dimensions and density.
+	/// The inertia tensor is computed about the local origin.
+	/// @param massData returns the mass data for this shape.
+	/// @param density the density in kilograms per meter squared.
+	virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;
+
+	Type m_type;
+
+	/// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for
+	/// making rounded polygons.
+	float32 m_radius;
+};
+
+inline b2Shape::Type b2Shape::GetType() const
+{
+	return m_type;
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/b2BroadPhase.cpp b/src/3rdparty/Box2D/Collision/b2BroadPhase.cpp
new file mode 100644
index 0000000..e96339d
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2BroadPhase.cpp
@@ -0,0 +1,119 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2BroadPhase.h"
+
+b2BroadPhase::b2BroadPhase()
+{
+	m_proxyCount = 0;
+
+	m_pairCapacity = 16;
+	m_pairCount = 0;
+	m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+
+	m_moveCapacity = 16;
+	m_moveCount = 0;
+	m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+}
+
+b2BroadPhase::~b2BroadPhase()
+{
+	b2Free(m_moveBuffer);
+	b2Free(m_pairBuffer);
+}
+
+int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
+{
+	int32 proxyId = m_tree.CreateProxy(aabb, userData);
+	++m_proxyCount;
+	BufferMove(proxyId);
+	return proxyId;
+}
+
+void b2BroadPhase::DestroyProxy(int32 proxyId)
+{
+	UnBufferMove(proxyId);
+	--m_proxyCount;
+	m_tree.DestroyProxy(proxyId);
+}
+
+void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
+{
+	bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
+	if (buffer)
+	{
+		BufferMove(proxyId);
+	}
+}
+
+void b2BroadPhase::TouchProxy(int32 proxyId)
+{
+	BufferMove(proxyId);
+}
+
+void b2BroadPhase::BufferMove(int32 proxyId)
+{
+	if (m_moveCount == m_moveCapacity)
+	{
+		int32* oldBuffer = m_moveBuffer;
+		m_moveCapacity *= 2;
+		m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
+		memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
+		b2Free(oldBuffer);
+	}
+
+	m_moveBuffer[m_moveCount] = proxyId;
+	++m_moveCount;
+}
+
+void b2BroadPhase::UnBufferMove(int32 proxyId)
+{
+	for (int32 i = 0; i < m_moveCount; ++i)
+	{
+		if (m_moveBuffer[i] == proxyId)
+		{
+			m_moveBuffer[i] = e_nullProxy;
+		}
+	}
+}
+
+// This is called from b2DynamicTree::Query when we are gathering pairs.
+bool b2BroadPhase::QueryCallback(int32 proxyId)
+{
+	// A proxy cannot form a pair with itself.
+	if (proxyId == m_queryProxyId)
+	{
+		return true;
+	}
+
+	// Grow the pair buffer as needed.
+	if (m_pairCount == m_pairCapacity)
+	{
+		b2Pair* oldBuffer = m_pairBuffer;
+		m_pairCapacity *= 2;
+		m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
+		memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
+		b2Free(oldBuffer);
+	}
+
+	m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
+	m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
+	++m_pairCount;
+
+	return true;
+}
diff --git a/src/3rdparty/Box2D/Collision/b2BroadPhase.h b/src/3rdparty/Box2D/Collision/b2BroadPhase.h
new file mode 100644
index 0000000..d2965ed
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2BroadPhase.h
@@ -0,0 +1,257 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_BROAD_PHASE_H
+#define B2_BROAD_PHASE_H
+
+#include "Box2D/Common/b2Settings.h"
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2DynamicTree.h"
+#include <algorithm>
+
+struct b2Pair
+{
+	int32 proxyIdA;
+	int32 proxyIdB;
+};
+
+/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
+/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
+/// It is up to the client to consume the new pairs and to track subsequent overlap.
+class b2BroadPhase
+{
+public:
+
+	enum
+	{
+		e_nullProxy = -1
+	};
+
+	b2BroadPhase();
+	~b2BroadPhase();
+
+	/// Create a proxy with an initial AABB. Pairs are not reported until
+	/// UpdatePairs is called.
+	int32 CreateProxy(const b2AABB& aabb, void* userData);
+
+	/// Destroy a proxy. It is up to the client to remove any pairs.
+	void DestroyProxy(int32 proxyId);
+
+	/// Call MoveProxy as many times as you like, then when you are done
+	/// call UpdatePairs to finalized the proxy pairs (for your time step).
+	void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
+
+	/// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
+	void TouchProxy(int32 proxyId);
+
+	/// Get the fat AABB for a proxy.
+	const b2AABB& GetFatAABB(int32 proxyId) const;
+
+	/// Get user data from a proxy. Returns nullptr if the id is invalid.
+	void* GetUserData(int32 proxyId) const;
+
+	/// Test overlap of fat AABBs.
+	bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
+
+	/// Get the number of proxies.
+	int32 GetProxyCount() const;
+
+	/// Update the pairs. This results in pair callbacks. This can only add pairs.
+	template <typename T>
+	void UpdatePairs(T* callback);
+
+	/// Query an AABB for overlapping proxies. The callback class
+	/// is called for each proxy that overlaps the supplied AABB.
+	template <typename T>
+	void Query(T* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast against the proxies in the tree. This relies on the callback
+	/// to perform a exact ray-cast in the case were the proxy contains a shape.
+	/// The callback also performs the any collision filtering. This has performance
+	/// roughly equal to k * log(n), where k is the number of collisions and n is the
+	/// number of proxies in the tree.
+	/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+	/// @param callback a callback class that is called for each proxy that is hit by the ray.
+	template <typename T>
+	void RayCast(T* callback, const b2RayCastInput& input) const;
+
+	/// Get the height of the embedded tree.
+	int32 GetTreeHeight() const;
+
+	/// Get the balance of the embedded tree.
+	int32 GetTreeBalance() const;
+
+	/// Get the quality metric of the embedded tree.
+	float32 GetTreeQuality() const;
+
+	/// Shift the world origin. Useful for large worlds.
+	/// The shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
+private:
+
+	friend class b2DynamicTree;
+
+	void BufferMove(int32 proxyId);
+	void UnBufferMove(int32 proxyId);
+
+	bool QueryCallback(int32 proxyId);
+
+	b2DynamicTree m_tree;
+
+	int32 m_proxyCount;
+
+	int32* m_moveBuffer;
+	int32 m_moveCapacity;
+	int32 m_moveCount;
+
+	b2Pair* m_pairBuffer;
+	int32 m_pairCapacity;
+	int32 m_pairCount;
+
+	int32 m_queryProxyId;
+};
+
+/// This is used to sort pairs.
+inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
+{
+	if (pair1.proxyIdA < pair2.proxyIdA)
+	{
+		return true;
+	}
+
+	if (pair1.proxyIdA == pair2.proxyIdA)
+	{
+		return pair1.proxyIdB < pair2.proxyIdB;
+	}
+
+	return false;
+}
+
+inline void* b2BroadPhase::GetUserData(int32 proxyId) const
+{
+	return m_tree.GetUserData(proxyId);
+}
+
+inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const
+{
+	const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
+	const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
+	return b2TestOverlap(aabbA, aabbB);
+}
+
+inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const
+{
+	return m_tree.GetFatAABB(proxyId);
+}
+
+inline int32 b2BroadPhase::GetProxyCount() const
+{
+	return m_proxyCount;
+}
+
+inline int32 b2BroadPhase::GetTreeHeight() const
+{
+	return m_tree.GetHeight();
+}
+
+inline int32 b2BroadPhase::GetTreeBalance() const
+{
+	return m_tree.GetMaxBalance();
+}
+
+inline float32 b2BroadPhase::GetTreeQuality() const
+{
+	return m_tree.GetAreaRatio();
+}
+
+template <typename T>
+void b2BroadPhase::UpdatePairs(T* callback)
+{
+	// Reset pair buffer
+	m_pairCount = 0;
+
+	// Perform tree queries for all moving proxies.
+	for (int32 i = 0; i < m_moveCount; ++i)
+	{
+		m_queryProxyId = m_moveBuffer[i];
+		if (m_queryProxyId == e_nullProxy)
+		{
+			continue;
+		}
+
+		// We have to query the tree with the fat AABB so that
+		// we don't fail to create a pair that may touch later.
+		const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
+
+		// Query tree, create pairs and add them pair buffer.
+		m_tree.Query(this, fatAABB);
+	}
+
+	// Reset move buffer
+	m_moveCount = 0;
+
+	// Sort the pair buffer to expose duplicates.
+	std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
+
+	// Send the pairs back to the client.
+	int32 i = 0;
+	while (i < m_pairCount)
+	{
+		b2Pair* primaryPair = m_pairBuffer + i;
+		void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
+		void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
+
+		callback->AddPair(userDataA, userDataB);
+		++i;
+
+		// Skip any duplicate pairs.
+		while (i < m_pairCount)
+		{
+			b2Pair* pair = m_pairBuffer + i;
+			if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
+			{
+				break;
+			}
+			++i;
+		}
+	}
+
+	// Try to keep the tree balanced.
+	//m_tree.Rebalance(4);
+}
+
+template <typename T>
+inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const
+{
+	m_tree.Query(callback, aabb);
+}
+
+template <typename T>
+inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const
+{
+	m_tree.RayCast(callback, input);
+}
+
+inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	m_tree.ShiftOrigin(newOrigin);
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/b2CollideCircle.cpp b/src/3rdparty/Box2D/Collision/b2CollideCircle.cpp
new file mode 100644
index 0000000..f39f057
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2CollideCircle.cpp
@@ -0,0 +1,154 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+void b2CollideCircles(
+	b2Manifold* manifold,
+	const b2CircleShape* circleA, const b2Transform& xfA,
+	const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+
+	b2Vec2 pA = b2Mul(xfA, circleA->m_p);
+	b2Vec2 pB = b2Mul(xfB, circleB->m_p);
+
+	b2Vec2 d = pB - pA;
+	float32 distSqr = b2Dot(d, d);
+	float32 rA = circleA->m_radius, rB = circleB->m_radius;
+	float32 radius = rA + rB;
+	if (distSqr > radius * radius)
+	{
+		return;
+	}
+
+	manifold->type = b2Manifold::e_circles;
+	manifold->localPoint = circleA->m_p;
+	manifold->localNormal.SetZero();
+	manifold->pointCount = 1;
+
+	manifold->points[0].localPoint = circleB->m_p;
+	manifold->points[0].id.key = 0;
+}
+
+void b2CollidePolygonAndCircle(
+	b2Manifold* manifold,
+	const b2PolygonShape* polygonA, const b2Transform& xfA,
+	const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+
+	// Compute circle position in the frame of the polygon.
+	b2Vec2 c = b2Mul(xfB, circleB->m_p);
+	b2Vec2 cLocal = b2MulT(xfA, c);
+
+	// Find the min separating edge.
+	int32 normalIndex = 0;
+	float32 separation = -b2_maxFloat;
+	float32 radius = polygonA->m_radius + circleB->m_radius;
+	int32 vertexCount = polygonA->m_count;
+	const b2Vec2* vertices = polygonA->m_vertices;
+	const b2Vec2* normals = polygonA->m_normals;
+
+	for (int32 i = 0; i < vertexCount; ++i)
+	{
+		float32 s = b2Dot(normals[i], cLocal - vertices[i]);
+
+		if (s > radius)
+		{
+			// Early out.
+			return;
+		}
+
+		if (s > separation)
+		{
+			separation = s;
+			normalIndex = i;
+		}
+	}
+
+	// Vertices that subtend the incident face.
+	int32 vertIndex1 = normalIndex;
+	int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
+	b2Vec2 v1 = vertices[vertIndex1];
+	b2Vec2 v2 = vertices[vertIndex2];
+
+	// If the center is inside the polygon ...
+	if (separation < b2_epsilon)
+	{
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = normals[normalIndex];
+		manifold->localPoint = 0.5f * (v1 + v2);
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+		return;
+	}
+
+	// Compute barycentric coordinates
+	float32 u1 = b2Dot(cLocal - v1, v2 - v1);
+	float32 u2 = b2Dot(cLocal - v2, v1 - v2);
+	if (u1 <= 0.0f)
+	{
+		if (b2DistanceSquared(cLocal, v1) > radius * radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = cLocal - v1;
+		manifold->localNormal.Normalize();
+		manifold->localPoint = v1;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+	else if (u2 <= 0.0f)
+	{
+		if (b2DistanceSquared(cLocal, v2) > radius * radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = cLocal - v2;
+		manifold->localNormal.Normalize();
+		manifold->localPoint = v2;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+	else
+	{
+		b2Vec2 faceCenter = 0.5f * (v1 + v2);
+		float32 s = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+		if (s > radius)
+		{
+			return;
+		}
+
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_faceA;
+		manifold->localNormal = normals[vertIndex1];
+		manifold->localPoint = faceCenter;
+		manifold->points[0].localPoint = circleB->m_p;
+		manifold->points[0].id.key = 0;
+	}
+}
diff --git a/src/3rdparty/Box2D/Collision/b2CollideEdge.cpp b/src/3rdparty/Box2D/Collision/b2CollideEdge.cpp
new file mode 100644
index 0000000..793d714
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2CollideEdge.cpp
@@ -0,0 +1,698 @@
+/*
+ * Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+ *
+ * This software is provided 'as-is', without any express or implied
+ * warranty.  In no event will the authors be held liable for any damages
+ * arising from the use of this software.
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 1. The origin of this software must not be misrepresented; you must not
+ * claim that you wrote the original software. If you use this software
+ * in a product, an acknowledgment in the product documentation would be
+ * appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ * misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+
+// Compute contact points for edge versus circle.
+// This accounts for edge connectivity.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							const b2EdgeShape* edgeA, const b2Transform& xfA,
+							const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	
+	// Compute circle in frame of edge
+	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
+	
+	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
+	b2Vec2 e = B - A;
+	
+	// Barycentric coordinates
+	float32 u = b2Dot(e, B - Q);
+	float32 v = b2Dot(e, Q - A);
+	
+	float32 radius = edgeA->m_radius + circleB->m_radius;
+	
+	b2ContactFeature cf;
+	cf.indexB = 0;
+	cf.typeB = b2ContactFeature::e_vertex;
+	
+	// Region A
+	if (v <= 0.0f)
+	{
+		b2Vec2 P = A;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to A?
+		if (edgeA->m_hasVertex0)
+		{
+			b2Vec2 A1 = edgeA->m_vertex0;
+			b2Vec2 B1 = A;
+			b2Vec2 e1 = B1 - A1;
+			float32 u1 = b2Dot(e1, B1 - Q);
+			
+			// Is the circle in Region AB of the previous edge?
+			if (u1 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 0;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region B
+	if (u <= 0.0f)
+	{
+		b2Vec2 P = B;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to B?
+		if (edgeA->m_hasVertex3)
+		{
+			b2Vec2 B2 = edgeA->m_vertex3;
+			b2Vec2 A2 = B;
+			b2Vec2 e2 = B2 - A2;
+			float32 v2 = b2Dot(e2, Q - A2);
+			
+			// Is the circle in Region AB of the next edge?
+			if (v2 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 1;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region AB
+	float32 den = b2Dot(e, e);
+	b2Assert(den > 0.0f);
+	b2Vec2 P = (1.0f / den) * (u * A + v * B);
+	b2Vec2 d = Q - P;
+	float32 dd = b2Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return;
+	}
+	
+	b2Vec2 n(-e.y, e.x);
+	if (b2Dot(n, Q - A) < 0.0f)
+	{
+		n.Set(-n.x, -n.y);
+	}
+	n.Normalize();
+	
+	cf.indexA = 0;
+	cf.typeA = b2ContactFeature::e_face;
+	manifold->pointCount = 1;
+	manifold->type = b2Manifold::e_faceA;
+	manifold->localNormal = n;
+	manifold->localPoint = A;
+	manifold->points[0].id.key = 0;
+	manifold->points[0].id.cf = cf;
+	manifold->points[0].localPoint = circleB->m_p;
+}
+
+// This structure is used to keep track of the best separating axis.
+struct b2EPAxis
+{
+	enum Type
+	{
+		e_unknown,
+		e_edgeA,
+		e_edgeB
+	};
+	
+	Type type;
+	int32 index;
+	float32 separation;
+};
+
+// This holds polygon B expressed in frame A.
+struct b2TempPolygon
+{
+	b2Vec2 vertices[b2_maxPolygonVertices];
+	b2Vec2 normals[b2_maxPolygonVertices];
+	int32 count;
+};
+
+// Reference face used for clipping
+struct b2ReferenceFace
+{
+	int32 i1, i2;
+	
+	b2Vec2 v1, v2;
+	
+	b2Vec2 normal;
+	
+	b2Vec2 sideNormal1;
+	float32 sideOffset1;
+	
+	b2Vec2 sideNormal2;
+	float32 sideOffset2;
+};
+
+// This class collides and edge and a polygon, taking into account edge adjacency.
+struct b2EPCollider
+{
+	void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+				 const b2PolygonShape* polygonB, const b2Transform& xfB);
+	b2EPAxis ComputeEdgeSeparation();
+	b2EPAxis ComputePolygonSeparation();
+	
+	enum VertexType
+	{
+		e_isolated,
+		e_concave,
+		e_convex
+	};
+	
+	b2TempPolygon m_polygonB;
+	
+	b2Transform m_xf;
+	b2Vec2 m_centroidB;
+	b2Vec2 m_v0, m_v1, m_v2, m_v3;
+	b2Vec2 m_normal0, m_normal1, m_normal2;
+	b2Vec2 m_normal;
+	VertexType m_type1, m_type2;
+	b2Vec2 m_lowerLimit, m_upperLimit;
+	float32 m_radius;
+	bool m_front;
+};
+
+// Algorithm:
+// 1. Classify v1 and v2
+// 2. Classify polygon centroid as front or back
+// 3. Flip normal if necessary
+// 4. Initialize normal range to [-pi, pi] about face normal
+// 5. Adjust normal range according to adjacent edges
+// 6. Visit each separating axes, only accept axes within the range
+// 7. Return if _any_ axis indicates separation
+// 8. Clip
+void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+						   const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	m_xf = b2MulT(xfA, xfB);
+	
+	m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
+	
+	m_v0 = edgeA->m_vertex0;
+	m_v1 = edgeA->m_vertex1;
+	m_v2 = edgeA->m_vertex2;
+	m_v3 = edgeA->m_vertex3;
+	
+	bool hasVertex0 = edgeA->m_hasVertex0;
+	bool hasVertex3 = edgeA->m_hasVertex3;
+	
+	b2Vec2 edge1 = m_v2 - m_v1;
+	edge1.Normalize();
+	m_normal1.Set(edge1.y, -edge1.x);
+	float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
+	float32 offset0 = 0.0f, offset2 = 0.0f;
+	bool convex1 = false, convex2 = false;
+	
+	// Is there a preceding edge?
+	if (hasVertex0)
+	{
+		b2Vec2 edge0 = m_v1 - m_v0;
+		edge0.Normalize();
+		m_normal0.Set(edge0.y, -edge0.x);
+		convex1 = b2Cross(edge0, edge1) >= 0.0f;
+		offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
+	}
+	
+	// Is there a following edge?
+	if (hasVertex3)
+	{
+		b2Vec2 edge2 = m_v3 - m_v2;
+		edge2.Normalize();
+		m_normal2.Set(edge2.y, -edge2.x);
+		convex2 = b2Cross(edge1, edge2) > 0.0f;
+		offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
+	}
+	
+	// Determine front or back collision. Determine collision normal limits.
+	if (hasVertex0 && hasVertex3)
+	{
+		if (convex1 && convex2)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex1)
+		{
+			m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex2)
+		{
+			m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex0)
+	{
+		if (convex1)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex3)
+	{
+		if (convex2)
+		{
+			m_front = offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = m_normal1;
+			}
+		}		
+	}
+	else
+	{
+		m_front = offset1 >= 0.0f;
+		if (m_front)
+		{
+			m_normal = m_normal1;
+			m_lowerLimit = -m_normal1;
+			m_upperLimit = -m_normal1;
+		}
+		else
+		{
+			m_normal = -m_normal1;
+			m_lowerLimit = m_normal1;
+			m_upperLimit = m_normal1;
+		}
+	}
+	
+	// Get polygonB in frameA
+	m_polygonB.count = polygonB->m_count;
+	for (int32 i = 0; i < polygonB->m_count; ++i)
+	{
+		m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
+		m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
+	}
+	
+	m_radius = polygonB->m_radius + edgeA->m_radius;
+	
+	manifold->pointCount = 0;
+	
+	b2EPAxis edgeAxis = ComputeEdgeSeparation();
+	
+	// If no valid normal can be found than this edge should not collide.
+	if (edgeAxis.type == b2EPAxis::e_unknown)
+	{
+		return;
+	}
+	
+	if (edgeAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	b2EPAxis polygonAxis = ComputePolygonSeparation();
+	if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	// Use hysteresis for jitter reduction.
+	const float32 k_relativeTol = 0.98f;
+	const float32 k_absoluteTol = 0.001f;
+	
+	b2EPAxis primaryAxis;
+	if (polygonAxis.type == b2EPAxis::e_unknown)
+	{
+		primaryAxis = edgeAxis;
+	}
+	else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
+	{
+		primaryAxis = polygonAxis;
+	}
+	else
+	{
+		primaryAxis = edgeAxis;
+	}
+	
+	b2ClipVertex ie[2];
+	b2ReferenceFace rf;
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->type = b2Manifold::e_faceA;
+		
+		// Search for the polygon normal that is most anti-parallel to the edge normal.
+		int32 bestIndex = 0;
+		float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
+		for (int32 i = 1; i < m_polygonB.count; ++i)
+		{
+			float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
+			if (value < bestValue)
+			{
+				bestValue = value;
+				bestIndex = i;
+			}
+		}
+		
+		int32 i1 = bestIndex;
+		int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
+		
+		ie[0].v = m_polygonB.vertices[i1];
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = static_cast<uint8>(i1);
+		ie[0].id.cf.typeA = b2ContactFeature::e_face;
+		ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
+		
+		ie[1].v = m_polygonB.vertices[i2];
+		ie[1].id.cf.indexA = 0;
+		ie[1].id.cf.indexB = static_cast<uint8>(i2);
+		ie[1].id.cf.typeA = b2ContactFeature::e_face;
+		ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
+		
+		if (m_front)
+		{
+			rf.i1 = 0;
+			rf.i2 = 1;
+			rf.v1 = m_v1;
+			rf.v2 = m_v2;
+			rf.normal = m_normal1;
+		}
+		else
+		{
+			rf.i1 = 1;
+			rf.i2 = 0;
+			rf.v1 = m_v2;
+			rf.v2 = m_v1;
+			rf.normal = -m_normal1;
+		}		
+	}
+	else
+	{
+		manifold->type = b2Manifold::e_faceB;
+		
+		ie[0].v = m_v1;
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
+		ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
+		ie[0].id.cf.typeB = b2ContactFeature::e_face;
+		
+		ie[1].v = m_v2;
+		ie[1].id.cf.indexA = 0;
+		ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
+		ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
+		ie[1].id.cf.typeB = b2ContactFeature::e_face;
+		
+		rf.i1 = primaryAxis.index;
+		rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
+		rf.v1 = m_polygonB.vertices[rf.i1];
+		rf.v2 = m_polygonB.vertices[rf.i2];
+		rf.normal = m_polygonB.normals[rf.i1];
+	}
+	
+	rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
+	rf.sideNormal2 = -rf.sideNormal1;
+	rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
+	rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
+	
+	// Clip incident edge against extruded edge1 side edges.
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int32 np;
+	
+	// Clip to box side 1
+	np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Now clipPoints2 contains the clipped points.
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->localNormal = rf.normal;
+		manifold->localPoint = rf.v1;
+	}
+	else
+	{
+		manifold->localNormal = polygonB->m_normals[rf.i1];
+		manifold->localPoint = polygonB->m_vertices[rf.i1];
+	}
+	
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation;
+		
+		separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
+		
+		if (separation <= m_radius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			
+			if (primaryAxis.type == b2EPAxis::e_edgeA)
+			{
+				cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
+				cp->id = clipPoints2[i].id;
+			}
+			else
+			{
+				cp->localPoint = clipPoints2[i].v;
+				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
+				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
+				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
+				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
+			}
+			
+			++pointCount;
+		}
+	}
+	
+	manifold->pointCount = pointCount;
+}
+
+b2EPAxis b2EPCollider::ComputeEdgeSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_edgeA;
+	axis.index = m_front ? 0 : 1;
+	axis.separation = FLT_MAX;
+	
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
+		if (s < axis.separation)
+		{
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+b2EPAxis b2EPCollider::ComputePolygonSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_unknown;
+	axis.index = -1;
+	axis.separation = -FLT_MAX;
+
+	b2Vec2 perp(-m_normal.y, m_normal.x);
+
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		b2Vec2 n = -m_polygonB.normals[i];
+		
+		float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
+		float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
+		float32 s = b2Min(s1, s2);
+		
+		if (s > m_radius)
+		{
+			// No collision
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+			return axis;
+		}
+		
+		// Adjacency
+		if (b2Dot(n, perp) >= 0.0f)
+		{
+			if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		else
+		{
+			if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		
+		if (s > axis.separation)
+		{
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+void b2CollideEdgeAndPolygon(	b2Manifold* manifold,
+							 const b2EdgeShape* edgeA, const b2Transform& xfA,
+							 const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	b2EPCollider collider;
+	collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
+}
diff --git a/src/3rdparty/Box2D/Collision/b2CollidePolygon.cpp b/src/3rdparty/Box2D/Collision/b2CollidePolygon.cpp
new file mode 100644
index 0000000..10211e7
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2CollidePolygon.cpp
@@ -0,0 +1,239 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+// Find the max separation between poly1 and poly2 using edge normals from poly1.
+static float32 b2FindMaxSeparation(int32* edgeIndex,
+								 const b2PolygonShape* poly1, const b2Transform& xf1,
+								 const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	int32 count1 = poly1->m_count;
+	int32 count2 = poly2->m_count;
+	const b2Vec2* n1s = poly1->m_normals;
+	const b2Vec2* v1s = poly1->m_vertices;
+	const b2Vec2* v2s = poly2->m_vertices;
+	b2Transform xf = b2MulT(xf2, xf1);
+
+	int32 bestIndex = 0;
+	float32 maxSeparation = -b2_maxFloat;
+	for (int32 i = 0; i < count1; ++i)
+	{
+		// Get poly1 normal in frame2.
+		b2Vec2 n = b2Mul(xf.q, n1s[i]);
+		b2Vec2 v1 = b2Mul(xf, v1s[i]);
+
+		// Find deepest point for normal i.
+		float32 si = b2_maxFloat;
+		for (int32 j = 0; j < count2; ++j)
+		{
+			float32 sij = b2Dot(n, v2s[j] - v1);
+			if (sij < si)
+			{
+				si = sij;
+			}
+		}
+
+		if (si > maxSeparation)
+		{
+			maxSeparation = si;
+			bestIndex = i;
+		}
+	}
+
+	*edgeIndex = bestIndex;
+	return maxSeparation;
+}
+
+static void b2FindIncidentEdge(b2ClipVertex c[2],
+							 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							 const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+	const b2Vec2* normals1 = poly1->m_normals;
+
+	int32 count2 = poly2->m_count;
+	const b2Vec2* vertices2 = poly2->m_vertices;
+	const b2Vec2* normals2 = poly2->m_normals;
+
+	b2Assert(0 <= edge1 && edge1 < poly1->m_count);
+
+	// Get the normal of the reference edge in poly2's frame.
+	b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
+
+	// Find the incident edge on poly2.
+	int32 index = 0;
+	float32 minDot = b2_maxFloat;
+	for (int32 i = 0; i < count2; ++i)
+	{
+		float32 dot = b2Dot(normal1, normals2[i]);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	int32 i1 = index;
+	int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+	c[0].v = b2Mul(xf2, vertices2[i1]);
+	c[0].id.cf.indexA = (uint8)edge1;
+	c[0].id.cf.indexB = (uint8)i1;
+	c[0].id.cf.typeA = b2ContactFeature::e_face;
+	c[0].id.cf.typeB = b2ContactFeature::e_vertex;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+	c[1].id.cf.indexA = (uint8)edge1;
+	c[1].id.cf.indexB = (uint8)i2;
+	c[1].id.cf.typeA = b2ContactFeature::e_face;
+	c[1].id.cf.typeB = b2ContactFeature::e_vertex;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(b2Manifold* manifold,
+					  const b2PolygonShape* polyA, const b2Transform& xfA,
+					  const b2PolygonShape* polyB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	float32 totalRadius = polyA->m_radius + polyB->m_radius;
+
+	int32 edgeA = 0;
+	float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > totalRadius)
+		return;
+
+	int32 edgeB = 0;
+	float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > totalRadius)
+		return;
+
+	const b2PolygonShape* poly1;	// reference polygon
+	const b2PolygonShape* poly2;	// incident polygon
+	b2Transform xf1, xf2;
+	int32 edge1;					// reference edge
+	uint8 flip;
+	const float32 k_tol = 0.1f * b2_linearSlop;
+
+	if (separationB > separationA + k_tol)
+	{
+		poly1 = polyB;
+		poly2 = polyA;
+		xf1 = xfB;
+		xf2 = xfA;
+		edge1 = edgeB;
+		manifold->type = b2Manifold::e_faceB;
+		flip = 1;
+	}
+	else
+	{
+		poly1 = polyA;
+		poly2 = polyB;
+		xf1 = xfA;
+		xf2 = xfB;
+		edge1 = edgeA;
+		manifold->type = b2Manifold::e_faceA;
+		flip = 0;
+	}
+
+	b2ClipVertex incidentEdge[2];
+	b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+	int32 count1 = poly1->m_count;
+	const b2Vec2* vertices1 = poly1->m_vertices;
+
+	int32 iv1 = edge1;
+	int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+	b2Vec2 v11 = vertices1[iv1];
+	b2Vec2 v12 = vertices1[iv2];
+
+	b2Vec2 localTangent = v12 - v11;
+	localTangent.Normalize();
+	
+	b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
+	b2Vec2 planePoint = 0.5f * (v11 + v12);
+
+	b2Vec2 tangent = b2Mul(xf1.q, localTangent);
+	b2Vec2 normal = b2Cross(tangent, 1.0f);
+	
+	v11 = b2Mul(xf1, v11);
+	v12 = b2Mul(xf1, v12);
+
+	// Face offset.
+	float32 frontOffset = b2Dot(normal, v11);
+
+	// Side offsets, extended by polytope skin thickness.
+	float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
+	float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
+
+	// Clip incident edge against extruded edge1 side edges.
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int np;
+
+	// Clip to box side 1
+	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);
+
+	if (np < 2)
+	{
+		return;
+	}
+
+	// Now clipPoints2 contains the clipped points.
+	manifold->localNormal = localNormal;
+	manifold->localPoint = planePoint;
+
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
+
+		if (separation <= totalRadius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
+			cp->id = clipPoints2[i].id;
+			if (flip)
+			{
+				// Swap features
+				b2ContactFeature cf = cp->id.cf;
+				cp->id.cf.indexA = cf.indexB;
+				cp->id.cf.indexB = cf.indexA;
+				cp->id.cf.typeA = cf.typeB;
+				cp->id.cf.typeB = cf.typeA;
+			}
+			++pointCount;
+		}
+	}
+
+	manifold->pointCount = pointCount;
+}
diff --git a/src/3rdparty/Box2D/Collision/b2Collision.cpp b/src/3rdparty/Box2D/Collision/b2Collision.cpp
new file mode 100644
index 0000000..10e0b59
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2Collision.cpp
@@ -0,0 +1,252 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2Distance.h"
+
+void b2WorldManifold::Initialize(const b2Manifold* manifold,
+						  const b2Transform& xfA, float32 radiusA,
+						  const b2Transform& xfB, float32 radiusB)
+{
+	if (manifold->pointCount == 0)
+	{
+		return;
+	}
+
+	switch (manifold->type)
+	{
+	case b2Manifold::e_circles:
+		{
+			normal.Set(1.0f, 0.0f);
+			b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
+			b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
+			if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+			{
+				normal = pointB - pointA;
+				normal.Normalize();
+			}
+
+			b2Vec2 cA = pointA + radiusA * normal;
+			b2Vec2 cB = pointB - radiusB * normal;
+			points[0] = 0.5f * (cA + cB);
+			separations[0] = b2Dot(cB - cA, normal);
+		}
+		break;
+
+	case b2Manifold::e_faceA:
+		{
+			normal = b2Mul(xfA.q, manifold->localNormal);
+			b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
+			
+			for (int32 i = 0; i < manifold->pointCount; ++i)
+			{
+				b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
+				b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
+				b2Vec2 cB = clipPoint - radiusB * normal;
+				points[i] = 0.5f * (cA + cB);
+				separations[i] = b2Dot(cB - cA, normal);
+			}
+		}
+		break;
+
+	case b2Manifold::e_faceB:
+		{
+			normal = b2Mul(xfB.q, manifold->localNormal);
+			b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
+
+			for (int32 i = 0; i < manifold->pointCount; ++i)
+			{
+				b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
+				b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
+				b2Vec2 cA = clipPoint - radiusA * normal;
+				points[i] = 0.5f * (cA + cB);
+				separations[i] = b2Dot(cA - cB, normal);
+			}
+
+			// Ensure normal points from A to B.
+			normal = -normal;
+		}
+		break;
+	}
+}
+
+void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
+					  const b2Manifold* manifold1, const b2Manifold* manifold2)
+{
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		state1[i] = b2_nullState;
+		state2[i] = b2_nullState;
+	}
+
+	// Detect persists and removes.
+	for (int32 i = 0; i < manifold1->pointCount; ++i)
+	{
+		b2ContactID id = manifold1->points[i].id;
+
+		state1[i] = b2_removeState;
+
+		for (int32 j = 0; j < manifold2->pointCount; ++j)
+		{
+			if (manifold2->points[j].id.key == id.key)
+			{
+				state1[i] = b2_persistState;
+				break;
+			}
+		}
+	}
+
+	// Detect persists and adds.
+	for (int32 i = 0; i < manifold2->pointCount; ++i)
+	{
+		b2ContactID id = manifold2->points[i].id;
+
+		state2[i] = b2_addState;
+
+		for (int32 j = 0; j < manifold1->pointCount; ++j)
+		{
+			if (manifold1->points[j].id.key == id.key)
+			{
+				state2[i] = b2_persistState;
+				break;
+			}
+		}
+	}
+}
+
+// From Real-time Collision Detection, p179.
+bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
+{
+	float32 tmin = -b2_maxFloat;
+	float32 tmax = b2_maxFloat;
+
+	b2Vec2 p = input.p1;
+	b2Vec2 d = input.p2 - input.p1;
+	b2Vec2 absD = b2Abs(d);
+
+	b2Vec2 normal;
+
+	for (int32 i = 0; i < 2; ++i)
+	{
+		if (absD(i) < b2_epsilon)
+		{
+			// Parallel.
+			if (p(i) < lowerBound(i) || upperBound(i) < p(i))
+			{
+				return false;
+			}
+		}
+		else
+		{
+			float32 inv_d = 1.0f / d(i);
+			float32 t1 = (lowerBound(i) - p(i)) * inv_d;
+			float32 t2 = (upperBound(i) - p(i)) * inv_d;
+
+			// Sign of the normal vector.
+			float32 s = -1.0f;
+
+			if (t1 > t2)
+			{
+				b2Swap(t1, t2);
+				s = 1.0f;
+			}
+
+			// Push the min up
+			if (t1 > tmin)
+			{
+				normal.SetZero();
+				normal(i) = s;
+				tmin = t1;
+			}
+
+			// Pull the max down
+			tmax = b2Min(tmax, t2);
+
+			if (tmin > tmax)
+			{
+				return false;
+			}
+		}
+	}
+
+	// Does the ray start inside the box?
+	// Does the ray intersect beyond the max fraction?
+	if (tmin < 0.0f || input.maxFraction < tmin)
+	{
+		return false;
+	}
+
+	// Intersection.
+	output->fraction = tmin;
+	output->normal = normal;
+	return true;
+}
+
+// Sutherland-Hodgman clipping.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+						const b2Vec2& normal, float32 offset, int32 vertexIndexA)
+{
+	// Start with no output points
+	int32 numOut = 0;
+
+	// Calculate the distance of end points to the line
+	float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
+	float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
+
+	// If the points are behind the plane
+	if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
+	if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
+
+	// If the points are on different sides of the plane
+	if (distance0 * distance1 < 0.0f)
+	{
+		// Find intersection point of edge and plane
+		float32 interp = distance0 / (distance0 - distance1);
+		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
+
+		// VertexA is hitting edgeB.
+		vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
+		vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
+		vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
+		vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
+		++numOut;
+	}
+
+	return numOut;
+}
+
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2DistanceInput input;
+	input.proxyA.Set(shapeA, indexA);
+	input.proxyB.Set(shapeB, indexB);
+	input.transformA = xfA;
+	input.transformB = xfB;
+	input.useRadii = true;
+
+	b2SimplexCache cache;
+	cache.count = 0;
+
+	b2DistanceOutput output;
+
+	b2Distance(&output, &cache, &input);
+
+	return output.distance < 10.0f * b2_epsilon;
+}
diff --git a/src/3rdparty/Box2D/Collision/b2Collision.h b/src/3rdparty/Box2D/Collision/b2Collision.h
new file mode 100644
index 0000000..fe1f4cd
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2Collision.h
@@ -0,0 +1,277 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_COLLISION_H
+#define B2_COLLISION_H
+
+#include "Box2D/Common/b2Math.h"
+#include <limits.h>
+
+/// @file
+/// Structures and functions used for computing contact points, distance
+/// queries, and TOI queries.
+
+class b2Shape;
+class b2CircleShape;
+class b2EdgeShape;
+class b2PolygonShape;
+
+const uint8 b2_nullFeature = UCHAR_MAX;
+
+/// The features that intersect to form the contact point
+/// This must be 4 bytes or less.
+struct b2ContactFeature
+{
+	enum Type
+	{
+		e_vertex = 0,
+		e_face = 1
+	};
+
+	uint8 indexA;		///< Feature index on shapeA
+	uint8 indexB;		///< Feature index on shapeB
+	uint8 typeA;		///< The feature type on shapeA
+	uint8 typeB;		///< The feature type on shapeB
+};
+
+/// Contact ids to facilitate warm starting.
+union b2ContactID
+{
+	b2ContactFeature cf;
+	uint32 key;					///< Used to quickly compare contact ids.
+};
+
+/// A manifold point is a contact point belonging to a contact
+/// manifold. It holds details related to the geometry and dynamics
+/// of the contact points.
+/// The local point usage depends on the manifold type:
+/// -e_circles: the local center of circleB
+/// -e_faceA: the local center of cirlceB or the clip point of polygonB
+/// -e_faceB: the clip point of polygonA
+/// This structure is stored across time steps, so we keep it small.
+/// Note: the impulses are used for internal caching and may not
+/// provide reliable contact forces, especially for high speed collisions.
+struct b2ManifoldPoint
+{
+	b2Vec2 localPoint;		///< usage depends on manifold type
+	float32 normalImpulse;	///< the non-penetration impulse
+	float32 tangentImpulse;	///< the friction impulse
+	b2ContactID id;			///< uniquely identifies a contact point between two shapes
+};
+
+/// A manifold for two touching convex shapes.
+/// Box2D supports multiple types of contact:
+/// - clip point versus plane with radius
+/// - point versus point with radius (circles)
+/// The local point usage depends on the manifold type:
+/// -e_circles: the local center of circleA
+/// -e_faceA: the center of faceA
+/// -e_faceB: the center of faceB
+/// Similarly the local normal usage:
+/// -e_circles: not used
+/// -e_faceA: the normal on polygonA
+/// -e_faceB: the normal on polygonB
+/// We store contacts in this way so that position correction can
+/// account for movement, which is critical for continuous physics.
+/// All contact scenarios must be expressed in one of these types.
+/// This structure is stored across time steps, so we keep it small.
+struct b2Manifold
+{
+	enum Type
+	{
+		e_circles,
+		e_faceA,
+		e_faceB
+	};
+
+	b2ManifoldPoint points[b2_maxManifoldPoints];	///< the points of contact
+	b2Vec2 localNormal;								///< not use for Type::e_points
+	b2Vec2 localPoint;								///< usage depends on manifold type
+	Type type;
+	int32 pointCount;								///< the number of manifold points
+};
+
+/// This is used to compute the current state of a contact manifold.
+struct b2WorldManifold
+{
+	/// Evaluate the manifold with supplied transforms. This assumes
+	/// modest motion from the original state. This does not change the
+	/// point count, impulses, etc. The radii must come from the shapes
+	/// that generated the manifold.
+	void Initialize(const b2Manifold* manifold,
+					const b2Transform& xfA, float32 radiusA,
+					const b2Transform& xfB, float32 radiusB);
+
+	b2Vec2 normal;								///< world vector pointing from A to B
+	b2Vec2 points[b2_maxManifoldPoints];		///< world contact point (point of intersection)
+	float32 separations[b2_maxManifoldPoints];	///< a negative value indicates overlap, in meters
+};
+
+/// This is used for determining the state of contact points.
+enum b2PointState
+{
+	b2_nullState,		///< point does not exist
+	b2_addState,		///< point was added in the update
+	b2_persistState,	///< point persisted across the update
+	b2_removeState		///< point was removed in the update
+};
+
+/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
+/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
+void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
+					  const b2Manifold* manifold1, const b2Manifold* manifold2);
+
+/// Used for computing contact manifolds.
+struct b2ClipVertex
+{
+	b2Vec2 v;
+	b2ContactID id;
+};
+
+/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+struct b2RayCastInput
+{
+	b2Vec2 p1, p2;
+	float32 maxFraction;
+};
+
+/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
+/// come from b2RayCastInput.
+struct b2RayCastOutput
+{
+	b2Vec2 normal;
+	float32 fraction;
+};
+
+/// An axis aligned bounding box.
+struct b2AABB
+{
+	/// Verify that the bounds are sorted.
+	bool IsValid() const;
+
+	/// Get the center of the AABB.
+	b2Vec2 GetCenter() const
+	{
+		return 0.5f * (lowerBound + upperBound);
+	}
+
+	/// Get the extents of the AABB (half-widths).
+	b2Vec2 GetExtents() const
+	{
+		return 0.5f * (upperBound - lowerBound);
+	}
+
+	/// Get the perimeter length
+	float32 GetPerimeter() const
+	{
+		float32 wx = upperBound.x - lowerBound.x;
+		float32 wy = upperBound.y - lowerBound.y;
+		return 2.0f * (wx + wy);
+	}
+
+	/// Combine an AABB into this one.
+	void Combine(const b2AABB& aabb)
+	{
+		lowerBound = b2Min(lowerBound, aabb.lowerBound);
+		upperBound = b2Max(upperBound, aabb.upperBound);
+	}
+
+	/// Combine two AABBs into this one.
+	void Combine(const b2AABB& aabb1, const b2AABB& aabb2)
+	{
+		lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
+		upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
+	}
+
+	/// Does this aabb contain the provided AABB.
+	bool Contains(const b2AABB& aabb) const
+	{
+		bool result = true;
+		result = result && lowerBound.x <= aabb.lowerBound.x;
+		result = result && lowerBound.y <= aabb.lowerBound.y;
+		result = result && aabb.upperBound.x <= upperBound.x;
+		result = result && aabb.upperBound.y <= upperBound.y;
+		return result;
+	}
+
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
+
+	b2Vec2 lowerBound;	///< the lower vertex
+	b2Vec2 upperBound;	///< the upper vertex
+};
+
+/// Compute the collision manifold between two circles.
+void b2CollideCircles(b2Manifold* manifold,
+					  const b2CircleShape* circleA, const b2Transform& xfA,
+					  const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between a polygon and a circle.
+void b2CollidePolygonAndCircle(b2Manifold* manifold,
+							   const b2PolygonShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between two polygons.
+void b2CollidePolygons(b2Manifold* manifold,
+					   const b2PolygonShape* polygonA, const b2Transform& xfA,
+					   const b2PolygonShape* polygonB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							   const b2EdgeShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndPolygon(b2Manifold* manifold,
+							   const b2EdgeShape* edgeA, const b2Transform& xfA,
+							   const b2PolygonShape* circleB, const b2Transform& xfB);
+
+/// Clipping for contact manifolds.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+							const b2Vec2& normal, float32 offset, int32 vertexIndexA);
+
+/// Determine if two generic shapes overlap.
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					const b2Transform& xfA, const b2Transform& xfB);
+
+// ---------------- Inline Functions ------------------------------------------
+
+inline bool b2AABB::IsValid() const
+{
+	b2Vec2 d = upperBound - lowerBound;
+	bool valid = d.x >= 0.0f && d.y >= 0.0f;
+	valid = valid && lowerBound.IsValid() && upperBound.IsValid();
+	return valid;
+}
+
+inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)
+{
+	b2Vec2 d1, d2;
+	d1 = b.lowerBound - a.upperBound;
+	d2 = a.lowerBound - b.upperBound;
+
+	if (d1.x > 0.0f || d1.y > 0.0f)
+		return false;
+
+	if (d2.x > 0.0f || d2.y > 0.0f)
+		return false;
+
+	return true;
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/b2Distance.cpp b/src/3rdparty/Box2D/Collision/b2Distance.cpp
new file mode 100644
index 0000000..194d747
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2Distance.cpp
@@ -0,0 +1,737 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Distance.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2EdgeShape.h"
+#include "Box2D/Collision/Shapes/b2ChainShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+
+// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
+int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
+
+void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
+{
+	switch (shape->GetType())
+	{
+	case b2Shape::e_circle:
+		{
+			const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
+			m_vertices = &circle->m_p;
+			m_count = 1;
+			m_radius = circle->m_radius;
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
+			m_vertices = polygon->m_vertices;
+			m_count = polygon->m_count;
+			m_radius = polygon->m_radius;
+		}
+		break;
+
+	case b2Shape::e_chain:
+		{
+			const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
+			b2Assert(0 <= index && index < chain->m_count);
+
+			m_buffer[0] = chain->m_vertices[index];
+			if (index + 1 < chain->m_count)
+			{
+				m_buffer[1] = chain->m_vertices[index + 1];
+			}
+			else
+			{
+				m_buffer[1] = chain->m_vertices[0];
+			}
+
+			m_vertices = m_buffer;
+			m_count = 2;
+			m_radius = chain->m_radius;
+		}
+		break;
+
+	case b2Shape::e_edge:
+		{
+			const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
+			m_vertices = &edge->m_vertex1;
+			m_count = 2;
+			m_radius = edge->m_radius;
+		}
+		break;
+
+	default:
+		b2Assert(false);
+	}
+}
+
+void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float32 radius)
+{
+    m_vertices = vertices;
+    m_count = count;
+    m_radius = radius;
+}
+
+struct b2SimplexVertex
+{
+	b2Vec2 wA;		// support point in proxyA
+	b2Vec2 wB;		// support point in proxyB
+	b2Vec2 w;		// wB - wA
+	float32 a;		// barycentric coordinate for closest point
+	int32 indexA;	// wA index
+	int32 indexB;	// wB index
+};
+
+struct b2Simplex
+{
+	void ReadCache(	const b2SimplexCache* cache,
+					const b2DistanceProxy* proxyA, const b2Transform& transformA,
+					const b2DistanceProxy* proxyB, const b2Transform& transformB)
+	{
+		b2Assert(cache->count <= 3);
+		
+		// Copy data from cache.
+		m_count = cache->count;
+		b2SimplexVertex* vertices = &m_v1;
+		for (int32 i = 0; i < m_count; ++i)
+		{
+			b2SimplexVertex* v = vertices + i;
+			v->indexA = cache->indexA[i];
+			v->indexB = cache->indexB[i];
+			b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
+			b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
+			v->wA = b2Mul(transformA, wALocal);
+			v->wB = b2Mul(transformB, wBLocal);
+			v->w = v->wB - v->wA;
+			v->a = 0.0f;
+		}
+
+		// Compute the new simplex metric, if it is substantially different than
+		// old metric then flush the simplex.
+		if (m_count > 1)
+		{
+			float32 metric1 = cache->metric;
+			float32 metric2 = GetMetric();
+			if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
+			{
+				// Reset the simplex.
+				m_count = 0;
+			}
+		}
+
+		// If the cache is empty or invalid ...
+		if (m_count == 0)
+		{
+			b2SimplexVertex* v = vertices + 0;
+			v->indexA = 0;
+			v->indexB = 0;
+			b2Vec2 wALocal = proxyA->GetVertex(0);
+			b2Vec2 wBLocal = proxyB->GetVertex(0);
+			v->wA = b2Mul(transformA, wALocal);
+			v->wB = b2Mul(transformB, wBLocal);
+			v->w = v->wB - v->wA;
+			v->a = 1.0f;
+			m_count = 1;
+		}
+	}
+
+	void WriteCache(b2SimplexCache* cache) const
+	{
+		cache->metric = GetMetric();
+		cache->count = uint16(m_count);
+		const b2SimplexVertex* vertices = &m_v1;
+		for (int32 i = 0; i < m_count; ++i)
+		{
+			cache->indexA[i] = uint8(vertices[i].indexA);
+			cache->indexB[i] = uint8(vertices[i].indexB);
+		}
+	}
+
+	b2Vec2 GetSearchDirection() const
+	{
+		switch (m_count)
+		{
+		case 1:
+			return -m_v1.w;
+
+		case 2:
+			{
+				b2Vec2 e12 = m_v2.w - m_v1.w;
+				float32 sgn = b2Cross(e12, -m_v1.w);
+				if (sgn > 0.0f)
+				{
+					// Origin is left of e12.
+					return b2Cross(1.0f, e12);
+				}
+				else
+				{
+					// Origin is right of e12.
+					return b2Cross(e12, 1.0f);
+				}
+			}
+
+		default:
+			b2Assert(false);
+			return b2Vec2_zero;
+		}
+	}
+
+	b2Vec2 GetClosestPoint() const
+	{
+		switch (m_count)
+		{
+		case 0:
+			b2Assert(false);
+			return b2Vec2_zero;
+
+		case 1:
+			return m_v1.w;
+
+		case 2:
+			return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
+
+		case 3:
+			return b2Vec2_zero;
+
+		default:
+			b2Assert(false);
+			return b2Vec2_zero;
+		}
+	}
+
+	void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
+	{
+		switch (m_count)
+		{
+		case 0:
+			b2Assert(false);
+			break;
+
+		case 1:
+			*pA = m_v1.wA;
+			*pB = m_v1.wB;
+			break;
+
+		case 2:
+			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
+			*pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
+			break;
+
+		case 3:
+			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
+			*pB = *pA;
+			break;
+
+		default:
+			b2Assert(false);
+			break;
+		}
+	}
+
+	float32 GetMetric() const
+	{
+		switch (m_count)
+		{
+		case 0:
+			b2Assert(false);
+			return 0.0f;
+
+		case 1:
+			return 0.0f;
+
+		case 2:
+			return b2Distance(m_v1.w, m_v2.w);
+
+		case 3:
+			return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
+
+		default:
+			b2Assert(false);
+			return 0.0f;
+		}
+	}
+
+	void Solve2();
+	void Solve3();
+
+	b2SimplexVertex m_v1, m_v2, m_v3;
+	int32 m_count;
+};
+
+
+// Solve a line segment using barycentric coordinates.
+//
+// p = a1 * w1 + a2 * w2
+// a1 + a2 = 1
+//
+// The vector from the origin to the closest point on the line is
+// perpendicular to the line.
+// e12 = w2 - w1
+// dot(p, e) = 0
+// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
+//
+// 2-by-2 linear system
+// [1      1     ][a1] = [1]
+// [w1.e12 w2.e12][a2] = [0]
+//
+// Define
+// d12_1 =  dot(w2, e12)
+// d12_2 = -dot(w1, e12)
+// d12 = d12_1 + d12_2
+//
+// Solution
+// a1 = d12_1 / d12
+// a2 = d12_2 / d12
+void b2Simplex::Solve2()
+{
+	b2Vec2 w1 = m_v1.w;
+	b2Vec2 w2 = m_v2.w;
+	b2Vec2 e12 = w2 - w1;
+
+	// w1 region
+	float32 d12_2 = -b2Dot(w1, e12);
+	if (d12_2 <= 0.0f)
+	{
+		// a2 <= 0, so we clamp it to 0
+		m_v1.a = 1.0f;
+		m_count = 1;
+		return;
+	}
+
+	// w2 region
+	float32 d12_1 = b2Dot(w2, e12);
+	if (d12_1 <= 0.0f)
+	{
+		// a1 <= 0, so we clamp it to 0
+		m_v2.a = 1.0f;
+		m_count = 1;
+		m_v1 = m_v2;
+		return;
+	}
+
+	// Must be in e12 region.
+	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+	m_v1.a = d12_1 * inv_d12;
+	m_v2.a = d12_2 * inv_d12;
+	m_count = 2;
+}
+
+// Possible regions:
+// - points[2]
+// - edge points[0]-points[2]
+// - edge points[1]-points[2]
+// - inside the triangle
+void b2Simplex::Solve3()
+{
+	b2Vec2 w1 = m_v1.w;
+	b2Vec2 w2 = m_v2.w;
+	b2Vec2 w3 = m_v3.w;
+
+	// Edge12
+	// [1      1     ][a1] = [1]
+	// [w1.e12 w2.e12][a2] = [0]
+	// a3 = 0
+	b2Vec2 e12 = w2 - w1;
+	float32 w1e12 = b2Dot(w1, e12);
+	float32 w2e12 = b2Dot(w2, e12);
+	float32 d12_1 = w2e12;
+	float32 d12_2 = -w1e12;
+
+	// Edge13
+	// [1      1     ][a1] = [1]
+	// [w1.e13 w3.e13][a3] = [0]
+	// a2 = 0
+	b2Vec2 e13 = w3 - w1;
+	float32 w1e13 = b2Dot(w1, e13);
+	float32 w3e13 = b2Dot(w3, e13);
+	float32 d13_1 = w3e13;
+	float32 d13_2 = -w1e13;
+
+	// Edge23
+	// [1      1     ][a2] = [1]
+	// [w2.e23 w3.e23][a3] = [0]
+	// a1 = 0
+	b2Vec2 e23 = w3 - w2;
+	float32 w2e23 = b2Dot(w2, e23);
+	float32 w3e23 = b2Dot(w3, e23);
+	float32 d23_1 = w3e23;
+	float32 d23_2 = -w2e23;
+	
+	// Triangle123
+	float32 n123 = b2Cross(e12, e13);
+
+	float32 d123_1 = n123 * b2Cross(w2, w3);
+	float32 d123_2 = n123 * b2Cross(w3, w1);
+	float32 d123_3 = n123 * b2Cross(w1, w2);
+
+	// w1 region
+	if (d12_2 <= 0.0f && d13_2 <= 0.0f)
+	{
+		m_v1.a = 1.0f;
+		m_count = 1;
+		return;
+	}
+
+	// e12
+	if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
+	{
+		float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+		m_v1.a = d12_1 * inv_d12;
+		m_v2.a = d12_2 * inv_d12;
+		m_count = 2;
+		return;
+	}
+
+	// e13
+	if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
+	{
+		float32 inv_d13 = 1.0f / (d13_1 + d13_2);
+		m_v1.a = d13_1 * inv_d13;
+		m_v3.a = d13_2 * inv_d13;
+		m_count = 2;
+		m_v2 = m_v3;
+		return;
+	}
+
+	// w2 region
+	if (d12_1 <= 0.0f && d23_2 <= 0.0f)
+	{
+		m_v2.a = 1.0f;
+		m_count = 1;
+		m_v1 = m_v2;
+		return;
+	}
+
+	// w3 region
+	if (d13_1 <= 0.0f && d23_1 <= 0.0f)
+	{
+		m_v3.a = 1.0f;
+		m_count = 1;
+		m_v1 = m_v3;
+		return;
+	}
+
+	// e23
+	if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
+	{
+		float32 inv_d23 = 1.0f / (d23_1 + d23_2);
+		m_v2.a = d23_1 * inv_d23;
+		m_v3.a = d23_2 * inv_d23;
+		m_count = 2;
+		m_v1 = m_v3;
+		return;
+	}
+
+	// Must be in triangle123
+	float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
+	m_v1.a = d123_1 * inv_d123;
+	m_v2.a = d123_2 * inv_d123;
+	m_v3.a = d123_3 * inv_d123;
+	m_count = 3;
+}
+
+void b2Distance(b2DistanceOutput* output,
+				b2SimplexCache* cache,
+				const b2DistanceInput* input)
+{
+	++b2_gjkCalls;
+
+	const b2DistanceProxy* proxyA = &input->proxyA;
+	const b2DistanceProxy* proxyB = &input->proxyB;
+
+	b2Transform transformA = input->transformA;
+	b2Transform transformB = input->transformB;
+
+	// Initialize the simplex.
+	b2Simplex simplex;
+	simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
+
+	// Get simplex vertices as an array.
+	b2SimplexVertex* vertices = &simplex.m_v1;
+	const int32 k_maxIters = 20;
+
+	// These store the vertices of the last simplex so that we
+	// can check for duplicates and prevent cycling.
+	int32 saveA[3], saveB[3];
+	int32 saveCount = 0;
+
+	// Main iteration loop.
+	int32 iter = 0;
+	while (iter < k_maxIters)
+	{
+		// Copy simplex so we can identify duplicates.
+		saveCount = simplex.m_count;
+		for (int32 i = 0; i < saveCount; ++i)
+		{
+			saveA[i] = vertices[i].indexA;
+			saveB[i] = vertices[i].indexB;
+		}
+
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+
+		case 2:
+			simplex.Solve2();
+			break;
+
+		case 3:
+			simplex.Solve3();
+			break;
+
+		default:
+			b2Assert(false);
+		}
+
+		// If we have 3 points, then the origin is in the corresponding triangle.
+		if (simplex.m_count == 3)
+		{
+			break;
+		}
+
+		// Get search direction.
+		b2Vec2 d = simplex.GetSearchDirection();
+
+		// Ensure the search direction is numerically fit.
+		if (d.LengthSquared() < b2_epsilon * b2_epsilon)
+		{
+			// The origin is probably contained by a line segment
+			// or triangle. Thus the shapes are overlapped.
+
+			// We can't return zero here even though there may be overlap.
+			// In case the simplex is a point, segment, or triangle it is difficult
+			// to determine if the origin is contained in the CSO or very close to it.
+			break;
+		}
+
+		// Compute a tentative new simplex vertex using support points.
+		b2SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
+		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
+		b2Vec2 wBLocal;
+		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
+		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
+		vertex->w = vertex->wB - vertex->wA;
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+		++b2_gjkIters;
+
+		// Check for duplicate support points. This is the main termination criteria.
+		bool duplicate = false;
+		for (int32 i = 0; i < saveCount; ++i)
+		{
+			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
+			{
+				duplicate = true;
+				break;
+			}
+		}
+
+		// If we found a duplicate support point we must exit to avoid cycling.
+		if (duplicate)
+		{
+			break;
+		}
+
+		// New vertex is ok and needed.
+		++simplex.m_count;
+	}
+
+	b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
+
+	// Prepare output.
+	simplex.GetWitnessPoints(&output->pointA, &output->pointB);
+	output->distance = b2Distance(output->pointA, output->pointB);
+	output->iterations = iter;
+
+	// Cache the simplex.
+	simplex.WriteCache(cache);
+
+	// Apply radii if requested.
+	if (input->useRadii)
+	{
+		float32 rA = proxyA->m_radius;
+		float32 rB = proxyB->m_radius;
+
+		if (output->distance > rA + rB && output->distance > b2_epsilon)
+		{
+			// Shapes are still no overlapped.
+			// Move the witness points to the outer surface.
+			output->distance -= rA + rB;
+			b2Vec2 normal = output->pointB - output->pointA;
+			normal.Normalize();
+			output->pointA += rA * normal;
+			output->pointB -= rB * normal;
+		}
+		else
+		{
+			// Shapes are overlapped when radii are considered.
+			// Move the witness points to the middle.
+			b2Vec2 p = 0.5f * (output->pointA + output->pointB);
+			output->pointA = p;
+			output->pointB = p;
+			output->distance = 0.0f;
+		}
+	}
+}
+
+// GJK-raycast
+// Algorithm by Gino van den Bergen.
+// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
+bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input)
+{
+    output->iterations = 0;
+    output->lambda = 1.0f;
+    output->normal.SetZero();
+    output->point.SetZero();
+
+	const b2DistanceProxy* proxyA = &input->proxyA;
+	const b2DistanceProxy* proxyB = &input->proxyB;
+
+    float32 radiusA = b2Max(proxyA->m_radius, b2_polygonRadius);
+    float32 radiusB = b2Max(proxyB->m_radius, b2_polygonRadius);
+    float32 radius = radiusA + radiusB;
+
+	b2Transform xfA = input->transformA;
+	b2Transform xfB = input->transformB;
+
+	b2Vec2 r = input->translationB;
+	b2Vec2 n(0.0f, 0.0f);
+	float32 lambda = 0.0f;
+
+	// Initial simplex
+	b2Simplex simplex;
+	simplex.m_count = 0;
+
+	// Get simplex vertices as an array.
+	b2SimplexVertex* vertices = &simplex.m_v1;
+
+	// Get support point in -r direction
+	int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r));
+	b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
+	int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r));
+	b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
+    b2Vec2 v = wA - wB;
+
+    // Sigma is the target distance between polygons
+    float32 sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius);
+	const float32 tolerance = 0.5f * b2_linearSlop;
+
+	// Main iteration loop.
+	const int32 k_maxIters = 20;
+	int32 iter = 0;
+	while (iter < k_maxIters && b2Abs(v.Length() - sigma) > tolerance)
+	{
+		b2Assert(simplex.m_count < 3);
+
+        output->iterations += 1;
+
+		// Support in direction -v (A - B)
+		indexA = proxyA->GetSupport(b2MulT(xfA.q, -v));
+		wA = b2Mul(xfA, proxyA->GetVertex(indexA));
+		indexB = proxyB->GetSupport(b2MulT(xfB.q, v));
+		wB = b2Mul(xfB, proxyB->GetVertex(indexB));
+        b2Vec2 p = wA - wB;
+
+        // -v is a normal at p
+        v.Normalize();
+
+        // Intersect ray with plane
+		float32 vp = b2Dot(v, p);
+        float32 vr = b2Dot(v, r);
+		if (vp - sigma > lambda * vr)
+		{
+			if (vr <= 0.0f)
+			{
+				return false;
+			}
+
+			lambda = (vp - sigma) / vr;
+			if (lambda > 1.0f)
+			{
+				return false;
+			}
+
+            n = -v;
+            simplex.m_count = 0;
+		}
+
+        // Reverse simplex since it works with B - A.
+        // Shift by lambda * r because we want the closest point to the current clip point.
+        // Note that the support point p is not shifted because we want the plane equation
+        // to be formed in unshifted space.
+		b2SimplexVertex* vertex = vertices + simplex.m_count;
+		vertex->indexA = indexB;
+		vertex->wA = wB + lambda * r;
+		vertex->indexB = indexA;
+		vertex->wB = wA;
+		vertex->w = vertex->wB - vertex->wA;
+		vertex->a = 1.0f;
+		simplex.m_count += 1;
+
+		switch (simplex.m_count)
+		{
+		case 1:
+			break;
+
+		case 2:
+			simplex.Solve2();
+			break;
+
+		case 3:
+			simplex.Solve3();
+			break;
+
+		default:
+			b2Assert(false);
+		}
+		
+		// If we have 3 points, then the origin is in the corresponding triangle.
+		if (simplex.m_count == 3)
+		{
+			// Overlap
+			return false;
+		}
+
+		// Get search direction.
+		v = simplex.GetClosestPoint();
+
+		// Iteration count is equated to the number of support point calls.
+		++iter;
+	}
+
+	// Prepare output.
+	b2Vec2 pointA, pointB;
+	simplex.GetWitnessPoints(&pointB, &pointA);
+
+	if (v.LengthSquared() > 0.0f)
+	{
+        n = -v;
+		n.Normalize();
+	}
+
+    output->point = pointA + radiusA * n;
+	output->normal = n;
+	output->lambda = lambda;
+	output->iterations = iter;
+	return true;
+}
diff --git a/src/3rdparty/Box2D/Collision/b2Distance.h b/src/3rdparty/Box2D/Collision/b2Distance.h
new file mode 100644
index 0000000..d6eb985
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2Distance.h
@@ -0,0 +1,166 @@
+
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_DISTANCE_H
+#define B2_DISTANCE_H
+
+#include "Box2D/Common/b2Math.h"
+
+class b2Shape;
+
+/// A distance proxy is used by the GJK algorithm.
+/// It encapsulates any shape.
+struct b2DistanceProxy
+{
+	b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {}
+
+	/// Initialize the proxy using the given shape. The shape
+	/// must remain in scope while the proxy is in use.
+	void Set(const b2Shape* shape, int32 index);
+
+    /// Initialize the proxy using a vertex cloud and radius. The vertices
+    /// must remain in scope while the proxy is in use.
+    void Set(const b2Vec2* vertices, int32 count, float32 radius);
+
+	/// Get the supporting vertex index in the given direction.
+	int32 GetSupport(const b2Vec2& d) const;
+
+	/// Get the supporting vertex in the given direction.
+	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
+
+	/// Get the vertex count.
+	int32 GetVertexCount() const;
+
+	/// Get a vertex by index. Used by b2Distance.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	b2Vec2 m_buffer[2];
+	const b2Vec2* m_vertices;
+	int32 m_count;
+	float32 m_radius;
+};
+
+/// Used to warm start b2Distance.
+/// Set count to zero on first call.
+struct b2SimplexCache
+{
+	float32 metric;		///< length or area
+	uint16 count;
+	uint8 indexA[3];	///< vertices on shape A
+	uint8 indexB[3];	///< vertices on shape B
+};
+
+/// Input for b2Distance.
+/// You have to option to use the shape radii
+/// in the computation. Even 
+struct b2DistanceInput
+{
+	b2DistanceProxy proxyA;
+	b2DistanceProxy proxyB;
+	b2Transform transformA;
+	b2Transform transformB;
+	bool useRadii;
+};
+
+/// Output for b2Distance.
+struct b2DistanceOutput
+{
+	b2Vec2 pointA;		///< closest point on shapeA
+	b2Vec2 pointB;		///< closest point on shapeB
+	float32 distance;
+	int32 iterations;	///< number of GJK iterations used
+};
+
+/// Compute the closest points between two shapes. Supports any combination of:
+/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
+/// On the first call set b2SimplexCache.count to zero.
+void b2Distance(b2DistanceOutput* output,
+				b2SimplexCache* cache, 
+				const b2DistanceInput* input);
+
+/// Input parameters for b2ShapeCast
+struct b2ShapeCastInput
+{
+	b2DistanceProxy proxyA;
+	b2DistanceProxy proxyB;
+	b2Transform transformA;
+	b2Transform transformB;
+	b2Vec2 translationB;
+};
+
+/// Output results for b2ShapeCast
+struct b2ShapeCastOutput
+{
+	b2Vec2 point;
+	b2Vec2 normal;
+	float32 lambda;
+	int32 iterations;
+};
+
+/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
+bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input);
+
+//////////////////////////////////////////////////////////////////////////
+
+inline int32 b2DistanceProxy::GetVertexCount() const
+{
+	return m_count;
+}
+
+inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
+{
+	b2Assert(0 <= index && index < m_count);
+	return m_vertices[index];
+}
+
+inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_count; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return bestIndex;
+}
+
+inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_count; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return m_vertices[bestIndex];
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/b2DynamicTree.cpp b/src/3rdparty/Box2D/Collision/b2DynamicTree.cpp
new file mode 100644
index 0000000..4432ec1
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2DynamicTree.cpp
@@ -0,0 +1,780 @@
+/*
+* Copyright (c) 2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2DynamicTree.h"
+#include <string.h>
+
+b2DynamicTree::b2DynamicTree()
+{
+	m_root = b2_nullNode;
+
+	m_nodeCapacity = 16;
+	m_nodeCount = 0;
+	m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
+
+	// Build a linked list for the free list.
+	for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
+	{
+		m_nodes[i].next = i + 1;
+		m_nodes[i].height = -1;
+	}
+	m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+	m_nodes[m_nodeCapacity-1].height = -1;
+	m_freeList = 0;
+
+	m_path = 0;
+
+	m_insertionCount = 0;
+}
+
+b2DynamicTree::~b2DynamicTree()
+{
+	// This frees the entire tree in one shot.
+	b2Free(m_nodes);
+}
+
+// Allocate a node from the pool. Grow the pool if necessary.
+int32 b2DynamicTree::AllocateNode()
+{
+	// Expand the node pool as needed.
+	if (m_freeList == b2_nullNode)
+	{
+		b2Assert(m_nodeCount == m_nodeCapacity);
+
+		// The free list is empty. Rebuild a bigger pool.
+		b2TreeNode* oldNodes = m_nodes;
+		m_nodeCapacity *= 2;
+		m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
+		b2Free(oldNodes);
+
+		// Build a linked list for the free list. The parent
+		// pointer becomes the "next" pointer.
+		for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
+		{
+			m_nodes[i].next = i + 1;
+			m_nodes[i].height = -1;
+		}
+		m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+		m_nodes[m_nodeCapacity-1].height = -1;
+		m_freeList = m_nodeCount;
+	}
+
+	// Peel a node off the free list.
+	int32 nodeId = m_freeList;
+	m_freeList = m_nodes[nodeId].next;
+	m_nodes[nodeId].parent = b2_nullNode;
+	m_nodes[nodeId].child1 = b2_nullNode;
+	m_nodes[nodeId].child2 = b2_nullNode;
+	m_nodes[nodeId].height = 0;
+	m_nodes[nodeId].userData = nullptr;
+	++m_nodeCount;
+	return nodeId;
+}
+
+// Return a node to the pool.
+void b2DynamicTree::FreeNode(int32 nodeId)
+{
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2Assert(0 < m_nodeCount);
+	m_nodes[nodeId].next = m_freeList;
+	m_nodes[nodeId].height = -1;
+	m_freeList = nodeId;
+	--m_nodeCount;
+}
+
+// Create a proxy in the tree as a leaf node. We return the index
+// of the node instead of a pointer so that we can grow
+// the node pool.
+int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
+{
+	int32 proxyId = AllocateNode();
+
+	// Fatten the aabb.
+	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
+	m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
+	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
+	m_nodes[proxyId].userData = userData;
+	m_nodes[proxyId].height = 0;
+
+	InsertLeaf(proxyId);
+
+	return proxyId;
+}
+
+void b2DynamicTree::DestroyProxy(int32 proxyId)
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	b2Assert(m_nodes[proxyId].IsLeaf());
+
+	RemoveLeaf(proxyId);
+	FreeNode(proxyId);
+}
+
+bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+
+	b2Assert(m_nodes[proxyId].IsLeaf());
+
+	if (m_nodes[proxyId].aabb.Contains(aabb))
+	{
+		return false;
+	}
+
+	RemoveLeaf(proxyId);
+
+	// Extend AABB.
+	b2AABB b = aabb;
+	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
+	b.lowerBound = b.lowerBound - r;
+	b.upperBound = b.upperBound + r;
+
+	// Predict AABB displacement.
+	b2Vec2 d = b2_aabbMultiplier * displacement;
+
+	if (d.x < 0.0f)
+	{
+		b.lowerBound.x += d.x;
+	}
+	else
+	{
+		b.upperBound.x += d.x;
+	}
+
+	if (d.y < 0.0f)
+	{
+		b.lowerBound.y += d.y;
+	}
+	else
+	{
+		b.upperBound.y += d.y;
+	}
+
+	m_nodes[proxyId].aabb = b;
+
+	InsertLeaf(proxyId);
+	return true;
+}
+
+void b2DynamicTree::InsertLeaf(int32 leaf)
+{
+	++m_insertionCount;
+
+	if (m_root == b2_nullNode)
+	{
+		m_root = leaf;
+		m_nodes[m_root].parent = b2_nullNode;
+		return;
+	}
+
+	// Find the best sibling for this node
+	b2AABB leafAABB = m_nodes[leaf].aabb;
+	int32 index = m_root;
+	while (m_nodes[index].IsLeaf() == false)
+	{
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		float32 area = m_nodes[index].aabb.GetPerimeter();
+
+		b2AABB combinedAABB;
+		combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
+		float32 combinedArea = combinedAABB.GetPerimeter();
+
+		// Cost of creating a new parent for this node and the new leaf
+		float32 cost = 2.0f * combinedArea;
+
+		// Minimum cost of pushing the leaf further down the tree
+		float32 inheritanceCost = 2.0f * (combinedArea - area);
+
+		// Cost of descending into child1
+		float32 cost1;
+		if (m_nodes[child1].IsLeaf())
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			cost1 = aabb.GetPerimeter() + inheritanceCost;
+		}
+		else
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost1 = (newArea - oldArea) + inheritanceCost;
+		}
+
+		// Cost of descending into child2
+		float32 cost2;
+		if (m_nodes[child2].IsLeaf())
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			cost2 = aabb.GetPerimeter() + inheritanceCost;
+		}
+		else
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost2 = newArea - oldArea + inheritanceCost;
+		}
+
+		// Descend according to the minimum cost.
+		if (cost < cost1 && cost < cost2)
+		{
+			break;
+		}
+
+		// Descend
+		if (cost1 < cost2)
+		{
+			index = child1;
+		}
+		else
+		{
+			index = child2;
+		}
+	}
+
+	int32 sibling = index;
+
+	// Create a new parent.
+	int32 oldParent = m_nodes[sibling].parent;
+	int32 newParent = AllocateNode();
+	m_nodes[newParent].parent = oldParent;
+	m_nodes[newParent].userData = nullptr;
+	m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
+	m_nodes[newParent].height = m_nodes[sibling].height + 1;
+
+	if (oldParent != b2_nullNode)
+	{
+		// The sibling was not the root.
+		if (m_nodes[oldParent].child1 == sibling)
+		{
+			m_nodes[oldParent].child1 = newParent;
+		}
+		else
+		{
+			m_nodes[oldParent].child2 = newParent;
+		}
+
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
+	}
+	else
+	{
+		// The sibling was the root.
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
+		m_root = newParent;
+	}
+
+	// Walk back up the tree fixing heights and AABBs
+	index = m_nodes[leaf].parent;
+	while (index != b2_nullNode)
+	{
+		index = Balance(index);
+
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		b2Assert(child1 != b2_nullNode);
+		b2Assert(child2 != b2_nullNode);
+
+		m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+		m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+		index = m_nodes[index].parent;
+	}
+
+	//Validate();
+}
+
+void b2DynamicTree::RemoveLeaf(int32 leaf)
+{
+	if (leaf == m_root)
+	{
+		m_root = b2_nullNode;
+		return;
+	}
+
+	int32 parent = m_nodes[leaf].parent;
+	int32 grandParent = m_nodes[parent].parent;
+	int32 sibling;
+	if (m_nodes[parent].child1 == leaf)
+	{
+		sibling = m_nodes[parent].child2;
+	}
+	else
+	{
+		sibling = m_nodes[parent].child1;
+	}
+
+	if (grandParent != b2_nullNode)
+	{
+		// Destroy parent and connect sibling to grandParent.
+		if (m_nodes[grandParent].child1 == parent)
+		{
+			m_nodes[grandParent].child1 = sibling;
+		}
+		else
+		{
+			m_nodes[grandParent].child2 = sibling;
+		}
+		m_nodes[sibling].parent = grandParent;
+		FreeNode(parent);
+
+		// Adjust ancestor bounds.
+		int32 index = grandParent;
+		while (index != b2_nullNode)
+		{
+			index = Balance(index);
+
+			int32 child1 = m_nodes[index].child1;
+			int32 child2 = m_nodes[index].child2;
+
+			m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+			m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+
+			index = m_nodes[index].parent;
+		}
+	}
+	else
+	{
+		m_root = sibling;
+		m_nodes[sibling].parent = b2_nullNode;
+		FreeNode(parent);
+	}
+
+	//Validate();
+}
+
+// Perform a left or right rotation if node A is imbalanced.
+// Returns the new root index.
+int32 b2DynamicTree::Balance(int32 iA)
+{
+	b2Assert(iA != b2_nullNode);
+
+	b2TreeNode* A = m_nodes + iA;
+	if (A->IsLeaf() || A->height < 2)
+	{
+		return iA;
+	}
+
+	int32 iB = A->child1;
+	int32 iC = A->child2;
+	b2Assert(0 <= iB && iB < m_nodeCapacity);
+	b2Assert(0 <= iC && iC < m_nodeCapacity);
+
+	b2TreeNode* B = m_nodes + iB;
+	b2TreeNode* C = m_nodes + iC;
+
+	int32 balance = C->height - B->height;
+
+	// Rotate C up
+	if (balance > 1)
+	{
+		int32 iF = C->child1;
+		int32 iG = C->child2;
+		b2TreeNode* F = m_nodes + iF;
+		b2TreeNode* G = m_nodes + iG;
+		b2Assert(0 <= iF && iF < m_nodeCapacity);
+		b2Assert(0 <= iG && iG < m_nodeCapacity);
+
+		// Swap A and C
+		C->child1 = iA;
+		C->parent = A->parent;
+		A->parent = iC;
+
+		// A's old parent should point to C
+		if (C->parent != b2_nullNode)
+		{
+			if (m_nodes[C->parent].child1 == iA)
+			{
+				m_nodes[C->parent].child1 = iC;
+			}
+			else
+			{
+				b2Assert(m_nodes[C->parent].child2 == iA);
+				m_nodes[C->parent].child2 = iC;
+			}
+		}
+		else
+		{
+			m_root = iC;
+		}
+
+		// Rotate
+		if (F->height > G->height)
+		{
+			C->child2 = iF;
+			A->child2 = iG;
+			G->parent = iA;
+			A->aabb.Combine(B->aabb, G->aabb);
+			C->aabb.Combine(A->aabb, F->aabb);
+
+			A->height = 1 + b2Max(B->height, G->height);
+			C->height = 1 + b2Max(A->height, F->height);
+		}
+		else
+		{
+			C->child2 = iG;
+			A->child2 = iF;
+			F->parent = iA;
+			A->aabb.Combine(B->aabb, F->aabb);
+			C->aabb.Combine(A->aabb, G->aabb);
+
+			A->height = 1 + b2Max(B->height, F->height);
+			C->height = 1 + b2Max(A->height, G->height);
+		}
+
+		return iC;
+	}
+	
+	// Rotate B up
+	if (balance < -1)
+	{
+		int32 iD = B->child1;
+		int32 iE = B->child2;
+		b2TreeNode* D = m_nodes + iD;
+		b2TreeNode* E = m_nodes + iE;
+		b2Assert(0 <= iD && iD < m_nodeCapacity);
+		b2Assert(0 <= iE && iE < m_nodeCapacity);
+
+		// Swap A and B
+		B->child1 = iA;
+		B->parent = A->parent;
+		A->parent = iB;
+
+		// A's old parent should point to B
+		if (B->parent != b2_nullNode)
+		{
+			if (m_nodes[B->parent].child1 == iA)
+			{
+				m_nodes[B->parent].child1 = iB;
+			}
+			else
+			{
+				b2Assert(m_nodes[B->parent].child2 == iA);
+				m_nodes[B->parent].child2 = iB;
+			}
+		}
+		else
+		{
+			m_root = iB;
+		}
+
+		// Rotate
+		if (D->height > E->height)
+		{
+			B->child2 = iD;
+			A->child1 = iE;
+			E->parent = iA;
+			A->aabb.Combine(C->aabb, E->aabb);
+			B->aabb.Combine(A->aabb, D->aabb);
+
+			A->height = 1 + b2Max(C->height, E->height);
+			B->height = 1 + b2Max(A->height, D->height);
+		}
+		else
+		{
+			B->child2 = iE;
+			A->child1 = iD;
+			D->parent = iA;
+			A->aabb.Combine(C->aabb, D->aabb);
+			B->aabb.Combine(A->aabb, E->aabb);
+
+			A->height = 1 + b2Max(C->height, D->height);
+			B->height = 1 + b2Max(A->height, E->height);
+		}
+
+		return iB;
+	}
+
+	return iA;
+}
+
+int32 b2DynamicTree::GetHeight() const
+{
+	if (m_root == b2_nullNode)
+	{
+		return 0;
+	}
+
+	return m_nodes[m_root].height;
+}
+
+//
+float32 b2DynamicTree::GetAreaRatio() const
+{
+	if (m_root == b2_nullNode)
+	{
+		return 0.0f;
+	}
+
+	const b2TreeNode* root = m_nodes + m_root;
+	float32 rootArea = root->aabb.GetPerimeter();
+
+	float32 totalArea = 0.0f;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height < 0)
+		{
+			// Free node in pool
+			continue;
+		}
+
+		totalArea += node->aabb.GetPerimeter();
+	}
+
+	return totalArea / rootArea;
+}
+
+// Compute the height of a sub-tree.
+int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
+{
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2TreeNode* node = m_nodes + nodeId;
+
+	if (node->IsLeaf())
+	{
+		return 0;
+	}
+
+	int32 height1 = ComputeHeight(node->child1);
+	int32 height2 = ComputeHeight(node->child2);
+	return 1 + b2Max(height1, height2);
+}
+
+int32 b2DynamicTree::ComputeHeight() const
+{
+	int32 height = ComputeHeight(m_root);
+	return height;
+}
+
+void b2DynamicTree::ValidateStructure(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	if (index == m_root)
+	{
+		b2Assert(m_nodes[index].parent == b2_nullNode);
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	b2Assert(m_nodes[child1].parent == index);
+	b2Assert(m_nodes[child2].parent == index);
+
+	ValidateStructure(child1);
+	ValidateStructure(child2);
+}
+
+void b2DynamicTree::ValidateMetrics(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	int32 height1 = m_nodes[child1].height;
+	int32 height2 = m_nodes[child2].height;
+	int32 height;
+	height = 1 + b2Max(height1, height2);
+	b2Assert(node->height == height);
+
+	b2AABB aabb;
+	aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+	b2Assert(aabb.lowerBound == node->aabb.lowerBound);
+	b2Assert(aabb.upperBound == node->aabb.upperBound);
+
+	ValidateMetrics(child1);
+	ValidateMetrics(child2);
+}
+
+void b2DynamicTree::Validate() const
+{
+#if defined(b2DEBUG)
+	ValidateStructure(m_root);
+	ValidateMetrics(m_root);
+
+	int32 freeCount = 0;
+	int32 freeIndex = m_freeList;
+	while (freeIndex != b2_nullNode)
+	{
+		b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
+		freeIndex = m_nodes[freeIndex].next;
+		++freeCount;
+	}
+
+	b2Assert(GetHeight() == ComputeHeight());
+
+	b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
+#endif
+}
+
+int32 b2DynamicTree::GetMaxBalance() const
+{
+	int32 maxBalance = 0;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height <= 1)
+		{
+			continue;
+		}
+
+		b2Assert(node->IsLeaf() == false);
+
+		int32 child1 = node->child1;
+		int32 child2 = node->child2;
+		int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
+		maxBalance = b2Max(maxBalance, balance);
+	}
+
+	return maxBalance;
+}
+
+void b2DynamicTree::RebuildBottomUp()
+{
+	int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
+	int32 count = 0;
+
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		if (m_nodes[i].height < 0)
+		{
+			// free node in pool
+			continue;
+		}
+
+		if (m_nodes[i].IsLeaf())
+		{
+			m_nodes[i].parent = b2_nullNode;
+			nodes[count] = i;
+			++count;
+		}
+		else
+		{
+			FreeNode(i);
+		}
+	}
+
+	while (count > 1)
+	{
+		float32 minCost = b2_maxFloat;
+		int32 iMin = -1, jMin = -1;
+		for (int32 i = 0; i < count; ++i)
+		{
+			b2AABB aabbi = m_nodes[nodes[i]].aabb;
+
+			for (int32 j = i + 1; j < count; ++j)
+			{
+				b2AABB aabbj = m_nodes[nodes[j]].aabb;
+				b2AABB b;
+				b.Combine(aabbi, aabbj);
+				float32 cost = b.GetPerimeter();
+				if (cost < minCost)
+				{
+					iMin = i;
+					jMin = j;
+					minCost = cost;
+				}
+			}
+		}
+
+		int32 index1 = nodes[iMin];
+		int32 index2 = nodes[jMin];
+		b2TreeNode* child1 = m_nodes + index1;
+		b2TreeNode* child2 = m_nodes + index2;
+
+		int32 parentIndex = AllocateNode();
+		b2TreeNode* parent = m_nodes + parentIndex;
+		parent->child1 = index1;
+		parent->child2 = index2;
+		parent->height = 1 + b2Max(child1->height, child2->height);
+		parent->aabb.Combine(child1->aabb, child2->aabb);
+		parent->parent = b2_nullNode;
+
+		child1->parent = parentIndex;
+		child2->parent = parentIndex;
+
+		nodes[jMin] = nodes[count-1];
+		nodes[iMin] = parentIndex;
+		--count;
+	}
+
+	m_root = nodes[0];
+	b2Free(nodes);
+
+	Validate();
+}
+
+void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		m_nodes[i].aabb.lowerBound -= newOrigin;
+		m_nodes[i].aabb.upperBound -= newOrigin;
+	}
+}
diff --git a/src/3rdparty/Box2D/Collision/b2DynamicTree.h b/src/3rdparty/Box2D/Collision/b2DynamicTree.h
new file mode 100644
index 0000000..e52b44b
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2DynamicTree.h
@@ -0,0 +1,289 @@
+/*
+* Copyright (c) 2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_DYNAMIC_TREE_H
+#define B2_DYNAMIC_TREE_H
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Common/b2GrowableStack.h"
+
+#define b2_nullNode (-1)
+
+/// A node in the dynamic tree. The client does not interact with this directly.
+struct b2TreeNode
+{
+	bool IsLeaf() const
+	{
+		return child1 == b2_nullNode;
+	}
+
+	/// Enlarged AABB
+	b2AABB aabb;
+
+	void* userData;
+
+	union
+	{
+		int32 parent;
+		int32 next;
+	};
+
+	int32 child1;
+	int32 child2;
+
+	// leaf = 0, free node = -1
+	int32 height;
+};
+
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+/// A dynamic tree arranges data in a binary tree to accelerate
+/// queries such as volume queries and ray casts. Leafs are proxies
+/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
+/// so that the proxy AABB is bigger than the client object. This allows the client
+/// object to move by small amounts without triggering a tree update.
+///
+/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
+class b2DynamicTree
+{
+public:
+	/// Constructing the tree initializes the node pool.
+	b2DynamicTree();
+
+	/// Destroy the tree, freeing the node pool.
+	~b2DynamicTree();
+
+	/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
+	int32 CreateProxy(const b2AABB& aabb, void* userData);
+
+	/// Destroy a proxy. This asserts if the id is invalid.
+	void DestroyProxy(int32 proxyId);
+
+	/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
+	/// then the proxy is removed from the tree and re-inserted. Otherwise
+	/// the function returns immediately.
+	/// @return true if the proxy was re-inserted.
+	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
+
+	/// Get proxy user data.
+	/// @return the proxy user data or 0 if the id is invalid.
+	void* GetUserData(int32 proxyId) const;
+
+	/// Get the fat AABB for a proxy.
+	const b2AABB& GetFatAABB(int32 proxyId) const;
+
+	/// Query an AABB for overlapping proxies. The callback class
+	/// is called for each proxy that overlaps the supplied AABB.
+	template <typename T>
+	void Query(T* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast against the proxies in the tree. This relies on the callback
+	/// to perform a exact ray-cast in the case were the proxy contains a shape.
+	/// The callback also performs the any collision filtering. This has performance
+	/// roughly equal to k * log(n), where k is the number of collisions and n is the
+	/// number of proxies in the tree.
+	/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+	/// @param callback a callback class that is called for each proxy that is hit by the ray.
+	template <typename T>
+	void RayCast(T* callback, const b2RayCastInput& input) const;
+
+	/// Validate this tree. For testing.
+	void Validate() const;
+
+	/// Compute the height of the binary tree in O(N) time. Should not be
+	/// called often.
+	int32 GetHeight() const;
+
+	/// Get the maximum balance of an node in the tree. The balance is the difference
+	/// in height of the two children of a node.
+	int32 GetMaxBalance() const;
+
+	/// Get the ratio of the sum of the node areas to the root area.
+	float32 GetAreaRatio() const;
+
+	/// Build an optimal tree. Very expensive. For testing.
+	void RebuildBottomUp();
+
+	/// Shift the world origin. Useful for large worlds.
+	/// The shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
+private:
+
+	int32 AllocateNode();
+	void FreeNode(int32 node);
+
+	void InsertLeaf(int32 node);
+	void RemoveLeaf(int32 node);
+
+	int32 Balance(int32 index);
+
+	int32 ComputeHeight() const;
+	int32 ComputeHeight(int32 nodeId) const;
+
+	void ValidateStructure(int32 index) const;
+	void ValidateMetrics(int32 index) const;
+
+	int32 m_root;
+
+	b2TreeNode* m_nodes;
+	int32 m_nodeCount;
+	int32 m_nodeCapacity;
+
+	int32 m_freeList;
+
+	/// This is used to incrementally traverse the tree for re-balancing.
+	uint32 m_path;
+
+	int32 m_insertionCount;
+};
+
+inline void* b2DynamicTree::GetUserData(int32 proxyId) const
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	return m_nodes[proxyId].userData;
+}
+
+inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
+{
+	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
+	return m_nodes[proxyId].aabb;
+}
+
+template <typename T>
+inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
+{
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.GetCount() > 0)
+	{
+		int32 nodeId = stack.Pop();
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2TreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, aabb))
+		{
+			if (node->IsLeaf())
+			{
+				bool proceed = callback->QueryCallback(nodeId);
+				if (proceed == false)
+				{
+					return;
+				}
+			}
+			else
+			{
+				stack.Push(node->child1);
+				stack.Push(node->child2);
+			}
+		}
+	}
+}
+
+template <typename T>
+inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
+{
+	b2Vec2 p1 = input.p1;
+	b2Vec2 p2 = input.p2;
+	b2Vec2 r = p2 - p1;
+	b2Assert(r.LengthSquared() > 0.0f);
+	r.Normalize();
+
+	// v is perpendicular to the segment.
+	b2Vec2 v = b2Cross(1.0f, r);
+	b2Vec2 abs_v = b2Abs(v);
+
+	// Separating axis for segment (Gino, p80).
+	// |dot(v, p1 - c)| > dot(|v|, h)
+
+	float32 maxFraction = input.maxFraction;
+
+	// Build a bounding box for the segment.
+	b2AABB segmentAABB;
+	{
+		b2Vec2 t = p1 + maxFraction * (p2 - p1);
+		segmentAABB.lowerBound = b2Min(p1, t);
+		segmentAABB.upperBound = b2Max(p1, t);
+	}
+
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
+
+	while (stack.GetCount() > 0)
+	{
+		int32 nodeId = stack.Pop();
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2TreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, segmentAABB) == false)
+		{
+			continue;
+		}
+
+		// Separating axis for segment (Gino, p80).
+		// |dot(v, p1 - c)| > dot(|v|, h)
+		b2Vec2 c = node->aabb.GetCenter();
+		b2Vec2 h = node->aabb.GetExtents();
+		float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
+		if (separation > 0.0f)
+		{
+			continue;
+		}
+
+		if (node->IsLeaf())
+		{
+			b2RayCastInput subInput;
+			subInput.p1 = input.p1;
+			subInput.p2 = input.p2;
+			subInput.maxFraction = maxFraction;
+
+			float32 value = callback->RayCastCallback(subInput, nodeId);
+
+			if (value == 0.0f)
+			{
+				// The client has terminated the ray cast.
+				return;
+			}
+
+			if (value > 0.0f)
+			{
+				// Update segment bounding box.
+				maxFraction = value;
+				b2Vec2 t = p1 + maxFraction * (p2 - p1);
+				segmentAABB.lowerBound = b2Min(p1, t);
+				segmentAABB.upperBound = b2Max(p1, t);
+			}
+		}
+		else
+		{
+			stack.Push(node->child1);
+			stack.Push(node->child2);
+		}
+	}
+}
+
+#endif
diff --git a/src/3rdparty/Box2D/Collision/b2TimeOfImpact.cpp b/src/3rdparty/Box2D/Collision/b2TimeOfImpact.cpp
new file mode 100644
index 0000000..4bc1769
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2TimeOfImpact.cpp
@@ -0,0 +1,486 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2Distance.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+#include "Box2D/Common/b2Timer.h"
+
+#include <stdio.h>
+
+float32 b2_toiTime, b2_toiMaxTime;
+int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
+int32 b2_toiRootIters, b2_toiMaxRootIters;
+
+//
+struct b2SeparationFunction
+{
+	enum Type
+	{
+		e_points,
+		e_faceA,
+		e_faceB
+	};
+
+	// TODO_ERIN might not need to return the separation
+
+	float32 Initialize(const b2SimplexCache* cache,
+		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
+		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
+		float32 t1)
+	{
+		m_proxyA = proxyA;
+		m_proxyB = proxyB;
+		int32 count = cache->count;
+		b2Assert(0 < count && count < 3);
+
+		m_sweepA = sweepA;
+		m_sweepB = sweepB;
+
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t1);
+		m_sweepB.GetTransform(&xfB, t1);
+
+		if (count == 1)
+		{
+			m_type = e_points;
+			b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 pointA = b2Mul(xfA, localPointA);
+			b2Vec2 pointB = b2Mul(xfB, localPointB);
+			m_axis = pointB - pointA;
+			float32 s = m_axis.Normalize();
+			return s;
+		}
+		else if (cache->indexA[0] == cache->indexA[1])
+		{
+			// Two points on B and one on A.
+			m_type = e_faceB;
+			b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
+
+			m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
+			m_axis.Normalize();
+			b2Vec2 normal = b2Mul(xfB.q, m_axis);
+
+			m_localPoint = 0.5f * (localPointB1 + localPointB2);
+			b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+			b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 pointA = b2Mul(xfA, localPointA);
+
+			float32 s = b2Dot(pointA - pointB, normal);
+			if (s < 0.0f)
+			{
+				m_axis = -m_axis;
+				s = -s;
+			}
+			return s;
+		}
+		else
+		{
+			// Two points on A and one or two points on B.
+			m_type = e_faceA;
+			b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
+			b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
+			
+			m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
+			m_axis.Normalize();
+			b2Vec2 normal = b2Mul(xfA.q, m_axis);
+
+			m_localPoint = 0.5f * (localPointA1 + localPointA2);
+			b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
+			b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+			float32 s = b2Dot(pointB - pointA, normal);
+			if (s < 0.0f)
+			{
+				m_axis = -m_axis;
+				s = -s;
+			}
+			return s;
+		}
+	}
+
+	//
+	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
+	{
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t);
+		m_sweepB.GetTransform(&xfB, t);
+
+		switch (m_type)
+		{
+		case e_points:
+			{
+				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
+
+				*indexA = m_proxyA->GetSupport(axisA);
+				*indexB = m_proxyB->GetSupport(axisB);
+
+				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
+				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
+				
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, m_axis);
+				return separation;
+			}
+
+		case e_faceA:
+			{
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.q, -normal);
+				
+				*indexA = -1;
+				*indexB = m_proxyB->GetSupport(axisB);
+
+				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, normal);
+				return separation;
+			}
+
+		case e_faceB:
+			{
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.q, -normal);
+
+				*indexB = -1;
+				*indexA = m_proxyA->GetSupport(axisA);
+
+				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+
+				float32 separation = b2Dot(pointA - pointB, normal);
+				return separation;
+			}
+
+		default:
+			b2Assert(false);
+			*indexA = -1;
+			*indexB = -1;
+			return 0.0f;
+		}
+	}
+
+	//
+	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
+	{
+		b2Transform xfA, xfB;
+		m_sweepA.GetTransform(&xfA, t);
+		m_sweepB.GetTransform(&xfB, t);
+
+		switch (m_type)
+		{
+		case e_points:
+			{
+				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
+				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
+
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+				float32 separation = b2Dot(pointB - pointA, m_axis);
+
+				return separation;
+			}
+
+		case e_faceA:
+			{
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
+				b2Vec2 pointB = b2Mul(xfB, localPointB);
+
+				float32 separation = b2Dot(pointB - pointA, normal);
+				return separation;
+			}
+
+		case e_faceB:
+			{
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
+				b2Vec2 pointA = b2Mul(xfA, localPointA);
+
+				float32 separation = b2Dot(pointA - pointB, normal);
+				return separation;
+			}
+
+		default:
+			b2Assert(false);
+			return 0.0f;
+		}
+	}
+
+	const b2DistanceProxy* m_proxyA;
+	const b2DistanceProxy* m_proxyB;
+	b2Sweep m_sweepA, m_sweepB;
+	Type m_type;
+	b2Vec2 m_localPoint;
+	b2Vec2 m_axis;
+};
+
+// CCD via the local separating axis method. This seeks progression
+// by computing the largest time at which separation is maintained.
+void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
+{
+	b2Timer timer;
+
+	++b2_toiCalls;
+
+	output->state = b2TOIOutput::e_unknown;
+	output->t = input->tMax;
+
+	const b2DistanceProxy* proxyA = &input->proxyA;
+	const b2DistanceProxy* proxyB = &input->proxyB;
+
+	b2Sweep sweepA = input->sweepA;
+	b2Sweep sweepB = input->sweepB;
+
+	// Large rotations can make the root finder fail, so we normalize the
+	// sweep angles.
+	sweepA.Normalize();
+	sweepB.Normalize();
+
+	float32 tMax = input->tMax;
+
+	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
+	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
+	float32 tolerance = 0.25f * b2_linearSlop;
+	b2Assert(target > tolerance);
+
+	float32 t1 = 0.0f;
+	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
+	int32 iter = 0;
+
+	// Prepare input for distance query.
+	b2SimplexCache cache;
+	cache.count = 0;
+	b2DistanceInput distanceInput;
+	distanceInput.proxyA = input->proxyA;
+	distanceInput.proxyB = input->proxyB;
+	distanceInput.useRadii = false;
+
+	// The outer loop progressively attempts to compute new separating axes.
+	// This loop terminates when an axis is repeated (no progress is made).
+	for(;;)
+	{
+		b2Transform xfA, xfB;
+		sweepA.GetTransform(&xfA, t1);
+		sweepB.GetTransform(&xfB, t1);
+
+		// Get the distance between shapes. We can also use the results
+		// to get a separating axis.
+		distanceInput.transformA = xfA;
+		distanceInput.transformB = xfB;
+		b2DistanceOutput distanceOutput;
+		b2Distance(&distanceOutput, &cache, &distanceInput);
+
+		// If the shapes are overlapped, we give up on continuous collision.
+		if (distanceOutput.distance <= 0.0f)
+		{
+			// Failure!
+			output->state = b2TOIOutput::e_overlapped;
+			output->t = 0.0f;
+			break;
+		}
+
+		if (distanceOutput.distance < target + tolerance)
+		{
+			// Victory!
+			output->state = b2TOIOutput::e_touching;
+			output->t = t1;
+			break;
+		}
+
+		// Initialize the separating axis.
+		b2SeparationFunction fcn;
+		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
+#if 0
+		// Dump the curve seen by the root finder
+		{
+			const int32 N = 100;
+			float32 dx = 1.0f / N;
+			float32 xs[N+1];
+			float32 fs[N+1];
+
+			float32 x = 0.0f;
+
+			for (int32 i = 0; i <= N; ++i)
+			{
+				sweepA.GetTransform(&xfA, x);
+				sweepB.GetTransform(&xfB, x);
+				float32 f = fcn.Evaluate(xfA, xfB) - target;
+
+				printf("%g %g\n", x, f);
+
+				xs[i] = x;
+				fs[i] = f;
+
+				x += dx;
+			}
+		}
+#endif
+
+		// Compute the TOI on the separating axis. We do this by successively
+		// resolving the deepest point. This loop is bounded by the number of vertices.
+		bool done = false;
+		float32 t2 = tMax;
+		int32 pushBackIter = 0;
+		for (;;)
+		{
+			// Find the deepest point at t2. Store the witness point indices.
+			int32 indexA, indexB;
+			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
+
+			// Is the final configuration separated?
+			if (s2 > target + tolerance)
+			{
+				// Victory!
+				output->state = b2TOIOutput::e_separated;
+				output->t = tMax;
+				done = true;
+				break;
+			}
+
+			// Has the separation reached tolerance?
+			if (s2 > target - tolerance)
+			{
+				// Advance the sweeps
+				t1 = t2;
+				break;
+			}
+
+			// Compute the initial separation of the witness points.
+			float32 s1 = fcn.Evaluate(indexA, indexB, t1);
+
+			// Check for initial overlap. This might happen if the root finder
+			// runs out of iterations.
+			if (s1 < target - tolerance)
+			{
+				output->state = b2TOIOutput::e_failed;
+				output->t = t1;
+				done = true;
+				break;
+			}
+
+			// Check for touching
+			if (s1 <= target + tolerance)
+			{
+				// Victory! t1 should hold the TOI (could be 0.0).
+				output->state = b2TOIOutput::e_touching;
+				output->t = t1;
+				done = true;
+				break;
+			}
+
+			// Compute 1D root of: f(x) - target = 0
+			int32 rootIterCount = 0;
+			float32 a1 = t1, a2 = t2;
+			for (;;)
+			{
+				// Use a mix of the secant rule and bisection.
+				float32 t;
+				if (rootIterCount & 1)
+				{
+					// Secant rule to improve convergence.
+					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
+				}
+				else
+				{
+					// Bisection to guarantee progress.
+					t = 0.5f * (a1 + a2);
+				}
+
+				++rootIterCount;
+				++b2_toiRootIters;
+
+				float32 s = fcn.Evaluate(indexA, indexB, t);
+
+				if (b2Abs(s - target) < tolerance)
+				{
+					// t2 holds a tentative value for t1
+					t2 = t;
+					break;
+				}
+
+				// Ensure we continue to bracket the root.
+				if (s > target)
+				{
+					a1 = t;
+					s1 = s;
+				}
+				else
+				{
+					a2 = t;
+					s2 = s;
+				}
+				
+				if (rootIterCount == 50)
+				{
+					break;
+				}
+			}
+
+			b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
+
+			++pushBackIter;
+
+			if (pushBackIter == b2_maxPolygonVertices)
+			{
+				break;
+			}
+		}
+
+		++iter;
+		++b2_toiIters;
+
+		if (done)
+		{
+			break;
+		}
+
+		if (iter == k_maxIterations)
+		{
+			// Root finder got stuck. Semi-victory.
+			output->state = b2TOIOutput::e_failed;
+			output->t = t1;
+			break;
+		}
+	}
+
+	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
+
+	float32 time = timer.GetMilliseconds();
+	b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
+	b2_toiTime += time;
+}
diff --git a/src/3rdparty/Box2D/Collision/b2TimeOfImpact.h b/src/3rdparty/Box2D/Collision/b2TimeOfImpact.h
new file mode 100644
index 0000000..3af2c32
--- /dev/null
+++ b/src/3rdparty/Box2D/Collision/b2TimeOfImpact.h
@@ -0,0 +1,58 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+*
+* 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 B2_TIME_OF_IMPACT_H
+#define B2_TIME_OF_IMPACT_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Distance.h"
+
+/// Input parameters for b2TimeOfImpact
+struct b2TOIInput
+{
+	b2DistanceProxy proxyA;
+	b2DistanceProxy proxyB;
+	b2Sweep sweepA;
+	b2Sweep sweepB;
+	float32 tMax;		// defines sweep interval [0, tMax]
+};
+
+/// Output parameters for b2TimeOfImpact.
+struct b2TOIOutput
+{
+	enum State
+	{
+		e_unknown,
+		e_failed,
+		e_overlapped,
+		e_touching,
+		e_separated
+	};
+
+	State state;
+	float32 t;
+};
+
+/// Compute the upper bound on time before two shapes penetrate. Time is represented as
+/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
+/// non-tunneling collisions. If you change the time interval, you should call this function
+/// again.
+/// Note: use b2Distance to compute the contact point and normal at the time of impact.
+void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
+
+#endif