bring in new version of Box2D and update #include paths

--HG--
branch : box2d-update

src/modules/physics/box2d/Body.h

@@ -28,7 +28,7 @@
 
 
 // Box2D
-#include "Include/Box2D.h"
+#include <Box2D/Box2D.h>
 
 namespace love
 {

src/modules/physics/box2d/Box2D/Box2D.h

@@ -0,0 +1,62 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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 BOX2D_H
+#define BOX2D_H
+
+/**
+\mainpage Box2D API Documentation
+
+\section intro_sec Getting Started
+
+For documentation please see http://box2d.org/documentation.html
+
+For discussion please visit http://box2d.org/forum
+*/
+
+// These include files constitute the main Box2D API
+
+#include <Box2D/Common/b2Settings.h>
+
+#include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+
+#include <Box2D/Collision/b2BroadPhase.h>
+#include <Box2D/Collision/b2Distance.h>
+#include <Box2D/Collision/b2DynamicTree.h>
+#include <Box2D/Collision/b2TimeOfImpact.h>
+
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+#include <Box2D/Dynamics/b2World.h>
+
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
+#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
+#include <Box2D/Dynamics/Joints/b2GearJoint.h>
+#include <Box2D/Dynamics/Joints/b2LineJoint.h>
+#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
+#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
+#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
+#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
+
+#endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp

@@ -0,0 +1,89 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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;
+}
+
+bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
+{
+	b2Vec2 center = transform.position + b2Mul(transform.R, 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) const
+{
+	b2Vec2 position = transform.position + b2Mul(transform.R, 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) const
+{
+	b2Vec2 p = transform.position + b2Mul(transform.R, 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));
+}

src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h

@@ -0,0 +1,87 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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;
+
+	/// Implement b2Shape.
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+
+	/// 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 { return 1; }
+
+	/// Get a vertex by index. Used by b2Distance.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	/// Position
+	b2Vec2 m_p;
+};
+
+inline b2CircleShape::b2CircleShape()
+{
+	m_type = e_circle;
+	m_radius = 0.0f;
+	m_p.SetZero();
+}
+
+inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const
+{
+	B2_NOT_USED(d);
+	return 0;
+}
+
+inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const
+{
+	B2_NOT_USED(d);
+	return m_p;
+}
+
+inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const
+{
+	B2_NOT_USED(index);
+	b2Assert(index == 0);
+	return m_p;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp

@@ -0,0 +1,434 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_vertexCount = 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_vertexCount = 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.position = center;
+	xf.R.Set(angle);
+
+	// Transform vertices and normals.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		m_vertices[i] = b2Mul(xf, m_vertices[i]);
+		m_normals[i] = b2Mul(xf.R, m_normals[i]);
+	}
+}
+
+void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)
+{
+	m_vertexCount = 2;
+	m_vertices[0] = v1;
+	m_vertices[1] = v2;
+	m_centroid = 0.5f * (v1 + v2);
+	m_normals[0] = b2Cross(v2 - v1, 1.0f);
+	m_normals[0].Normalize();
+	m_normals[1] = -m_normals[0];
+}
+
+static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
+{
+	b2Assert(count >= 2);
+
+	b2Vec2 c; c.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+
+	if (count == 2)
+	{
+		c = 0.5f * (vs[0] + vs[1]);
+		return c;
+	}
+
+	// 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(2 <= count && count <= b2_maxPolygonVertices);
+	m_vertexCount = count;
+
+	// Copy vertices.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		m_vertices[i] = vertices[i];
+	}
+
+	// Compute normals. Ensure the edges have non-zero length.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i + 1 < m_vertexCount ? 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();
+	}
+
+#ifdef _DEBUG
+	// Ensure the polygon is convex and the interior
+	// is to the left of each edge.
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
+
+		for (int32 j = 0; j < m_vertexCount; ++j)
+		{
+			// Don't check vertices on the current edge.
+			if (j == i1 || j == i2)
+			{
+				continue;
+			}
+			
+			b2Vec2 r = m_vertices[j] - m_vertices[i1];
+
+			// Your polygon is non-convex (it has an indentation) or
+			// has colinear edges.
+			float32 s = b2Cross(edge, r);
+			b2Assert(s > 0.0f);
+		}
+	}
+#endif
+
+	// Compute the polygon centroid.
+	m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
+}
+
+bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
+
+	for (int32 i = 0; i < m_vertexCount; ++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) const
+{
+	// Put the ray into the polygon's frame of reference.
+	b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
+	b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
+	b2Vec2 d = p2 - p1;
+
+	if (m_vertexCount == 2)
+	{
+		b2Vec2 v1 = m_vertices[0];
+		b2Vec2 v2 = m_vertices[1];
+		b2Vec2 normal = m_normals[0];
+
+		// 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 || 1.0f < 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 = -normal;
+		}
+		else
+		{
+			output->normal = normal;
+		}
+		return true;
+	}
+	else
+	{
+		float32 lower = 0.0f, upper = input.maxFraction;
+
+		int32 index = -1;
+
+		for (int32 i = 0; i < m_vertexCount; ++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.R, m_normals[index]);
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const
+{
+	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
+	b2Vec2 upper = lower;
+
+	for (int32 i = 1; i < m_vertexCount; ++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_vertexCount >= 2);
+
+	// A line segment has zero mass.
+	if (m_vertexCount == 2)
+	{
+		massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
+		massData->mass = 0.0f;
+		massData->I = 0.0f;
+		return;
+	}
+
+	b2Vec2 center; center.Set(0.0f, 0.0f);
+	float32 area = 0.0f;
+	float32 I = 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 < m_vertexCount; ++i)
+	{
+		pRef += m_vertices[i];
+	}
+	pRef *= 1.0f / count;
+#endif
+
+	const float32 k_inv3 = 1.0f / 3.0f;
+
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		// Triangle vertices.
+		b2Vec2 p1 = pRef;
+		b2Vec2 p2 = m_vertices[i];
+		b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
+
+		b2Vec2 e1 = p2 - p1;
+		b2Vec2 e2 = p3 - p1;
+
+		float32 D = b2Cross(e1, e2);
+
+		float32 triangleArea = 0.5f * D;
+		area += triangleArea;
+
+		// Area weighted centroid
+		center += triangleArea * k_inv3 * (p1 + p2 + p3);
+
+		float32 px = p1.x, py = p1.y;
+		float32 ex1 = e1.x, ey1 = e1.y;
+		float32 ex2 = e2.x, ey2 = e2.y;
+
+		float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
+		float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
+
+		I += D * (intx2 + inty2);
+	}
+
+	// Total mass
+	massData->mass = density * area;
+
+	// Center of mass
+	b2Assert(area > b2_epsilon);
+	center *= 1.0f / area;
+	massData->center = center;
+
+	// Inertia tensor relative to the local origin.
+	massData->I = density * I;
+}

src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h

@@ -0,0 +1,131 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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.
+class b2PolygonShape : public b2Shape
+{
+public:
+	b2PolygonShape();
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+	/// Copy vertices. This assumes the vertices define a convex polygon.
+	/// It is assumed that the exterior is the the right of each edge.
+	void Set(const b2Vec2* vertices, int32 vertexCount);
+
+	/// Build vertices to represent an axis-aligned box.
+	/// @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);
+
+	/// Set this as a single edge.
+	void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2);
+
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+
+	/// 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 { return m_vertexCount; }
+
+	/// Get a vertex by index.
+	const b2Vec2& GetVertex(int32 index) const;
+
+	b2Vec2 m_centroid;
+	b2Vec2 m_vertices[b2_maxPolygonVertices];
+	b2Vec2 m_normals[b2_maxPolygonVertices];
+	int32 m_vertexCount;
+};
+
+inline b2PolygonShape::b2PolygonShape()
+{
+	m_type = e_polygon;
+	m_radius = b2_polygonRadius;
+	m_vertexCount = 0;
+	m_centroid.SetZero();
+}
+
+inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_vertexCount; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return bestIndex;
+}
+
+inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const
+{
+	int32 bestIndex = 0;
+	float32 bestValue = b2Dot(m_vertices[0], d);
+	for (int32 i = 1; i < m_vertexCount; ++i)
+	{
+		float32 value = b2Dot(m_vertices[i], d);
+		if (value > bestValue)
+		{
+			bestIndex = i;
+			bestValue = value;
+		}
+	}
+
+	return m_vertices[bestIndex];
+}
+
+inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
+{
+	b2Assert(0 <= index && index < m_vertexCount);
+	return m_vertices[index];
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h

@@ -0,0 +1,95 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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.
+class b2Shape
+{
+public:
+	
+	enum Type
+	{
+		e_unknown= -1,
+		e_circle = 0,
+		e_polygon = 1,
+		e_typeCount = 2,
+	};
+
+	b2Shape() { m_type = e_unknown; }
+	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;
+
+	/// 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 this shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	/// @param transform the transform to be applied to the shape.
+	virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0;
+
+	/// Given a transform, compute the associated axis aligned bounding box for this shape.
+	/// @param aabb returns the axis aligned box.
+	/// @param xf the world transform of the shape.
+	virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) 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;
+	float32 m_radius;
+};
+
+inline b2Shape::Type b2Shape::GetType() const
+{
+	return m_type;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp

@@ -0,0 +1,116 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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>
+#include <cstring>
+
+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::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;
+			return;
+		}
+	}
+}
+
+// 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;
+}

src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h

@@ -0,0 +1,229 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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;
+	int32 next;
+};
+
+/// 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);
+
+	/// Get the fat AABB for a proxy.
+	const b2AABB& GetFatAABB(int32 proxyId) const;
+
+	/// Get user data from a proxy. Returns NULL 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;
+
+	/// Compute the height of the embedded tree.
+	int32 ComputeHeight() const;
+
+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::ComputeHeight() const
+{
+	return m_tree.ComputeHeight();
+}
+
+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);
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp

@@ -0,0 +1,154 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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_vertexCount;
+	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 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+		if (separation > 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;
+	}
+}

src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,59 +16,19 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2Collision.h"
-#include "Shapes/b2PolygonShape.h"
-
-struct ClipVertex
-{
-	b2Vec2 v;
-	b2ContactID id;
-};
-
-static int32 ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2],
-					  const b2Vec2& normal, float32 offset)
-{
-	// 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);
-		if (distance0 > 0.0f)
-		{
-			vOut[numOut].id = vIn[0].id;
-		}
-		else
-		{
-			vOut[numOut].id = vIn[1].id;
-		}
-		++numOut;
-	}
-
-	return numOut;
-}
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
 // Find the separation between poly1 and poly2 for a give edge normal on poly1.
-static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
-							  const b2PolygonShape* poly2, const b2XForm& xf2)
+static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							  const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->GetVertexCount();
-	const b2Vec2* vertices1 = poly1->GetVertices();
-	const b2Vec2* normals1 = poly1->GetNormals();
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* vertices1 = poly1->m_vertices;
+	const b2Vec2* normals1 = poly1->m_normals;
 
-	int32 count2 = poly2->GetVertexCount();
-	const b2Vec2* vertices2 = poly2->GetVertices();
+	int32 count2 = poly2->m_vertexCount;
+	const b2Vec2* vertices2 = poly2->m_vertices;
 
 	b2Assert(0 <= edge1 && edge1 < count1);
 
@@ -78,7 +38,7 @@ static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, i
 
 	// Find support vertex on poly2 for -normal.
 	int32 index = 0;
-	float32 minDot = B2_FLT_MAX;
+	float32 minDot = b2_maxFloat;
 
 	for (int32 i = 0; i < count2; ++i)
 	{
@@ -97,20 +57,20 @@ static float32 EdgeSeparation(const b2PolygonShape* poly1, const b2XForm& xf1, i
 }
 
 // Find the max separation between poly1 and poly2 using edge normals from poly1.
-static float32 FindMaxSeparation(int32* edgeIndex,
-								 const b2PolygonShape* poly1, const b2XForm& xf1,
-								 const b2PolygonShape* poly2, const b2XForm& xf2)
+static float32 b2FindMaxSeparation(int32* edgeIndex,
+								 const b2PolygonShape* poly1, const b2Transform& xf1,
+								 const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->GetVertexCount();
-	const b2Vec2* normals1 = poly1->GetNormals();
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* normals1 = poly1->m_normals;
 
 	// Vector pointing from the centroid of poly1 to the centroid of poly2.
-	b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
+	b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
 	b2Vec2 dLocal1 = b2MulT(xf1.R, d);
 
 	// Find edge normal on poly1 that has the largest projection onto d.
 	int32 edge = 0;
-	float32 maxDot = -B2_FLT_MAX;
+	float32 maxDot = -b2_maxFloat;
 	for (int32 i = 0; i < count1; ++i)
 	{
 		float32 dot = b2Dot(normals1[i], dLocal1);
@@ -122,27 +82,15 @@ static float32 FindMaxSeparation(int32* edgeIndex,
 	}
 
 	// Get the separation for the edge normal.
-	float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
-	if (s > 0.0f)
-	{
-		return s;
-	}
+	float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 
 	// Check the separation for the previous edge normal.
 	int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
-	float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
-	if (sPrev > 0.0f)
-	{
-		return sPrev;
-	}
+	float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
 
 	// Check the separation for the next edge normal.
 	int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
-	float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
-	if (sNext > 0.0f)
-	{
-		return sNext;
-	}
+	float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
 
 	// Find the best edge and the search direction.
 	int32 bestEdge;
@@ -174,11 +122,7 @@ static float32 FindMaxSeparation(int32* edgeIndex,
 		else
 			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
 
-		s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
-		if (s > 0.0f)
-		{
-			return s;
-		}
+		s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
 
 		if (s > bestSeparation)
 		{
@@ -195,16 +139,16 @@ static float32 FindMaxSeparation(int32* edgeIndex,
 	return bestSeparation;
 }
 
-static void FindIncidentEdge(ClipVertex c[2],
-							 const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
-							 const b2PolygonShape* poly2, const b2XForm& xf2)
+static void b2FindIncidentEdge(b2ClipVertex c[2],
+							 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+							 const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->GetVertexCount();
-	const b2Vec2* normals1 = poly1->GetNormals();
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* normals1 = poly1->m_normals;
 
-	int32 count2 = poly2->GetVertexCount();
-	const b2Vec2* vertices2 = poly2->GetVertices();
-	const b2Vec2* normals2 = poly2->GetNormals();
+	int32 count2 = poly2->m_vertexCount;
+	const b2Vec2* vertices2 = poly2->m_vertices;
+	const b2Vec2* normals2 = poly2->m_normals;
 
 	b2Assert(0 <= edge1 && edge1 < count1);
 
@@ -213,7 +157,7 @@ static void FindIncidentEdge(ClipVertex c[2],
 
 	// Find the incident edge on poly2.
 	int32 index = 0;
-	float32 minDot = B2_FLT_MAX;
+	float32 minDot = b2_maxFloat;
 	for (int32 i = 0; i < count2; ++i)
 	{
 		float32 dot = b2Dot(normal1, normals2[i]);
@@ -247,30 +191,30 @@ static void FindIncidentEdge(ClipVertex c[2],
 
 // The normal points from 1 to 2
 void b2CollidePolygons(b2Manifold* manifold,
-					  const b2PolygonShape* polyA, const b2XForm& xfA,
-					  const b2PolygonShape* polyB, const b2XForm& xfB)
+					  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 = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
-	if (separationA > 0.0f)
+	float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > totalRadius)
 		return;
 
 	int32 edgeB = 0;
-	float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
-	if (separationB > 0.0f)
+	float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > totalRadius)
 		return;
 
-	const b2PolygonShape* poly1;	// reference poly
-	const b2PolygonShape* poly2;	// incident poly
-	b2XForm xf1, xf2;
+	const b2PolygonShape* poly1;	// reference polygon
+	const b2PolygonShape* poly2;	// incident polygon
+	b2Transform xf1, xf2;
 	int32 edge1;		// reference edge
 	uint8 flip;
 	const float32 k_relativeTol = 0.98f;
 	const float32 k_absoluteTol = 0.001f;
 
-	// TODO_ERIN use "radius" of poly for absolute tolerance.
 	if (separationB > k_relativeTol * separationA + k_absoluteTol)
 	{
 		poly1 = polyB;
@@ -278,6 +222,7 @@ void b2CollidePolygons(b2Manifold* manifold,
 		xf1 = xfB;
 		xf2 = xfA;
 		edge1 = edgeB;
+		manifold->type = b2Manifold::e_faceB;
 		flip = 1;
 	}
 	else
@@ -287,61 +232,70 @@ void b2CollidePolygons(b2Manifold* manifold,
 		xf1 = xfA;
 		xf2 = xfB;
 		edge1 = edgeA;
+		manifold->type = b2Manifold::e_faceA;
 		flip = 0;
 	}
 
-	ClipVertex incidentEdge[2];
-	FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+	b2ClipVertex incidentEdge[2];
+	b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
 
-	int32 count1 = poly1->GetVertexCount();
-	const b2Vec2* vertices1 = poly1->GetVertices();
+	int32 count1 = poly1->m_vertexCount;
+	const b2Vec2* vertices1 = poly1->m_vertices;
 
 	b2Vec2 v11 = vertices1[edge1];
 	b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
 
-	b2Vec2 dv = v12 - v11;
-	b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
-	sideNormal.Normalize();
-	b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
+	b2Vec2 localTangent = v12 - v11;
+	localTangent.Normalize();
+	
+	b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
+	b2Vec2 planePoint = 0.5f * (v11 + v12);
+
+	b2Vec2 tangent = b2Mul(xf1.R, localTangent);
+	b2Vec2 normal = b2Cross(tangent, 1.0f);
 	
 	v11 = b2Mul(xf1, v11);
 	v12 = b2Mul(xf1, v12);
 
-	float32 frontOffset = b2Dot(frontNormal, v11);
-	float32 sideOffset1 = -b2Dot(sideNormal, v11);
-	float32 sideOffset2 = b2Dot(sideNormal, 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.
-	ClipVertex clipPoints1[2];
-	ClipVertex clipPoints2[2];
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
 	int np;
 
 	// Clip to box side 1
-	np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
 
 	if (np < 2)
 		return;
 
 	// Clip to negative box side 1
-	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2);
 
 	if (np < 2)
+	{
 		return;
+	}
 
 	// Now clipPoints2 contains the clipped points.
-	manifold->normal = flip ? -frontNormal : frontNormal;
+	manifold->localNormal = localNormal;
+	manifold->localPoint = planePoint;
 
 	int32 pointCount = 0;
 	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
 	{
-		float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+		float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
 
-		if (separation <= 0.0f)
+		if (separation <= totalRadius)
 		{
 			b2ManifoldPoint* cp = manifold->points + pointCount;
-			cp->separation = separation;
-			cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
-			cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+			cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
 			cp->id = clipPoints2[i].id;
 			cp->id.features.flip = flip;
 			++pointCount;

src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp

@@ -0,0 +1,250 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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);
+		}
+		break;
+
+	case b2Manifold::e_faceA:
+		{
+			normal = b2Mul(xfA.R, 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);
+			}
+		}
+		break;
+
+	case b2Manifold::e_faceB:
+		{
+			normal = b2Mul(xfB.R, 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);
+			}
+
+			// 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)
+{
+	// 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);
+		if (distance0 > 0.0f)
+		{
+			vOut[numOut].id = vIn[0].id;
+		}
+		else
+		{
+			vOut[numOut].id = vIn[1].id;
+		}
+		++numOut;
+	}
+
+	return numOut;
+}
+
+bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
+				   const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2DistanceInput input;
+	input.proxyA.Set(shapeA);
+	input.proxyB.Set(shapeB);
+	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;
+}

src/modules/physics/box2d/Box2D/Collision/b2Collision.h

@@ -0,0 +1,240 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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 <climits>
+
+/// @file
+/// Structures and functions used for computing contact points, distance
+/// queries, and TOI queries.
+
+class b2Shape;
+class b2CircleShape;
+class b2PolygonShape;
+
+const uint8 b2_nullFeature = UCHAR_MAX;
+
+/// Contact ids to facilitate warm starting.
+union b2ContactID
+{
+	/// The features that intersect to form the contact point
+	struct Features
+	{
+		uint8 referenceEdge;	///< The edge that defines the outward contact normal.
+		uint8 incidentEdge;		///< The edge most anti-parallel to the reference edge.
+		uint8 incidentVertex;	///< The vertex (0 or 1) on the incident edge that was clipped.
+		uint8 flip;				///< A value of 1 indicates that the reference edge is on shape2.
+	} features;
+	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)
+};
+
+/// 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);
+	}
+
+	/// 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* circle1, const b2Transform& xf1,
+					  const b2CircleShape* circle2, const b2Transform& xf2);
+
+/// Compute the collision manifold between a polygon and a circle.
+void b2CollidePolygonAndCircle(b2Manifold* manifold,
+							   const b2PolygonShape* polygon, const b2Transform& xf1,
+							   const b2CircleShape* circle, const b2Transform& xf2);
+
+/// Compute the collision manifold between two polygons.
+void b2CollidePolygons(b2Manifold* manifold,
+					   const b2PolygonShape* polygon1, const b2Transform& xf1,
+					   const b2PolygonShape* polygon2, const b2Transform& xf2);
+
+/// Clipping for contact manifolds.
+int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
+							const b2Vec2& normal, float32 offset);
+
+/// Determine if two generic shapes overlap.
+bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
+				   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

src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp

@@ -0,0 +1,571 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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/b2PolygonShape.h>
+
+// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
+int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
+
+void b2DistanceProxy::Set(const b2Shape* shape)
+{
+	switch (shape->GetType())
+	{
+	case b2Shape::e_circle:
+		{
+			const b2CircleShape* circle = (b2CircleShape*)shape;
+			m_vertices = &circle->m_p;
+			m_count = 1;
+			m_radius = circle->m_radius;
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			const b2PolygonShape* polygon = (b2PolygonShape*)shape;
+			m_vertices = polygon->m_vertices;
+			m_count = polygon->m_vertexCount;
+			m_radius = polygon->m_radius;
+		}
+		break;
+
+	default:
+		b2Assert(false);
+	}
+}
+
+
+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;
+			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.0;
+
+		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;
+
+	b2Vec2 closestPoint = simplex.GetClosestPoint();
+	float32 distanceSqr1 = closestPoint.LengthSquared();
+	float32 distanceSqr2 = distanceSqr1;
+
+	// 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;
+		}
+
+		// Compute closest point.
+		b2Vec2 p = simplex.GetClosestPoint();
+		distanceSqr2 = p.LengthSquared();
+
+		// Ensure progress
+		if (distanceSqr2 >= distanceSqr1)
+		{
+			//break;
+		}
+		distanceSqr1 = distanceSqr2;
+
+		// 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.R, -d));
+		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
+		b2Vec2 wBLocal;
+		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, 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;
+		}
+	}
+}

src/modules/physics/box2d/Box2D/Collision/b2Distance.h

@@ -0,0 +1,141 @@
+
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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>
+#include <climits>
+
+class b2Shape;
+
+/// A distance proxy is used by the GJK algorithm.
+/// It encapsulates any shape.
+struct b2DistanceProxy
+{
+	b2DistanceProxy() : m_vertices(NULL), 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);
+
+	/// 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;
+
+	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);
+
+
+//////////////////////////////////////////////////////////////////////////
+
+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

src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp

@@ -0,0 +1,365 @@
+/*
+* Copyright (c) 2009 Erin Catto http://www.gphysics.com
+*
+* 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 <cstring>
+#include <cfloat>
+
+b2DynamicTree::b2DynamicTree()
+{
+	m_root = b2_nullNode;
+
+	m_nodeCapacity = 16;
+	m_nodeCount = 0;
+	m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
+
+	// 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[m_nodeCapacity-1].next = b2_nullNode;
+	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.
+		b2DynamicTreeNode* oldNodes = m_nodes;
+		m_nodeCapacity *= 2;
+		m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
+		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[m_nodeCapacity-1].next = b2_nullNode;
+		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_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_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;
+
+	InsertLeaf(proxyId);
+
+	// Rebalance if necessary.
+	int32 iterationCount = m_nodeCount >> 4;
+	int32 tryCount = 0;
+	int32 height = ComputeHeight();
+	while (height > 64 && tryCount < 10)
+	{
+		Rebalance(iterationCount);
+		height = ComputeHeight();
+		++tryCount;
+	}
+
+	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.
+	b2Vec2 center = m_nodes[leaf].aabb.GetCenter();
+	int32 sibling = m_root;
+	if (m_nodes[sibling].IsLeaf() == false)
+	{
+		do 
+		{
+			int32 child1 = m_nodes[sibling].child1;
+			int32 child2 = m_nodes[sibling].child2;
+
+			b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);
+			b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);
+
+			float32 norm1 = delta1.x + delta1.y;
+			float32 norm2 = delta2.x + delta2.y;
+
+			if (norm1 < norm2)
+			{
+				sibling = child1;
+			}
+			else
+			{
+				sibling = child2;
+			}
+
+		}
+		while(m_nodes[sibling].IsLeaf() == false);
+	}
+
+	// Create a parent for the siblings.
+	int32 node1 = m_nodes[sibling].parent;
+	int32 node2 = AllocateNode();
+	m_nodes[node2].parent = node1;
+	m_nodes[node2].userData = NULL;
+	m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb);
+
+	if (node1 != b2_nullNode)
+	{
+		if (m_nodes[m_nodes[sibling].parent].child1 == sibling)
+		{
+			m_nodes[node1].child1 = node2;
+		}
+		else
+		{
+			m_nodes[node1].child2 = node2;
+		}
+
+		m_nodes[node2].child1 = sibling;
+		m_nodes[node2].child2 = leaf;
+		m_nodes[sibling].parent = node2;
+		m_nodes[leaf].parent = node2;
+
+		do 
+		{
+			if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb))
+			{
+				break;
+			}
+
+			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
+			node2 = node1;
+			node1 = m_nodes[node1].parent;
+		}
+		while(node1 != b2_nullNode);
+	}
+	else
+	{
+		m_nodes[node2].child1 = sibling;
+		m_nodes[node2].child2 = leaf;
+		m_nodes[sibling].parent = node2;
+		m_nodes[leaf].parent = node2;
+		m_root = node2;
+	}
+}
+
+void b2DynamicTree::RemoveLeaf(int32 leaf)
+{
+	if (leaf == m_root)
+	{
+		m_root = b2_nullNode;
+		return;
+	}
+
+	int32 node2 = m_nodes[leaf].parent;
+	int32 node1 = m_nodes[node2].parent;
+	int32 sibling;
+	if (m_nodes[node2].child1 == leaf)
+	{
+		sibling = m_nodes[node2].child2;
+	}
+	else
+	{
+		sibling = m_nodes[node2].child1;
+	}
+
+	if (node1 != b2_nullNode)
+	{
+		// Destroy node2 and connect node1 to sibling.
+		if (m_nodes[node1].child1 == node2)
+		{
+			m_nodes[node1].child1 = sibling;
+		}
+		else
+		{
+			m_nodes[node1].child2 = sibling;
+		}
+		m_nodes[sibling].parent = node1;
+		FreeNode(node2);
+
+		// Adjust ancestor bounds.
+		while (node1 != b2_nullNode)
+		{
+			b2AABB oldAABB = m_nodes[node1].aabb;
+			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
+
+			if (oldAABB.Contains(m_nodes[node1].aabb))
+			{
+				break;
+			}
+
+			node1 = m_nodes[node1].parent;
+		}
+	}
+	else
+	{
+		m_root = sibling;
+		m_nodes[sibling].parent = b2_nullNode;
+		FreeNode(node2);
+	}
+}
+
+void b2DynamicTree::Rebalance(int32 iterations)
+{
+	if (m_root == b2_nullNode)
+	{
+		return;
+	}
+
+	for (int32 i = 0; i < iterations; ++i)
+	{
+		int32 node = m_root;
+
+		uint32 bit = 0;
+		while (m_nodes[node].IsLeaf() == false)
+		{
+			int32* children = &m_nodes[node].child1;
+			node = children[(m_path >> bit) & 1];
+			bit = (bit + 1) & (8* sizeof(uint32) - 1);
+		}
+		++m_path;
+
+		RemoveLeaf(node);
+		InsertLeaf(node);
+	}
+}
+
+// Compute the height of a sub-tree.
+int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
+{
+	if (nodeId == b2_nullNode)
+	{
+		return 0;
+	}
+
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2DynamicTreeNode* node = m_nodes + nodeId;
+	int32 height1 = ComputeHeight(node->child1);
+	int32 height2 = ComputeHeight(node->child2);
+	return 1 + b2Max(height1, height2);
+}
+
+int32 b2DynamicTree::ComputeHeight() const
+{
+	return ComputeHeight(m_root);
+}

src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h

@@ -0,0 +1,286 @@
+/*
+* Copyright (c) 2009 Erin Catto http://www.gphysics.com
+*
+* 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>
+
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+
+#define b2_nullNode (-1)
+
+/// A node in the dynamic tree. The client does not interact with this directly.
+struct b2DynamicTreeNode
+{
+	bool IsLeaf() const
+	{
+		return child1 == b2_nullNode;
+	}
+
+	/// This is the fattened AABB.
+	b2AABB aabb;
+
+	//int32 userData;
+	void* userData;
+
+	union
+	{
+		int32 parent;
+		int32 next;
+	};
+
+	int32 child1;
+	int32 child2;
+};
+
+/// 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);
+
+	/// Perform some iterations to re-balance the tree.
+	void Rebalance(int32 iterations);
+
+	/// 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;
+
+	/// Compute the height of the tree.
+	int32 ComputeHeight() 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;
+
+private:
+
+	int32 AllocateNode();
+	void FreeNode(int32 node);
+
+	void InsertLeaf(int32 node);
+	void RemoveLeaf(int32 node);
+
+	int32 ComputeHeight(int32 nodeId) const;
+
+	int32 m_root;
+
+	b2DynamicTreeNode* m_nodes;
+	int32 m_nodeCount;
+	int32 m_nodeCapacity;
+
+	int32 m_freeList;
+
+	/// This is used 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
+{
+	const int32 k_stackSize = 128;
+	int32 stack[k_stackSize];
+
+	int32 count = 0;
+	stack[count++] = m_root;
+
+	while (count > 0)
+	{
+		int32 nodeId = stack[--count];
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2DynamicTreeNode* node = m_nodes + nodeId;
+
+		if (b2TestOverlap(node->aabb, aabb))
+		{
+			if (node->IsLeaf())
+			{
+				bool proceed = callback->QueryCallback(nodeId);
+				if (proceed == false)
+				{
+					return;
+				}
+			}
+			else
+			{
+				if (count < k_stackSize)
+				{
+					stack[count++] = node->child1;
+				}
+
+				if (count < k_stackSize)
+				{
+					stack[count++] = 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);
+	}
+
+	const int32 k_stackSize = 128;
+	int32 stack[k_stackSize];
+
+	int32 count = 0;
+	stack[count++] = m_root;
+
+	while (count > 0)
+	{
+		int32 nodeId = stack[--count];
+		if (nodeId == b2_nullNode)
+		{
+			continue;
+		}
+
+		const b2DynamicTreeNode* 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
+		{
+			if (count < k_stackSize)
+			{
+				stack[count++] = node->child1;
+			}
+
+			if (count < k_stackSize)
+			{
+				stack[count++] = node->child2;
+			}
+		}
+	}
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp

@@ -0,0 +1,483 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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 <cstdio>
+
+int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
+int32 b2_toiRootIters, b2_toiMaxRootIters;
+
+int32 b2_toiMaxOptIters;
+
+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)
+	{
+		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, 0.0f);
+		m_sweepB.GetTransform(&xfB, 0.0f);
+
+		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.R, 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.R, 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.R,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.R, -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.R, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.R, -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 axisA = b2MulT(xfA.R,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
+
+				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.R, m_axis);
+				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+
+				b2Vec2 axisB = b2MulT(xfB.R, -normal);
+
+				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.R, m_axis);
+				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+
+				b2Vec2 axisA = b2MulT(xfA.R, -normal);
+
+				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)
+{
+	++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);
+#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);
+				}
+
+				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;
+				}
+
+				++rootIterCount;
+				++b2_toiRootIters;
+
+				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);
+}

src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h

@@ -0,0 +1,59 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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>
+#include <climits>
+
+/// 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 collision. 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

src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,11 +16,11 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2BlockAllocator.h"
+#include <Box2D/Common/b2BlockAllocator.h>
 #include <cstdlib>
-#include <memory>
 #include <climits>
-#include <string.h>
+#include <cstring>
+#include <memory>
 
 int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
 {
@@ -164,14 +164,13 @@ void b2BlockAllocator::Free(void* p, int32 size)
 	// Verify the memory address and size is valid.
 	int32 blockSize = s_blockSizes[index];
 	bool found = false;
-	int32 gap = (int32)((int8*)&m_chunks->blocks - (int8*)m_chunks);
 	for (int32 i = 0; i < m_chunkCount; ++i)
 	{
 		b2Chunk* chunk = m_chunks + i;
 		if (chunk->blockSize != blockSize)
 		{
 			b2Assert(	(int8*)p + blockSize <= (int8*)chunk->blocks ||
-						(int8*)chunk->blocks + b2_chunkSize + gap <= (int8*)p);
+						(int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
 		}
 		else
 		{

src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,7 +19,7 @@
 #ifndef B2_BLOCK_ALLOCATOR_H
 #define B2_BLOCK_ALLOCATOR_H
 
-#include "b2Settings.h"
+#include <Box2D/Common/b2Settings.h>
 
 const int32 b2_chunkSize = 4096;
 const int32 b2_maxBlockSize = 640;

src/modules/physics/box2d/Box2D/Common/b2Math.cpp

@@ -0,0 +1,55 @@
+/*
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Common/b2Math.h>
+
+const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
+const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f);
+const b2Transform b2Transform_identity(b2Vec2_zero, b2Mat22_identity);
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
+{
+	float32 det = b2Dot(col1, b2Cross(col2, col3));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	b2Vec3 x;
+	x.x = det * b2Dot(b, b2Cross(col2, col3));
+	x.y = det * b2Dot(col1, b2Cross(b, col3));
+	x.z = det * b2Dot(col1, b2Cross(col2, b));
+	return x;
+}
+
+/// Solve A * x = b, where b is a column vector. This is more efficient
+/// than computing the inverse in one-shot cases.
+b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
+{
+	float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+	float32 det = a11 * a22 - a12 * a21;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+	b2Vec2 x;
+	x.x = det * (a22 * b.x - a12 * b.y);
+	x.y = det * (a11 * b.y - a21 * b.x);
+	return x;
+}

src/modules/physics/box2d/Box2D/Common/b2Math.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,49 +19,25 @@
 #ifndef B2_MATH_H
 #define B2_MATH_H
 
-#include "b2Settings.h"
+#include <Box2D/Common/b2Settings.h>
+
 #include <cmath>
 #include <cfloat>
-#include <cstdlib>
-
-#include <stdio.h>
-
-#ifdef TARGET_FLOAT32_IS_FIXED
-
-inline Fixed b2Min(const Fixed& a, const Fixed& b)
-{
-  return a < b ? a : b;
-}
-
-inline Fixed b2Max(const Fixed& a, const Fixed& b)
-{
-  return a > b ? a : b;
-}
-
-inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
-{
-	return b2Max(low, b2Min(a, high));
-}
-
-inline bool b2IsValid(Fixed x)
-{
-	return true;
-}
-
-#define	b2Sqrt(x)	sqrt(x)
-#define	b2Atan2(y, x)	atan2(y, x)
-
-#else
+#include <cstddef>
+#include <limits>
 
 /// This function is used to ensure that a floating point number is
 /// not a NaN or infinity.
 inline bool b2IsValid(float32 x)
 {
-#ifdef _MSC_VER
-	return _finite(x) != 0;
-#else
-	return finite(x) != 0;
-#endif
+	if (x != x)
+	{
+		// NaN.
+		return false;
+	}
+
+	float32 infinity = std::numeric_limits<float32>::infinity();
+	return -infinity < x && x < infinity;
 }
 
 /// This is a approximate yet fast inverse square-root.
@@ -84,15 +60,12 @@ inline float32 b2InvSqrt(float32 x)
 #define	b2Sqrt(x)	sqrtf(x)
 #define	b2Atan2(y, x)	atan2f(y, x)
 
-#endif
-
 inline float32 b2Abs(float32 a)
 {
 	return a > 0.0f ? a : -a;
 }
 
 /// A 2D column vector.
-
 struct b2Vec2
 {
 	/// Default constructor does nothing (for performance).
@@ -110,6 +83,18 @@ struct b2Vec2
 	/// Negate this vector.
 	b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
 	
+	/// Read from and indexed element.
+	float32 operator () (int32 i) const
+	{
+		return (&x)[i];
+	}
+
+	/// Write to an indexed element.
+	float32& operator () (int32 i)
+	{
+		return (&x)[i];
+	}
+
 	/// Add a vector to this vector.
 	void operator += (const b2Vec2& v)
 	{
@@ -131,20 +116,7 @@ struct b2Vec2
 	/// Get the length of this vector (the norm).
 	float32 Length() const
 	{
-#ifdef TARGET_FLOAT32_IS_FIXED
-		float est = b2Abs(x) + b2Abs(y);
-		if(est == 0.0f) {
-			return 0.0;
-		} else if(est < 0.1) {
-			return (1.0/256.0) * b2Vec2(x<<8, y<<8).Length();
-		} else if(est < 180.0f) {
-			return b2Sqrt(x * x + y * y);
-		} else {
-			return 256.0 * (b2Vec2(x>>8, y>>8).Length());
-		}
-#else
 		return b2Sqrt(x * x + y * y);
-#endif 
 	}
 
 	/// Get the length squared. For performance, use this instead of
@@ -155,38 +127,10 @@ struct b2Vec2
 	}
 
 	/// Convert this vector into a unit vector. Returns the length.
-#ifdef TARGET_FLOAT32_IS_FIXED
 	float32 Normalize()
 	{
 		float32 length = Length();
-		if (length < B2_FLT_EPSILON)
-		{
-			return 0.0f;
-		} 
-#ifdef NORMALIZE_BY_INVERT_MULTIPLY
-		if (length < (1.0/16.0)) {
-			x = x << 4;
-			y = y << 4;
-			return (1.0/16.0)*Normalize();
-		} else if(length > 16.0) {
-			x = x >> 4;
-			y = y >> 4;
-			return 16.0*Normalize();
-		}
-		float32 invLength = 1.0f / length;
-		x *= invLength;
-		y *= invLength;
-#else
-		x /= length;
-		y /= length;
-#endif
-		return length;
-	}
-#else
-	float32 Normalize()
-	{
-		float32 length = Length();
-		if (length < B2_FLT_EPSILON)
+		if (length < b2_epsilon)
 		{
 			return 0.0f;
 		}
@@ -196,7 +140,6 @@ struct b2Vec2
 
 		return length;
 	}
-#endif
 
 	/// Does this vector contain finite coordinates?
 	bool IsValid() const
@@ -207,6 +150,45 @@ struct b2Vec2
 	float32 x, y;
 };
 
+/// A 2D column vector with 3 elements.
+struct b2Vec3
+{
+	/// Default constructor does nothing (for performance).
+	b2Vec3() {}
+
+	/// Construct using coordinates.
+	b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
+
+	/// Set this vector to all zeros.
+	void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
+
+	/// Set this vector to some specified coordinates.
+	void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
+
+	/// Negate this vector.
+	b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
+
+	/// Add a vector to this vector.
+	void operator += (const b2Vec3& v)
+	{
+		x += v.x; y += v.y; z += v.z;
+	}
+
+	/// Subtract a vector from this vector.
+	void operator -= (const b2Vec3& v)
+	{
+		x -= v.x; y -= v.y; z -= v.z;
+	}
+
+	/// Multiply this vector by a scalar.
+	void operator *= (float32 s)
+	{
+		x *= s; y *= s; z *= s;
+	}
+
+	float32 x, y, z;
+};
+
 /// A 2-by-2 matrix. Stored in column-major order.
 struct b2Mat22
 {
@@ -231,6 +213,7 @@ struct b2Mat22
 	/// an orthonormal rotation matrix.
 	explicit b2Mat22(float32 angle)
 	{
+		// TODO_ERIN compute sin+cos together.
 		float32 c = cosf(angle), s = sinf(angle);
 		col1.x = c; col2.x = -s;
 		col1.y = s; col2.y = c;
@@ -273,79 +256,15 @@ struct b2Mat22
 		return b2Atan2(col1.y, col1.x);
 	}
 
-#ifdef TARGET_FLOAT32_IS_FIXED
-
-	/// Compute the inverse of this matrix, such that inv(A) * A = identity.
-	b2Mat22 Invert() const
-	{
-		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
-		float32 det = a * d - b * c;
-		b2Mat22 B;
-		int n = 0;
-
-		if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
-		{
-			n = 3;
-			a = a<<n; b = b<<n; 
-			c = c<<n; d = d<<n;
-			det = a * d - b * c;
-			b2Assert(det != 0.0f);
-			det = float32(1) / det;
-			B.col1.x = ( det * d) << n;	B.col2.x = (-det * b) << n;
-			B.col1.y = (-det * c) << n;	B.col2.y = ( det * a) << n;
-		} 
-		else
-		{
-			n = (b2Abs(det) >= 16.0)? 4 : 0;
-			b2Assert(det != 0.0f);
-			det = float32(1<<n) / det;
-			B.col1.x = ( det * d) >> n;	B.col2.x = (-det * b) >> n;
-			B.col1.y = (-det * c) >> n;	B.col2.y = ( det * a) >> n;
-		}
-		
-		return B;
-	}
-
-	// Solve A * x = b
-	b2Vec2 Solve(const b2Vec2& b) const
-	{
-		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
-		float32 det = a11 * a22 - a12 * a21;
-		int n = 0;
-		b2Vec2 x;
-
-		
-		if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
-		{
-			n = 3;
-			a11 = col1.x<<n; a12 = col2.x<<n;
-			a21 = col1.y<<n; a22 = col2.y<<n;
-			det = a11 * a22 - a12 * a21;
-			b2Assert(det != 0.0f);
-			det = float32(1) / det;
-			x.x = (det * (a22 * b.x - a12 * b.y)) << n;
-			x.y = (det * (a11 * b.y - a21 * b.x)) << n;
-		} 
-		else 
-		{
-			n = (b2Abs(det) >= 16.0) ? 4 : 0;
-			b2Assert(det != 0.0f);
-			det = float32(1<<n) / det;
-			x.x = (det * (a22 * b.x - a12 * b.y)) >> n;
-			x.y = (det * (a11 * b.y - a21 * b.x)) >> n;
-		}
-
-		return x;
-	}
-
-#else
-	b2Mat22 Invert() const
+	b2Mat22 GetInverse() const
 	{
 		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
 		b2Mat22 B;
 		float32 det = a * d - b * c;
-		b2Assert(det != 0.0f);
-		det = float32(1.0f) / det;
+		if (det != 0.0f)
+		{
+			det = 1.0f / det;
+		}
 		B.col1.x =  det * d;	B.col2.x = -det * b;
 		B.col1.y = -det * c;	B.col2.y =  det * a;
 		return B;
@@ -357,27 +276,62 @@ struct b2Mat22
 	{
 		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
 		float32 det = a11 * a22 - a12 * a21;
-		b2Assert(det != 0.0f);
-		det = 1.0f / det;
+		if (det != 0.0f)
+		{
+			det = 1.0f / det;
+		}
 		b2Vec2 x;
 		x.x = det * (a22 * b.x - a12 * b.y);
 		x.y = det * (a11 * b.y - a21 * b.x);
 		return x;
 	}
-#endif
 
 	b2Vec2 col1, col2;
 };
 
-/// A transform contains translation and rotation. It is used to represent
-/// the position and orientation of rigid frames.
-struct b2XForm
+/// A 3-by-3 matrix. Stored in column-major order.
+struct b2Mat33
 {
 	/// The default constructor does nothing (for performance).
-	b2XForm() {}
+	b2Mat33() {}
+
+	/// Construct this matrix using columns.
+	b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
+	{
+		col1 = c1;
+		col2 = c2;
+		col3 = c3;
+	}
+
+	/// Set this matrix to all zeros.
+	void SetZero()
+	{
+		col1.SetZero();
+		col2.SetZero();
+		col3.SetZero();
+	}
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases.
+	b2Vec3 Solve33(const b2Vec3& b) const;
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases. Solve only the upper
+	/// 2-by-2 matrix equation.
+	b2Vec2 Solve22(const b2Vec2& b) const;
+
+	b2Vec3 col1, col2, col3;
+};
+
+/// A transform contains translation and rotation. It is used to represent
+/// the position and orientation of rigid frames.
+struct b2Transform
+{
+	/// The default constructor does nothing (for performance).
+	b2Transform() {}
 
 	/// Initialize using a position vector and a rotation matrix.
-	b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
+	b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
 
 	/// Set this to the identity transform.
 	void SetIdentity()
@@ -386,6 +340,19 @@ struct b2XForm
 		R.SetIdentity();
 	}
 
+	/// Set this based on the position and angle.
+	void Set(const b2Vec2& p, float32 angle)
+	{
+		position = p;
+		R.Set(angle);
+	}
+
+	/// Calculate the angle that the rotation matrix represents.
+	float32 GetAngle() const
+	{
+		return b2Atan2(R.col1.y, R.col1.x);
+	}
+
 	b2Vec2 position;
 	b2Mat22 R;
 };
@@ -397,25 +364,27 @@ struct b2XForm
 struct b2Sweep
 {
 	/// Get the interpolated transform at a specific time.
-	/// @param t the normalized time in [0,1].
-	void GetXForm(b2XForm* xf, float32 t) const;
+	/// @param alpha is a factor in [0,1], where 0 indicates t0.
+	void GetTransform(b2Transform* xf, float32 alpha) const;
 
 	/// Advance the sweep forward, yielding a new initial state.
 	/// @param t the new initial time.
 	void Advance(float32 t);
 
+	/// Normalize the angles.
+	void Normalize();
+
 	b2Vec2 localCenter;	///< local center of mass position
 	b2Vec2 c0, c;		///< center world positions
 	float32 a0, a;		///< world angles
-	float32 t0;			///< time interval = [t0,1], where t0 is in [0,1]
 };
 
 
 extern const b2Vec2 b2Vec2_zero;
 extern const b2Mat22 b2Mat22_identity;
-extern const b2XForm b2XForm_identity;
+extern const b2Transform b2Transform_identity;
 
-/// Peform the dot product on two vectors.
+/// Perform the dot product on two vectors.
 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
 {
 	return a.x * b.x + a.y * b.y;
@@ -431,54 +400,45 @@ inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
 /// a vector.
 inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
 {
-	b2Vec2 v; v.Set(s * a.y, -s * a.x);
-	return v;
+	return b2Vec2(s * a.y, -s * a.x);
 }
 
 /// Perform the cross product on a scalar and a vector. In 2D this produces
 /// a vector.
 inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
 {
-	b2Vec2 v; v.Set(-s * a.y, s * a.x);
-	return v;
+	return b2Vec2(-s * a.y, s * a.x);
 }
 
 /// Multiply a matrix times a vector. If a rotation matrix is provided,
 /// then this transforms the vector from one frame to another.
 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
 {
-	b2Vec2 u;
-	u.Set(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
-	return u;
+	return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
 }
 
 /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
 /// then this transforms the vector from one frame to another (inverse transform).
 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
 {
-	b2Vec2 u;
-	u.Set(b2Dot(v, A.col1), b2Dot(v, A.col2));
-	return u;
+	return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
 }
 
 /// Add two vectors component-wise.
 inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
 {
-	b2Vec2 v; v.Set(a.x + b.x, a.y + b.y);
-	return v;
+	return b2Vec2(a.x + b.x, a.y + b.y);
 }
 
 /// Subtract two vectors component-wise.
 inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
 {
-	b2Vec2 v; v.Set(a.x - b.x, a.y - b.y);
-	return v;
+	return b2Vec2(a.x - b.x, a.y - b.y);
 }
 
 inline b2Vec2 operator * (float32 s, const b2Vec2& a)
 {
-	b2Vec2 v; v.Set(s * a.x, s * a.y);
-	return v;
+	return b2Vec2(s * a.x, s * a.y);
 }
 
 inline bool operator == (const b2Vec2& a, const b2Vec2& b)
@@ -498,52 +458,81 @@ inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
 	return b2Dot(c, c);
 }
 
+inline b2Vec3 operator * (float32 s, const b2Vec3& a)
+{
+	return b2Vec3(s * a.x, s * a.y, s * a.z);
+}
+
+/// Add two vectors component-wise.
+inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
+}
+
+/// Subtract two vectors component-wise.
+inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
+}
+
+/// Perform the dot product on two vectors.
+inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
+{
+	return a.x * b.x + a.y * b.y + a.z * b.z;
+}
+
+/// Perform the cross product on two vectors.
+inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
+{
+	return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
+}
+
 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
 {
-	b2Mat22 C;
-	C.Set(A.col1 + B.col1, A.col2 + B.col2);
-	return C;
+	return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
 }
 
 // A * B
 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
 {
-	b2Mat22 C;
-	C.Set(b2Mul(A, B.col1), b2Mul(A, B.col2));
-	return C;
+	return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
 }
 
 // A^T * B
 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
 {
-	b2Vec2 c1; c1.Set(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
-	b2Vec2 c2; c2.Set(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
-	b2Mat22 C;
-	C.Set(c1, c2);
-	return C;
+	b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
+	b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
+	return b2Mat22(c1, c2);
 }
 
-inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
+/// Multiply a matrix times a vector.
+inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
 {
-	return T.position + b2Mul(T.R, v);
+	return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
 }
 
-inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& v)
+inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
+{
+	float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
+	float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
+
+	return b2Vec2(x, y);
+}
+
+inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
 {
 	return b2MulT(T.R, v - T.position);
 }
 
 inline b2Vec2 b2Abs(const b2Vec2& a)
 {
-	b2Vec2 b; b.Set(b2Abs(a.x), b2Abs(a.y));
-	return b;
+	return b2Vec2(b2Abs(a.x), b2Abs(a.y));
 }
 
 inline b2Mat22 b2Abs(const b2Mat22& A)
 {
-	b2Mat22 B;
-	B.Set(b2Abs(A.col1), b2Abs(A.col2));
-	return B;
+	return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
 }
 
 template <typename T>
@@ -554,10 +543,7 @@ inline T b2Min(T a, T b)
 
 inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
 {
-	b2Vec2 c;
-	c.x = b2Min(a.x, b.x);
-	c.y = b2Min(a.y, b.y);
-	return c;
+	return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
 }
 
 template <typename T>
@@ -568,10 +554,7 @@ inline T b2Max(T a, T b)
 
 inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
 {
-	b2Vec2 c;
-	c.x = b2Max(a.x, b.x);
-	c.y = b2Max(a.y, b.y);
-	return c;
+	return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
 }
 
 template <typename T>
@@ -592,26 +575,6 @@ template<typename T> inline void b2Swap(T& a, T& b)
 	b = tmp;
 }
 
-#define	RAND_LIMIT	32767
-
-// Random number in range [-1,1]
-inline float32 b2Random()
-{
-	float32 r = (float32)(rand() & (RAND_LIMIT));
-	r /= RAND_LIMIT;
-	r = 2.0f * r - 1.0f;
-	return r;
-}
-
-/// Random floating point number in range [lo, hi]
-inline float32 b2Random(float32 lo, float32 hi)
-{
-	float32 r = (float32)(rand() & (RAND_LIMIT));
-	r /= RAND_LIMIT;
-	r = (hi - lo) * r + lo;
-	return r;
-}
-
 /// "Next Largest Power of 2
 /// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
 /// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
@@ -633,4 +596,29 @@ inline bool b2IsPowerOfTwo(uint32 x)
 	return result;
 }
 
+inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
+{
+	xf->position = (1.0f - alpha) * c0 + alpha * c;
+	float32 angle = (1.0f - alpha) * a0 + alpha * a;
+	xf->R.Set(angle);
+
+	// Shift to origin
+	xf->position -= b2Mul(xf->R, localCenter);
+}
+
+inline void b2Sweep::Advance(float32 t)
+{
+	c0 = (1.0f - t) * c0 + t * c;
+	a0 = (1.0f - t) * a0 + t * a;
+}
+
+/// Normalize an angle in radians to be between -pi and pi
+inline void b2Sweep::Normalize()
+{
+	float32 twoPi = 2.0f * b2_pi;
+	float32 d =  twoPi * floorf(a0 / twoPi);
+	a0 -= d;
+	a -= d;
+}
+
 #endif

src/modules/physics/box2d/Box2D/Common/b2Settings.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,18 +16,18 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef B2_NULL_CONTACT_H
-#define B2_NULL_CONTACT_H
+#include <Box2D/Common/b2Settings.h>
+#include <cstdlib>
 
-#include "../../Common/b2Math.h"
-#include "b2Contact.h"
+b2Version b2_version = {2, 1, 2};
 
-class b2NullContact : public b2Contact
+// Memory allocators. Modify these to use your own allocator.
+void* b2Alloc(int32 size)
 {
-public:
-	b2NullContact() {}
-	void Evaluate(b2ContactListener*) {}
-	b2Manifold* GetManifolds() { return NULL; }
-};
+	return malloc(size);
+}
 
-#endif
+void b2Free(void* mem)
+{
+	free(mem);
+}

src/modules/physics/box2d/Box2D/Common/b2Settings.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,23 +19,11 @@
 #ifndef B2_SETTINGS_H
 #define B2_SETTINGS_H
 
-#include <assert.h>
-//#include <common/Exception.h>
+#include <cassert>
 #include <cmath>
 
-#define B2_NOT_USED(x) x
+#define B2_NOT_USED(x) ((void)(x))
 #define b2Assert(A) assert(A)
-//#define b2Assert(A) {if(!(A)) throw love::Exception("Box2D error: " #A);}
-
-
-// need to include NDS jtypes.h instead of
-// usual typedefs because NDS jtypes defines
-// them slightly differently, oh well.
-#ifdef TARGET_IS_NDS
-
-#include "jtypes.h"
-
-#else
 
 typedef signed char	int8;
 typedef signed short int16;
@@ -43,108 +31,93 @@ typedef signed int int32;
 typedef unsigned char uint8;
 typedef unsigned short uint16;
 typedef unsigned int uint32;
-
-#endif
-
-#ifdef	TARGET_FLOAT32_IS_FIXED
-
-#include "Fixed.h"
-
-typedef Fixed float32;
-#define	B2_FLT_MAX	FIXED_MAX
-#define	B2_FLT_EPSILON	FIXED_EPSILON
-#define	B2FORCE_SCALE(x)	((x)<<7)
-#define	B2FORCE_INV_SCALE(x)	((x)>>7)
-
-#else
-
 typedef float float32;
-#define	B2_FLT_MAX	FLT_MAX
-#define	B2_FLT_EPSILON	FLT_EPSILON
-#define	B2FORCE_SCALE(x)	(x)
-#define	B2FORCE_INV_SCALE(x)	(x)
 
-#endif
-
-const float32 b2_pi = 3.14159265359f;
+#define	b2_maxFloat		FLT_MAX
+#define	b2_epsilon		FLT_EPSILON
+#define b2_pi			3.14159265359f
 
 /// @file
 /// Global tuning constants based on meters-kilograms-seconds (MKS) units.
 ///
 
 // Collision
-const int32 b2_maxManifoldPoints = 2;
-const int32 b2_maxPolygonVertices = 8;
-const int32 b2_maxProxies = 2048;				// this must be a power of two
-const int32 b2_maxPairs = 8 * b2_maxProxies;	// this must be a power of two
 
-// Dynamics
+/// The maximum number of contact points between two convex shapes.
+#define b2_maxManifoldPoints	2
+
+/// The maximum number of vertices on a convex polygon.
+#define b2_maxPolygonVertices	8
+
+/// This is used to fatten AABBs in the dynamic tree. This allows proxies
+/// to move by a small amount without triggering a tree adjustment.
+/// This is in meters.
+#define b2_aabbExtension		0.1f
+
+/// This is used to fatten AABBs in the dynamic tree. This is used to predict
+/// the future position based on the current displacement.
+/// This is a dimensionless multiplier.
+#define b2_aabbMultiplier		2.0f
 
 /// A small length used as a collision and constraint tolerance. Usually it is
 /// chosen to be numerically significant, but visually insignificant.
-const float32 b2_linearSlop = 0.005f;	// 0.5 cm
+#define b2_linearSlop			0.005f
 
 /// A small angle used as a collision and constraint tolerance. Usually it is
 /// chosen to be numerically significant, but visually insignificant.
-const float32 b2_angularSlop = 2.0f / 180.0f * b2_pi;			// 2 degrees
+#define b2_angularSlop			(2.0f / 180.0f * b2_pi)
 
-/// Continuous collision detection (CCD) works with core, shrunken shapes. This is the
-/// amount by which shapes are automatically shrunk to work with CCD. This must be
-/// larger than b2_linearSlop.
-const float32 b2_toiSlop = 8.0f * b2_linearSlop;
+/// The radius of the polygon/edge shape skin. This should not be modified. Making
+/// this smaller means polygons will have an insufficient buffer for continuous collision.
+/// Making it larger may create artifacts for vertex collision.
+#define b2_polygonRadius		(2.0f * b2_linearSlop)
 
-/// Maximum number of contacts to be handled to solve a TOI island.
-const int32 b2_maxTOIContactsPerIsland = 32;
+
+// Dynamics
+
+/// Maximum number of contacts to be handled to solve a TOI impact.
+#define b2_maxTOIContacts			32
 
 /// A velocity threshold for elastic collisions. Any collision with a relative linear
 /// velocity below this threshold will be treated as inelastic.
-const float32 b2_velocityThreshold = 1.0f;		// 1 m/s
+#define b2_velocityThreshold		1.0f
 
 /// The maximum linear position correction used when solving constraints. This helps to
 /// prevent overshoot.
-const float32 b2_maxLinearCorrection = 0.2f;	// 20 cm
+#define b2_maxLinearCorrection		0.2f
 
 /// The maximum angular position correction used when solving constraints. This helps to
 /// prevent overshoot.
-const float32 b2_maxAngularCorrection = 8.0f / 180.0f * b2_pi;			// 8 degrees
+#define b2_maxAngularCorrection		(8.0f / 180.0f * b2_pi)
 
 /// The maximum linear velocity of a body. This limit is very large and is used
 /// to prevent numerical problems. You shouldn't need to adjust this.
-#ifdef TARGET_FLOAT32_IS_FIXED
-const float32 b2_maxLinearVelocity = 100.0f;
-#else
-const float32 b2_maxLinearVelocity = 200.0f;
-const float32 b2_maxLinearVelocitySquared = b2_maxLinearVelocity * b2_maxLinearVelocity;
-#endif
+#define b2_maxTranslation			2.0f
+#define b2_maxTranslationSquared	(b2_maxTranslation * b2_maxTranslation)
 
 /// The maximum angular velocity of a body. This limit is very large and is used
 /// to prevent numerical problems. You shouldn't need to adjust this.
-const float32 b2_maxAngularVelocity = 250.0f;
-#ifndef TARGET_FLOAT32_IS_FIXED
-const float32 b2_maxAngularVelocitySquared = b2_maxAngularVelocity * b2_maxAngularVelocity;
-#endif
+#define b2_maxRotation				(0.5f * b2_pi)
+#define b2_maxRotationSquared		(b2_maxRotation * b2_maxRotation)
 
 /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
 /// that overlap is removed in one time step. However using values close to 1 often lead
 /// to overshoot.
-const float32 b2_contactBaumgarte = 0.2f;
+#define b2_contactBaumgarte			0.2f
 
 // Sleep
 
 /// The time that a body must be still before it will go to sleep.
-const float32 b2_timeToSleep = 0.5f;									// half a second
+#define b2_timeToSleep				0.5f
 
 /// A body cannot sleep if its linear velocity is above this tolerance.
-const float32 b2_linearSleepTolerance = 0.01f;		// 1 cm/s
+#define b2_linearSleepTolerance		0.01f
 
 /// A body cannot sleep if its angular velocity is above this tolerance.
-const float32 b2_angularSleepTolerance = 2.0f / 180.0f;		// 2 degrees/s
+#define b2_angularSleepTolerance	(2.0f / 180.0f * b2_pi)
 
 // Memory Allocation
 
-/// The current number of bytes allocated through b2Alloc.
-extern int32 b2_byteCount;
-
 /// Implement this function to use your own memory allocator.
 void* b2Alloc(int32 size);
 

src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,8 +16,8 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2StackAllocator.h"
-#include "b2Math.h"
+#include <Box2D/Common/b2StackAllocator.h>
+#include <Box2D/Common/b2Math.h>
 
 b2StackAllocator::b2StackAllocator()
 {

src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,7 +19,7 @@
 #ifndef B2_STACK_ALLOCATOR_H
 #define B2_STACK_ALLOCATOR_H
 
-#include "b2Settings.h"
+#include <Box2D/Common/b2Settings.h>
 
 const int32 b2_stackSize = 100 * 1024;	// 100k
 const int32 b2_maxStackEntries = 32;

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2CircleContact.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Collision/b2TimeOfImpact.h>
+
+#include <new>
+
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2CircleContact));
+	return new (mem) b2CircleContact(fixtureA, fixtureB);
+}
+
+void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2CircleContact*)contact)->~b2CircleContact();
+	allocator->Free(contact, sizeof(b2CircleContact));
+}
+
+b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideCircles(manifold,
+					(b2CircleShape*)m_fixtureA->GetShape(), xfA,
+					(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,31 +16,23 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef CIRCLE_CONTACT_H
-#define CIRCLE_CONTACT_H
+#ifndef B2_CIRCLE_CONTACT_H
+#define B2_CIRCLE_CONTACT_H
 
-#include "../../Common/b2Math.h"
-#include "../../Collision/b2Collision.h"
-#include "b2Contact.h"
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
 
 class b2BlockAllocator;
 
 class b2CircleContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
-	b2CircleContact(b2Shape* shape1, b2Shape* shape2);
+	b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
 	~b2CircleContact() {}
 
-	void Evaluate(b2ContactListener* listener);
-	b2Manifold* GetManifolds()
-	{
-		return &m_manifold;
-	}
-
-	b2Manifold m_manifold;
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp

@@ -0,0 +1,226 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/Contacts/b2CircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
+#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
+
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Collision/b2TimeOfImpact.h>
+#include <Box2D/Collision/Shapes/b2Shape.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2World.h>
+
+b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
+bool b2Contact::s_initialized = false;
+
+void b2Contact::InitializeRegisters()
+{
+	AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
+	AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
+	AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
+}
+
+void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
+						b2Shape::Type type1, b2Shape::Type type2)
+{
+	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	
+	s_registers[type1][type2].createFcn = createFcn;
+	s_registers[type1][type2].destroyFcn = destoryFcn;
+	s_registers[type1][type2].primary = true;
+
+	if (type1 != type2)
+	{
+		s_registers[type2][type1].createFcn = createFcn;
+		s_registers[type2][type1].destroyFcn = destoryFcn;
+		s_registers[type2][type1].primary = false;
+	}
+}
+
+b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	if (s_initialized == false)
+	{
+		InitializeRegisters();
+		s_initialized = true;
+	}
+
+	b2Shape::Type type1 = fixtureA->GetType();
+	b2Shape::Type type2 = fixtureB->GetType();
+
+	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	
+	b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
+	if (createFcn)
+	{
+		if (s_registers[type1][type2].primary)
+		{
+			return createFcn(fixtureA, fixtureB, allocator);
+		}
+		else
+		{
+			return createFcn(fixtureB, fixtureA, allocator);
+		}
+	}
+	else
+	{
+		return NULL;
+	}
+}
+
+void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	b2Assert(s_initialized == true);
+
+	if (contact->m_manifold.pointCount > 0)
+	{
+		contact->GetFixtureA()->GetBody()->SetAwake(true);
+		contact->GetFixtureB()->GetBody()->SetAwake(true);
+	}
+
+	b2Shape::Type typeA = contact->GetFixtureA()->GetType();
+	b2Shape::Type typeB = contact->GetFixtureB()->GetType();
+
+	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+
+	b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
+	destroyFcn(contact, allocator);
+}
+
+b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB)
+{
+	m_flags = e_enabledFlag;
+
+	m_fixtureA = fA;
+	m_fixtureB = fB;
+
+	m_manifold.pointCount = 0;
+
+	m_prev = NULL;
+	m_next = NULL;
+
+	m_nodeA.contact = NULL;
+	m_nodeA.prev = NULL;
+	m_nodeA.next = NULL;
+	m_nodeA.other = NULL;
+
+	m_nodeB.contact = NULL;
+	m_nodeB.prev = NULL;
+	m_nodeB.next = NULL;
+	m_nodeB.other = NULL;
+
+	m_toiCount = 0;
+}
+
+// Update the contact manifold and touching status.
+// Note: do not assume the fixture AABBs are overlapping or are valid.
+void b2Contact::Update(b2ContactListener* listener)
+{
+	b2Manifold oldManifold = m_manifold;
+
+	// Re-enable this contact.
+	m_flags |= e_enabledFlag;
+
+	bool touching = false;
+	bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
+
+	bool sensorA = m_fixtureA->IsSensor();
+	bool sensorB = m_fixtureB->IsSensor();
+	bool sensor = sensorA || sensorB;
+
+	b2Body* bodyA = m_fixtureA->GetBody();
+	b2Body* bodyB = m_fixtureB->GetBody();
+	const b2Transform& xfA = bodyA->GetTransform();
+	const b2Transform& xfB = bodyB->GetTransform();
+
+	// Is this contact a sensor?
+	if (sensor)
+	{
+		const b2Shape* shapeA = m_fixtureA->GetShape();
+		const b2Shape* shapeB = m_fixtureB->GetShape();
+		touching = b2TestOverlap(shapeA, shapeB, xfA, xfB);
+
+		// Sensors don't generate manifolds.
+		m_manifold.pointCount = 0;
+	}
+	else
+	{
+		Evaluate(&m_manifold, xfA, xfB);
+		touching = m_manifold.pointCount > 0;
+
+		// Match old contact ids to new contact ids and copy the
+		// stored impulses to warm start the solver.
+		for (int32 i = 0; i < m_manifold.pointCount; ++i)
+		{
+			b2ManifoldPoint* mp2 = m_manifold.points + i;
+			mp2->normalImpulse = 0.0f;
+			mp2->tangentImpulse = 0.0f;
+			b2ContactID id2 = mp2->id;
+
+			for (int32 j = 0; j < oldManifold.pointCount; ++j)
+			{
+				b2ManifoldPoint* mp1 = oldManifold.points + j;
+
+				if (mp1->id.key == id2.key)
+				{
+					mp2->normalImpulse = mp1->normalImpulse;
+					mp2->tangentImpulse = mp1->tangentImpulse;
+					break;
+				}
+			}
+		}
+
+		if (touching != wasTouching)
+		{
+			bodyA->SetAwake(true);
+			bodyB->SetAwake(true);
+		}
+	}
+
+	if (touching)
+	{
+		m_flags |= e_touchingFlag;
+	}
+	else
+	{
+		m_flags &= ~e_touchingFlag;
+	}
+
+	if (wasTouching == false && touching == true && listener)
+	{
+		listener->BeginContact(this);
+	}
+
+	if (wasTouching == true && touching == false && listener)
+	{
+		listener->EndContact(this);
+	}
+
+	if (sensor == false && touching && listener)
+	{
+		listener->PreSolve(this, &oldManifold);
+	}
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h

@@ -0,0 +1,242 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_CONTACT_H
+#define B2_CONTACT_H
+
+#include <Box2D/Common/b2Math.h>
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Collision/Shapes/b2Shape.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+
+class b2Body;
+class b2Contact;
+class b2Fixture;
+class b2World;
+class b2BlockAllocator;
+class b2StackAllocator;
+class b2ContactListener;
+
+typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
+
+struct b2ContactRegister
+{
+	b2ContactCreateFcn* createFcn;
+	b2ContactDestroyFcn* destroyFcn;
+	bool primary;
+};
+
+/// A contact edge is used to connect bodies and contacts together
+/// in a contact graph where each body is a node and each contact
+/// is an edge. A contact edge belongs to a doubly linked list
+/// maintained in each attached body. Each contact has two contact
+/// nodes, one for each attached body.
+struct b2ContactEdge
+{
+	b2Body* other;			///< provides quick access to the other body attached.
+	b2Contact* contact;		///< the contact
+	b2ContactEdge* prev;	///< the previous contact edge in the body's contact list
+	b2ContactEdge* next;	///< the next contact edge in the body's contact list
+};
+
+/// The class manages contact between two shapes. A contact exists for each overlapping
+/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
+/// that has no contact points.
+class b2Contact
+{
+public:
+
+	/// Get the contact manifold. Do not modify the manifold unless you understand the
+	/// internals of Box2D.
+	b2Manifold* GetManifold();
+	const b2Manifold* GetManifold() const;
+
+	/// Get the world manifold.
+	void GetWorldManifold(b2WorldManifold* worldManifold) const;
+
+	/// Is this contact touching?
+	bool IsTouching() const;
+
+	/// Enable/disable this contact. This can be used inside the pre-solve
+	/// contact listener. The contact is only disabled for the current
+	/// time step (or sub-step in continuous collisions).
+	void SetEnabled(bool flag);
+
+	/// Has this contact been disabled?
+	bool IsEnabled() const;
+
+	/// Get the next contact in the world's contact list.
+	b2Contact* GetNext();
+	const b2Contact* GetNext() const;
+
+	/// Get the first fixture in this contact.
+	b2Fixture* GetFixtureA();
+	const b2Fixture* GetFixtureA() const;
+
+	/// Get the second fixture in this contact.
+	b2Fixture* GetFixtureB();
+	const b2Fixture* GetFixtureB() const;
+
+	/// Evaluate this contact with your own manifold and transforms.
+	virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
+
+protected:
+	friend class b2ContactManager;
+	friend class b2World;
+	friend class b2ContactSolver;
+	friend class b2Body;
+	friend class b2Fixture;
+
+	// Flags stored in m_flags
+	enum
+	{
+		// Used when crawling contact graph when forming islands.
+		e_islandFlag		= 0x0001,
+
+        // Set when the shapes are touching.
+		e_touchingFlag		= 0x0002,
+
+		// This contact can be disabled (by user)
+		e_enabledFlag		= 0x0004,
+
+		// This contact needs filtering because a fixture filter was changed.
+		e_filterFlag		= 0x0008,
+
+		// This bullet contact had a TOI event
+		e_bulletHitFlag		= 0x0010,
+
+	};
+
+	/// Flag this contact for filtering. Filtering will occur the next time step.
+	void FlagForFiltering();
+
+	static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
+						b2Shape::Type typeA, b2Shape::Type typeB);
+	static void InitializeRegisters();
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}
+	b2Contact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	virtual ~b2Contact() {}
+
+	void Update(b2ContactListener* listener);
+
+	static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
+	static bool s_initialized;
+
+	uint32 m_flags;
+
+	// World pool and list pointers.
+	b2Contact* m_prev;
+	b2Contact* m_next;
+
+	// Nodes for connecting bodies.
+	b2ContactEdge m_nodeA;
+	b2ContactEdge m_nodeB;
+
+	b2Fixture* m_fixtureA;
+	b2Fixture* m_fixtureB;
+
+	b2Manifold m_manifold;
+
+	int32 m_toiCount;
+//	float32 m_toi;
+};
+
+inline b2Manifold* b2Contact::GetManifold()
+{
+	return &m_manifold;
+}
+
+inline const b2Manifold* b2Contact::GetManifold() const
+{
+	return &m_manifold;
+}
+
+inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const
+{
+	const b2Body* bodyA = m_fixtureA->GetBody();
+	const b2Body* bodyB = m_fixtureB->GetBody();
+	const b2Shape* shapeA = m_fixtureA->GetShape();
+	const b2Shape* shapeB = m_fixtureB->GetShape();
+
+	worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
+}
+
+inline void b2Contact::SetEnabled(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_enabledFlag;
+	}
+	else
+	{
+		m_flags &= ~e_enabledFlag;
+	}
+}
+
+inline bool b2Contact::IsEnabled() const
+{
+	return (m_flags & e_enabledFlag) == e_enabledFlag;
+}
+
+inline bool b2Contact::IsTouching() const
+{
+	return (m_flags & e_touchingFlag) == e_touchingFlag;
+}
+
+inline b2Contact* b2Contact::GetNext()
+{
+	return m_next;
+}
+
+inline const b2Contact* b2Contact::GetNext() const
+{
+	return m_next;
+}
+
+inline b2Fixture* b2Contact::GetFixtureA()
+{
+	return m_fixtureA;
+}
+
+inline const b2Fixture* b2Contact::GetFixtureA() const
+{
+	return m_fixtureA;
+}
+
+inline b2Fixture* b2Contact::GetFixtureB()
+{
+	return m_fixtureB;
+}
+
+inline const b2Fixture* b2Contact::GetFixtureB() const
+{
+	return m_fixtureB;
+}
+
+inline void b2Contact::FlagForFiltering()
+{
+	m_flags |= e_filterFlag;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp

@@ -0,0 +1,623 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2ContactSolver.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2World.h>
+#include <Box2D/Common/b2StackAllocator.h>
+
+#define B2_DEBUG_SOLVER 0
+
+b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,
+								b2StackAllocator* allocator, float32 impulseRatio)
+{
+	m_allocator = allocator;
+
+	m_constraintCount = contactCount;
+	m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
+
+	for (int32 i = 0; i < m_constraintCount; ++i)
+	{
+		b2Contact* contact = contacts[i];
+
+		b2Fixture* fixtureA = contact->m_fixtureA;
+		b2Fixture* fixtureB = contact->m_fixtureB;
+		b2Shape* shapeA = fixtureA->GetShape();
+		b2Shape* shapeB = fixtureB->GetShape();
+		float32 radiusA = shapeA->m_radius;
+		float32 radiusB = shapeB->m_radius;
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+		b2Manifold* manifold = contact->GetManifold();
+
+		float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
+		float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
+
+		b2Vec2 vA = bodyA->m_linearVelocity;
+		b2Vec2 vB = bodyB->m_linearVelocity;
+		float32 wA = bodyA->m_angularVelocity;
+		float32 wB = bodyB->m_angularVelocity;
+
+		b2Assert(manifold->pointCount > 0);
+
+		b2WorldManifold worldManifold;
+		worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
+
+		b2ContactConstraint* cc = m_constraints + i;
+		cc->bodyA = bodyA;
+		cc->bodyB = bodyB;
+		cc->manifold = manifold;
+		cc->normal = worldManifold.normal;
+		cc->pointCount = manifold->pointCount;
+		cc->friction = friction;
+
+		cc->localNormal = manifold->localNormal;
+		cc->localPoint = manifold->localPoint;
+		cc->radius = radiusA + radiusB;
+		cc->type = manifold->type;
+
+		for (int32 j = 0; j < cc->pointCount; ++j)
+		{
+			b2ManifoldPoint* cp = manifold->points + j;
+			b2ContactConstraintPoint* ccp = cc->points + j;
+
+			ccp->normalImpulse = impulseRatio * cp->normalImpulse;
+			ccp->tangentImpulse = impulseRatio * cp->tangentImpulse;
+
+			ccp->localPoint = cp->localPoint;
+
+			ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;
+			ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;
+
+			float32 rnA = b2Cross(ccp->rA, cc->normal);
+			float32 rnB = b2Cross(ccp->rB, cc->normal);
+			rnA *= rnA;
+			rnB *= rnB;
+
+			float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
+
+			b2Assert(kNormal > b2_epsilon);
+			ccp->normalMass = 1.0f / kNormal;
+
+			b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
+
+			float32 rtA = b2Cross(ccp->rA, tangent);
+			float32 rtB = b2Cross(ccp->rB, tangent);
+			rtA *= rtA;
+			rtB *= rtB;
+
+			float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
+
+			b2Assert(kTangent > b2_epsilon);
+			ccp->tangentMass = 1.0f /  kTangent;
+
+			// Setup a velocity bias for restitution.
+			ccp->velocityBias = 0.0f;
+			float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
+			if (vRel < -b2_velocityThreshold)
+			{
+				ccp->velocityBias = -restitution * vRel;
+			}
+		}
+
+		// If we have two points, then prepare the block solver.
+		if (cc->pointCount == 2)
+		{
+			b2ContactConstraintPoint* ccp1 = cc->points + 0;
+			b2ContactConstraintPoint* ccp2 = cc->points + 1;
+			
+			float32 invMassA = bodyA->m_invMass;
+			float32 invIA = bodyA->m_invI;
+			float32 invMassB = bodyB->m_invMass;
+			float32 invIB = bodyB->m_invI;
+
+			float32 rn1A = b2Cross(ccp1->rA, cc->normal);
+			float32 rn1B = b2Cross(ccp1->rB, cc->normal);
+			float32 rn2A = b2Cross(ccp2->rA, cc->normal);
+			float32 rn2B = b2Cross(ccp2->rB, cc->normal);
+
+			float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
+			float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
+			float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
+
+			// Ensure a reasonable condition number.
+			const float32 k_maxConditionNumber = 100.0f;
+			if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
+			{
+				// K is safe to invert.
+				cc->K.col1.Set(k11, k12);
+				cc->K.col2.Set(k12, k22);
+				cc->normalMass = cc->K.GetInverse();
+			}
+			else
+			{
+				// The constraints are redundant, just use one.
+				// TODO_ERIN use deepest?
+				cc->pointCount = 1;
+			}
+		}
+	}
+}
+
+b2ContactSolver::~b2ContactSolver()
+{
+	m_allocator->Free(m_constraints);
+}
+
+void b2ContactSolver::WarmStart()
+{
+	// Warm start.
+	for (int32 i = 0; i < m_constraintCount; ++i)
+	{
+		b2ContactConstraint* c = m_constraints + i;
+
+		b2Body* bodyA = c->bodyA;
+		b2Body* bodyB = c->bodyB;
+		float32 invMassA = bodyA->m_invMass;
+		float32 invIA = bodyA->m_invI;
+		float32 invMassB = bodyB->m_invMass;
+		float32 invIB = bodyB->m_invI;
+		b2Vec2 normal = c->normal;
+		b2Vec2 tangent = b2Cross(normal, 1.0f);
+
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			b2ContactConstraintPoint* ccp = c->points + j;
+			b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
+			bodyA->m_angularVelocity -= invIA * b2Cross(ccp->rA, P);
+			bodyA->m_linearVelocity -= invMassA * P;
+			bodyB->m_angularVelocity += invIB * b2Cross(ccp->rB, P);
+			bodyB->m_linearVelocity += invMassB * P;
+		}
+	}
+}
+
+void b2ContactSolver::SolveVelocityConstraints()
+{
+	for (int32 i = 0; i < m_constraintCount; ++i)
+	{
+		b2ContactConstraint* c = m_constraints + i;
+		b2Body* bodyA = c->bodyA;
+		b2Body* bodyB = c->bodyB;
+		float32 wA = bodyA->m_angularVelocity;
+		float32 wB = bodyB->m_angularVelocity;
+		b2Vec2 vA = bodyA->m_linearVelocity;
+		b2Vec2 vB = bodyB->m_linearVelocity;
+		float32 invMassA = bodyA->m_invMass;
+		float32 invIA = bodyA->m_invI;
+		float32 invMassB = bodyB->m_invMass;
+		float32 invIB = bodyB->m_invI;
+		b2Vec2 normal = c->normal;
+		b2Vec2 tangent = b2Cross(normal, 1.0f);
+		float32 friction = c->friction;
+
+		b2Assert(c->pointCount == 1 || c->pointCount == 2);
+
+		// Solve tangent constraints
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			b2ContactConstraintPoint* ccp = c->points + j;
+
+			// Relative velocity at contact
+			b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
+
+			// Compute tangent force
+			float32 vt = b2Dot(dv, tangent);
+			float32 lambda = ccp->tangentMass * (-vt);
+
+			// b2Clamp the accumulated force
+			float32 maxFriction = friction * ccp->normalImpulse;
+			float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
+			lambda = newImpulse - ccp->tangentImpulse;
+
+			// Apply contact impulse
+			b2Vec2 P = lambda * tangent;
+
+			vA -= invMassA * P;
+			wA -= invIA * b2Cross(ccp->rA, P);
+
+			vB += invMassB * P;
+			wB += invIB * b2Cross(ccp->rB, P);
+
+			ccp->tangentImpulse = newImpulse;
+		}
+
+		// Solve normal constraints
+		if (c->pointCount == 1)
+		{
+			b2ContactConstraintPoint* ccp = c->points + 0;
+
+			// Relative velocity at contact
+			b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
+
+			// Compute normal impulse
+			float32 vn = b2Dot(dv, normal);
+			float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
+
+			// b2Clamp the accumulated impulse
+			float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
+			lambda = newImpulse - ccp->normalImpulse;
+
+			// Apply contact impulse
+			b2Vec2 P = lambda * normal;
+			vA -= invMassA * P;
+			wA -= invIA * b2Cross(ccp->rA, P);
+
+			vB += invMassB * P;
+			wB += invIB * b2Cross(ccp->rB, P);
+			ccp->normalImpulse = newImpulse;
+		}
+		else
+		{
+			// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
+			// Build the mini LCP for this contact patch
+			//
+			// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
+			//
+			// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
+			// b = vn_0 - velocityBias
+			//
+			// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
+			// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
+			// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
+			// solution that satisfies the problem is chosen.
+			// 
+			// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
+			// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
+			//
+			// Substitute:
+			// 
+			// x = x' - a
+			// 
+			// Plug into above equation:
+			//
+			// vn = A * x + b
+			//    = A * (x' - a) + b
+			//    = A * x' + b - A * a
+			//    = A * x' + b'
+			// b' = b - A * a;
+
+			b2ContactConstraintPoint* cp1 = c->points + 0;
+			b2ContactConstraintPoint* cp2 = c->points + 1;
+
+			b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
+			b2Assert(a.x >= 0.0f && a.y >= 0.0f);
+
+			// Relative velocity at contact
+			b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+			b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+			// Compute normal velocity
+			float32 vn1 = b2Dot(dv1, normal);
+			float32 vn2 = b2Dot(dv2, normal);
+
+			b2Vec2 b;
+			b.x = vn1 - cp1->velocityBias;
+			b.y = vn2 - cp2->velocityBias;
+			b -= b2Mul(c->K, a);
+
+			const float32 k_errorTol = 1e-3f;
+			B2_NOT_USED(k_errorTol);
+
+			for (;;)
+			{
+				//
+				// Case 1: vn = 0
+				//
+				// 0 = A * x' + b'
+				//
+				// Solve for x':
+				//
+				// x' = - inv(A) * b'
+				//
+				b2Vec2 x = - b2Mul(c->normalMass, b);
+
+				if (x.x >= 0.0f && x.y >= 0.0f)
+				{
+					// Resubstitute for the incremental impulse
+					b2Vec2 d = x - a;
+
+					// Apply incremental impulse
+					b2Vec2 P1 = d.x * normal;
+					b2Vec2 P2 = d.y * normal;
+					vA -= invMassA * (P1 + P2);
+					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+					vB += invMassB * (P1 + P2);
+					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+					// Accumulate
+					cp1->normalImpulse = x.x;
+					cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+					// Postconditions
+					dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+					dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+					// Compute normal velocity
+					vn1 = b2Dot(dv1, normal);
+					vn2 = b2Dot(dv2, normal);
+
+					b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
+					b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
+#endif
+					break;
+				}
+
+				//
+				// Case 2: vn1 = 0 and x2 = 0
+				//
+				//   0 = a11 * x1' + a12 * 0 + b1' 
+				// vn2 = a21 * x1' + a22 * 0 + b2'
+				//
+				x.x = - cp1->normalMass * b.x;
+				x.y = 0.0f;
+				vn1 = 0.0f;
+				vn2 = c->K.col1.y * x.x + b.y;
+
+				if (x.x >= 0.0f && vn2 >= 0.0f)
+				{
+					// Resubstitute for the incremental impulse
+					b2Vec2 d = x - a;
+
+					// Apply incremental impulse
+					b2Vec2 P1 = d.x * normal;
+					b2Vec2 P2 = d.y * normal;
+					vA -= invMassA * (P1 + P2);
+					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+					vB += invMassB * (P1 + P2);
+					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+					// Accumulate
+					cp1->normalImpulse = x.x;
+					cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+					// Postconditions
+					dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
+
+					// Compute normal velocity
+					vn1 = b2Dot(dv1, normal);
+
+					b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
+#endif
+					break;
+				}
+
+
+				//
+				// Case 3: vn2 = 0 and x1 = 0
+				//
+				// vn1 = a11 * 0 + a12 * x2' + b1' 
+				//   0 = a21 * 0 + a22 * x2' + b2'
+				//
+				x.x = 0.0f;
+				x.y = - cp2->normalMass * b.y;
+				vn1 = c->K.col2.x * x.y + b.x;
+				vn2 = 0.0f;
+
+				if (x.y >= 0.0f && vn1 >= 0.0f)
+				{
+					// Resubstitute for the incremental impulse
+					b2Vec2 d = x - a;
+
+					// Apply incremental impulse
+					b2Vec2 P1 = d.x * normal;
+					b2Vec2 P2 = d.y * normal;
+					vA -= invMassA * (P1 + P2);
+					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+					vB += invMassB * (P1 + P2);
+					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+					// Accumulate
+					cp1->normalImpulse = x.x;
+					cp2->normalImpulse = x.y;
+
+#if B2_DEBUG_SOLVER == 1
+					// Postconditions
+					dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
+
+					// Compute normal velocity
+					vn2 = b2Dot(dv2, normal);
+
+					b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
+#endif
+					break;
+				}
+
+				//
+				// Case 4: x1 = 0 and x2 = 0
+				// 
+				// vn1 = b1
+				// vn2 = b2;
+				x.x = 0.0f;
+				x.y = 0.0f;
+				vn1 = b.x;
+				vn2 = b.y;
+
+				if (vn1 >= 0.0f && vn2 >= 0.0f )
+				{
+					// Resubstitute for the incremental impulse
+					b2Vec2 d = x - a;
+
+					// Apply incremental impulse
+					b2Vec2 P1 = d.x * normal;
+					b2Vec2 P2 = d.y * normal;
+					vA -= invMassA * (P1 + P2);
+					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+
+					vB += invMassB * (P1 + P2);
+					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+
+					// Accumulate
+					cp1->normalImpulse = x.x;
+					cp2->normalImpulse = x.y;
+
+					break;
+				}
+
+				// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
+				break;
+			}
+		}
+
+		bodyA->m_linearVelocity = vA;
+		bodyA->m_angularVelocity = wA;
+		bodyB->m_linearVelocity = vB;
+		bodyB->m_angularVelocity = wB;
+	}
+}
+
+void b2ContactSolver::StoreImpulses()
+{
+	for (int32 i = 0; i < m_constraintCount; ++i)
+	{
+		b2ContactConstraint* c = m_constraints + i;
+		b2Manifold* m = c->manifold;
+
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			m->points[j].normalImpulse = c->points[j].normalImpulse;
+			m->points[j].tangentImpulse = c->points[j].tangentImpulse;
+		}
+	}
+}
+
+struct b2PositionSolverManifold
+{
+	void Initialize(b2ContactConstraint* cc, int32 index)
+	{
+		b2Assert(cc->pointCount > 0);
+
+		switch (cc->type)
+		{
+		case b2Manifold::e_circles:
+			{
+				b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
+				b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint);
+				if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+				{
+					normal = pointB - pointA;
+					normal.Normalize();
+				}
+				else
+				{
+					normal.Set(1.0f, 0.0f);
+				}
+
+				point = 0.5f * (pointA + pointB);
+				separation = b2Dot(pointB - pointA, normal) - cc->radius;
+			}
+			break;
+
+		case b2Manifold::e_faceA:
+			{
+				normal = cc->bodyA->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+			}
+			break;
+
+		case b2Manifold::e_faceB:
+			{
+				normal = cc->bodyB->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+
+				// Ensure normal points from A to B
+				normal = -normal;
+			}
+			break;
+		}
+	}
+
+	b2Vec2 normal;
+	b2Vec2 point;
+	float32 separation;
+};
+
+// Sequential solver.
+bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
+{
+	float32 minSeparation = 0.0f;
+
+	for (int32 i = 0; i < m_constraintCount; ++i)
+	{
+		b2ContactConstraint* c = m_constraints + i;
+		b2Body* bodyA = c->bodyA;
+		b2Body* bodyB = c->bodyB;
+
+		float32 invMassA = bodyA->m_mass * bodyA->m_invMass;
+		float32 invIA = bodyA->m_mass * bodyA->m_invI;
+		float32 invMassB = bodyB->m_mass * bodyB->m_invMass;
+		float32 invIB = bodyB->m_mass * bodyB->m_invI;
+
+		// Solve normal constraints
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			b2PositionSolverManifold psm;
+			psm.Initialize(c, j);
+			b2Vec2 normal = psm.normal;
+
+			b2Vec2 point = psm.point;
+			float32 separation = psm.separation;
+
+			b2Vec2 rA = point - bodyA->m_sweep.c;
+			b2Vec2 rB = point - bodyB->m_sweep.c;
+
+			// Track max constraint error.
+			minSeparation = b2Min(minSeparation, separation);
+
+			// Prevent large corrections and allow slop.
+			float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+			// Compute the effective mass.
+			float32 rnA = b2Cross(rA, normal);
+			float32 rnB = b2Cross(rB, normal);
+			float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
+
+			// Compute normal impulse
+			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+			b2Vec2 P = impulse * normal;
+
+			bodyA->m_sweep.c -= invMassA * P;
+			bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
+			bodyA->SynchronizeTransform();
+
+			bodyB->m_sweep.c += invMassB * P;
+			bodyB->m_sweep.a += invIB * b2Cross(rB, P);
+			bodyB->SynchronizeTransform();
+		}
+	}
+
+	// We can't expect minSpeparation >= -b2_linearSlop because we don't
+	// push the separation above -b2_linearSlop.
+	return minSeparation >= -1.5f * b2_linearSlop;
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,59 +16,60 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef CONTACT_SOLVER_H
-#define CONTACT_SOLVER_H
+#ifndef B2_CONTACT_SOLVER_H
+#define B2_CONTACT_SOLVER_H
 
-#include "../../Common/b2Math.h"
-#include "../../Collision/b2Collision.h"
-#include "../b2World.h"
+#include <Box2D/Common/b2Math.h>
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Dynamics/b2Island.h>
 
 class b2Contact;
 class b2Body;
-class b2Island;
 class b2StackAllocator;
 
 struct b2ContactConstraintPoint
 {
-	b2Vec2 localAnchor1;
-	b2Vec2 localAnchor2;
-	b2Vec2 r1;
-	b2Vec2 r2;
+	b2Vec2 localPoint;
+	b2Vec2 rA;
+	b2Vec2 rB;
 	float32 normalImpulse;
 	float32 tangentImpulse;
-	float32 positionImpulse;
 	float32 normalMass;
 	float32 tangentMass;
-	float32 equalizedMass;
-	float32 separation;
 	float32 velocityBias;
 };
 
 struct b2ContactConstraint
 {
 	b2ContactConstraintPoint points[b2_maxManifoldPoints];
+	b2Vec2 localNormal;
+	b2Vec2 localPoint;
 	b2Vec2 normal;
-	b2Manifold* manifold;
-	b2Body* body1;
-	b2Body* body2;
+	b2Mat22 normalMass;
+	b2Mat22 K;
+	b2Body* bodyA;
+	b2Body* bodyB;
+	b2Manifold::Type type;
+	float32 radius;
 	float32 friction;
-	float32 restitution;
 	int32 pointCount;
+	b2Manifold* manifold;
 };
 
 class b2ContactSolver
 {
 public:
-	b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator);
+	b2ContactSolver(b2Contact** contacts, int32 contactCount,
+					b2StackAllocator* allocator, float32 impulseRatio);
+
 	~b2ContactSolver();
 
-	void InitVelocityConstraints(const b2TimeStep& step);
+	void WarmStart();
 	void SolveVelocityConstraints();
-	void FinalizeVelocityConstraints();
+	void StoreImpulses();
 
 	bool SolvePositionConstraints(float32 baumgarte);
 
-	b2TimeStep m_step;
 	b2StackAllocator* m_allocator;
 	b2ContactConstraint* m_constraints;
 	int m_constraintCount;

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2PolygonAndCircleContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Collision/b2TimeOfImpact.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+
+#include <new>
+
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
+	return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
+	allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
+}
+
+b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygonAndCircle(	manifold,
+								(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,29 +16,23 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef POLY_AND_CIRCLE_CONTACT_H
-#define POLY_AND_CIRCLE_CONTACT_H
+#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H
+#define B2_POLYGON_AND_CIRCLE_CONTACT_H
 
-#include "b2Contact.h"
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
 
 class b2BlockAllocator;
 
-class b2PolyAndCircleContact : public b2Contact
+class b2PolygonAndCircleContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
-	b2PolyAndCircleContact(b2Shape* shape1, b2Shape* shape2);
-	~b2PolyAndCircleContact() {}
+	b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2PolygonAndCircleContact() {}
 
-	void Evaluate(b2ContactListener* listener);
-	b2Manifold* GetManifolds()
-	{
-		return &m_manifold;
-	}
-
-	b2Manifold m_manifold;
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2PolygonContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Collision/b2TimeOfImpact.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+
+#include <new>
+
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
+	return new (mem) b2PolygonContact(fixtureA, fixtureB);
+}
+
+void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonContact*)contact)->~b2PolygonContact();
+	allocator->Free(contact, sizeof(b2PolygonContact));
+}
+
+b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygons(	manifold,
+						(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+						(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,29 +16,23 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef POLYCONTACT_H
-#define POLYCONTACT_H
+#ifndef B2_POLYGON_CONTACT_H
+#define B2_POLYGON_CONTACT_H
 
-#include "b2Contact.h"
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
 
 class b2BlockAllocator;
 
 class b2PolygonContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
-	b2PolygonContact(b2Shape* shape1, b2Shape* shape2);
+	b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
 	~b2PolygonContact() {}
 
-	void Evaluate(b2ContactListener* listener);
-	b2Manifold* GetManifolds()
-	{
-		return &m_manifold;
-	}
-
-	b2Manifold m_manifold;
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.cpp

@@ -0,0 +1,231 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Contacts/b2TOISolver.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Common/b2StackAllocator.h>
+
+struct b2TOIConstraint
+{
+	b2Vec2 localPoints[b2_maxManifoldPoints];
+	b2Vec2 localNormal;
+	b2Vec2 localPoint;
+	b2Manifold::Type type;
+	float32 radius;
+	int32 pointCount;
+	b2Body* bodyA;
+	b2Body* bodyB;
+};
+
+b2TOISolver::b2TOISolver(b2StackAllocator* allocator)
+{
+	m_allocator = allocator;
+	m_constraints = NULL;
+	m_count = NULL;
+	m_toiBody = NULL;
+}
+
+b2TOISolver::~b2TOISolver()
+{
+	Clear();
+}
+
+void b2TOISolver::Clear()
+{
+	if (m_allocator && m_constraints)
+	{
+		m_allocator->Free(m_constraints);
+		m_constraints = NULL;
+	}
+}
+
+void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)
+{
+	Clear();
+
+	m_count = count;
+	m_toiBody = toiBody;
+
+	m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2Contact* contact = contacts[i];
+
+		b2Fixture* fixtureA = contact->GetFixtureA();
+		b2Fixture* fixtureB = contact->GetFixtureB();
+		b2Shape* shapeA = fixtureA->GetShape();
+		b2Shape* shapeB = fixtureB->GetShape();
+		float32 radiusA = shapeA->m_radius;
+		float32 radiusB = shapeB->m_radius;
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+		b2Manifold* manifold = contact->GetManifold();
+
+		b2Assert(manifold->pointCount > 0);
+
+		b2TOIConstraint* constraint = m_constraints + i;
+		constraint->bodyA = bodyA;
+		constraint->bodyB = bodyB;
+		constraint->localNormal = manifold->localNormal;
+		constraint->localPoint = manifold->localPoint;
+		constraint->type = manifold->type;
+		constraint->pointCount = manifold->pointCount;
+		constraint->radius = radiusA + radiusB;
+
+		for (int32 j = 0; j < constraint->pointCount; ++j)
+		{
+			b2ManifoldPoint* cp = manifold->points + j;
+			constraint->localPoints[j] = cp->localPoint;
+		}
+	}
+}
+
+struct b2TOISolverManifold
+{
+	void Initialize(b2TOIConstraint* cc, int32 index)
+	{
+		b2Assert(cc->pointCount > 0);
+
+		switch (cc->type)
+		{
+		case b2Manifold::e_circles:
+			{
+				b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
+				b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
+				if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+				{
+					normal = pointB - pointA;
+					normal.Normalize();
+				}
+				else
+				{
+					normal.Set(1.0f, 0.0f);
+				}
+
+				point = 0.5f * (pointA + pointB);
+				separation = b2Dot(pointB - pointA, normal) - cc->radius;
+			}
+			break;
+
+		case b2Manifold::e_faceA:
+			{
+				normal = cc->bodyA->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+			}
+			break;
+
+		case b2Manifold::e_faceB:
+			{
+				normal = cc->bodyB->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+
+				// Ensure normal points from A to B
+				normal = -normal;
+			}
+			break;
+		}
+	}
+
+	b2Vec2 normal;
+	b2Vec2 point;
+	float32 separation;
+};
+
+// Push out the toi body to provide clearance for further simulation.
+bool b2TOISolver::Solve(float32 baumgarte)
+{
+	float32 minSeparation = 0.0f;
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2TOIConstraint* c = m_constraints + i;
+		b2Body* bodyA = c->bodyA;
+		b2Body* bodyB = c->bodyB;
+
+		float32 massA = bodyA->m_mass;
+		float32 massB = bodyB->m_mass;
+
+		// Only the TOI body should move.
+		if (bodyA == m_toiBody)
+		{
+			massB = 0.0f;
+		}
+		else
+		{
+			massA = 0.0f;
+		}
+
+		float32 invMassA = massA * bodyA->m_invMass;
+		float32 invIA = massA * bodyA->m_invI;
+		float32 invMassB = massB * bodyB->m_invMass;
+		float32 invIB = massB * bodyB->m_invI;
+
+		// Solve normal constraints
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			b2TOISolverManifold psm;
+			psm.Initialize(c, j);
+			b2Vec2 normal = psm.normal;
+
+			b2Vec2 point = psm.point;
+			float32 separation = psm.separation;
+
+			b2Vec2 rA = point - bodyA->m_sweep.c;
+			b2Vec2 rB = point - bodyB->m_sweep.c;
+
+			// Track max constraint error.
+			minSeparation = b2Min(minSeparation, separation);
+
+			// Prevent large corrections and allow slop.
+			float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+			// Compute the effective mass.
+			float32 rnA = b2Cross(rA, normal);
+			float32 rnB = b2Cross(rB, normal);
+			float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
+
+			// Compute normal impulse
+			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+			b2Vec2 P = impulse * normal;
+
+			bodyA->m_sweep.c -= invMassA * P;
+			bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
+			bodyA->SynchronizeTransform();
+
+			bodyB->m_sweep.c += invMassB * P;
+			bodyB->m_sweep.a += invIB * b2Cross(rB, P);
+			bodyB->SynchronizeTransform();
+		}
+	}
+
+	// We can't expect minSpeparation >= -b2_linearSlop because we don't
+	// push the separation above -b2_linearSlop.
+	return minSeparation >= -1.5f * b2_linearSlop;
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.h

@@ -0,0 +1,51 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* 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_TOI_SOLVER_H
+#define B2_TOI_SOLVER_H
+
+#include <Box2D/Common/b2Math.h>
+
+class b2Contact;
+class b2Body;
+struct b2TOIConstraint;
+class b2StackAllocator;
+
+/// This is a pure position solver for a single movable body in contact with
+/// multiple non-moving bodies.
+class b2TOISolver
+{
+public:
+	b2TOISolver(b2StackAllocator* allocator);
+	~b2TOISolver();
+
+	void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);
+	void Clear();
+
+	// Perform one solver iteration. Returns true if converged.
+	bool Solve(float32 baumgarte);
+
+private:
+
+	b2TOIConstraint* m_constraints;
+	int32 m_count;
+	b2Body* m_toiBody;
+	b2StackAllocator* m_allocator;
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.cpp

@@ -16,9 +16,9 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2DistanceJoint.h"
-#include "../b2Body.h"
-#include "../b2World.h"
+#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 // 1-D constrained system
 // m (v2 - v1) = lambda
@@ -38,10 +38,10 @@
 void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
 									const b2Vec2& anchor1, const b2Vec2& anchor2)
 {
-	body1 = b1;
-	body2 = b2;
-	localAnchor1 = body1->GetLocalPoint(anchor1);
-	localAnchor2 = body2->GetLocalPoint(anchor2);
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor1);
+	localAnchorB = bodyB->GetLocalPoint(anchor2);
 	b2Vec2 d = anchor2 - anchor1;
 	length = d.Length();
 }
@@ -50,27 +50,24 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
 b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
 : b2Joint(def)
 {
-	m_localAnchor1 = def->localAnchor1;
-	m_localAnchor2 = def->localAnchor2;
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
 	m_length = def->length;
 	m_frequencyHz = def->frequencyHz;
 	m_dampingRatio = def->dampingRatio;
 	m_impulse = 0.0f;
 	m_gamma = 0.0f;
 	m_bias = 0.0f;
-	m_inv_dt = 0.0f;
 }
 
 void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
 {
-	m_inv_dt = step.inv_dt;
-
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
 	// Compute the effective mass matrix.
-	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 	m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
 
 	// Handle singularity.
@@ -87,8 +84,8 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
 	float32 cr1u = b2Cross(r1, m_u);
 	float32 cr2u = b2Cross(r2, m_u);
 	float32 invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u;
-	b2Assert(invMass > B2_FLT_EPSILON);
-	m_mass = 1.0f / invMass;
+
+	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
 
 	if (m_frequencyHz > 0.0f)
 	{
@@ -104,15 +101,19 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
 		float32 k = m_mass * omega * omega;
 
 		// magic formulas
-		m_gamma = 1.0f / (step.dt * (d + step.dt * k));
+		m_gamma = step.dt * (d + step.dt * k);
+		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
 		m_bias = C * step.dt * k * m_gamma;
 
-		m_mass = 1.0f / (invMass + m_gamma);
+		m_mass = invMass + m_gamma;
+		m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
 	}
 
 	if (step.warmStarting)
 	{
+		// Scale the impulse to support a variable time step.
 		m_impulse *= step.dtRatio;
+
 		b2Vec2 P = m_impulse * m_u;
 		b1->m_linearVelocity -= b1->m_invMass * P;
 		b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
@@ -129,11 +130,11 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
 {
 	B2_NOT_USED(step);
 
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
-	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 
 	// Cdot = dot(u, v + cross(w, r))
 	b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
@@ -150,18 +151,21 @@ void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
 	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
 }
 
-bool b2DistanceJoint::SolvePositionConstraints()
+bool b2DistanceJoint::SolvePositionConstraints(float32 baumgarte)
 {
+	B2_NOT_USED(baumgarte);
+
 	if (m_frequencyHz > 0.0f)
 	{
+		// There is no position correction for soft distance constraints.
 		return true;
 	}
 
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
-	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 
 	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
 
@@ -184,23 +188,24 @@ bool b2DistanceJoint::SolvePositionConstraints()
 	return b2Abs(C) < b2_linearSlop;
 }
 
-b2Vec2 b2DistanceJoint::GetAnchor1() const
+b2Vec2 b2DistanceJoint::GetAnchorA() const
 {
-	return m_body1->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
 }
 
-b2Vec2 b2DistanceJoint::GetAnchor2() const
+b2Vec2 b2DistanceJoint::GetAnchorB() const
 {
-	return m_body2->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
 }
 
-b2Vec2 b2DistanceJoint::GetReactionForce() const
+b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
 {
-	b2Vec2 F = (m_inv_dt * m_impulse) * m_u;
+	b2Vec2 F = (inv_dt * m_impulse) * m_u;
 	return F;
 }
 
-float32 b2DistanceJoint::GetReactionTorque() const
+float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
 {
+	B2_NOT_USED(inv_dt);
 	return 0.0f;
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2DistanceJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_DISTANCE_JOINT_H
 #define B2_DISTANCE_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 /// Distance joint definition. This requires defining an
 /// anchor point on both bodies and the non-zero length of the
@@ -32,8 +32,8 @@ struct b2DistanceJointDef : public b2JointDef
 	b2DistanceJointDef()
 	{
 		type = e_distanceJoint;
-		localAnchor1.Set(0.0f, 0.0f);
-		localAnchor2.Set(0.0f, 0.0f);
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.Set(0.0f, 0.0f);
 		length = 1.0f;
 		frequencyHz = 0.0f;
 		dampingRatio = 0.0f;
@@ -41,19 +41,19 @@ struct b2DistanceJointDef : public b2JointDef
 
 	/// Initialize the bodies, anchors, and length using the world
 	/// anchors.
-	void Initialize(b2Body* body1, b2Body* body2,
-					const b2Vec2& anchor1, const b2Vec2& anchor2);
+	void Initialize(b2Body* bodyA, b2Body* bodyB,
+					const b2Vec2& anchorA, const b2Vec2& anchorB);
 
 	/// The local anchor point relative to body1's origin.
-	b2Vec2 localAnchor1;
+	b2Vec2 localAnchorA;
 
 	/// The local anchor point relative to body2's origin.
-	b2Vec2 localAnchor2;
+	b2Vec2 localAnchorB;
 
-	/// The equilibrium length between the anchor points.
+	/// The natural length between the anchor points.
 	float32 length;
 
-	/// The response speed.
+	/// The mass-spring-damper frequency in Hertz.
 	float32 frequencyHz;
 
 	/// The damping ratio. 0 = no damping, 1 = critical damping.
@@ -67,19 +67,33 @@ class b2DistanceJoint : public b2Joint
 {
 public:
 
-	b2Vec2 GetAnchor1() const;
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce() const;
-	float32 GetReactionTorque() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
-	//--------------- Internals Below -------------------
+	/// Set/get the natural length.
+	/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
+	void SetLength(float32 length);
+	float32 GetLength() const;
 
+	// Set/get frequency in Hz.
+	void SetFrequency(float32 hz);
+	float32 GetFrequency() const;
+
+	// Set/get damping ratio.
+	void SetDampingRatio(float32 ratio);
+	float32 GetDampingRatio() const;
+
+protected:
+
+	friend class b2Joint;
 	b2DistanceJoint(const b2DistanceJointDef* data);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints();
+	bool SolvePositionConstraints(float32 baumgarte);
 
 	b2Vec2 m_localAnchor1;
 	b2Vec2 m_localAnchor2;
@@ -89,8 +103,38 @@ public:
 	float32 m_gamma;
 	float32 m_bias;
 	float32 m_impulse;
-	float32 m_mass;		// effective mass for the constraint.
+	float32 m_mass;
 	float32 m_length;
 };
 
+inline void b2DistanceJoint::SetLength(float32 length)
+{
+	m_length = length;
+}
+
+inline float32 b2DistanceJoint::GetLength() const
+{
+	return m_length;
+}
+
+inline void b2DistanceJoint::SetFrequency(float32 hz)
+{
+	m_frequencyHz = hz;
+}
+
+inline float32 b2DistanceJoint::GetFrequency() const
+{
+	return m_frequencyHz;
+}
+
+inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
+{
+	m_dampingRatio = ratio;
+}
+
+inline float32 b2DistanceJoint::GetDampingRatio() const
+{
+	return m_dampingRatio;
+}
+
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp

@@ -0,0 +1,229 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2FrictionJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Point-to-point constraint
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+}
+
+b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+
+	m_linearImpulse.SetZero();
+	m_angularImpulse = 0.0f;
+
+	m_maxForce = def->maxForce;
+	m_maxTorque = def->maxTorque;
+}
+
+void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	// Compute the effective mass matrix.
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = mA + mB;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;		K1.col2.y = mA + mB;
+
+	b2Mat22 K2;
+	K2.col1.x =  iA * rA.y * rA.y;	K2.col2.x = -iA * rA.x * rA.y;
+	K2.col1.y = -iA * rA.x * rA.y;	K2.col2.y =  iA * rA.x * rA.x;
+
+	b2Mat22 K3;
+	K3.col1.x =  iB * rB.y * rB.y;	K3.col2.x = -iB * rB.x * rB.y;
+	K3.col1.y = -iB * rB.x * rB.y;	K3.col2.y =  iB * rB.x * rB.x;
+
+	b2Mat22 K = K1 + K2 + K3;
+	m_linearMass = K.GetInverse();
+
+	m_angularMass = iA + iB;
+	if (m_angularMass > 0.0f)
+	{
+		m_angularMass = 1.0f / m_angularMass;
+	}
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_linearImpulse *= step.dtRatio;
+		m_angularImpulse *= step.dtRatio;
+
+		b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
+
+		bA->m_linearVelocity -= mA * P;
+		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
+
+		bB->m_linearVelocity += mB * P;
+		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
+	}
+	else
+	{
+		m_linearImpulse.SetZero();
+		m_angularImpulse = 0.0f;
+	}
+}
+
+void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	float32 wA = bA->m_angularVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float32 wB = bB->m_angularVelocity;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// Solve angular friction
+	{
+		float32 Cdot = wB - wA;
+		float32 impulse = -m_angularMass * Cdot;
+
+		float32 oldImpulse = m_angularImpulse;
+		float32 maxImpulse = step.dt * m_maxTorque;
+		m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_angularImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// Solve linear friction
+	{
+		b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+
+		b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
+		b2Vec2 oldImpulse = m_linearImpulse;
+		m_linearImpulse += impulse;
+
+		float32 maxImpulse = step.dt * m_maxForce;
+
+		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
+		{
+			m_linearImpulse.Normalize();
+			m_linearImpulse *= maxImpulse;
+		}
+
+		impulse = m_linearImpulse - oldImpulse;
+
+		vA -= mA * impulse;
+		wA -= iA * b2Cross(rA, impulse);
+
+		vB += mB * impulse;
+		wB += iB * b2Cross(rB, impulse);
+	}
+
+	bA->m_linearVelocity = vA;
+	bA->m_angularVelocity = wA;
+	bB->m_linearVelocity = vB;
+	bB->m_angularVelocity = wB;
+}
+
+bool b2FrictionJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	return true;
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * m_linearImpulse;
+}
+
+float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_angularImpulse;
+}
+
+void b2FrictionJoint::SetMaxForce(float32 force)
+{
+	b2Assert(b2IsValid(force) && force >= 0.0f);
+	m_maxForce = force;
+}
+
+float32 b2FrictionJoint::GetMaxForce() const
+{
+	return m_maxForce;
+}
+
+void b2FrictionJoint::SetMaxTorque(float32 torque)
+{
+	b2Assert(b2IsValid(torque) && torque >= 0.0f);
+	m_maxTorque = torque;
+}
+
+float32 b2FrictionJoint::GetMaxTorque() const
+{
+	return m_maxTorque;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h

@@ -0,0 +1,99 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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_FRICTION_JOINT_H
+#define B2_FRICTION_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Friction joint definition.
+struct b2FrictionJointDef : public b2JointDef
+{
+	b2FrictionJointDef()
+	{
+		type = e_frictionJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		maxForce = 0.0f;
+		maxTorque = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
+
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
+
+	/// The maximum friction force in N.
+	float32 maxForce;
+
+	/// The maximum friction torque in N-m.
+	float32 maxTorque;
+};
+
+/// Friction joint. This is used for top-down friction.
+/// It provides 2D translational friction and angular friction.
+class b2FrictionJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Set the maximum friction force in N.
+	void SetMaxForce(float32 force);
+
+	/// Get the maximum friction force in N.
+	float32 GetMaxForce() const;
+
+	/// Set the maximum friction torque in N*m.
+	void SetMaxTorque(float32 torque);
+
+	/// Get the maximum friction torque in N*m.
+	float32 GetMaxTorque() const;
+
+protected:
+
+	friend class b2Joint;
+
+	b2FrictionJoint(const b2FrictionJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+
+	b2Mat22 m_linearMass;
+	float32 m_angularMass;
+
+	b2Vec2 m_linearImpulse;
+	float32 m_angularImpulse;
+
+	float32 m_maxForce;
+	float32 m_maxTorque;
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.cpp

@@ -16,11 +16,11 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2GearJoint.h"
-#include "b2RevoluteJoint.h"
-#include "b2PrismaticJoint.h"
-#include "../b2Body.h"
-#include "../b2World.h"
+#include <Box2D/Dynamics/Joints/b2GearJoint.h>
+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
+#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 // Gear Joint:
 // C0 = (coordinate1 + ratio * coordinate2)_initial
@@ -50,8 +50,8 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 
 	b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
 	b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
-	b2Assert(def->joint1->GetBody1()->IsStatic());
-	b2Assert(def->joint2->GetBody1()->IsStatic());
+	b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody);
+	b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
 
 	m_revolute1 = NULL;
 	m_prismatic1 = NULL;
@@ -60,8 +60,8 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 
 	float32 coordinate1, coordinate2;
 
-	m_ground1 = def->joint1->GetBody1();
-	m_body1 = def->joint1->GetBody2();
+	m_ground1 = def->joint1->GetBodyA();
+	m_bodyA = def->joint1->GetBodyB();
 	if (type1 == e_revoluteJoint)
 	{
 		m_revolute1 = (b2RevoluteJoint*)def->joint1;
@@ -77,8 +77,8 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 		coordinate1 = m_prismatic1->GetJointTranslation();
 	}
 
-	m_ground2 = def->joint2->GetBody1();
-	m_body2 = def->joint2->GetBody2();
+	m_ground2 = def->joint2->GetBodyA();
+	m_bodyB = def->joint2->GetBodyB();
 	if (type2 == e_revoluteJoint)
 	{
 		m_revolute2 = (b2RevoluteJoint*)def->joint2;
@@ -98,92 +98,93 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 
 	m_constant = coordinate1 + m_ratio * coordinate2;
 
-	m_force = 0.0f;
+	m_impulse = 0.0f;
 }
 
 void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
 {
 	b2Body* g1 = m_ground1;
 	b2Body* g2 = m_ground2;
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
 	float32 K = 0.0f;
 	m_J.SetZero();
 
 	if (m_revolute1)
 	{
-		m_J.angular1 = -1.0f;
+		m_J.angularA = -1.0f;
 		K += b1->m_invI;
 	}
 	else
 	{
-		b2Vec2 ug = b2Mul(g1->GetXForm().R, m_prismatic1->m_localXAxis1);
-		b2Vec2 r = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
+		b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
 		float32 crug = b2Cross(r, ug);
-		m_J.linear1 = -ug;
-		m_J.angular1 = -crug;
+		m_J.linearA = -ug;
+		m_J.angularA = -crug;
 		K += b1->m_invMass + b1->m_invI * crug * crug;
 	}
 
 	if (m_revolute2)
 	{
-		m_J.angular2 = -m_ratio;
+		m_J.angularB = -m_ratio;
 		K += m_ratio * m_ratio * b2->m_invI;
 	}
 	else
 	{
-		b2Vec2 ug = b2Mul(g2->GetXForm().R, m_prismatic2->m_localXAxis1);
-		b2Vec2 r = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
+		b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 		float32 crug = b2Cross(r, ug);
-		m_J.linear2 = -m_ratio * ug;
-		m_J.angular2 = -m_ratio * crug;
+		m_J.linearB = -m_ratio * ug;
+		m_J.angularB = -m_ratio * crug;
 		K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
 	}
 
 	// Compute effective mass.
-	b2Assert(K > 0.0f);
-	m_mass = 1.0f / K;
+	m_mass = K > 0.0f ? 1.0f / K : 0.0f;
 
 	if (step.warmStarting)
 	{
 		// Warm starting.
-		float32 P = B2FORCE_SCALE(step.dt) * m_force;
-		b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
-		b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
-		b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
-		b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+		b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
+		b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
+		b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
+		b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
 	}
 	else
 	{
-		m_force = 0.0f;
+		m_impulse = 0.0f;
 	}
 }
 
 void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
 {
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	B2_NOT_USED(step);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
 	float32 Cdot = m_J.Compute(	b1->m_linearVelocity, b1->m_angularVelocity,
 								b2->m_linearVelocity, b2->m_angularVelocity);
 
-	float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_mass * Cdot;
-	m_force += force;
+	float32 impulse = m_mass * (-Cdot);
+	m_impulse += impulse;
 
-	float32 P = B2FORCE_SCALE(step.dt) * force;
-	b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
-	b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
-	b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
-	b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+	b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
+	b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
+	b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
+	b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
 }
 
-bool b2GearJoint::SolvePositionConstraints()
+bool b2GearJoint::SolvePositionConstraints(float32 baumgarte)
 {
+	B2_NOT_USED(baumgarte);
+	
 	float32 linearError = 0.0f;
 
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
 	float32 coordinate1, coordinate2;
 	if (m_revolute1)
@@ -206,48 +207,53 @@ bool b2GearJoint::SolvePositionConstraints()
 
 	float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
 
-	float32 impulse = -m_mass * C;
+	float32 impulse = m_mass * (-C);
 
-	b1->m_sweep.c += b1->m_invMass * impulse * m_J.linear1;
-	b1->m_sweep.a += b1->m_invI * impulse * m_J.angular1;
-	b2->m_sweep.c += b2->m_invMass * impulse * m_J.linear2;
-	b2->m_sweep.a += b2->m_invI * impulse * m_J.angular2;
+	b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA;
+	b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
+	b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
+	b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
 
 	b1->SynchronizeTransform();
 	b2->SynchronizeTransform();
 
+	// TODO_ERIN not implemented
 	return linearError < b2_linearSlop;
 }
 
-b2Vec2 b2GearJoint::GetAnchor1() const
+b2Vec2 b2GearJoint::GetAnchorA() const
 {
-	return m_body1->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
 }
 
-b2Vec2 b2GearJoint::GetAnchor2() const
+b2Vec2 b2GearJoint::GetAnchorB() const
 {
-	return m_body2->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
 }
 
-b2Vec2 b2GearJoint::GetReactionForce() const
+b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
 {
 	// TODO_ERIN not tested
-	b2Vec2 F = B2FORCE_SCALE(m_force) * m_J.linear2;
-	return F;
+	b2Vec2 P = m_impulse * m_J.linearB;
+	return inv_dt * P;
 }
 
-float32 b2GearJoint::GetReactionTorque() const
+float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
 {
 	// TODO_ERIN not tested
-	b2Vec2 r = b2Mul(m_body2->GetXForm().R, m_localAnchor2 - m_body2->GetLocalCenter());
-	b2Vec2 F = m_force * m_J.linear2;
-	float32 T = B2FORCE_SCALE(m_force * m_J.angular2 - b2Cross(r, F));
-	return T;
+	b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
+	b2Vec2 P = m_impulse * m_J.linearB;
+	float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
+	return inv_dt * L;
+}
+
+void b2GearJoint::SetRatio(float32 ratio)
+{
+	b2Assert(b2IsValid(ratio));
+	m_ratio = ratio;
 }
 
 float32 b2GearJoint::GetRatio() const
 {
 	return m_ratio;
 }
-
-

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2GearJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_GEAR_JOINT_H
 #define B2_GEAR_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 class b2RevoluteJoint;
 class b2PrismaticJoint;
@@ -60,22 +60,24 @@ struct b2GearJointDef : public b2JointDef
 class b2GearJoint : public b2Joint
 {
 public:
-	b2Vec2 GetAnchor1() const;
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce() const;
-	float32 GetReactionTorque() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
-	/// Get the gear ratio.
+	/// Set/Get the gear ratio.
+	void SetRatio(float32 ratio);
 	float32 GetRatio() const;
 
-	//--------------- Internals Below -------------------
+protected:
 
+	friend class b2Joint;
 	b2GearJoint(const b2GearJointDef* data);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints();
+	bool SolvePositionConstraints(float32 baumgarte);
 
 	b2Body* m_ground1;
 	b2Body* m_ground2;
@@ -103,7 +105,7 @@ public:
 	float32 m_mass;
 
 	// Impulse for accumulation/warm starting.
-	float32 m_force;
+	float32 m_impulse;
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.cpp

@@ -16,17 +16,19 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2Joint.h"
-#include "b2DistanceJoint.h"
-#include "b2MouseJoint.h"
-#include "b2RevoluteJoint.h"
-#include "b2PrismaticJoint.h"
-#include "b2PulleyJoint.h"
-#include "b2GearJoint.h"
-#include "../b2Body.h"
-#include "../b2World.h"
-#include "../../Common/b2BlockAllocator.h"
-#include "../../Collision/b2BroadPhase.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
+#include <Box2D/Dynamics/Joints/b2LineJoint.h>
+#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
+#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
+#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
+#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
+#include <Box2D/Dynamics/Joints/b2GearJoint.h>
+#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
+#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2World.h>
+#include <Box2D/Common/b2BlockAllocator.h>
 
 #include <new>
 
@@ -78,6 +80,27 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
 		}
 		break;
 
+	case e_lineJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2LineJoint));
+			joint = new (mem) b2LineJoint((b2LineJointDef*)def);
+		}
+		break;
+
+	case e_weldJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2WeldJoint));
+			joint = new (mem) b2WeldJoint((b2WeldJointDef*)def);
+		}
+		break;
+        
+	case e_frictionJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
+			joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def);
+		}
+		break;
+
 	default:
 		b2Assert(false);
 		break;
@@ -115,6 +138,18 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
 		allocator->Free(joint, sizeof(b2GearJoint));
 		break;
 
+	case e_lineJoint:
+		allocator->Free(joint, sizeof(b2LineJoint));
+		break;
+    
+	case e_weldJoint:
+		allocator->Free(joint, sizeof(b2WeldJoint));
+		break;
+
+	case e_frictionJoint:
+		allocator->Free(joint, sizeof(b2FrictionJoint));
+		break;
+
 	default:
 		b2Assert(false);
 		break;
@@ -123,12 +158,29 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
 
 b2Joint::b2Joint(const b2JointDef* def)
 {
+	b2Assert(def->bodyA != def->bodyB);
+
 	m_type = def->type;
 	m_prev = NULL;
 	m_next = NULL;
-	m_body1 = def->body1;
-	m_body2 = def->body2;
+	m_bodyA = def->bodyA;
+	m_bodyB = def->bodyB;
 	m_collideConnected = def->collideConnected;
 	m_islandFlag = false;
 	m_userData = def->userData;
+
+	m_edgeA.joint = NULL;
+	m_edgeA.other = NULL;
+	m_edgeA.prev = NULL;
+	m_edgeA.next = NULL;
+
+	m_edgeB.joint = NULL;
+	m_edgeB.other = NULL;
+	m_edgeB.prev = NULL;
+	m_edgeB.next = NULL;
+}
+
+bool b2Joint::IsActive() const
+{
+	return m_bodyA->IsActive() && m_bodyB->IsActive();
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2Joint.h

@@ -16,10 +16,10 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef JOINT_H
-#define JOINT_H
+#ifndef B2_JOINT_H
+#define B2_JOINT_H
 
-#include "../../Common/b2Math.h"
+#include <Box2D/Common/b2Math.h>
 
 class b2Body;
 class b2Joint;
@@ -34,7 +34,10 @@ enum b2JointType
 	e_distanceJoint,
 	e_pulleyJoint,
 	e_mouseJoint,
-	e_gearJoint
+	e_gearJoint,
+	e_lineJoint,
+    e_weldJoint,
+	e_frictionJoint,
 };
 
 enum b2LimitState
@@ -47,10 +50,10 @@ enum b2LimitState
 
 struct b2Jacobian
 {
-	b2Vec2 linear1;
-	float32 angular1;
-	b2Vec2 linear2;
-	float32 angular2;
+	b2Vec2 linearA;
+	float32 angularA;
+	b2Vec2 linearB;
+	float32 angularB;
 
 	void SetZero();
 	void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
@@ -77,8 +80,8 @@ struct b2JointDef
 	{
 		type = e_unknownJoint;
 		userData = NULL;
-		body1 = NULL;
-		body2 = NULL;
+		bodyA = NULL;
+		bodyB = NULL;
 		collideConnected = false;
 	}
 
@@ -89,10 +92,10 @@ struct b2JointDef
 	void* userData;
 
 	/// The first attached body.
-	b2Body* body1;
+	b2Body* bodyA;
 
 	/// The second attached body.
-	b2Body* body2;
+	b2Body* bodyB;
 
 	/// Set this flag to true if the attached bodies should collide.
 	bool collideConnected;
@@ -108,33 +111,35 @@ public:
 	b2JointType GetType() const;
 
 	/// Get the first body attached to this joint.
-	b2Body* GetBody1();
+	b2Body* GetBodyA();
 
 	/// Get the second body attached to this joint.
-	b2Body* GetBody2();
+	b2Body* GetBodyB();
 
-	/// Get the anchor point on body1 in world coordinates.
-	virtual b2Vec2 GetAnchor1() const = 0;
+	/// Get the anchor point on bodyA in world coordinates.
+	virtual b2Vec2 GetAnchorA() const = 0;
 
-	/// Get the anchor point on body2 in world coordinates.
-	virtual b2Vec2 GetAnchor2() const = 0;
+	/// Get the anchor point on bodyB in world coordinates.
+	virtual b2Vec2 GetAnchorB() const = 0;
 
-	/// Get the reaction force on body2 at the joint anchor.
-	virtual b2Vec2 GetReactionForce() const = 0;
+	/// Get the reaction force on body2 at the joint anchor in Newtons.
+	virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
 
-	/// Get the reaction torque on body2.
-	virtual float32 GetReactionTorque() const = 0;
+	/// Get the reaction torque on body2 in N*m.
+	virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
 
 	/// Get the next joint the world joint list.
 	b2Joint* GetNext();
 
 	/// Get the user data pointer.
-	void* GetUserData();
+	void* GetUserData() const;
 
 	/// Set the user data pointer.
 	void SetUserData(void* data);
 
-	//--------------- Internals Below -------------------
+	/// Short-cut function to determine if either body is inactive.
+	bool IsActive() const;
+
 protected:
 	friend class b2World;
 	friend class b2Body;
@@ -150,42 +155,42 @@ protected:
 	virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
 
 	// This returns true if the position errors are within tolerance.
-	virtual void InitPositionConstraints() {}
-	virtual bool SolvePositionConstraints() = 0;
+	virtual bool SolvePositionConstraints(float32 baumgarte) = 0;
 
 	b2JointType m_type;
 	b2Joint* m_prev;
 	b2Joint* m_next;
-	b2JointEdge m_node1;
-	b2JointEdge m_node2;
-	b2Body* m_body1;
-	b2Body* m_body2;
-
-	float32 m_inv_dt;
+	b2JointEdge m_edgeA;
+	b2JointEdge m_edgeB;
+	b2Body* m_bodyA;
+	b2Body* m_bodyB;
 
 	bool m_islandFlag;
-	
+	bool m_collideConnected;
 
 	void* m_userData;
-public:
-	bool m_collideConnected;
+
+	// Cache here per time step to reduce cache misses.
+	b2Vec2 m_localCenterA, m_localCenterB;
+	float32 m_invMassA, m_invIA;
+	float32 m_invMassB, m_invIB;
 };
 
 inline void b2Jacobian::SetZero()
 {
-	linear1.SetZero(); angular1 = 0.0f;
-	linear2.SetZero(); angular2 = 0.0f;
+	linearA.SetZero(); angularA = 0.0f;
+	linearB.SetZero(); angularB = 0.0f;
 }
 
 inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
 {
-	linear1 = x1; angular1 = a1;
-	linear2 = x2; angular2 = a2;
+	linearA = x1; angularA = a1;
+	linearB = x2; angularB = a2;
 }
 
 inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
 {
-	return b2Dot(linear1, x1) + angular1 * a1 + b2Dot(linear2, x2) + angular2 * a2;
+	return b2Dot(linearA, x1) + angularA * a1 + b2Dot(linearB, x2) + angularB * a2;
 }
 
 inline b2JointType b2Joint::GetType() const
@@ -193,14 +198,14 @@ inline b2JointType b2Joint::GetType() const
 	return m_type;
 }
 
-inline b2Body* b2Joint::GetBody1()
+inline b2Body* b2Joint::GetBodyA()
 {
-	return m_body1;
+	return m_bodyA;
 }
 
-inline b2Body* b2Joint::GetBody2()
+inline b2Body* b2Joint::GetBodyB()
 {
-	return m_body2;
+	return m_bodyB;
 }
 
 inline b2Joint* b2Joint::GetNext()
@@ -208,7 +213,7 @@ inline b2Joint* b2Joint::GetNext()
 	return m_next;
 }
 
-inline void* b2Joint::GetUserData()
+inline void* b2Joint::GetUserData() const
 {
 	return m_userData;
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.cpp

@@ -0,0 +1,591 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2LineJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(perp, d)
+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+// a = perp
+// s1 = cross(d + r1, a) = cross(p2 - x1, a)
+// s2 = cross(r2, a) = cross(p2 - x2, a)
+
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+// Block Solver
+// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
+// when the mass has poor distribution (leading to large torques about the joint anchor points).
+//
+// The Jacobian has 3 rows:
+// J = [-uT -s1 uT s2] // linear
+//     [-vT -a1 vT a2] // limit
+//
+// u = perp
+// v = axis
+// s1 = cross(d + r1, u), s2 = cross(r2, u)
+// a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+// M * (v2 - v1) = JT * df
+// J * v2 = bias
+//
+// v2 = v1 + invM * JT * df
+// J * (v1 + invM * JT * df) = bias
+// K * df = bias - J * v1 = -Cdot
+// K = J * invM * JT
+// Cdot = J * v1 - bias
+//
+// Now solve for f2.
+// df = f2 - f1
+// K * (f2 - f1) = -Cdot
+// f2 = invK * (-Cdot) + f1
+//
+// Clamp accumulated limit impulse.
+// lower: f2(2) = max(f2(2), 0)
+// upper: f2(2) = min(f2(2), 0)
+//
+// Solve for correct f2(1)
+// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1
+//                = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2)
+// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1)
+// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
+//
+// Now compute impulse to be applied:
+// df = f2 - f1
+
+void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	localAxisA = bodyA->GetLocalVector(axis);
+}
+
+b2LineJoint::b2LineJoint(const b2LineJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+	m_localXAxis1 = def->localAxisA;
+	m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+
+	m_impulse.SetZero();
+	m_motorMass = 0.0;
+	m_motorImpulse = 0.0f;
+
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+	m_maxMotorForce = def->maxMotorForce;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+	m_limitState = e_inactiveLimit;
+
+	m_axis.SetZero();
+	m_perp.SetZero();
+}
+
+void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	m_localCenterA = b1->GetLocalCenter();
+	m_localCenterB = b2->GetLocalCenter();
+
+	b2Transform xf1 = b1->GetTransform();
+	b2Transform xf2 = b2->GetTransform();
+
+	// Compute the effective masses.
+	b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
+	b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
+	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	m_invMassA = b1->m_invMass;
+	m_invIA = b1->m_invI;
+	m_invMassB = b2->m_invMass;
+	m_invIB = b2->m_invI;
+
+	// Compute motor Jacobian and effective mass.
+	{
+		m_axis = b2Mul(xf1.R, m_localXAxis1);
+		m_a1 = b2Cross(d + r1, m_axis);
+		m_a2 = b2Cross(r2, m_axis);
+
+		m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
+		if (m_motorMass > b2_epsilon)
+		{
+			m_motorMass = 1.0f / m_motorMass;
+		}
+		else
+		{
+			m_motorMass = 0.0f;
+		}
+	}
+
+	// Prismatic constraint.
+	{
+		m_perp = b2Mul(xf1.R, m_localYAxis1);
+
+		m_s1 = b2Cross(d + r1, m_perp);
+		m_s2 = b2Cross(r2, m_perp);
+
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+		float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+		float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+		m_K.col1.Set(k11, k12);
+		m_K.col2.Set(k12, k22);
+	}
+
+	// Compute motor and limit terms.
+	if (m_enableLimit)
+	{
+		float32 jointTranslation = b2Dot(m_axis, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			m_limitState = e_equalLimits;
+		}
+		else if (jointTranslation <= m_lowerTranslation)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_limitState = e_atLowerLimit;
+				m_impulse.y = 0.0f;
+			}
+		}
+		else if (jointTranslation >= m_upperTranslation)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_limitState = e_atUpperLimit;
+				m_impulse.y = 0.0f;
+			}
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_impulse.y = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		// Account for variable time step.
+		m_impulse *= step.dtRatio;
+		m_motorImpulse *= step.dtRatio;
+
+		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;
+		float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
+		float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;
+
+		b1->m_linearVelocity -= m_invMassA * P;
+		b1->m_angularVelocity -= m_invIA * L1;
+
+		b2->m_linearVelocity += m_invMassB * P;
+		b2->m_angularVelocity += m_invIB * L2;
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+	}
+}
+
+void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	// Solve linear motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = step.dt * m_maxMotorForce;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		b2Vec2 P = impulse * m_axis;
+		float32 L1 = impulse * m_a1;
+		float32 L2 = impulse * m_a2;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+
+	float32 Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		// Solve prismatic and limit constraint in block form.
+		float32 Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		b2Vec2 Cdot(Cdot1, Cdot2);
+
+		b2Vec2 f1 = m_impulse;
+		b2Vec2 df =  m_K.Solve(-Cdot);
+		m_impulse += df;
+
+		if (m_limitState == e_atLowerLimit)
+		{
+			m_impulse.y = b2Max(m_impulse.y, 0.0f);
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			m_impulse.y = b2Min(m_impulse.y, 0.0f);
+		}
+
+		// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
+		float32 b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;
+		float32 f2r;
+		if (m_K.col1.x != 0.0f)
+		{
+			f2r = b / m_K.col1.x + f1.x;
+		}
+		else
+		{
+			f2r = f1.x;	
+		}
+
+		m_impulse.x = f2r;
+
+		df = m_impulse - f1;
+
+		b2Vec2 P = df.x * m_perp + df.y * m_axis;
+		float32 L1 = df.x * m_s1 + df.y * m_a1;
+		float32 L2 = df.x * m_s2 + df.y * m_a2;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+	else
+	{
+		// Limit is inactive, just solve the prismatic constraint in block form.
+		float32 df;
+		if (m_K.col1.x != 0.0f)
+		{
+			df = - Cdot1 / m_K.col1.x;
+		}
+		else
+		{
+			df = 0.0f;
+		}
+		m_impulse.x += df;
+
+		b2Vec2 P = df * m_perp;
+		float32 L1 = df * m_s1;
+		float32 L2 = df * m_s2;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+
+	b1->m_linearVelocity = v1;
+	b1->m_angularVelocity = w1;
+	b2->m_linearVelocity = v2;
+	b2->m_angularVelocity = w2;
+}
+
+bool b2LineJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 c1 = b1->m_sweep.c;
+	float32 a1 = b1->m_sweep.a;
+
+	b2Vec2 c2 = b2->m_sweep.c;
+	float32 a2 = b2->m_sweep.a;
+
+	// Solve linear limit constraint.
+	float32 linearError = 0.0f, angularError = 0.0f;
+	bool active = false;
+	float32 C2 = 0.0f;
+
+	b2Mat22 R1(a1), R2(a2);
+
+	b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
+	b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
+	b2Vec2 d = c2 + r2 - c1 - r1;
+
+	if (m_enableLimit)
+	{
+		m_axis = b2Mul(R1, m_localXAxis1);
+
+		m_a1 = b2Cross(d + r1, m_axis);
+		m_a2 = b2Cross(r2, m_axis);
+
+		float32 translation = b2Dot(m_axis, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			// Prevent large angular corrections
+			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+			linearError = b2Abs(translation);
+			active = true;
+		}
+		else if (translation <= m_lowerTranslation)
+		{
+			// Prevent large linear corrections and allow some slop.
+			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+			linearError = m_lowerTranslation - translation;
+			active = true;
+		}
+		else if (translation >= m_upperTranslation)
+		{
+			// Prevent large linear corrections and allow some slop.
+			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+			linearError = translation - m_upperTranslation;
+			active = true;
+		}
+	}
+
+	m_perp = b2Mul(R1, m_localYAxis1);
+
+	m_s1 = b2Cross(d + r1, m_perp);
+	m_s2 = b2Cross(r2, m_perp);
+
+	b2Vec2 impulse;
+	float32 C1;
+	C1 = b2Dot(m_perp, d);
+
+	linearError = b2Max(linearError, b2Abs(C1));
+	angularError = 0.0f;
+
+	if (active)
+	{
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+		float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+		float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+		m_K.col1.Set(k11, k12);
+		m_K.col2.Set(k12, k22);
+
+		b2Vec2 C;
+		C.x = C1;
+		C.y = C2;
+
+		impulse = m_K.Solve(-C);
+	}
+	else
+	{
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+
+		float32 impulse1;
+		if (k11 != 0.0f)
+		{
+			impulse1 = - C1 / k11;
+		}
+		else
+		{
+			impulse1 = 0.0f;
+		}
+
+		impulse.x = impulse1;
+		impulse.y = 0.0f;
+	}
+
+	b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
+	float32 L1 = impulse.x * m_s1 + impulse.y * m_a1;
+	float32 L2 = impulse.x * m_s2 + impulse.y * m_a2;
+
+	c1 -= m_invMassA * P;
+	a1 -= m_invIA * L1;
+	c2 += m_invMassB * P;
+	a2 += m_invIB * L2;
+
+	// TODO_ERIN remove need for this.
+	b1->m_sweep.c = c1;
+	b1->m_sweep.a = a1;
+	b2->m_sweep.c = c2;
+	b2->m_sweep.a = a2;
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2LineJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2LineJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2LineJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis);
+}
+
+float32 b2LineJoint::GetReactionTorque(float32 inv_dt) const
+{
+	B2_NOT_USED(inv_dt);
+	return 0.0f;
+}
+
+float32 b2LineJoint::GetJointTranslation() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+	b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	float32 translation = b2Dot(d, axis);
+	return translation;
+}
+
+float32 b2LineJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+	return speed;
+}
+
+bool b2LineJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2LineJoint::EnableLimit(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableLimit = flag;
+}
+
+float32 b2LineJoint::GetLowerLimit() const
+{
+	return m_lowerTranslation;
+}
+
+float32 b2LineJoint::GetUpperLimit() const
+{
+	return m_upperTranslation;
+}
+
+void b2LineJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_lowerTranslation = lower;
+	m_upperTranslation = upper;
+}
+
+bool b2LineJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2LineJoint::EnableMotor(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableMotor = flag;
+}
+
+void b2LineJoint::SetMotorSpeed(float32 speed)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+void b2LineJoint::SetMaxMotorForce(float32 force)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_maxMotorForce = force;
+}
+
+float32 b2LineJoint::GetMotorForce() const
+{
+	return m_motorImpulse;
+}
+
+
+
+
+

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.h

@@ -0,0 +1,170 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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_LINE_JOINT_H
+#define B2_LINE_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Line joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space. Using local
+/// anchors and a local axis helps when saving and loading a game.
+struct b2LineJointDef : public b2JointDef
+{
+	b2LineJointDef()
+	{
+		type = e_lineJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		localAxisA.Set(1.0f, 0.0f);
+		enableLimit = false;
+		lowerTranslation = 0.0f;
+		upperTranslation = 0.0f;
+		enableMotor = false;
+		maxMotorForce = 0.0f;
+		motorSpeed = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The local translation axis in body1.
+	b2Vec2 localAxisA;
+
+	/// Enable/disable the joint limit.
+	bool enableLimit;
+
+	/// The lower translation limit, usually in meters.
+	float32 lowerTranslation;
+
+	/// The upper translation limit, usually in meters.
+	float32 upperTranslation;
+
+	/// Enable/disable the joint motor.
+	bool enableMotor;
+
+	/// The maximum motor torque, usually in N-m.
+	float32 maxMotorForce;
+
+	/// The desired motor speed in radians per second.
+	float32 motorSpeed;
+};
+
+/// A line joint. This joint provides two degrees of freedom: translation
+/// along an axis fixed in body1 and rotation in the plane. You can use a
+/// joint limit to restrict the range of motion and a joint motor to drive
+/// the motion or to model joint friction.
+class b2LineJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the current joint translation, usually in meters.
+	float32 GetJointTranslation() const;
+
+	/// Get the current joint translation speed, usually in meters per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	/// Enable/disable the joint limit.
+	void EnableLimit(bool flag);
+
+	/// Get the lower joint limit, usually in meters.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit, usually in meters.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits, usually in meters.
+	void SetLimits(float32 lower, float32 upper);
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed, usually in meters per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed, usually in meters per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set/Get the maximum motor force, usually in N.
+	void SetMaxMotorForce(float32 force);
+	float32 GetMaxMotorForce() const;
+
+	/// Get the current motor force, usually in N.
+	float32 GetMotorForce() const;
+
+protected:
+
+	friend class b2Joint;
+	b2LineJoint(const b2LineJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_localXAxis1;
+	b2Vec2 m_localYAxis1;
+
+	b2Vec2 m_axis, m_perp;
+	float32 m_s1, m_s2;
+	float32 m_a1, m_a2;
+
+	b2Mat22 m_K;
+	b2Vec2 m_impulse;
+
+	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
+	float32 m_motorImpulse;
+
+	float32 m_lowerTranslation;
+	float32 m_upperTranslation;
+	float32 m_maxMotorForce;
+	float32 m_motorSpeed;
+
+	bool m_enableLimit;
+	bool m_enableMotor;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2LineJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.cpp

@@ -0,0 +1,197 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2MouseJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// p = attached point, m = mouse point
+// C = p - m
+// Cdot = v
+//      = v + cross(w, r)
+// J = [I r_skew]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
+: b2Joint(def)
+{
+	b2Assert(def->target.IsValid());
+	b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
+	b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
+	b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
+
+	m_target = def->target;
+	m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target);
+
+	m_maxForce = def->maxForce;
+	m_impulse.SetZero();
+
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+
+	m_beta = 0.0f;
+	m_gamma = 0.0f;
+}
+
+void b2MouseJoint::SetTarget(const b2Vec2& target)
+{
+	if (m_bodyB->IsAwake() == false)
+	{
+		m_bodyB->SetAwake(true);
+	}
+	m_target = target;
+}
+
+const b2Vec2& b2MouseJoint::GetTarget() const
+{
+	return m_target;
+}
+
+void b2MouseJoint::SetMaxForce(float32 force)
+{
+	m_maxForce = force;
+}
+
+float32 b2MouseJoint::GetMaxForce() const
+{
+	return m_maxForce;
+}
+
+void b2MouseJoint::SetFrequency(float32 hz)
+{
+	m_frequencyHz = hz;
+}
+
+float32 b2MouseJoint::GetFrequency() const
+{
+	return m_frequencyHz;
+}
+
+void b2MouseJoint::SetDampingRatio(float32 ratio)
+{
+	m_dampingRatio = ratio;
+}
+
+float32 b2MouseJoint::GetDampingRatio() const
+{
+	return m_dampingRatio;
+}
+
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_bodyB;
+
+	float32 mass = b->GetMass();
+
+	// Frequency
+	float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+	// Damping coefficient
+	float32 d = 2.0f * mass * m_dampingRatio * omega;
+
+	// Spring stiffness
+	float32 k = mass * (omega * omega);
+
+	// magic formulas
+	// gamma has units of inverse mass.
+	// beta has units of inverse time.
+	b2Assert(d + step.dt * k > b2_epsilon);
+	m_gamma = step.dt * (d + step.dt * k);
+	if (m_gamma != 0.0f)
+	{
+		m_gamma = 1.0f / m_gamma;
+	}
+	m_beta = step.dt * k * m_gamma;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+
+	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	float32 invMass = b->m_invMass;
+	float32 invI = b->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = invMass;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;		K1.col2.y = invMass;
+
+	b2Mat22 K2;
+	K2.col1.x =  invI * r.y * r.y;	K2.col2.x = -invI * r.x * r.y;
+	K2.col1.y = -invI * r.x * r.y;	K2.col2.y =  invI * r.x * r.x;
+
+	b2Mat22 K = K1 + K2;
+	K.col1.x += m_gamma;
+	K.col2.y += m_gamma;
+
+	m_mass = K.GetInverse();
+
+	m_C = b->m_sweep.c + r - m_target;
+
+	// Cheat with some damping
+	b->m_angularVelocity *= 0.98f;
+
+	// Warm starting.
+	m_impulse *= step.dtRatio;
+	b->m_linearVelocity += invMass * m_impulse;
+	b->m_angularVelocity += invI * b2Cross(r, m_impulse);
+}
+
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_bodyB;
+
+	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+
+	// Cdot = v + cross(w, r)
+	b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+	b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
+
+	b2Vec2 oldImpulse = m_impulse;
+	m_impulse += impulse;
+	float32 maxImpulse = step.dt * m_maxForce;
+	if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
+	{
+		m_impulse *= maxImpulse / m_impulse.Length();
+	}
+	impulse = m_impulse - oldImpulse;
+
+	b->m_linearVelocity += b->m_invMass * impulse;
+	b->m_angularVelocity += b->m_invI * b2Cross(r, impulse);
+}
+
+b2Vec2 b2MouseJoint::GetAnchorA() const
+{
+	return m_target;
+}
+
+b2Vec2 b2MouseJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor);
+}
+
+b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * m_impulse;
+}
+
+float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * 0.0f;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2MouseJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_MOUSE_JOINT_H
 #define B2_MOUSE_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 /// Mouse joint definition. This requires a world target point,
 /// tuning parameters, and the time step.
@@ -32,7 +32,6 @@ struct b2MouseJointDef : public b2JointDef
 		maxForce = 0.0f;
 		frequencyHz = 5.0f;
 		dampingRatio = 0.7f;
-		timeStep = 1.0f / 60.0f;
 	}
 
 	/// The initial world target point. This is assumed
@@ -49,44 +48,55 @@ struct b2MouseJointDef : public b2JointDef
 
 	/// The damping ratio. 0 = no damping, 1 = critical damping.
 	float32 dampingRatio;
-
-	/// The time step used in the simulation.
-	float32 timeStep;
 };
 
 /// A mouse joint is used to make a point on a body track a
 /// specified world point. This a soft constraint with a maximum
 /// force. This allows the constraint to stretch and without
 /// applying huge forces.
+/// NOTE: this joint is not documented in the manual because it was
+/// developed to be used in the testbed. If you want to learn how to
+/// use the mouse joint, look at the testbed.
 class b2MouseJoint : public b2Joint
 {
 public:
 
 	/// Implements b2Joint.
-	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchorA() const;
 
 	/// Implements b2Joint.
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorB() const;
 
 	/// Implements b2Joint.
-	b2Vec2 GetReactionForce() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
 
 	/// Implements b2Joint.
-	float32 GetReactionTorque() const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
 	/// Use this to update the target point.
 	void SetTarget(const b2Vec2& target);
+	const b2Vec2& GetTarget() const;
 
-	//--------------- Internals Below -------------------
+	/// Set/get the maximum force in Newtons.
+	void SetMaxForce(float32 force);
+	float32 GetMaxForce() const;
+
+	/// Set/get the frequency in Hertz.
+	void SetFrequency(float32 hz);
+	float32 GetFrequency() const;
+
+	/// Set/get the damping ratio (dimensionless).
+	void SetDampingRatio(float32 ratio);
+	float32 GetDampingRatio() const;
+
+protected:
+	friend class b2Joint;
 
 	b2MouseJoint(const b2MouseJointDef* def);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints()
-	{
-		return true;
-	}
+	bool SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); return true; }
 
 	b2Vec2 m_localAnchor;
 	b2Vec2 m_target;
@@ -95,8 +105,10 @@ public:
 	b2Mat22 m_mass;		// effective mass for point-to-point constraint.
 	b2Vec2 m_C;				// position error
 	float32 m_maxForce;
-	float32 m_beta;			// bias factor
-	float32 m_gamma;		// softness
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+	float32 m_beta;
+	float32 m_gamma;
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp

@@ -0,0 +1,586 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2PrismaticJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(perp, d)
+// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
+// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+//     [0  -1  0  1]
+// a = perp
+// s1 = cross(d + r1, a) = cross(p2 - x1, a)
+// s2 = cross(r2, a) = cross(p2 - x2, a)
+
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+// Block Solver
+// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
+// when the mass has poor distribution (leading to large torques about the joint anchor points).
+//
+// The Jacobian has 3 rows:
+// J = [-uT -s1 uT s2] // linear
+//     [0   -1   0  1] // angular
+//     [-vT -a1 vT a2] // limit
+//
+// u = perp
+// v = axis
+// s1 = cross(d + r1, u), s2 = cross(r2, u)
+// a1 = cross(d + r1, v), a2 = cross(r2, v)
+
+// M * (v2 - v1) = JT * df
+// J * v2 = bias
+//
+// v2 = v1 + invM * JT * df
+// J * (v1 + invM * JT * df) = bias
+// K * df = bias - J * v1 = -Cdot
+// K = J * invM * JT
+// Cdot = J * v1 - bias
+//
+// Now solve for f2.
+// df = f2 - f1
+// K * (f2 - f1) = -Cdot
+// f2 = invK * (-Cdot) + f1
+//
+// Clamp accumulated limit impulse.
+// lower: f2(3) = max(f2(3), 0)
+// upper: f2(3) = min(f2(3), 0)
+//
+// Solve for correct f2(1:2)
+// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
+//                       = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
+// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
+// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+//
+// Now compute impulse to be applied:
+// df = f2 - f1
+
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	localAxis1 = bodyA->GetLocalVector(axis);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+	m_localXAxis1 = def->localAxis1;
+	m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+	m_refAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+	m_motorMass = 0.0;
+	m_motorImpulse = 0.0f;
+
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+	m_maxMotorForce = def->maxMotorForce;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+	m_limitState = e_inactiveLimit;
+
+	m_axis.SetZero();
+	m_perp.SetZero();
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	m_localCenterA = b1->GetLocalCenter();
+	m_localCenterB = b2->GetLocalCenter();
+
+	b2Transform xf1 = b1->GetTransform();
+	b2Transform xf2 = b2->GetTransform();
+
+	// Compute the effective masses.
+	b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
+	b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
+	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	m_invMassA = b1->m_invMass;
+	m_invIA = b1->m_invI;
+	m_invMassB = b2->m_invMass;
+	m_invIB = b2->m_invI;
+
+	// Compute motor Jacobian and effective mass.
+	{
+		m_axis = b2Mul(xf1.R, m_localXAxis1);
+		m_a1 = b2Cross(d + r1, m_axis);
+		m_a2 = b2Cross(r2, m_axis);
+
+		m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
+		if (m_motorMass > b2_epsilon)
+		{
+			m_motorMass = 1.0f / m_motorMass;
+		}
+	}
+
+	// Prismatic constraint.
+	{
+		m_perp = b2Mul(xf1.R, m_localYAxis1);
+
+		m_s1 = b2Cross(d + r1, m_perp);
+		m_s2 = b2Cross(r2, m_perp);
+
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+		float32 k12 = i1 * m_s1 + i2 * m_s2;
+		float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+		float32 k22 = i1 + i2;
+		float32 k23 = i1 * m_a1 + i2 * m_a2;
+		float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+		m_K.col1.Set(k11, k12, k13);
+		m_K.col2.Set(k12, k22, k23);
+		m_K.col3.Set(k13, k23, k33);
+	}
+
+	// Compute motor and limit terms.
+	if (m_enableLimit)
+	{
+		float32 jointTranslation = b2Dot(m_axis, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			m_limitState = e_equalLimits;
+		}
+		else if (jointTranslation <= m_lowerTranslation)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_limitState = e_atLowerLimit;
+				m_impulse.z = 0.0f;
+			}
+		}
+		else if (jointTranslation >= m_upperTranslation)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_limitState = e_atUpperLimit;
+				m_impulse.z = 0.0f;
+			}
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_impulse.z = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+		m_impulse.z = 0.0f;
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		// Account for variable time step.
+		m_impulse *= step.dtRatio;
+		m_motorImpulse *= step.dtRatio;
+
+		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
+		float32 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
+		float32 L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
+
+		b1->m_linearVelocity -= m_invMassA * P;
+		b1->m_angularVelocity -= m_invIA * L1;
+
+		b2->m_linearVelocity += m_invMassB * P;
+		b2->m_angularVelocity += m_invIB * L2;
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+	}
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	// Solve linear motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = step.dt * m_maxMotorForce;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		b2Vec2 P = impulse * m_axis;
+		float32 L1 = impulse * m_a1;
+		float32 L2 = impulse * m_a2;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+
+	b2Vec2 Cdot1;
+	Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
+	Cdot1.y = w2 - w1;
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		// Solve prismatic and limit constraint in block form.
+		float32 Cdot2;
+		Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+
+		b2Vec3 f1 = m_impulse;
+		b2Vec3 df =  m_K.Solve33(-Cdot);
+		m_impulse += df;
+
+		if (m_limitState == e_atLowerLimit)
+		{
+			m_impulse.z = b2Max(m_impulse.z, 0.0f);
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			m_impulse.z = b2Min(m_impulse.z, 0.0f);
+		}
+
+		// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
+		b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y);
+		b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
+		m_impulse.x = f2r.x;
+		m_impulse.y = f2r.y;
+
+		df = m_impulse - f1;
+
+		b2Vec2 P = df.x * m_perp + df.z * m_axis;
+		float32 L1 = df.x * m_s1 + df.y + df.z * m_a1;
+		float32 L2 = df.x * m_s2 + df.y + df.z * m_a2;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+	else
+	{
+		// Limit is inactive, just solve the prismatic constraint in block form.
+		b2Vec2 df = m_K.Solve22(-Cdot1);
+		m_impulse.x += df.x;
+		m_impulse.y += df.y;
+
+		b2Vec2 P = df.x * m_perp;
+		float32 L1 = df.x * m_s1 + df.y;
+		float32 L2 = df.x * m_s2 + df.y;
+
+		v1 -= m_invMassA * P;
+		w1 -= m_invIA * L1;
+
+		v2 += m_invMassB * P;
+		w2 += m_invIB * L2;
+	}
+
+	b1->m_linearVelocity = v1;
+	b1->m_angularVelocity = w1;
+	b2->m_linearVelocity = v2;
+	b2->m_angularVelocity = w2;
+}
+
+bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 c1 = b1->m_sweep.c;
+	float32 a1 = b1->m_sweep.a;
+
+	b2Vec2 c2 = b2->m_sweep.c;
+	float32 a2 = b2->m_sweep.a;
+
+	// Solve linear limit constraint.
+	float32 linearError = 0.0f, angularError = 0.0f;
+	bool active = false;
+	float32 C2 = 0.0f;
+
+	b2Mat22 R1(a1), R2(a2);
+
+	b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
+	b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
+	b2Vec2 d = c2 + r2 - c1 - r1;
+
+	if (m_enableLimit)
+	{
+		m_axis = b2Mul(R1, m_localXAxis1);
+
+		m_a1 = b2Cross(d + r1, m_axis);
+		m_a2 = b2Cross(r2, m_axis);
+
+		float32 translation = b2Dot(m_axis, d);
+		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+		{
+			// Prevent large angular corrections
+			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+			linearError = b2Abs(translation);
+			active = true;
+		}
+		else if (translation <= m_lowerTranslation)
+		{
+			// Prevent large linear corrections and allow some slop.
+			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+			linearError = m_lowerTranslation - translation;
+			active = true;
+		}
+		else if (translation >= m_upperTranslation)
+		{
+			// Prevent large linear corrections and allow some slop.
+			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+			linearError = translation - m_upperTranslation;
+			active = true;
+		}
+	}
+
+	m_perp = b2Mul(R1, m_localYAxis1);
+
+	m_s1 = b2Cross(d + r1, m_perp);
+	m_s2 = b2Cross(r2, m_perp);
+
+	b2Vec3 impulse;
+	b2Vec2 C1;
+	C1.x = b2Dot(m_perp, d);
+	C1.y = a2 - a1 - m_refAngle;
+
+	linearError = b2Max(linearError, b2Abs(C1.x));
+	angularError = b2Abs(C1.y);
+
+	if (active)
+	{
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+		float32 k12 = i1 * m_s1 + i2 * m_s2;
+		float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+		float32 k22 = i1 + i2;
+		float32 k23 = i1 * m_a1 + i2 * m_a2;
+		float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+		m_K.col1.Set(k11, k12, k13);
+		m_K.col2.Set(k12, k22, k23);
+		m_K.col3.Set(k13, k23, k33);
+
+		b2Vec3 C;
+		C.x = C1.x;
+		C.y = C1.y;
+		C.z = C2;
+
+		impulse = m_K.Solve33(-C);
+	}
+	else
+	{
+		float32 m1 = m_invMassA, m2 = m_invMassB;
+		float32 i1 = m_invIA, i2 = m_invIB;
+
+		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+		float32 k12 = i1 * m_s1 + i2 * m_s2;
+		float32 k22 = i1 + i2;
+
+		m_K.col1.Set(k11, k12, 0.0f);
+		m_K.col2.Set(k12, k22, 0.0f);
+
+		b2Vec2 impulse1 = m_K.Solve22(-C1);
+		impulse.x = impulse1.x;
+		impulse.y = impulse1.y;
+		impulse.z = 0.0f;
+	}
+
+	b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis;
+	float32 L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
+	float32 L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
+
+	c1 -= m_invMassA * P;
+	a1 -= m_invIA * L1;
+	c2 += m_invMassB * P;
+	a2 += m_invIB * L2;
+
+	// TODO_ERIN remove need for this.
+	b1->m_sweep.c = c1;
+	b1->m_sweep.a = a1;
+	b2->m_sweep.c = c2;
+	b2->m_sweep.a = a2;
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+	
+	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis);
+}
+
+float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_impulse.y;
+}
+
+float32 b2PrismaticJoint::GetJointTranslation() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+	b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	float32 translation = b2Dot(d, axis);
+	return translation;
+}
+
+float32 b2PrismaticJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+	return speed;
+}
+
+bool b2PrismaticJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2PrismaticJoint::EnableLimit(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableLimit = flag;
+}
+
+float32 b2PrismaticJoint::GetLowerLimit() const
+{
+	return m_lowerTranslation;
+}
+
+float32 b2PrismaticJoint::GetUpperLimit() const
+{
+	return m_upperTranslation;
+}
+
+void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_lowerTranslation = lower;
+	m_upperTranslation = upper;
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableMotor = flag;
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_maxMotorForce = force;
+}
+
+float32 b2PrismaticJoint::GetMotorForce() const
+{
+	return m_motorImpulse;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PrismaticJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_PRISMATIC_JOINT_H
 #define B2_PRISMATIC_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 /// Prismatic joint definition. This requires defining a line of
 /// motion using an axis and an anchor point. The definition uses local
@@ -27,13 +27,14 @@
 /// can violate the constraint slightly. The joint translation is zero
 /// when the local anchor points coincide in world space. Using local
 /// anchors and a local axis helps when saving and loading a game.
+/// @warning at least one body should by dynamic with a non-fixed rotation.
 struct b2PrismaticJointDef : public b2JointDef
 {
 	b2PrismaticJointDef()
 	{
 		type = e_prismaticJoint;
-		localAnchor1.SetZero();
-		localAnchor2.SetZero();
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
 		localAxis1.Set(1.0f, 0.0f);
 		referenceAngle = 0.0f;
 		enableLimit = false;
@@ -46,13 +47,13 @@ struct b2PrismaticJointDef : public b2JointDef
 
 	/// Initialize the bodies, anchors, axis, and reference angle using the world
 	/// anchor and world axis.
-	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor, const b2Vec2& axis);
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
 
 	/// The local anchor point relative to body1's origin.
-	b2Vec2 localAnchor1;
+	b2Vec2 localAnchorA;
 
 	/// The local anchor point relative to body2's origin.
-	b2Vec2 localAnchor2;
+	b2Vec2 localAnchorB;
 
 	/// The local translation axis in body1.
 	b2Vec2 localAxis1;
@@ -86,11 +87,11 @@ struct b2PrismaticJointDef : public b2JointDef
 class b2PrismaticJoint : public b2Joint
 {
 public:
-	b2Vec2 GetAnchor1() const;
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce() const;
-	float32 GetReactionTorque() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
 	/// Get the current joint translation, usually in meters.
 	float32 GetJointTranslation() const;
@@ -131,13 +132,14 @@ public:
 	/// Get the current motor force, usually in N.
 	float32 GetMotorForce() const;
 
-	//--------------- Internals Below -------------------
-
+protected:
+	friend class b2Joint;
+	friend class b2GearJoint;
 	b2PrismaticJoint(const b2PrismaticJointDef* def);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints();
+	bool SolvePositionConstraints(float32 baumgarte);
 
 	b2Vec2 m_localAnchor1;
 	b2Vec2 m_localAnchor2;
@@ -145,18 +147,15 @@ public:
 	b2Vec2 m_localYAxis1;
 	float32 m_refAngle;
 
-	b2Jacobian m_linearJacobian;
-	float32 m_linearMass;				// effective mass for point-to-line constraint.
-	float32 m_force;
-	
-	float32 m_angularMass;			// effective mass for angular constraint.
-	float32 m_torque;
+	b2Vec2 m_axis, m_perp;
+	float32 m_s1, m_s2;
+	float32 m_a1, m_a2;
+
+	b2Mat33 m_K;
+	b2Vec3 m_impulse;
 
-	b2Jacobian m_motorJacobian;
 	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
-	float32 m_motorForce;
-	float32 m_limitForce;
-	float32 m_limitPositionImpulse;
+	float32 m_motorImpulse;
 
 	float32 m_lowerTranslation;
 	float32 m_upperTranslation;

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.cpp

@@ -16,9 +16,9 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2PulleyJoint.h"
-#include "../b2Body.h"
-#include "../b2World.h"
+#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 // Pulley:
 // length1 = norm(p1 - s1)
@@ -44,58 +44,57 @@ void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
 				const b2Vec2& anchor1, const b2Vec2& anchor2,
 				float32 r)
 {
-	body1 = b1;
-	body2 = b2;
-	groundAnchor1 = ga1;
-	groundAnchor2 = ga2;
-	localAnchor1 = body1->GetLocalPoint(anchor1);
-	localAnchor2 = body2->GetLocalPoint(anchor2);
+	bodyA = b1;
+	bodyB = b2;
+	groundAnchorA = ga1;
+	groundAnchorB = ga2;
+	localAnchorA = bodyA->GetLocalPoint(anchor1);
+	localAnchorB = bodyB->GetLocalPoint(anchor2);
 	b2Vec2 d1 = anchor1 - ga1;
-	length1 = d1.Length();
+	lengthA = d1.Length();
 	b2Vec2 d2 = anchor2 - ga2;
-	length2 = d2.Length();
+	lengthB = d2.Length();
 	ratio = r;
-	b2Assert(ratio > B2_FLT_EPSILON);
-	float32 C = length1 + ratio * length2;
-	maxLength1 = C - ratio * b2_minPulleyLength;
-	maxLength2 = (C - b2_minPulleyLength) / ratio;
+	b2Assert(ratio > b2_epsilon);
+	float32 C = lengthA + ratio * lengthB;
+	maxLengthA = C - ratio * b2_minPulleyLength;
+	maxLengthB = (C - b2_minPulleyLength) / ratio;
 }
 
 b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
 : b2Joint(def)
 {
-	m_ground = m_body1->GetWorld()->GetGroundBody();
-	m_groundAnchor1 = def->groundAnchor1 - m_ground->GetXForm().position;
-	m_groundAnchor2 = def->groundAnchor2 - m_ground->GetXForm().position;
-	m_localAnchor1 = def->localAnchor1;
-	m_localAnchor2 = def->localAnchor2;
+	m_groundAnchor1 = def->groundAnchorA;
+	m_groundAnchor2 = def->groundAnchorB;
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
 
 	b2Assert(def->ratio != 0.0f);
 	m_ratio = def->ratio;
 
-	m_constant = def->length1 + m_ratio * def->length2;
+	m_constant = def->lengthA + m_ratio * def->lengthB;
 
-	m_maxLength1 = b2Min(def->maxLength1, m_constant - m_ratio * b2_minPulleyLength);
-	m_maxLength2 = b2Min(def->maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
+	m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength);
+	m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio);
 
-	m_force = 0.0f;
-	m_limitForce1 = 0.0f;
-	m_limitForce2 = 0.0f;
+	m_impulse = 0.0f;
+	m_limitImpulse1 = 0.0f;
+	m_limitImpulse2 = 0.0f;
 }
 
 void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
 {
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
 
-	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 
 	b2Vec2 p1 = b1->m_sweep.c + r1;
 	b2Vec2 p2 = b2->m_sweep.c + r2;
 
-	b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
-	b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+	b2Vec2 s1 = m_groundAnchor1;
+	b2Vec2 s2 = m_groundAnchor2;
 
 	// Get the pulley axes.
 	m_u1 = p1 - s1;
@@ -126,34 +125,31 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
 	if (C > 0.0f)
 	{
 		m_state = e_inactiveLimit;
-		m_force = 0.0f;
+		m_impulse = 0.0f;
 	}
 	else
 	{
 		m_state = e_atUpperLimit;
-		m_positionImpulse = 0.0f;
 	}
 
 	if (length1 < m_maxLength1)
 	{
 		m_limitState1 = e_inactiveLimit;
-		m_limitForce1 = 0.0f;
+		m_limitImpulse1 = 0.0f;
 	}
 	else
 	{
 		m_limitState1 = e_atUpperLimit;
-		m_limitPositionImpulse1 = 0.0f;
 	}
 
 	if (length2 < m_maxLength2)
 	{
 		m_limitState2 = e_inactiveLimit;
-		m_limitForce2 = 0.0f;
+		m_limitImpulse2 = 0.0f;
 	}
 	else
 	{
 		m_limitState2 = e_atUpperLimit;
-		m_limitPositionImpulse2 = 0.0f;
 	}
 
 	// Compute effective mass.
@@ -163,18 +159,23 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
 	m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
 	m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
 	m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
-	b2Assert(m_limitMass1 > B2_FLT_EPSILON);
-	b2Assert(m_limitMass2 > B2_FLT_EPSILON);
-	b2Assert(m_pulleyMass > B2_FLT_EPSILON);
+	b2Assert(m_limitMass1 > b2_epsilon);
+	b2Assert(m_limitMass2 > b2_epsilon);
+	b2Assert(m_pulleyMass > b2_epsilon);
 	m_limitMass1 = 1.0f / m_limitMass1;
 	m_limitMass2 = 1.0f / m_limitMass2;
 	m_pulleyMass = 1.0f / m_pulleyMass;
 
 	if (step.warmStarting)
 	{
+		// Scale impulses to support variable time steps.
+		m_impulse *= step.dtRatio;
+		m_limitImpulse1 *= step.dtRatio;
+		m_limitImpulse2 *= step.dtRatio;
+
 		// Warm starting.
-		b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
-		b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
+		b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;
+		b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;
 		b1->m_linearVelocity += b1->m_invMass * P1;
 		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 		b2->m_linearVelocity += b2->m_invMass * P2;
@@ -182,19 +183,21 @@ void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
 	}
 	else
 	{
-		m_force = 0.0f;
-		m_limitForce1 = 0.0f;
-		m_limitForce2 = 0.0f;
+		m_impulse = 0.0f;
+		m_limitImpulse1 = 0.0f;
+		m_limitImpulse2 = 0.0f;
 	}
 }
 
 void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
 {
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	B2_NOT_USED(step);
 
-	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 
 	if (m_state == e_atUpperLimit)
 	{
@@ -202,13 +205,13 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
 
 		float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
-		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_pulleyMass * Cdot;
-		float32 oldForce = m_force;
-		m_force = b2Max(0.0f, m_force + force);
-		force = m_force - oldForce;
+		float32 impulse = m_pulleyMass * (-Cdot);
+		float32 oldImpulse = m_impulse;
+		m_impulse = b2Max(0.0f, m_impulse + impulse);
+		impulse = m_impulse - oldImpulse;
 
-		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
-		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * m_u2;
+		b2Vec2 P1 = -impulse * m_u1;
+		b2Vec2 P2 = -m_ratio * impulse * m_u2;
 		b1->m_linearVelocity += b1->m_invMass * P1;
 		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 		b2->m_linearVelocity += b2->m_invMass * P2;
@@ -220,12 +223,12 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
 
 		float32 Cdot = -b2Dot(m_u1, v1);
-		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass1 * Cdot;
-		float32 oldForce = m_limitForce1;
-		m_limitForce1 = b2Max(0.0f, m_limitForce1 + force);
-		force = m_limitForce1 - oldForce;
+		float32 impulse = -m_limitMass1 * Cdot;
+		float32 oldImpulse = m_limitImpulse1;
+		m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse);
+		impulse = m_limitImpulse1 - oldImpulse;
 
-		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+		b2Vec2 P1 = -impulse * m_u1;
 		b1->m_linearVelocity += b1->m_invMass * P1;
 		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
 	}
@@ -235,31 +238,33 @@ void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
 
 		float32 Cdot = -b2Dot(m_u2, v2);
-		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass2 * Cdot;
-		float32 oldForce = m_limitForce2;
-		m_limitForce2 = b2Max(0.0f, m_limitForce2 + force);
-		force = m_limitForce2 - oldForce;
+		float32 impulse = -m_limitMass2 * Cdot;
+		float32 oldImpulse = m_limitImpulse2;
+		m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse);
+		impulse = m_limitImpulse2 - oldImpulse;
 
-		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
+		b2Vec2 P2 = -impulse * m_u2;
 		b2->m_linearVelocity += b2->m_invMass * P2;
 		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
 	}
 }
 
-bool b2PulleyJoint::SolvePositionConstraints()
+bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
 {
-	b2Body* b1 = m_body1;
-	b2Body* b2 = m_body2;
+	B2_NOT_USED(baumgarte);
 
-	b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
-	b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 s1 = m_groundAnchor1;
+	b2Vec2 s2 = m_groundAnchor2;
 
 	float32 linearError = 0.0f;
 
 	if (m_state == e_atUpperLimit)
 	{
-		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 
 		b2Vec2 p1 = b1->m_sweep.c + r1;
 		b2Vec2 p2 = b2->m_sweep.c + r2;
@@ -294,9 +299,6 @@ bool b2PulleyJoint::SolvePositionConstraints()
 
 		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 		float32 impulse = -m_pulleyMass * C;
-		float32 oldImpulse = m_positionImpulse;
-		m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse);
-		impulse = m_positionImpulse - oldImpulse;
 
 		b2Vec2 P1 = -impulse * m_u1;
 		b2Vec2 P2 = -m_ratio * impulse * m_u2;
@@ -312,7 +314,7 @@ bool b2PulleyJoint::SolvePositionConstraints()
 
 	if (m_limitState1 == e_atUpperLimit)
 	{
-		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
 		b2Vec2 p1 = b1->m_sweep.c + r1;
 
 		m_u1 = p1 - s1;
@@ -331,9 +333,6 @@ bool b2PulleyJoint::SolvePositionConstraints()
 		linearError = b2Max(linearError, -C);
 		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 		float32 impulse = -m_limitMass1 * C;
-		float32 oldLimitPositionImpulse = m_limitPositionImpulse1;
-		m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse);
-		impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse;
 
 		b2Vec2 P1 = -impulse * m_u1;
 		b1->m_sweep.c += b1->m_invMass * P1;
@@ -344,7 +343,7 @@ bool b2PulleyJoint::SolvePositionConstraints()
 
 	if (m_limitState2 == e_atUpperLimit)
 	{
-		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
 		b2Vec2 p2 = b2->m_sweep.c + r2;
 
 		m_u2 = p2 - s2;
@@ -363,9 +362,6 @@ bool b2PulleyJoint::SolvePositionConstraints()
 		linearError = b2Max(linearError, -C);
 		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
 		float32 impulse = -m_limitMass2 * C;
-		float32 oldLimitPositionImpulse = m_limitPositionImpulse2;
-		m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse);
-		impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse;
 
 		b2Vec2 P2 = -impulse * m_u2;
 		b2->m_sweep.c += b2->m_invMass * P2;
@@ -377,49 +373,50 @@ bool b2PulleyJoint::SolvePositionConstraints()
 	return linearError < b2_linearSlop;
 }
 
-b2Vec2 b2PulleyJoint::GetAnchor1() const
+b2Vec2 b2PulleyJoint::GetAnchorA() const
 {
-	return m_body1->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
 }
 
-b2Vec2 b2PulleyJoint::GetAnchor2() const
+b2Vec2 b2PulleyJoint::GetAnchorB() const
 {
-	return m_body2->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
 }
 
-b2Vec2 b2PulleyJoint::GetReactionForce() const
+b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
 {
-	b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
-	return F;
+	b2Vec2 P = m_impulse * m_u2;
+	return inv_dt * P;
 }
 
-float32 b2PulleyJoint::GetReactionTorque() const
+float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
 {
+	B2_NOT_USED(inv_dt);
 	return 0.0f;
 }
 
-b2Vec2 b2PulleyJoint::GetGroundAnchor1() const
+b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
 {
-	return m_ground->GetXForm().position + m_groundAnchor1;
+	return m_groundAnchor1;
 }
 
-b2Vec2 b2PulleyJoint::GetGroundAnchor2() const
+b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
 {
-	return m_ground->GetXForm().position + m_groundAnchor2;
+	return m_groundAnchor2;
 }
 
 float32 b2PulleyJoint::GetLength1() const
 {
-	b2Vec2 p = m_body1->GetWorldPoint(m_localAnchor1);
-	b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor1;
+	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchor1);
+	b2Vec2 s = m_groundAnchor1;
 	b2Vec2 d = p - s;
 	return d.Length();
 }
 
 float32 b2PulleyJoint::GetLength2() const
 {
-	b2Vec2 p = m_body2->GetWorldPoint(m_localAnchor2);
-	b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor2;
+	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchor2);
+	b2Vec2 s = m_groundAnchor2;
 	b2Vec2 d = p - s;
 	return d.Length();
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2PulleyJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_PULLEY_JOINT_H
 #define B2_PULLEY_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 const float32 b2_minPulleyLength = 2.0f;
 
@@ -31,47 +31,47 @@ struct b2PulleyJointDef : public b2JointDef
 	b2PulleyJointDef()
 	{
 		type = e_pulleyJoint;
-		groundAnchor1.Set(-1.0f, 1.0f);
-		groundAnchor2.Set(1.0f, 1.0f);
-		localAnchor1.Set(-1.0f, 0.0f);
-		localAnchor2.Set(1.0f, 0.0f);
-		length1 = 0.0f;
-		maxLength1 = 0.0f;
-		length2 = 0.0f;
-		maxLength2 = 0.0f;
+		groundAnchorA.Set(-1.0f, 1.0f);
+		groundAnchorB.Set(1.0f, 1.0f);
+		localAnchorA.Set(-1.0f, 0.0f);
+		localAnchorB.Set(1.0f, 0.0f);
+		lengthA = 0.0f;
+		maxLengthA = 0.0f;
+		lengthB = 0.0f;
+		maxLengthB = 0.0f;
 		ratio = 1.0f;
 		collideConnected = true;
 	}
 
 	/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
-	void Initialize(b2Body* body1, b2Body* body2,
-					const b2Vec2& groundAnchor1, const b2Vec2& groundAnchor2,
-					const b2Vec2& anchor1, const b2Vec2& anchor2,
+	void Initialize(b2Body* bodyA, b2Body* bodyB,
+					const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,
+					const b2Vec2& anchorA, const b2Vec2& anchorB,
 					float32 ratio);
 
 	/// The first ground anchor in world coordinates. This point never moves.
-	b2Vec2 groundAnchor1;
+	b2Vec2 groundAnchorA;
 
 	/// The second ground anchor in world coordinates. This point never moves.
-	b2Vec2 groundAnchor2;
+	b2Vec2 groundAnchorB;
 
-	/// The local anchor point relative to body1's origin.
-	b2Vec2 localAnchor1;
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
-	b2Vec2 localAnchor2;
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
 
-	/// The a reference length for the segment attached to body1.
-	float32 length1;
+	/// The a reference length for the segment attached to bodyA.
+	float32 lengthA;
 
-	/// The maximum length of the segment attached to body1.
-	float32 maxLength1;
+	/// The maximum length of the segment attached to bodyA.
+	float32 maxLengthA;
 
-	/// The a reference length for the segment attached to body2.
-	float32 length2;
+	/// The a reference length for the segment attached to bodyB.
+	float32 lengthB;
 
-	/// The maximum length of the segment attached to body2.
-	float32 maxLength2;
+	/// The maximum length of the segment attached to bodyB.
+	float32 maxLengthB;
 
 	/// The pulley ratio, used to simulate a block-and-tackle.
 	float32 ratio;
@@ -86,17 +86,17 @@ struct b2PulleyJointDef : public b2JointDef
 class b2PulleyJoint : public b2Joint
 {
 public:
-	b2Vec2 GetAnchor1() const;
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce() const;
-	float32 GetReactionTorque() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
 	/// Get the first ground anchor.
-	b2Vec2 GetGroundAnchor1() const;
+	b2Vec2 GetGroundAnchorA() const;
 
 	/// Get the second ground anchor.
-	b2Vec2 GetGroundAnchor2() const;
+	b2Vec2 GetGroundAnchorB() const;
 
 	/// Get the current length of the segment attached to body1.
 	float32 GetLength1() const;
@@ -107,15 +107,15 @@ public:
 	/// Get the pulley ratio.
 	float32 GetRatio() const;
 
-	//--------------- Internals Below -------------------
+protected:
 
+	friend class b2Joint;
 	b2PulleyJoint(const b2PulleyJointDef* data);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints();
+	bool SolvePositionConstraints(float32 baumgarte);
 
-	b2Body* m_ground;
 	b2Vec2 m_groundAnchor1;
 	b2Vec2 m_groundAnchor2;
 	b2Vec2 m_localAnchor1;
@@ -136,14 +136,9 @@ public:
 	float32 m_limitMass2;
 
 	// Impulses for accumulation/warm starting.
-	float32 m_force;
-	float32 m_limitForce1;
-	float32 m_limitForce2;
-
-	// Position impulses for accumulation.
-	float32 m_positionImpulse;
-	float32 m_limitPositionImpulse1;
-	float32 m_limitPositionImpulse2;
+	float32 m_impulse;
+	float32 m_limitImpulse1;
+	float32 m_limitImpulse2;
 
 	b2LimitState m_state;
 	b2LimitState m_limitState1;

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp

@@ -0,0 +1,478 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2RevoluteJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
+{
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+	m_motorImpulse = 0.0f;
+
+	m_lowerAngle = def->lowerAngle;
+	m_upperAngle = def->upperAngle;
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+	m_limitState = e_inactiveLimit;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	if (m_enableMotor || m_enableLimit)
+	{
+		// You cannot create a rotation limit between bodies that
+		// both have fixed rotation.
+		b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f);
+	}
+
+	// Compute the effective mass matrix.
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
+	//     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
+	//     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+
+	float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
+	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+
+	m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
+	m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
+	m_mass.col3.x = -r1.y * i1 - r2.y * i2;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
+	m_mass.col3.y = r1.x * i1 + r2.x * i2;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = i1 + i2;
+
+	m_motorMass = i1 + i2;
+	if (m_motorMass > 0.0f)
+	{
+		m_motorMass = 1.0f / m_motorMass;
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (m_enableLimit)
+	{
+		float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
+		{
+			m_limitState = e_equalLimits;
+		}
+		else if (jointAngle <= m_lowerAngle)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_impulse.z = 0.0f;
+			}
+			m_limitState = e_atLowerLimit;
+		}
+		else if (jointAngle >= m_upperAngle)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_impulse.z = 0.0f;
+			}
+			m_limitState = e_atUpperLimit;
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_impulse.z = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+	}
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_impulse *= step.dtRatio;
+		m_motorImpulse *= step.dtRatio;
+
+		b2Vec2 P(m_impulse.x, m_impulse.y);
+
+		b1->m_linearVelocity -= m1 * P;
+		b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z);
+
+		b2->m_linearVelocity += m2 * P;
+		b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z);
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+	}
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
+	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+
+	// Solve motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 Cdot = w2 - w1 - m_motorSpeed;
+		float32 impulse = m_motorMass * (-Cdot);
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = step.dt * m_maxMotorTorque;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		w1 -= i1 * impulse;
+		w2 += i2 * impulse;
+	}
+
+	// Solve limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		// Solve point-to-point constraint
+		b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+		float32 Cdot2 = w2 - w1;
+		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+
+		b2Vec3 impulse = m_mass.Solve33(-Cdot);
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_impulse += impulse;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 newImpulse = m_impulse.z + impulse.z;
+			if (newImpulse < 0.0f)
+			{
+				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				impulse.x = reduced.x;
+				impulse.y = reduced.y;
+				impulse.z = -m_impulse.z;
+				m_impulse.x += reduced.x;
+				m_impulse.y += reduced.y;
+				m_impulse.z = 0.0f;
+			}
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 newImpulse = m_impulse.z + impulse.z;
+			if (newImpulse > 0.0f)
+			{
+				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				impulse.x = reduced.x;
+				impulse.y = reduced.y;
+				impulse.z = -m_impulse.z;
+				m_impulse.x += reduced.x;
+				m_impulse.y += reduced.y;
+				m_impulse.z = 0.0f;
+			}
+		}
+
+		b2Vec2 P(impulse.x, impulse.y);
+
+		v1 -= m1 * P;
+		w1 -= i1 * (b2Cross(r1, P) + impulse.z);
+
+		v2 += m2 * P;
+		w2 += i2 * (b2Cross(r2, P) + impulse.z);
+	}
+	else
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		// Solve point-to-point constraint
+		b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+		b2Vec2 impulse = m_mass.Solve22(-Cdot);
+
+		m_impulse.x += impulse.x;
+		m_impulse.y += impulse.y;
+
+		v1 -= m1 * impulse;
+		w1 -= i1 * b2Cross(r1, impulse);
+
+		v2 += m2 * impulse;
+		w2 += i2 * b2Cross(r2, impulse);
+	}
+
+	b1->m_linearVelocity = v1;
+	b1->m_angularVelocity = w1;
+	b2->m_linearVelocity = v2;
+	b2->m_angularVelocity = w2;
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	// TODO_ERIN block solve with limit.
+
+	B2_NOT_USED(baumgarte);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	float32 angularError = 0.0f;
+	float32 positionError = 0.0f;
+
+	// Solve angular limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		float32 limitImpulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			// Prevent large angular corrections
+			float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * C;
+			angularError = b2Abs(C);
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 C = angle - m_lowerAngle;
+			angularError = -C;
+
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+			limitImpulse = -m_motorMass * C;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 C = angle - m_upperAngle;
+			angularError = C;
+
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * C;
+		}
+
+		b1->m_sweep.a -= b1->m_invI * limitImpulse;
+		b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	// Solve point-to-point constraint.
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+		positionError = C.Length();
+
+		float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+		float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+		// Handle large detachment.
+		const float32 k_allowedStretch = 10.0f * b2_linearSlop;
+		if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
+		{
+			// Use a particle solution (no rotation).
+			b2Vec2 u = C; u.Normalize();
+			float32 m = invMass1 + invMass2;
+			if (m > 0.0f)
+			{
+				m = 1.0f / m;
+			}
+			b2Vec2 impulse = m * (-C);
+			const float32 k_beta = 0.5f;
+			b1->m_sweep.c -= k_beta * invMass1 * impulse;
+			b2->m_sweep.c += k_beta * invMass2 * impulse;
+
+			C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+		}
+
+		b2Mat22 K1;
+		K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
+		K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;
+
+		b2Mat22 K2;
+		K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
+		K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;
+
+		b2Mat22 K3;
+		K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
+		K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;
+
+		b2Mat22 K = K1 + K2 + K3;
+		b2Vec2 impulse = K.Solve(-C);
+
+		b1->m_sweep.c -= b1->m_invMass * impulse;
+		b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
+
+		b2->m_sweep.c += b2->m_invMass * impulse;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+	
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 P(m_impulse.x, m_impulse.y);
+	return inv_dt * P;
+}
+
+float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_impulse.z;
+}
+
+float32 b2RevoluteJoint::GetJointAngle() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+	return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+}
+
+float32 b2RevoluteJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+	return b2->m_angularVelocity - b1->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableMotor = flag;
+}
+
+float32 b2RevoluteJoint::GetMotorTorque() const
+{
+	return m_motorImpulse;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_maxMotorTorque = torque;
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableLimit = flag;
+}
+
+float32 b2RevoluteJoint::GetLowerLimit() const
+{
+	return m_lowerAngle;
+}
+
+float32 b2RevoluteJoint::GetUpperLimit() const
+{
+	return m_upperAngle;
+}
+
+void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_lowerAngle = lower;
+	m_upperAngle = upper;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RevoluteJoint.h

@@ -19,7 +19,7 @@
 #ifndef B2_REVOLUTE_JOINT_H
 #define B2_REVOLUTE_JOINT_H
 
-#include "b2Joint.h"
+#include <Box2D/Dynamics/Joints/b2Joint.h>
 
 /// Revolute joint definition. This requires defining an
 /// anchor point where the bodies are joined. The definition
@@ -37,8 +37,8 @@ struct b2RevoluteJointDef : public b2JointDef
 	b2RevoluteJointDef()
 	{
 		type = e_revoluteJoint;
-		localAnchor1.Set(0.0f, 0.0f);
-		localAnchor2.Set(0.0f, 0.0f);
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.Set(0.0f, 0.0f);
 		referenceAngle = 0.0f;
 		lowerAngle = 0.0f;
 		upperAngle = 0.0f;
@@ -48,15 +48,15 @@ struct b2RevoluteJointDef : public b2JointDef
 		enableMotor = false;
 	}
 
-	/// Initialize the bodies, anchors, and reference angle using the world
-	/// anchor.
-	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+	/// Initialize the bodies, anchors, and reference angle using a world
+	/// anchor point.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
 
 	/// The local anchor point relative to body1's origin.
-	b2Vec2 localAnchor1;
+	b2Vec2 localAnchorA;
 
 	/// The local anchor point relative to body2's origin.
-	b2Vec2 localAnchor2;
+	b2Vec2 localAnchorB;
 
 	/// The body2 angle minus body1 angle in the reference state (radians).
 	float32 referenceAngle;
@@ -81,7 +81,7 @@ struct b2RevoluteJointDef : public b2JointDef
 	float32 maxMotorTorque;
 };
 
-/// A revolute joint constrains to bodies to share a common point while they
+/// A revolute joint constrains two bodies to share a common point while they
 /// are free to rotate about the point. The relative rotation about the shared
 /// point is the joint angle. You can limit the relative rotation with
 /// a joint limit that specifies a lower and upper angle. You can use a motor
@@ -90,11 +90,11 @@ struct b2RevoluteJointDef : public b2JointDef
 class b2RevoluteJoint : public b2Joint
 {
 public:
-	b2Vec2 GetAnchor1() const;
-	b2Vec2 GetAnchor2() const;
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce() const;
-	float32 GetReactionTorque() const;
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
 
 	/// Get the current joint angle in radians.
 	float32 GetJointAngle() const;
@@ -135,22 +135,24 @@ public:
 	/// Get the current motor torque, usually in N-m.
 	float32 GetMotorTorque() const;
 
-	//--------------- Internals Below -------------------
+protected:
+	
+	friend class b2Joint;
+	friend class b2GearJoint;
+
 	b2RevoluteJoint(const b2RevoluteJointDef* def);
 
 	void InitVelocityConstraints(const b2TimeStep& step);
 	void SolveVelocityConstraints(const b2TimeStep& step);
 
-	bool SolvePositionConstraints();
+	bool SolvePositionConstraints(float32 baumgarte);
 
 	b2Vec2 m_localAnchor1;	// relative
 	b2Vec2 m_localAnchor2;
-	b2Vec2 m_pivotForce;
-	float32 m_motorForce;
-	float32 m_limitForce;
-	float32 m_limitPositionImpulse;
+	b2Vec3 m_impulse;
+	float32 m_motorImpulse;
 
-	b2Mat22 m_pivotMass;		// effective mass for point-to-point constraint.
+	b2Mat33 m_mass;			// effective mass for point-to-point constraint.
 	float32 m_motorMass;	// effective mass for motor/limit angular constraint.
 	
 	bool m_enableMotor;

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp

@@ -0,0 +1,219 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/Joints/b2WeldJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// C = angle2 - angle1 - referenceAngle
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+}
+
+void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	// Compute the effective mass matrix.
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	m_mass.col3.x = -rA.y * iA - rB.y * iB;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	m_mass.col3.y = rA.x * iA + rB.x * iB;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = iA + iB;
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_impulse *= step.dtRatio;
+
+		b2Vec2 P(m_impulse.x, m_impulse.y);
+
+		bA->m_linearVelocity -= mA * P;
+		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z);
+
+		bB->m_linearVelocity += mB * P;
+		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_impulse.z);
+	}
+	else
+	{
+		m_impulse.SetZero();
+	}
+}
+
+void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	float32 wA = bA->m_angularVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float32 wB = bB->m_angularVelocity;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// Solve point-to-point constraint
+	b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+	float32 Cdot2 = wB - wA;
+	b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+
+	b2Vec3 impulse = m_mass.Solve33(-Cdot);
+	m_impulse += impulse;
+
+	b2Vec2 P(impulse.x, impulse.y);
+
+	vA -= mA * P;
+	wA -= iA * (b2Cross(rA, P) + impulse.z);
+
+	vB += mB * P;
+	wB += iB * (b2Cross(rB, P) + impulse.z);
+
+	bA->m_linearVelocity = vA;
+	bA->m_angularVelocity = wA;
+	bB->m_linearVelocity = vB;
+	bB->m_angularVelocity = wB;
+}
+
+bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	b2Vec2 C1 =  bB->m_sweep.c + rB - bA->m_sweep.c - rA;
+	float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
+
+	// Handle large detachment.
+	const float32 k_allowedStretch = 10.0f * b2_linearSlop;
+	float32 positionError = C1.Length();
+	float32 angularError = b2Abs(C2);
+	if (positionError > k_allowedStretch)
+	{
+		iA *= 1.0f;
+		iB *= 1.0f;
+	}
+
+	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	m_mass.col3.x = -rA.y * iA - rB.y * iB;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	m_mass.col3.y = rA.x * iA + rB.x * iB;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = iA + iB;
+
+	b2Vec3 C(C1.x, C1.y, C2);
+
+	b2Vec3 impulse = m_mass.Solve33(-C);
+
+	b2Vec2 P(impulse.x, impulse.y);
+
+	bA->m_sweep.c -= mA * P;
+	bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);
+
+	bB->m_sweep.c += mB * P;
+	bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
+
+	bA->SynchronizeTransform();
+	bB->SynchronizeTransform();
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2WeldJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2WeldJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 P(m_impulse.x, m_impulse.y);
+	return inv_dt * P;
+}
+
+float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_impulse.z;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h

@@ -0,0 +1,82 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_WELD_JOINT_H
+#define B2_WELD_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Weld joint definition. You need to specify local anchor points
+/// where they are attached and the relative body angle. The position
+/// of the anchor points is important for computing the reaction torque.
+struct b2WeldJointDef : public b2JointDef
+{
+	b2WeldJointDef()
+	{
+		type = e_weldJoint;
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.Set(0.0f, 0.0f);
+		referenceAngle = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, and reference angle using a world
+	/// anchor point.
+	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The body2 angle minus body1 angle in the reference state (radians).
+	float32 referenceAngle;
+};
+
+/// A weld joint essentially glues two bodies together. A weld joint may
+/// distort somewhat because the island constraint solver is approximate.
+class b2WeldJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+protected:
+
+	friend class b2Joint;
+
+	b2WeldJoint(const b2WeldJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	float32 m_referenceAngle;
+
+	b2Vec3 m_impulse;
+
+	b2Mat33 m_mass;
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp

@@ -0,0 +1,470 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2World.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+b2Body::b2Body(const b2BodyDef* bd, b2World* world)
+{
+	b2Assert(bd->position.IsValid());
+	b2Assert(bd->linearVelocity.IsValid());
+	b2Assert(b2IsValid(bd->angle));
+	b2Assert(b2IsValid(bd->angularVelocity));
+	b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
+	b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
+	b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
+
+	m_flags = 0;
+
+	if (bd->bullet)
+	{
+		m_flags |= e_bulletFlag;
+	}
+	if (bd->fixedRotation)
+	{
+		m_flags |= e_fixedRotationFlag;
+	}
+	if (bd->allowSleep)
+	{
+		m_flags |= e_autoSleepFlag;
+	}
+	if (bd->awake)
+	{
+		m_flags |= e_awakeFlag;
+	}
+	if (bd->active)
+	{
+		m_flags |= e_activeFlag;
+	}
+
+	m_world = world;
+
+	m_xf.position = bd->position;
+	m_xf.R.Set(bd->angle);
+
+	m_sweep.localCenter.SetZero();
+	m_sweep.a0 = m_sweep.a = bd->angle;
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+	m_jointList = NULL;
+	m_contactList = NULL;
+	m_prev = NULL;
+	m_next = NULL;
+
+	m_linearVelocity = bd->linearVelocity;
+	m_angularVelocity = bd->angularVelocity;
+
+	m_linearDamping = bd->linearDamping;
+	m_angularDamping = bd->angularDamping;
+
+	m_force.SetZero();
+	m_torque = 0.0f;
+
+	m_sleepTime = 0.0f;
+
+	m_type = bd->type;
+
+	if (m_type == b2_dynamicBody)
+	{
+		m_mass = 1.0f;
+		m_invMass = 1.0f;
+	}
+	else
+	{
+		m_mass = 0.0f;
+		m_invMass = 0.0f;
+	}
+
+	m_I = 0.0f;
+	m_invI = 0.0f;
+
+	m_userData = bd->userData;
+
+	m_fixtureList = NULL;
+	m_fixtureCount = 0;
+}
+
+b2Body::~b2Body()
+{
+	// shapes and joints are destroyed in b2World::Destroy
+}
+
+void b2Body::SetType(b2BodyType type)
+{
+	if (m_type == type)
+	{
+		return;
+	}
+
+	m_type = type;
+
+	ResetMassData();
+
+	if (m_type == b2_staticBody)
+	{
+		m_linearVelocity.SetZero();
+		m_angularVelocity = 0.0f;
+	}
+
+	SetAwake(true);
+
+	m_force.SetZero();
+	m_torque = 0.0f;
+
+	// Since the body type changed, we need to flag contacts for filtering.
+	for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next)
+	{
+		ce->contact->FlagForFiltering();
+	}
+}
+
+b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
+{
+	b2Assert(m_world->IsLocked() == false);
+	if (m_world->IsLocked() == true)
+	{
+		return NULL;
+	}
+
+	b2BlockAllocator* allocator = &m_world->m_blockAllocator;
+
+	void* memory = allocator->Allocate(sizeof(b2Fixture));
+	b2Fixture* fixture = new (memory) b2Fixture;
+	fixture->Create(allocator, this, def);
+
+	if (m_flags & e_activeFlag)
+	{
+		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+		fixture->CreateProxy(broadPhase, m_xf);
+	}
+
+	fixture->m_next = m_fixtureList;
+	m_fixtureList = fixture;
+	++m_fixtureCount;
+
+	fixture->m_body = this;
+
+	// Adjust mass properties if needed.
+	if (fixture->m_density > 0.0f)
+	{
+		ResetMassData();
+	}
+
+	// Let the world know we have a new fixture. This will cause new contacts
+	// to be created at the beginning of the next time step.
+	m_world->m_flags |= b2World::e_newFixture;
+
+	return fixture;
+}
+
+b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
+{
+	b2FixtureDef def;
+	def.shape = shape;
+	def.density = density;
+
+	return CreateFixture(&def);
+}
+
+void b2Body::DestroyFixture(b2Fixture* fixture)
+{
+	b2Assert(m_world->IsLocked() == false);
+	if (m_world->IsLocked() == true)
+	{
+		return;
+	}
+
+	b2Assert(fixture->m_body == this);
+
+	// Remove the fixture from this body's singly linked list.
+	b2Assert(m_fixtureCount > 0);
+	b2Fixture** node = &m_fixtureList;
+	bool found = false;
+	while (*node != NULL)
+	{
+		if (*node == fixture)
+		{
+			*node = fixture->m_next;
+			found = true;
+			break;
+		}
+
+		node = &(*node)->m_next;
+	}
+
+	// You tried to remove a shape that is not attached to this body.
+	b2Assert(found);
+
+	// Destroy any contacts associated with the fixture.
+	b2ContactEdge* edge = m_contactList;
+	while (edge)
+	{
+		b2Contact* c = edge->contact;
+		edge = edge->next;
+
+		b2Fixture* fixtureA = c->GetFixtureA();
+		b2Fixture* fixtureB = c->GetFixtureB();
+
+		if (fixture == fixtureA || fixture == fixtureB)
+		{
+			// This destroys the contact and removes it from
+			// this body's contact list.
+			m_world->m_contactManager.Destroy(c);
+		}
+	}
+
+	b2BlockAllocator* allocator = &m_world->m_blockAllocator;
+
+	if (m_flags & e_activeFlag)
+	{
+		b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy);
+		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+		fixture->DestroyProxy(broadPhase);
+	}
+	else
+	{
+		b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy);
+	}
+
+	fixture->Destroy(allocator);
+	fixture->m_body = NULL;
+	fixture->m_next = NULL;
+	fixture->~b2Fixture();
+	allocator->Free(fixture, sizeof(b2Fixture));
+
+	--m_fixtureCount;
+
+	// Reset the mass data.
+	ResetMassData();
+}
+
+void b2Body::ResetMassData()
+{
+	// Compute mass data from shapes. Each shape has its own density.
+	m_mass = 0.0f;
+	m_invMass = 0.0f;
+	m_I = 0.0f;
+	m_invI = 0.0f;
+	m_sweep.localCenter.SetZero();
+
+	// Static and kinematic bodies have zero mass.
+	if (m_type == b2_staticBody || m_type == b2_kinematicBody)
+	{
+		m_sweep.c0 = m_sweep.c = m_xf.position;
+		return;
+	}
+
+	b2Assert(m_type == b2_dynamicBody);
+
+	// Accumulate mass over all fixtures.
+	b2Vec2 center = b2Vec2_zero;
+	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+	{
+		if (f->m_density == 0.0f)
+		{
+			continue;
+		}
+
+		b2MassData massData;
+		f->GetMassData(&massData);
+		m_mass += massData.mass;
+		center += massData.mass * massData.center;
+		m_I += massData.I;
+	}
+
+	// Compute center of mass.
+	if (m_mass > 0.0f)
+	{
+		m_invMass = 1.0f / m_mass;
+		center *= m_invMass;
+	}
+	else
+	{
+		// Force all dynamic bodies to have a positive mass.
+		m_mass = 1.0f;
+		m_invMass = 1.0f;
+	}
+
+	if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
+	{
+		// Center the inertia about the center of mass.
+		m_I -= m_mass * b2Dot(center, center);
+		b2Assert(m_I > 0.0f);
+		m_invI = 1.0f / m_I;
+
+	}
+	else
+	{
+		m_I = 0.0f;
+		m_invI = 0.0f;
+	}
+
+	// Move center of mass.
+	b2Vec2 oldCenter = m_sweep.c;
+	m_sweep.localCenter = center;
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+	// Update center of mass velocity.
+	m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
+}
+
+void b2Body::SetMassData(const b2MassData* massData)
+{
+	b2Assert(m_world->IsLocked() == false);
+	if (m_world->IsLocked() == true)
+	{
+		return;
+	}
+
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	m_invMass = 0.0f;
+	m_I = 0.0f;
+	m_invI = 0.0f;
+
+	m_mass = massData->mass;
+	if (m_mass <= 0.0f)
+	{
+		m_mass = 1.0f;
+	}
+
+	m_invMass = 1.0f / m_mass;
+
+	if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
+	{
+		m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
+		b2Assert(m_I > 0.0f);
+		m_invI = 1.0f / m_I;
+	}
+
+	// Move center of mass.
+	b2Vec2 oldCenter = m_sweep.c;
+	m_sweep.localCenter = massData->center;
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+	// Update center of mass velocity.
+	m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
+}
+
+bool b2Body::ShouldCollide(const b2Body* other) const
+{
+	// At least one body should be dynamic.
+	if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
+	{
+		return false;
+	}
+
+	// Does a joint prevent collision?
+	for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
+	{
+		if (jn->other == other)
+		{
+			if (jn->joint->m_collideConnected == false)
+			{
+				return false;
+			}
+		}
+	}
+
+	return true;
+}
+
+void b2Body::SetTransform(const b2Vec2& position, float32 angle)
+{
+	b2Assert(m_world->IsLocked() == false);
+	if (m_world->IsLocked() == true)
+	{
+		return;
+	}
+
+	m_xf.R.Set(angle);
+	m_xf.position = position;
+
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+	m_sweep.a0 = m_sweep.a = angle;
+
+	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+	{
+		f->Synchronize(broadPhase, m_xf, m_xf);
+	}
+
+	m_world->m_contactManager.FindNewContacts();
+}
+
+void b2Body::SynchronizeFixtures()
+{
+	b2Transform xf1;
+	xf1.R.Set(m_sweep.a0);
+	xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
+
+	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+	{
+		f->Synchronize(broadPhase, xf1, m_xf);
+	}
+}
+
+void b2Body::SetActive(bool flag)
+{
+	if (flag == IsActive())
+	{
+		return;
+	}
+
+	if (flag)
+	{
+		m_flags |= e_activeFlag;
+
+		// Create all proxies.
+		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->CreateProxy(broadPhase, m_xf);
+		}
+
+		// Contacts are created the next time step.
+	}
+	else
+	{
+		m_flags &= ~e_activeFlag;
+
+		// Destroy all proxies.
+		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
+		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+		{
+			f->DestroyProxy(broadPhase);
+		}
+
+		// Destroy the attached contacts.
+		b2ContactEdge* ce = m_contactList;
+		while (ce)
+		{
+			b2ContactEdge* ce0 = ce;
+			ce = ce->next;
+			m_world->m_contactManager.Destroy(ce0->contact);
+		}
+		m_contactList = NULL;
+	}
+}

src/modules/physics/box2d/Box2D/Dynamics/b2Body.h

@@ -0,0 +1,802 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_BODY_H
+#define B2_BODY_H
+
+#include <Box2D/Common/b2Math.h>
+#include <Box2D/Collision/Shapes/b2Shape.h>
+#include <memory>
+
+class b2Fixture;
+class b2Joint;
+class b2Contact;
+class b2Controller;
+class b2World;
+struct b2FixtureDef;
+struct b2JointEdge;
+struct b2ContactEdge;
+
+/// The body type.
+/// static: zero mass, zero velocity, may be manually moved
+/// kinematic: zero mass, non-zero velocity set by user, moved by solver
+/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
+enum b2BodyType
+{
+	b2_staticBody = 0,
+	b2_kinematicBody,
+	b2_dynamicBody,
+};
+
+/// A body definition holds all the data needed to construct a rigid body.
+/// You can safely re-use body definitions. Shapes are added to a body after construction.
+struct b2BodyDef
+{
+	/// This constructor sets the body definition default values.
+	b2BodyDef()
+	{
+		userData = NULL;
+		position.Set(0.0f, 0.0f);
+		angle = 0.0f;
+		linearVelocity.Set(0.0f, 0.0f);
+		angularVelocity = 0.0f;
+		linearDamping = 0.0f;
+		angularDamping = 0.0f;
+		allowSleep = true;
+		awake = true;
+		fixedRotation = false;
+		bullet = false;
+		type = b2_staticBody;
+		active = true;
+		inertiaScale = 1.0f;
+	}
+
+	/// The body type: static, kinematic, or dynamic.
+	/// Note: if a dynamic body would have zero mass, the mass is set to one.
+	b2BodyType type;
+
+	/// The world position of the body. Avoid creating bodies at the origin
+	/// since this can lead to many overlapping shapes.
+	b2Vec2 position;
+
+	/// The world angle of the body in radians.
+	float32 angle;
+
+	/// The linear velocity of the body's origin in world co-ordinates.
+	b2Vec2 linearVelocity;
+
+	/// The angular velocity of the body.
+	float32 angularVelocity;
+
+	/// Linear damping is use to reduce the linear velocity. The damping parameter
+	/// can be larger than 1.0f but the damping effect becomes sensitive to the
+	/// time step when the damping parameter is large.
+	float32 linearDamping;
+
+	/// Angular damping is use to reduce the angular velocity. The damping parameter
+	/// can be larger than 1.0f but the damping effect becomes sensitive to the
+	/// time step when the damping parameter is large.
+	float32 angularDamping;
+
+	/// Set this flag to false if this body should never fall asleep. Note that
+	/// this increases CPU usage.
+	bool allowSleep;
+
+	/// Is this body initially awake or sleeping?
+	bool awake;
+
+	/// Should this body be prevented from rotating? Useful for characters.
+	bool fixedRotation;
+
+	/// Is this a fast moving body that should be prevented from tunneling through
+	/// other moving bodies? Note that all bodies are prevented from tunneling through
+	/// kinematic and static bodies. This setting is only considered on dynamic bodies.
+	/// @warning You should use this flag sparingly since it increases processing time.
+	bool bullet;
+
+	/// Does this body start out active?
+	bool active;
+
+	/// Use this to store application specific body data.
+	void* userData;
+
+	/// Experimental: scales the inertia tensor.
+	float32 inertiaScale;
+};
+
+/// A rigid body. These are created via b2World::CreateBody.
+class b2Body
+{
+public:
+	/// Creates a fixture and attach it to this body. Use this function if you need
+	/// to set some fixture parameters, like friction. Otherwise you can create the
+	/// fixture directly from a shape.
+	/// If the density is non-zero, this function automatically updates the mass of the body.
+	/// Contacts are not created until the next time step.
+	/// @param def the fixture definition.
+	/// @warning This function is locked during callbacks.
+	b2Fixture* CreateFixture(const b2FixtureDef* def);
+
+	/// Creates a fixture from a shape and attach it to this body.
+	/// This is a convenience function. Use b2FixtureDef if you need to set parameters
+	/// like friction, restitution, user data, or filtering.
+	/// If the density is non-zero, this function automatically updates the mass of the body.
+	/// @param shape the shape to be cloned.
+	/// @param density the shape density (set to zero for static bodies).
+	/// @warning This function is locked during callbacks.
+	b2Fixture* CreateFixture(const b2Shape* shape, float32 density);
+
+	/// Destroy a fixture. This removes the fixture from the broad-phase and
+	/// destroys all contacts associated with this fixture. This will
+	/// automatically adjust the mass of the body if the body is dynamic and the
+	/// fixture has positive density.
+	/// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
+	/// @param fixture the fixture to be removed.
+	/// @warning This function is locked during callbacks.
+	void DestroyFixture(b2Fixture* fixture);
+
+	/// Set the position of the body's origin and rotation.
+	/// This breaks any contacts and wakes the other bodies.
+	/// Manipulating a body's transform may cause non-physical behavior.
+	/// @param position the world position of the body's local origin.
+	/// @param angle the world rotation in radians.
+	void SetTransform(const b2Vec2& position, float32 angle);
+
+	/// Get the body transform for the body's origin.
+	/// @return the world transform of the body's origin.
+	const b2Transform& GetTransform() const;
+
+	/// Get the world body origin position.
+	/// @return the world position of the body's origin.
+	const b2Vec2& GetPosition() const;
+
+	/// Get the angle in radians.
+	/// @return the current world rotation angle in radians.
+	float32 GetAngle() const;
+
+	/// Get the world position of the center of mass.
+	const b2Vec2& GetWorldCenter() const;
+
+	/// Get the local position of the center of mass.
+	const b2Vec2& GetLocalCenter() const;
+
+	/// Set the linear velocity of the center of mass.
+	/// @param v the new linear velocity of the center of mass.
+	void SetLinearVelocity(const b2Vec2& v);
+
+	/// Get the linear velocity of the center of mass.
+	/// @return the linear velocity of the center of mass.
+	b2Vec2 GetLinearVelocity() const;
+
+	/// Set the angular velocity.
+	/// @param omega the new angular velocity in radians/second.
+	void SetAngularVelocity(float32 omega);
+
+	/// Get the angular velocity.
+	/// @return the angular velocity in radians/second.
+	float32 GetAngularVelocity() const;
+
+	/// Apply a force at a world point. If the force is not
+	/// applied at the center of mass, it will generate a torque and
+	/// affect the angular velocity. This wakes up the body.
+	/// @param force the world force vector, usually in Newtons (N).
+	/// @param point the world position of the point of application.
+	void ApplyForce(const b2Vec2& force, const b2Vec2& point);
+
+	/// Apply a torque. This affects the angular velocity
+	/// without affecting the linear velocity of the center of mass.
+	/// This wakes up the body.
+	/// @param torque about the z-axis (out of the screen), usually in N-m.
+	void ApplyTorque(float32 torque);
+
+	/// Apply an impulse at a point. This immediately modifies the velocity.
+	/// It also modifies the angular velocity if the point of application
+	/// is not at the center of mass. This wakes up the body.
+	/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
+	/// @param point the world position of the point of application.
+	void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point);
+
+	/// Apply an angular impulse.
+	/// @param impulse the angular impulse in units of kg*m*m/s
+	void ApplyAngularImpulse(float32 impulse);
+
+	/// Get the total mass of the body.
+	/// @return the mass, usually in kilograms (kg).
+	float32 GetMass() const;
+
+	/// Get the rotational inertia of the body about the local origin.
+	/// @return the rotational inertia, usually in kg-m^2.
+	float32 GetInertia() const;
+
+	/// Get the mass data of the body.
+	/// @return a struct containing the mass, inertia and center of the body.
+	void GetMassData(b2MassData* data) const;
+
+	/// Set the mass properties to override the mass properties of the fixtures.
+	/// Note that this changes the center of mass position.
+	/// Note that creating or destroying fixtures can also alter the mass.
+	/// This function has no effect if the body isn't dynamic.
+	/// @param massData the mass properties.
+	void SetMassData(const b2MassData* data);
+
+	/// This resets the mass properties to the sum of the mass properties of the fixtures.
+	/// This normally does not need to be called unless you called SetMassData to override
+	/// the mass and you later want to reset the mass.
+	void ResetMassData();
+
+	/// Get the world coordinates of a point given the local coordinates.
+	/// @param localPoint a point on the body measured relative the the body's origin.
+	/// @return the same point expressed in world coordinates.
+	b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const;
+
+	/// Get the world coordinates of a vector given the local coordinates.
+	/// @param localVector a vector fixed in the body.
+	/// @return the same vector expressed in world coordinates.
+	b2Vec2 GetWorldVector(const b2Vec2& localVector) const;
+
+	/// Gets a local point relative to the body's origin given a world point.
+	/// @param a point in world coordinates.
+	/// @return the corresponding local point relative to the body's origin.
+	b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const;
+
+	/// Gets a local vector given a world vector.
+	/// @param a vector in world coordinates.
+	/// @return the corresponding local vector.
+	b2Vec2 GetLocalVector(const b2Vec2& worldVector) const;
+
+	/// Get the world linear velocity of a world point attached to this body.
+	/// @param a point in world coordinates.
+	/// @return the world velocity of a point.
+	b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;
+
+	/// Get the world velocity of a local point.
+	/// @param a point in local coordinates.
+	/// @return the world velocity of a point.
+	b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;
+
+	/// Get the linear damping of the body.
+	float32 GetLinearDamping() const;
+
+	/// Set the linear damping of the body.
+	void SetLinearDamping(float32 linearDamping);
+
+	/// Get the angular damping of the body.
+	float32 GetAngularDamping() const;
+
+	/// Set the angular damping of the body.
+	void SetAngularDamping(float32 angularDamping);
+
+	/// Set the type of this body. This may alter the mass and velocity.
+	void SetType(b2BodyType type);
+
+	/// Get the type of this body.
+	b2BodyType GetType() const;
+
+	/// Should this body be treated like a bullet for continuous collision detection?
+	void SetBullet(bool flag);
+
+	/// Is this body treated like a bullet for continuous collision detection?
+	bool IsBullet() const;
+
+	/// You can disable sleeping on this body. If you disable sleeping, the
+	/// body will be woken.
+	void SetSleepingAllowed(bool flag);
+
+	/// Is this body allowed to sleep
+	bool IsSleepingAllowed() const;
+
+	/// Set the sleep state of the body. A sleeping body has very
+	/// low CPU cost.
+	/// @param flag set to true to put body to sleep, false to wake it.
+	void SetAwake(bool flag);
+
+	/// Get the sleeping state of this body.
+	/// @return true if the body is sleeping.
+	bool IsAwake() const;
+
+	/// Set the active state of the body. An inactive body is not
+	/// simulated and cannot be collided with or woken up.
+	/// If you pass a flag of true, all fixtures will be added to the
+	/// broad-phase.
+	/// If you pass a flag of false, all fixtures will be removed from
+	/// the broad-phase and all contacts will be destroyed.
+	/// Fixtures and joints are otherwise unaffected. You may continue
+	/// to create/destroy fixtures and joints on inactive bodies.
+	/// Fixtures on an inactive body are implicitly inactive and will
+	/// not participate in collisions, ray-casts, or queries.
+	/// Joints connected to an inactive body are implicitly inactive.
+	/// An inactive body is still owned by a b2World object and remains
+	/// in the body list.
+	void SetActive(bool flag);
+
+	/// Get the active state of the body.
+	bool IsActive() const;
+
+	/// Set this body to have fixed rotation. This causes the mass
+	/// to be reset.
+	void SetFixedRotation(bool flag);
+
+	/// Does this body have fixed rotation?
+	bool IsFixedRotation() const;
+
+	/// Get the list of all fixtures attached to this body.
+	b2Fixture* GetFixtureList();
+	const b2Fixture* GetFixtureList() const;
+
+	/// Get the list of all joints attached to this body.
+	b2JointEdge* GetJointList();
+	const b2JointEdge* GetJointList() const;
+
+	/// Get the list of all contacts attached to this body.
+	/// @warning this list changes during the time step and you may
+	/// miss some collisions if you don't use b2ContactListener.
+	b2ContactEdge* GetContactList();
+	const b2ContactEdge* GetContactList() const;
+
+	/// Get the next body in the world's body list.
+	b2Body* GetNext();
+	const b2Body* GetNext() const;
+
+	/// Get the user data pointer that was provided in the body definition.
+	void* GetUserData() const;
+
+	/// Set the user data. Use this to store your application specific data.
+	void SetUserData(void* data);
+
+	/// Get the parent world of this body.
+	b2World* GetWorld();
+	const b2World* GetWorld() const;
+
+private:
+
+	friend class b2World;
+	friend class b2Island;
+	friend class b2ContactManager;
+	friend class b2ContactSolver;
+	friend class b2TOISolver;
+	
+	friend class b2DistanceJoint;
+	friend class b2GearJoint;
+	friend class b2LineJoint;
+	friend class b2MouseJoint;
+	friend class b2PrismaticJoint;
+	friend class b2PulleyJoint;
+	friend class b2RevoluteJoint;
+	friend class b2WeldJoint;
+	friend class b2FrictionJoint;
+
+	// m_flags
+	enum
+	{
+		e_islandFlag		= 0x0001,
+		e_awakeFlag			= 0x0002,
+		e_autoSleepFlag		= 0x0004,
+		e_bulletFlag		= 0x0008,
+		e_fixedRotationFlag	= 0x0010,
+		e_activeFlag		= 0x0020,
+		e_toiFlag			= 0x0040,
+	};
+
+	b2Body(const b2BodyDef* bd, b2World* world);
+	~b2Body();
+
+	void SynchronizeFixtures();
+	void SynchronizeTransform();
+
+	// This is used to prevent connected bodies from colliding.
+	// It may lie, depending on the collideConnected flag.
+	bool ShouldCollide(const b2Body* other) const;
+
+	void Advance(float32 t);
+
+	b2BodyType m_type;
+
+	uint16 m_flags;
+
+	int32 m_islandIndex;
+
+	b2Transform m_xf;		// the body origin transform
+	b2Sweep m_sweep;		// the swept motion for CCD
+
+	b2Vec2 m_linearVelocity;
+	float32 m_angularVelocity;
+
+	b2Vec2 m_force;
+	float32 m_torque;
+
+	b2World* m_world;
+	b2Body* m_prev;
+	b2Body* m_next;
+
+	b2Fixture* m_fixtureList;
+	int32 m_fixtureCount;
+
+	b2JointEdge* m_jointList;
+	b2ContactEdge* m_contactList;
+
+	float32 m_mass, m_invMass;
+
+	// Rotational inertia about the center of mass.
+	float32 m_I, m_invI;
+
+	float32 m_linearDamping;
+	float32 m_angularDamping;
+
+	float32 m_sleepTime;
+
+	void* m_userData;
+};
+
+inline b2BodyType b2Body::GetType() const
+{
+	return m_type;
+}
+
+inline const b2Transform& b2Body::GetTransform() const
+{
+	return m_xf;
+}
+
+inline const b2Vec2& b2Body::GetPosition() const
+{
+	return m_xf.position;
+}
+
+inline float32 b2Body::GetAngle() const
+{
+	return m_sweep.a;
+}
+
+inline const b2Vec2& b2Body::GetWorldCenter() const
+{
+	return m_sweep.c;
+}
+
+inline const b2Vec2& b2Body::GetLocalCenter() const
+{
+	return m_sweep.localCenter;
+}
+
+inline void b2Body::SetLinearVelocity(const b2Vec2& v)
+{
+	if (m_type == b2_staticBody)
+	{
+		return;
+	}
+
+	if (b2Dot(v,v) > 0.0f)
+	{
+		SetAwake(true);
+	}
+
+	m_linearVelocity = v;
+}
+
+inline b2Vec2 b2Body::GetLinearVelocity() const
+{
+	return m_linearVelocity;
+}
+
+inline void b2Body::SetAngularVelocity(float32 w)
+{
+	if (m_type == b2_staticBody)
+	{
+		return;
+	}
+
+	if (w * w > 0.0f)
+	{
+		SetAwake(true);
+	}
+
+	m_angularVelocity = w;
+}
+
+inline float32 b2Body::GetAngularVelocity() const
+{
+	return m_angularVelocity;
+}
+
+inline float32 b2Body::GetMass() const
+{
+	return m_mass;
+}
+
+inline float32 b2Body::GetInertia() const
+{
+	return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
+}
+
+inline void b2Body::GetMassData(b2MassData* data) const
+{
+	data->mass = m_mass;
+	data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
+	data->center = m_sweep.localCenter;
+}
+
+inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const
+{
+	return b2Mul(m_xf, localPoint);
+}
+
+inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const
+{
+	return b2Mul(m_xf.R, localVector);
+}
+
+inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
+{
+	return b2MulT(m_xf, worldPoint);
+}
+
+inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const
+{
+	return b2MulT(m_xf.R, worldVector);
+}
+
+inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
+{
+	return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
+}
+
+inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const
+{
+	return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
+}
+
+inline float32 b2Body::GetLinearDamping() const
+{
+	return m_linearDamping;
+}
+
+inline void b2Body::SetLinearDamping(float32 linearDamping)
+{
+	m_linearDamping = linearDamping;
+}
+
+inline float32 b2Body::GetAngularDamping() const
+{
+	return m_angularDamping;
+}
+
+inline void b2Body::SetAngularDamping(float32 angularDamping)
+{
+	m_angularDamping = angularDamping;
+}
+
+inline void b2Body::SetBullet(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_bulletFlag;
+	}
+	else
+	{
+		m_flags &= ~e_bulletFlag;
+	}
+}
+
+inline bool b2Body::IsBullet() const
+{
+	return (m_flags & e_bulletFlag) == e_bulletFlag;
+}
+
+inline void b2Body::SetAwake(bool flag)
+{
+	if (flag)
+	{
+		if ((m_flags & e_awakeFlag) == 0)
+		{
+			m_flags |= e_awakeFlag;
+			m_sleepTime = 0.0f;
+		}
+	}
+	else
+	{
+		m_flags &= ~e_awakeFlag;
+		m_sleepTime = 0.0f;
+		m_linearVelocity.SetZero();
+		m_angularVelocity = 0.0f;
+		m_force.SetZero();
+		m_torque = 0.0f;
+	}
+}
+
+inline bool b2Body::IsAwake() const
+{
+	return (m_flags & e_awakeFlag) == e_awakeFlag;
+}
+
+inline bool b2Body::IsActive() const
+{
+	return (m_flags & e_activeFlag) == e_activeFlag;
+}
+
+inline void b2Body::SetFixedRotation(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_fixedRotationFlag;
+	}
+	else
+	{
+		m_flags &= ~e_fixedRotationFlag;
+	}
+
+	ResetMassData();
+}
+
+inline bool b2Body::IsFixedRotation() const
+{
+	return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
+}
+
+inline void b2Body::SetSleepingAllowed(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_autoSleepFlag;
+	}
+	else
+	{
+		m_flags &= ~e_autoSleepFlag;
+		SetAwake(true);
+	}
+}
+
+inline bool b2Body::IsSleepingAllowed() const
+{
+	return (m_flags & e_autoSleepFlag) == e_autoSleepFlag;
+}
+
+inline b2Fixture* b2Body::GetFixtureList()
+{
+	return m_fixtureList;
+}
+
+inline const b2Fixture* b2Body::GetFixtureList() const
+{
+	return m_fixtureList;
+}
+
+inline b2JointEdge* b2Body::GetJointList()
+{
+	return m_jointList;
+}
+
+inline const b2JointEdge* b2Body::GetJointList() const
+{
+	return m_jointList;
+}
+
+inline b2ContactEdge* b2Body::GetContactList()
+{
+	return m_contactList;
+}
+
+inline const b2ContactEdge* b2Body::GetContactList() const
+{
+	return m_contactList;
+}
+
+inline b2Body* b2Body::GetNext()
+{
+	return m_next;
+}
+
+inline const b2Body* b2Body::GetNext() const
+{
+	return m_next;
+}
+
+inline void b2Body::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+inline void* b2Body::GetUserData() const
+{
+	return m_userData;
+}
+
+inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
+{
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	if (IsAwake() == false)
+	{
+		SetAwake(true);
+	}
+
+	m_force += force;
+	m_torque += b2Cross(point - m_sweep.c, force);
+}
+
+inline void b2Body::ApplyTorque(float32 torque)
+{
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	if (IsAwake() == false)
+	{
+		SetAwake(true);
+	}
+
+	m_torque += torque;
+}
+
+inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point)
+{
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	if (IsAwake() == false)
+	{
+		SetAwake(true);
+	}
+	m_linearVelocity += m_invMass * impulse;
+	m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
+}
+
+inline void b2Body::ApplyAngularImpulse(float32 impulse)
+{
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	if (IsAwake() == false)
+	{
+		SetAwake(true);
+	}
+	m_angularVelocity += m_invI * impulse;
+}
+
+inline void b2Body::SynchronizeTransform()
+{
+	m_xf.R.Set(m_sweep.a);
+	m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
+}
+
+inline void b2Body::Advance(float32 t)
+{
+	// Advance to the new safe time.
+	m_sweep.Advance(t);
+	m_sweep.c = m_sweep.c0;
+	m_sweep.a = m_sweep.a0;
+	SynchronizeTransform();
+}
+
+inline b2World* b2Body::GetWorld()
+{
+	return m_world;
+}
+
+inline const b2World* b2Body::GetWorld() const
+{
+	return m_world;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp

@@ -0,0 +1,266 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/b2ContactManager.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+b2ContactFilter b2_defaultFilter;
+b2ContactListener b2_defaultListener;
+
+b2ContactManager::b2ContactManager()
+{
+	m_contactList = NULL;
+	m_contactCount = 0;
+	m_contactFilter = &b2_defaultFilter;
+	m_contactListener = &b2_defaultListener;
+	m_allocator = NULL;
+}
+
+void b2ContactManager::Destroy(b2Contact* c)
+{
+	b2Fixture* fixtureA = c->GetFixtureA();
+	b2Fixture* fixtureB = c->GetFixtureB();
+	b2Body* bodyA = fixtureA->GetBody();
+	b2Body* bodyB = fixtureB->GetBody();
+
+	if (m_contactListener && c->IsTouching())
+	{
+		m_contactListener->EndContact(c);
+	}
+
+	// Remove from the world.
+	if (c->m_prev)
+	{
+		c->m_prev->m_next = c->m_next;
+	}
+
+	if (c->m_next)
+	{
+		c->m_next->m_prev = c->m_prev;
+	}
+
+	if (c == m_contactList)
+	{
+		m_contactList = c->m_next;
+	}
+
+	// Remove from body 1
+	if (c->m_nodeA.prev)
+	{
+		c->m_nodeA.prev->next = c->m_nodeA.next;
+	}
+
+	if (c->m_nodeA.next)
+	{
+		c->m_nodeA.next->prev = c->m_nodeA.prev;
+	}
+
+	if (&c->m_nodeA == bodyA->m_contactList)
+	{
+		bodyA->m_contactList = c->m_nodeA.next;
+	}
+
+	// Remove from body 2
+	if (c->m_nodeB.prev)
+	{
+		c->m_nodeB.prev->next = c->m_nodeB.next;
+	}
+
+	if (c->m_nodeB.next)
+	{
+		c->m_nodeB.next->prev = c->m_nodeB.prev;
+	}
+
+	if (&c->m_nodeB == bodyB->m_contactList)
+	{
+		bodyB->m_contactList = c->m_nodeB.next;
+	}
+
+	// Call the factory.
+	b2Contact::Destroy(c, m_allocator);
+	--m_contactCount;
+}
+
+// This is the top level collision call for the time step. Here
+// all the narrow phase collision is processed for the world
+// contact list.
+void b2ContactManager::Collide()
+{
+	// Update awake contacts.
+	b2Contact* c = m_contactList;
+	while (c)
+	{
+		b2Fixture* fixtureA = c->GetFixtureA();
+		b2Fixture* fixtureB = c->GetFixtureB();
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+
+		if (bodyA->IsAwake() == false && bodyB->IsAwake() == false)
+		{
+			c = c->GetNext();
+			continue;
+		}
+
+		// Is this contact flagged for filtering?
+		if (c->m_flags & b2Contact::e_filterFlag)
+		{
+			// Should these bodies collide?
+			if (bodyB->ShouldCollide(bodyA) == false)
+			{
+				b2Contact* cNuke = c;
+				c = cNuke->GetNext();
+				Destroy(cNuke);
+				continue;
+			}
+
+			// Check user filtering.
+			if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
+			{
+				b2Contact* cNuke = c;
+				c = cNuke->GetNext();
+				Destroy(cNuke);
+				continue;
+			}
+
+			// Clear the filtering flag.
+			c->m_flags &= ~b2Contact::e_filterFlag;
+		}
+
+		int32 proxyIdA = fixtureA->m_proxyId;
+		int32 proxyIdB = fixtureB->m_proxyId;
+		bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
+
+		// Here we destroy contacts that cease to overlap in the broad-phase.
+		if (overlap == false)
+		{
+			b2Contact* cNuke = c;
+			c = cNuke->GetNext();
+			Destroy(cNuke);
+			continue;
+		}
+
+		// The contact persists.
+		c->Update(m_contactListener);
+		c = c->GetNext();
+	}
+}
+
+void b2ContactManager::FindNewContacts()
+{
+	m_broadPhase.UpdatePairs(this);
+}
+
+void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
+{
+	b2Fixture* fixtureA = (b2Fixture*)proxyUserDataA;
+	b2Fixture* fixtureB = (b2Fixture*)proxyUserDataB;
+
+	b2Body* bodyA = fixtureA->GetBody();
+	b2Body* bodyB = fixtureB->GetBody();
+
+	// Are the fixtures on the same body?
+	if (bodyA == bodyB)
+	{
+		return;
+	}
+
+	// Does a contact already exist?
+	b2ContactEdge* edge = bodyB->GetContactList();
+	while (edge)
+	{
+		if (edge->other == bodyA)
+		{
+			b2Fixture* fA = edge->contact->GetFixtureA();
+			b2Fixture* fB = edge->contact->GetFixtureB();
+			if (fA == fixtureA && fB == fixtureB)
+			{
+				// A contact already exists.
+				return;
+			}
+
+			if (fA == fixtureB && fB == fixtureA)
+			{
+				// A contact already exists.
+				return;
+			}
+		}
+
+		edge = edge->next;
+	}
+
+	// Does a joint override collision? Is at least one body dynamic?
+	if (bodyB->ShouldCollide(bodyA) == false)
+	{
+		return;
+	}
+
+	// Check user filtering.
+	if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
+	{
+		return;
+	}
+
+	// Call the factory.
+	b2Contact* c = b2Contact::Create(fixtureA, fixtureB, m_allocator);
+
+	// Contact creation may swap fixtures.
+	fixtureA = c->GetFixtureA();
+	fixtureB = c->GetFixtureB();
+	bodyA = fixtureA->GetBody();
+	bodyB = fixtureB->GetBody();
+
+	// Insert into the world.
+	c->m_prev = NULL;
+	c->m_next = m_contactList;
+	if (m_contactList != NULL)
+	{
+		m_contactList->m_prev = c;
+	}
+	m_contactList = c;
+
+	// Connect to island graph.
+
+	// Connect to body A
+	c->m_nodeA.contact = c;
+	c->m_nodeA.other = bodyB;
+
+	c->m_nodeA.prev = NULL;
+	c->m_nodeA.next = bodyA->m_contactList;
+	if (bodyA->m_contactList != NULL)
+	{
+		bodyA->m_contactList->prev = &c->m_nodeA;
+	}
+	bodyA->m_contactList = &c->m_nodeA;
+
+	// Connect to body B
+	c->m_nodeB.contact = c;
+	c->m_nodeB.other = bodyA;
+
+	c->m_nodeB.prev = NULL;
+	c->m_nodeB.next = bodyB->m_contactList;
+	if (bodyB->m_contactList != NULL)
+	{
+		bodyB->m_contactList->prev = &c->m_nodeB;
+	}
+	bodyB->m_contactList = &c->m_nodeB;
+
+	++m_contactCount;
+}

src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,36 +19,34 @@
 #ifndef B2_CONTACT_MANAGER_H
 #define B2_CONTACT_MANAGER_H
 
-#include "../Collision/b2BroadPhase.h"
-#include "../Dynamics/Contacts/b2NullContact.h"
+#include <Box2D/Collision/b2BroadPhase.h>
 
-class b2World;
 class b2Contact;
-struct b2TimeStep;
+class b2ContactFilter;
+class b2ContactListener;
+class b2BlockAllocator;
 
 // Delegate of b2World.
-class b2ContactManager : public b2PairCallback
+class b2ContactManager
 {
 public:
-	b2ContactManager() : m_world(NULL), m_destroyImmediate(false) {}
+	b2ContactManager();
 
-	// Implements PairCallback
-	void* PairAdded(void* proxyUserData1, void* proxyUserData2);
+	// Broad-phase callback.
+	void AddPair(void* proxyUserDataA, void* proxyUserDataB);
 
-	// Implements PairCallback
-	void PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData);
+	void FindNewContacts();
 
 	void Destroy(b2Contact* c);
 
 	void Collide();
-
-	b2World* m_world;
-
-	// This lets us provide broadphase proxy pair user data for
-	// contacts that shouldn't exist.
-	b2NullContact m_nullContact;
-
-	bool m_destroyImmediate;
+            
+	b2BroadPhase m_broadPhase;
+	b2Contact* m_contactList;
+	int32 m_contactCount;
+	b2ContactFilter* m_contactFilter;
+	b2ContactListener* m_contactListener;
+	b2BlockAllocator* m_allocator;
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp

@@ -0,0 +1,163 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+#include <Box2D/Collision/b2BroadPhase.h>
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+
+
+b2Fixture::b2Fixture()
+{
+	m_userData = NULL;
+	m_body = NULL;
+	m_next = NULL;
+	m_proxyId = b2BroadPhase::e_nullProxy;
+	m_shape = NULL;
+	m_density = 0.0f;
+}
+
+b2Fixture::~b2Fixture()
+{
+	b2Assert(m_shape == NULL);
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+}
+
+void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def)
+{
+	m_userData = def->userData;
+	m_friction = def->friction;
+	m_restitution = def->restitution;
+
+	m_body = body;
+	m_next = NULL;
+
+	m_filter = def->filter;
+
+	m_isSensor = def->isSensor;
+
+	m_shape = def->shape->Clone(allocator);
+
+	m_density = def->density;
+}
+
+void b2Fixture::Destroy(b2BlockAllocator* allocator)
+{
+	// The proxy must be destroyed before calling this.
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+
+	// Free the child shape.
+	switch (m_shape->m_type)
+	{
+	case b2Shape::e_circle:
+		{
+			b2CircleShape* s = (b2CircleShape*)m_shape;
+			s->~b2CircleShape();
+			allocator->Free(s, sizeof(b2CircleShape));
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			b2PolygonShape* s = (b2PolygonShape*)m_shape;
+			s->~b2PolygonShape();
+			allocator->Free(s, sizeof(b2PolygonShape));
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	m_shape = NULL;
+}
+
+void b2Fixture::CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf)
+{
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+
+	// Create proxy in the broad-phase.
+	m_shape->ComputeAABB(&m_aabb, xf);
+	m_proxyId = broadPhase->CreateProxy(m_aabb, this);
+}
+
+void b2Fixture::DestroyProxy(b2BroadPhase* broadPhase)
+{
+	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	{
+		return;
+	}
+
+	// Destroy proxy in the broad-phase.
+	broadPhase->DestroyProxy(m_proxyId);
+	m_proxyId = b2BroadPhase::e_nullProxy;
+}
+
+void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2)
+{
+	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	{	
+		return;
+	}
+
+	// Compute an AABB that covers the swept shape (may miss some rotation effect).
+	b2AABB aabb1, aabb2;
+	m_shape->ComputeAABB(&aabb1, transform1);
+	m_shape->ComputeAABB(&aabb2, transform2);
+	
+	m_aabb.Combine(aabb1, aabb2);
+
+	b2Vec2 displacement = transform2.position - transform1.position;
+
+	broadPhase->MoveProxy(m_proxyId, m_aabb, displacement);
+}
+
+void b2Fixture::SetFilterData(const b2Filter& filter)
+{
+	m_filter = filter;
+
+	if (m_body == NULL)
+	{
+		return;
+	}
+
+	// Flag associated contacts for filtering.
+	b2ContactEdge* edge = m_body->GetContactList();
+	while (edge)
+	{
+		b2Contact* contact = edge->contact;
+		b2Fixture* fixtureA = contact->GetFixtureA();
+		b2Fixture* fixtureB = contact->GetFixtureB();
+		if (fixtureA == this || fixtureB == this)
+		{
+			contact->FlagForFiltering();
+		}
+
+		edge = edge->next;
+	}
+}
+
+void b2Fixture::SetSensor(bool sensor)
+{
+	m_isSensor = sensor;
+}
+

src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h

@@ -0,0 +1,326 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_FIXTURE_H
+#define B2_FIXTURE_H
+
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Collision/b2Collision.h>
+#include <Box2D/Collision/Shapes/b2Shape.h>
+
+class b2BlockAllocator;
+class b2Body;
+class b2BroadPhase;
+
+/// This holds contact filtering data.
+struct b2Filter
+{
+	/// The collision category bits. Normally you would just set one bit.
+	uint16 categoryBits;
+
+	/// The collision mask bits. This states the categories that this
+	/// shape would accept for collision.
+	uint16 maskBits;
+
+	/// Collision groups allow a certain group of objects to never collide (negative)
+	/// or always collide (positive). Zero means no collision group. Non-zero group
+	/// filtering always wins against the mask bits.
+	int16 groupIndex;
+};
+
+/// A fixture definition is used to create a fixture. This class defines an
+/// abstract fixture definition. You can reuse fixture definitions safely.
+struct b2FixtureDef
+{
+	/// The constructor sets the default fixture definition values.
+	b2FixtureDef()
+	{
+		shape = NULL;
+		userData = NULL;
+		friction = 0.2f;
+		restitution = 0.0f;
+		density = 0.0f;
+		filter.categoryBits = 0x0001;
+		filter.maskBits = 0xFFFF;
+		filter.groupIndex = 0;
+		isSensor = false;
+	}
+
+	virtual ~b2FixtureDef() {}
+
+	/// The shape, this must be set. The shape will be cloned, so you
+	/// can create the shape on the stack.
+	const b2Shape* shape;
+
+	/// Use this to store application specific fixture data.
+	void* userData;
+
+	/// The friction coefficient, usually in the range [0,1].
+	float32 friction;
+
+	/// The restitution (elasticity) usually in the range [0,1].
+	float32 restitution;
+
+	/// The density, usually in kg/m^2.
+	float32 density;
+
+	/// A sensor shape collects contact information but never generates a collision
+	/// response.
+	bool isSensor;
+
+	/// Contact filtering data.
+	b2Filter filter;
+};
+
+
+/// A fixture is used to attach a shape to a body for collision detection. A fixture
+/// inherits its transform from its parent. Fixtures hold additional non-geometric data
+/// such as friction, collision filters, etc.
+/// Fixtures are created via b2Body::CreateFixture.
+/// @warning you cannot reuse fixtures.
+class b2Fixture
+{
+public:
+	/// Get the type of the child shape. You can use this to down cast to the concrete shape.
+	/// @return the shape type.
+	b2Shape::Type GetType() const;
+
+	/// Get the child shape. You can modify the child shape, however you should not change the
+	/// number of vertices because this will crash some collision caching mechanisms.
+	/// Manipulating the shape may lead to non-physical behavior.
+	b2Shape* GetShape();
+	const b2Shape* GetShape() const;
+
+	/// Set if this fixture is a sensor.
+	void SetSensor(bool sensor);
+
+	/// Is this fixture a sensor (non-solid)?
+	/// @return the true if the shape is a sensor.
+	bool IsSensor() const;
+
+	/// Set the contact filtering data. This will not update contacts until the next time
+	/// step when either parent body is active and awake.
+	void SetFilterData(const b2Filter& filter);
+
+	/// Get the contact filtering data.
+	const b2Filter& GetFilterData() const;
+
+	/// Get the parent body of this fixture. This is NULL if the fixture is not attached.
+	/// @return the parent body.
+	b2Body* GetBody();
+	const b2Body* GetBody() const;
+
+	/// Get the next fixture in the parent body's fixture list.
+	/// @return the next shape.
+	b2Fixture* GetNext();
+	const b2Fixture* GetNext() const;
+
+	/// Get the user data that was assigned in the fixture definition. Use this to
+	/// store your application specific data.
+	void* GetUserData() const;
+
+	/// Set the user data. Use this to store your application specific data.
+	void SetUserData(void* data);
+
+	/// Test a point for containment in this fixture.
+	/// @param xf the shape world transform.
+	/// @param p a point in world coordinates.
+	bool TestPoint(const b2Vec2& p) const;
+
+	/// Cast a ray against this shape.
+	/// @param output the ray-cast results.
+	/// @param input the ray-cast input parameters.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
+
+	/// Get the mass data for this fixture. The mass data is based on the density and
+	/// the shape. The rotational inertia is about the shape's origin. This operation
+	/// may be expensive.
+	void GetMassData(b2MassData* massData) const;
+
+	/// Set the density of this fixture. This will _not_ automatically adjust the mass
+	/// of the body. You must call b2Body::ResetMassData to update the body's mass.
+	void SetDensity(float32 density);
+
+	/// Get the density of this fixture.
+	float32 GetDensity() const;
+
+	/// Get the coefficient of friction.
+	float32 GetFriction() const;
+
+	/// Set the coefficient of friction.
+	void SetFriction(float32 friction);
+
+	/// Get the coefficient of restitution.
+	float32 GetRestitution() const;
+
+	/// Set the coefficient of restitution.
+	void SetRestitution(float32 restitution);
+
+	/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
+	/// If you need a more accurate AABB, compute it using the shape and
+	/// the body transform.
+	const b2AABB& GetAABB() const;
+
+protected:
+
+	friend class b2Body;
+	friend class b2World;
+	friend class b2Contact;
+	friend class b2ContactManager;
+
+	b2Fixture();
+	~b2Fixture();
+
+	// We need separation create/destroy functions from the constructor/destructor because
+	// the destructor cannot access the allocator (no destructor arguments allowed by C++).
+	void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def);
+	void Destroy(b2BlockAllocator* allocator);
+
+	// These support body activation/deactivation.
+	void CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf);
+	void DestroyProxy(b2BroadPhase* broadPhase);
+
+	void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
+
+	b2AABB m_aabb;
+
+	float32 m_density;
+
+	b2Fixture* m_next;
+	b2Body* m_body;
+
+	b2Shape* m_shape;
+
+	float32 m_friction;
+	float32 m_restitution;
+
+	int32 m_proxyId;
+	b2Filter m_filter;
+
+	bool m_isSensor;
+
+	void* m_userData;
+};
+
+inline b2Shape::Type b2Fixture::GetType() const
+{
+	return m_shape->GetType();
+}
+
+inline b2Shape* b2Fixture::GetShape()
+{
+	return m_shape;
+}
+
+inline const b2Shape* b2Fixture::GetShape() const
+{
+	return m_shape;
+}
+
+inline bool b2Fixture::IsSensor() const
+{
+	return m_isSensor;
+}
+
+inline const b2Filter& b2Fixture::GetFilterData() const
+{
+	return m_filter;
+}
+
+inline void* b2Fixture::GetUserData() const
+{
+	return m_userData;
+}
+
+inline void b2Fixture::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+inline b2Body* b2Fixture::GetBody()
+{
+	return m_body;
+}
+
+inline const b2Body* b2Fixture::GetBody() const
+{
+	return m_body;
+}
+
+inline b2Fixture* b2Fixture::GetNext()
+{
+	return m_next;
+}
+
+inline const b2Fixture* b2Fixture::GetNext() const
+{
+	return m_next;
+}
+
+inline void b2Fixture::SetDensity(float32 density)
+{
+	b2Assert(b2IsValid(density) && density >= 0.0f);
+	m_density = density;
+}
+
+inline float32 b2Fixture::GetDensity() const
+{
+	return m_density;
+}
+
+inline float32 b2Fixture::GetFriction() const
+{
+	return m_friction;
+}
+
+inline void b2Fixture::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b2Fixture::GetRestitution() const
+{
+	return m_restitution;
+}
+
+inline void b2Fixture::SetRestitution(float32 restitution)
+{
+	m_restitution = restitution;
+}
+
+inline bool b2Fixture::TestPoint(const b2Vec2& p) const
+{
+	return m_shape->TestPoint(m_body->GetTransform(), p);
+}
+
+inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
+{
+	return m_shape->RayCast(output, input, m_body->GetTransform());
+}
+
+inline void b2Fixture::GetMassData(b2MassData* massData) const
+{
+	m_shape->ComputeMass(massData, m_density);
+}
+
+inline const b2AABB& b2Fixture::GetAABB() const
+{
+	return m_aabb;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,13 +16,14 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2Island.h"
-#include "b2Body.h"
-#include "b2World.h"
-#include "Contacts/b2Contact.h"
-#include "Contacts/b2ContactSolver.h"
-#include "Joints/b2Joint.h"
-#include "../Common/b2StackAllocator.h"
+#include <Box2D/Dynamics/b2Island.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Dynamics/b2World.h>
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+#include <Box2D/Common/b2StackAllocator.h>
 
 /*
 Position Correction Notes
@@ -103,6 +104,45 @@ probably default to the slower Full NGS and let the user select the faster
 Baumgarte method in performance critical scenarios.
 */
 
+/*
+Cache Performance
+
+The Box2D solvers are dominated by cache misses. Data structures are designed
+to increase the number of cache hits. Much of misses are due to random access
+to body data. The constraint structures are iterated over linearly, which leads
+to few cache misses.
+
+The bodies are not accessed during iteration. Instead read only data, such as
+the mass values are stored with the constraints. The mutable data are the constraint
+impulses and the bodies velocities/positions. The impulses are held inside the
+constraint structures. The body velocities/positions are held in compact, temporary
+arrays to increase the number of cache hits. Linear and angular velocity are
+stored in a single array since multiple arrays lead to multiple misses.
+*/
+
+/*
+2D Rotation
+
+R = [cos(theta) -sin(theta)]
+    [sin(theta) cos(theta) ]
+
+thetaDot = omega
+
+Let q1 = cos(theta), q2 = sin(theta).
+R = [q1 -q2]
+    [q2  q1]
+
+q1Dot = -thetaDot * q2
+q2Dot = thetaDot * q1
+
+q1_new = q1_old - dt * w * q2
+q2_new = q2_old + dt * w * q1
+then normalize.
+
+This might be faster than computing sin+cos.
+However, we can compute sin+cos of the same angle fast.
+*/
+
 b2Island::b2Island(
 	int32 bodyCapacity,
 	int32 contactCapacity,
@@ -124,35 +164,36 @@ b2Island::b2Island(
 	m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity	 * sizeof(b2Contact*));
 	m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
 
-	m_positionIterationCount = 0;
+	m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
+	m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
 }
 
 b2Island::~b2Island()
 {
 	// Warning: the order should reverse the constructor order.
+	m_allocator->Free(m_positions);
+	m_allocator->Free(m_velocities);
 	m_allocator->Free(m_joints);
 	m_allocator->Free(m_contacts);
 	m_allocator->Free(m_bodies);
 }
 
-void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep)
+void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
 {
 	// Integrate velocities and apply damping.
 	for (int32 i = 0; i < m_bodyCount; ++i)
 	{
 		b2Body* b = m_bodies[i];
 
-		if (b->IsStatic())
+		if (b->GetType() != b2_dynamicBody)
+		{
 			continue;
+		}
 
 		// Integrate velocities.
 		b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
 		b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
 
-		// Reset forces.
-		b->m_force.Set(0.0f, 0.0f);
-		b->m_torque = 0.0f;
-
 		// Apply damping.
 		// ODE: dv/dt + c * v = 0
 		// Solution: v(t) = v0 * exp(-c * t)
@@ -162,72 +203,70 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correct
 		// v2 = (1.0f - c * dt) * v1
 		b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
 		b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
-
-		// Check for large velocities.
-#ifdef TARGET_FLOAT32_IS_FIXED
-				// Fixed point code written this way to prevent
-				// overflows, float code is optimized for speed
-
-		float32 vMagnitude = b->m_linearVelocity.Length();
-		if(vMagnitude > b2_maxLinearVelocity) {
-			b->m_linearVelocity *= b2_maxLinearVelocity/vMagnitude;
-		}
-		b->m_angularVelocity = b2Clamp(b->m_angularVelocity, 
-			-b2_maxAngularVelocity, b2_maxAngularVelocity);
-
-#else
-
-		if (b2Dot(b->m_linearVelocity, b->m_linearVelocity) > b2_maxLinearVelocitySquared)
-		{
-			b->m_linearVelocity.Normalize();
-			b->m_linearVelocity *= b2_maxLinearVelocity;
-		}
-		if (b->m_angularVelocity * b->m_angularVelocity > b2_maxAngularVelocitySquared)
-		{
-			if (b->m_angularVelocity < 0.0f)
-			{
-				b->m_angularVelocity = -b2_maxAngularVelocity;
-			}
-			else
-			{
-				b->m_angularVelocity = b2_maxAngularVelocity;
-			}
-		}
-#endif
-
 	}
 
-	b2ContactSolver contactSolver(step, m_contacts, m_contactCount, m_allocator);
+	// Partition contacts so that contacts with static bodies are solved last.
+	int32 i1 = -1;
+	for (int32 i2 = 0; i2 < m_contactCount; ++i2)
+	{
+		b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
+		b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+		bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
+		if (nonStatic)
+		{
+			++i1;
+			b2Swap(m_contacts[i1], m_contacts[i2]);
+		}
+	}
 
 	// Initialize velocity constraints.
-	contactSolver.InitVelocityConstraints(step);
-
+	b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
+	contactSolver.WarmStart();
 	for (int32 i = 0; i < m_jointCount; ++i)
 	{
 		m_joints[i]->InitVelocityConstraints(step);
 	}
 
 	// Solve velocity constraints.
-	for (int32 i = 0; i < step.maxIterations; ++i)
+	for (int32 i = 0; i < step.velocityIterations; ++i)
 	{
-		contactSolver.SolveVelocityConstraints();
-
 		for (int32 j = 0; j < m_jointCount; ++j)
 		{
 			m_joints[j]->SolveVelocityConstraints(step);
 		}
+
+		contactSolver.SolveVelocityConstraints();
 	}
 
 	// Post-solve (store impulses for warm starting).
-	contactSolver.FinalizeVelocityConstraints();
+	contactSolver.StoreImpulses();
 
 	// Integrate positions.
 	for (int32 i = 0; i < m_bodyCount; ++i)
 	{
 		b2Body* b = m_bodies[i];
 
-		if (b->IsStatic())
+		if (b->GetType() == b2_staticBody)
+		{
 			continue;
+		}
+
+		// Check for large velocities.
+		b2Vec2 translation = step.dt * b->m_linearVelocity;
+		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
+		{
+			float32 ratio = b2_maxTranslation / translation.Length();
+			b->m_linearVelocity *= ratio;
+		}
+
+		float32 rotation = step.dt * b->m_angularVelocity;
+		if (rotation * rotation > b2_maxRotationSquared)
+		{
+			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			b->m_angularVelocity *= ratio;
+		}
 
 		// Store positions for continuous collision.
 		b->m_sweep.c0 = b->m_sweep.c;
@@ -243,31 +282,22 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correct
 		// Note: shapes are synchronized later.
 	}
 
-	if (correctPositions)
+	// Iterate over constraints.
+	for (int32 i = 0; i < step.positionIterations; ++i)
 	{
-		// Initialize position constraints.
-		// Contacts don't need initialization.
+		bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
+
+		bool jointsOkay = true;
 		for (int32 i = 0; i < m_jointCount; ++i)
 		{
-			m_joints[i]->InitPositionConstraints();
+			bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
+			jointsOkay = jointsOkay && jointOkay;
 		}
 
-		// Iterate over constraints.
-		for (m_positionIterationCount = 0; m_positionIterationCount < step.maxIterations; ++m_positionIterationCount)
+		if (contactsOkay && jointsOkay)
 		{
-			bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
-
-			bool jointsOkay = true;
-			for (int i = 0; i < m_jointCount; ++i)
-			{
-				bool jointOkay = m_joints[i]->SolvePositionConstraints();
-				jointsOkay = jointsOkay && jointOkay;
-			}
-
-			if (contactsOkay && jointsOkay)
-			{
-				break;
-			}
+			// Exit early if the position errors are small.
+			break;
 		}
 	}
 
@@ -275,36 +305,28 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correct
 
 	if (allowSleep)
 	{
-		float32 minSleepTime = B2_FLT_MAX;
+		float32 minSleepTime = b2_maxFloat;
 
-#ifndef TARGET_FLOAT32_IS_FIXED
 		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
 		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
-#endif
 
 		for (int32 i = 0; i < m_bodyCount; ++i)
 		{
 			b2Body* b = m_bodies[i];
-			if (b->m_invMass == 0.0f)
+			if (b->GetType() == b2_staticBody)
 			{
 				continue;
 			}
 
-			if ((b->m_flags & b2Body::e_allowSleepFlag) == 0)
+			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
 			{
 				b->m_sleepTime = 0.0f;
 				minSleepTime = 0.0f;
 			}
 
-			if ((b->m_flags & b2Body::e_allowSleepFlag) == 0 ||
-#ifdef TARGET_FLOAT32_IS_FIXED
-				b2Abs(b->m_angularVelocity) > b2_angularSleepTolerance ||
-				b2Abs(b->m_linearVelocity.x) > b2_linearSleepTolerance ||
-				b2Abs(b->m_linearVelocity.y) > b2_linearSleepTolerance)
-#else
+			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
 				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
 				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
-#endif
 			{
 				b->m_sleepTime = 0.0f;
 				minSleepTime = 0.0f;
@@ -321,66 +343,13 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correct
 			for (int32 i = 0; i < m_bodyCount; ++i)
 			{
 				b2Body* b = m_bodies[i];
-				b->m_flags |= b2Body::e_sleepFlag;
-				b->m_linearVelocity = b2Vec2_zero;
-				b->m_angularVelocity = 0.0f;
+				b->SetAwake(false);
 			}
 		}
 	}
 }
 
-void b2Island::SolveTOI(const b2TimeStep& subStep)
-{
-	b2ContactSolver contactSolver(subStep, m_contacts, m_contactCount, m_allocator);
-
-	// No warm starting needed for TOI events.
-
-	// Solve velocity constraints.
-	for (int32 i = 0; i < subStep.maxIterations; ++i)
-	{
-		contactSolver.SolveVelocityConstraints();
-	}
-
-	// Don't store the TOI contact forces for warm starting
-	// because they can be quite large.
-
-	// Integrate positions.
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Body* b = m_bodies[i];
-
-		if (b->IsStatic())
-			continue;
-
-		// Store positions for continuous collision.
-		b->m_sweep.c0 = b->m_sweep.c;
-		b->m_sweep.a0 = b->m_sweep.a;
-
-		// Integrate
-		b->m_sweep.c += subStep.dt * b->m_linearVelocity;
-		b->m_sweep.a += subStep.dt * b->m_angularVelocity;
-
-		// Compute new transform
-		b->SynchronizeTransform();
-
-		// Note: shapes are synchronized later.
-	}
-
-	// Solve position constraints.
-	const float32 k_toiBaumgarte = 0.75f;
-	for (int32 i = 0; i < subStep.maxIterations; ++i)
-	{
-		bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
-		if (contactsOkay)
-		{
-			break;
-		}
-	}
-
-	Report(contactSolver.m_constraints);
-}
-
-void b2Island::Report(b2ContactConstraint* constraints)
+void b2Island::Report(const b2ContactConstraint* constraints)
 {
 	if (m_listener == NULL)
 	{
@@ -390,31 +359,16 @@ void b2Island::Report(b2ContactConstraint* constraints)
 	for (int32 i = 0; i < m_contactCount; ++i)
 	{
 		b2Contact* c = m_contacts[i];
-		b2ContactConstraint* cc = constraints + i;
-		b2ContactResult cr;
-		cr.shape1 = c->GetShape1();
-		cr.shape2 = c->GetShape2();
-		b2Body* b1 = cr.shape1->GetBody();
-		int32 manifoldCount = c->GetManifoldCount();
-		b2Manifold* manifolds = c->GetManifolds();
-		for (int32 j = 0; j < manifoldCount; ++j)
+
+		const b2ContactConstraint* cc = constraints + i;
+		
+		b2ContactImpulse impulse;
+		for (int32 j = 0; j < cc->pointCount; ++j)
 		{
-			b2Manifold* manifold = manifolds + j;
-			cr.normal = manifold->normal;
-			for (int32 k = 0; k < manifold->pointCount; ++k)
-			{
-				b2ManifoldPoint* point = manifold->points + k;
-				b2ContactConstraintPoint* ccp = cc->points + k;
-				cr.position = b1->GetWorldPoint(point->localPoint1);
-
-				// TOI constraint results are not stored, so get
-				// the result from the constraint.
-				cr.normalImpulse = ccp->normalImpulse;
-				cr.tangentImpulse = ccp->tangentImpulse;
-				cr.id = point->id;
-
-				m_listener->Result(&cr);
-			}
+			impulse.normalImpulses[j] = cc->points[j].normalImpulse;
+			impulse.tangentImpulses[j] = cc->points[j].tangentImpulse;
 		}
+
+		m_listener->PostSolve(c, &impulse);
 	}
 }

src/modules/physics/box2d/Box2D/Dynamics/b2Island.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,16 +19,31 @@
 #ifndef B2_ISLAND_H
 #define B2_ISLAND_H
 
-#include "../Common/b2Math.h"
+#include <Box2D/Common/b2Math.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 class b2Contact;
-class b2Body;
 class b2Joint;
 class b2StackAllocator;
 class b2ContactListener;
 struct b2ContactConstraint;
-struct b2TimeStep;
 
+/// This is an internal structure.
+struct b2Position
+{
+	b2Vec2 x;
+	float32 a;
+};
+
+/// This is an internal structure.
+struct b2Velocity
+{
+	b2Vec2 v;
+	float32 w;
+};
+
+/// This is an internal class.
 class b2Island
 {
 public:
@@ -43,13 +58,12 @@ public:
 		m_jointCount = 0;
 	}
 
-	void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep);
-
-	void SolveTOI(const b2TimeStep& subStep);
+	void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
 
 	void Add(b2Body* body)
 	{
 		b2Assert(m_bodyCount < m_bodyCapacity);
+		body->m_islandIndex = m_bodyCount;
 		m_bodies[m_bodyCount++] = body;
 	}
 
@@ -65,7 +79,7 @@ public:
 		m_joints[m_jointCount++] = joint;
 	}
 
-	void Report(b2ContactConstraint* constraints);
+	void Report(const b2ContactConstraint* constraints);
 
 	b2StackAllocator* m_allocator;
 	b2ContactListener* m_listener;
@@ -74,6 +88,9 @@ public:
 	b2Contact** m_contacts;
 	b2Joint** m_joints;
 
+	b2Position* m_positions;
+	b2Velocity* m_velocities;
+
 	int32 m_bodyCount;
 	int32 m_jointCount;
 	int32 m_contactCount;

src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,36 +16,20 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2Settings.h"
-#include <cstdlib>
+#ifndef B2_TIME_STEP_H
+#define B2_TIME_STEP_H
 
-b2Version b2_version = {2, 0, 1};
+#include <Box2D/Common/b2Settings.h>
 
-int32 b2_byteCount = 0;
+/// This is an internal structure.
+struct b2TimeStep
+{
+	float32 dt;			// time step
+	float32 inv_dt;		// inverse time step (0 if dt == 0).
+	float32 dtRatio;	// dt * inv_dt0
+	int32 velocityIterations;
+	int32 positionIterations;
+	bool warmStarting;
+};
 
-
-
-// Memory allocators. Modify these to use your own allocator.
-void* b2Alloc(int32 size)
-{
-	size += 4;
-	b2_byteCount += size;
-	char* bytes = (char*)malloc(size);
-	*(int32*)bytes = size;
-	return bytes + 4;
-}
-
-void b2Free(void* mem)
-{
-	if (mem == NULL)
-	{
-		return;
-	}
-
-	char* bytes = (char*)mem;
-	bytes -= 4;
-	int32 size = *(int32*)bytes;
-	b2Assert(b2_byteCount >= size);
-	b2_byteCount -= size;
-	free(bytes);
-}
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2World.h

@@ -1,253 +1,285 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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_WORLD_H
-#define B2_WORLD_H
-
-#include "../Common/b2Math.h"
-#include "../Common/b2BlockAllocator.h"
-#include "../Common/b2StackAllocator.h"
-#include "b2ContactManager.h"
-#include "b2WorldCallbacks.h"
-
-struct b2AABB;
-struct b2ShapeDef;
-struct b2BodyDef;
-struct b2JointDef;
-class b2Body;
-class b2Joint;
-class b2Shape;
-class b2Contact;
-class b2BroadPhase;
-
-struct b2TimeStep
-{
-	float32 dt;			// time step
-	float32 inv_dt;		// inverse time step (0 if dt == 0).
-	float32 dtRatio;	// dt * inv_dt0
-	int32 maxIterations;
-	bool warmStarting;
-	bool positionCorrection;
-};
-
-/// The world class manages all physics entities, dynamic simulation,
-/// and asynchronous queries. The world also contains efficient memory
-/// management facilities.
-class b2World
-{
-public:
-	/// Construct a world object.
-	/// @param worldAABB a bounding box that completely encompasses all your shapes.
-	/// @param gravity the world gravity vector.
-	/// @param doSleep improve performance by not simulating inactive bodies.
-	b2World(const b2AABB& worldAABB, const b2Vec2& gravity, bool doSleep);
-
-	/// Destruct the world. All physics entities are destroyed and all heap memory is released.
-	~b2World();
-
-	/// Register a destruction listener.
-	void SetDestructionListener(b2DestructionListener* listener);
-
-	/// Register a broad-phase boundary listener.
-	void SetBoundaryListener(b2BoundaryListener* listener);
-
-	/// Register a contact filter to provide specific control over collision.
-	/// Otherwise the default filter is used (b2_defaultFilter).
-	void SetContactFilter(b2ContactFilter* filter);
-
-	/// Register a contact event listener
-	void SetContactListener(b2ContactListener* listener);
-
-	/// Register a routine for debug drawing. The debug draw functions are called
-	/// inside the b2World::Step method, so make sure your renderer is ready to
-	/// consume draw commands when you call Step().
-	void SetDebugDraw(b2DebugDraw* debugDraw);
-
-	/// Create a rigid body given a definition. No reference to the definition
-	/// is retained.
-	/// @warning This function is locked during callbacks.
-	b2Body* CreateBody(const b2BodyDef* def);
-
-	/// Destroy a rigid body given a definition. No reference to the definition
-	/// is retained. This function is locked during callbacks.
-	/// @warning This automatically deletes all associated shapes and joints.
-	/// @warning This function is locked during callbacks.
-	void DestroyBody(b2Body* body);
-
-	/// Create a joint to constrain bodies together. No reference to the definition
-	/// is retained. This may cause the connected bodies to cease colliding.
-	/// @warning This function is locked during callbacks.
-	b2Joint* CreateJoint(const b2JointDef* def);
-
-	/// Destroy a joint. This may cause the connected bodies to begin colliding.
-	/// @warning This function is locked during callbacks.
-	void DestroyJoint(b2Joint* joint);
-
-	/// The world provides a single static ground body with no collision shapes.
-	/// You can use this to simplify the creation of joints and static shapes.
-	b2Body* GetGroundBody();
-
-	/// Take a time step. This performs collision detection, integration,
-	/// and constraint solution.
-	/// @param timeStep the amount of time to simulate, this should not vary.
-	/// @param iterations the number of iterations to be used by the constraint solver.
-	void Step(float32 timeStep, int32 iterations);
-
-	/// Query the world for all shapes that potentially overlap the
-	/// provided AABB. You provide a shape pointer buffer of specified
-	/// size. The number of shapes found is returned.
-	/// @param aabb the query box.
-	/// @param shapes a user allocated shape pointer array of size maxCount (or greater).
-	/// @param maxCount the capacity of the shapes array.
-	/// @return the number of shapes found in aabb.
-	int32 Query(const b2AABB& aabb, b2Shape** shapes, int32 maxCount);
-
-	/// Get the world body list. With the returned body, use b2Body::GetNext to get
-	/// the next body in the world list. A NULL body indicates the end of the list.
-	/// @return the head of the world body list.
-	b2Body* GetBodyList();
-
-	/// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
-	/// the next joint in the world list. A NULL joint indicates the end of the list.
-	/// @return the head of the world joint list.
-	b2Joint* GetJointList();
-
-	/// Re-filter a shape. This re-runs contact filtering on a shape.
-	void Refilter(b2Shape* shape);
-
-	/// Enable/disable warm starting. For testing.
-	void SetWarmStarting(bool flag) { m_warmStarting = flag; }
-
-	/// Enable/disable position correction. For testing.
-	void SetPositionCorrection(bool flag) { m_positionCorrection = flag; }
-
-	/// Enable/disable continuous physics. For testing.
-	void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
-
-	/// Perform validation of internal data structures.
-	void Validate();
-
-	/// Get the number of broad-phase proxies.
-	int32 GetProxyCount() const;
-
-	/// Get the number of broad-phase pairs.
-	int32 GetPairCount() const;
-
-	/// Get the number of bodies.
-	int32 GetBodyCount() const;
-
-	/// Get the number joints.
-	int32 GetJointCount() const;
-
-	/// Get the number of contacts (each may have 0 or more contact points).
-	int32 GetContactCount() const;
-
-	/// Change the global gravity vector.
-	void SetGravity(const b2Vec2& gravity);
-
-public:
-
-	friend class b2Body;
-	friend class b2ContactManager;
-
-	void Solve(const b2TimeStep& step);
-	void SolveTOI(const b2TimeStep& step);
-
-	void DrawJoint(b2Joint* joint);
-	void DrawShape(b2Shape* shape, const b2XForm& xf, const b2Color& color, bool core);
-	void DrawDebugData();
-
-	b2BlockAllocator m_blockAllocator;
-	b2StackAllocator m_stackAllocator;
-
-	bool m_lock;
-
-	b2BroadPhase* m_broadPhase;
-	b2ContactManager m_contactManager;
-
-	b2Body* m_bodyList;
-	b2Joint* m_jointList;
-
-	// Do not access
-	b2Contact* m_contactList;
-
-	int32 m_bodyCount;
-	int32 m_contactCount;
-	int32 m_jointCount;
-
-	b2Vec2 m_gravity;
-	bool m_allowSleep;
-
-	b2Body* m_groundBody;
-
-	b2DestructionListener* m_destructionListener;
-	b2BoundaryListener* m_boundaryListener;
-	b2ContactFilter* m_contactFilter;
-	b2ContactListener* m_contactListener;
-	b2DebugDraw* m_debugDraw;
-
-	float32 m_inv_dt0;
-
-	int32 m_positionIterationCount;
-
-	// This is for debugging the solver.
-	bool m_positionCorrection;
-
-	// This is for debugging the solver.
-	bool m_warmStarting;
-
-	// This is for debugging the solver.
-	bool m_continuousPhysics;
-};
-
-inline b2Body* b2World::GetGroundBody()
-{
-	return m_groundBody;
-}
-
-inline b2Body* b2World::GetBodyList()
-{
-	return m_bodyList;
-}
-
-inline b2Joint* b2World::GetJointList()
-{
-	return m_jointList;
-}
-
-inline int32 b2World::GetBodyCount() const
-{
-	return m_bodyCount;
-}
-
-inline int32 b2World::GetJointCount() const
-{
-	return m_jointCount;
-}
-
-inline int32 b2World::GetContactCount() const
-{
-	return m_contactCount;
-}
-
-inline void b2World::SetGravity(const b2Vec2& gravity)
-{
-	m_gravity = gravity;
-}
-
-#endif
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* 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_WORLD_H
+#define B2_WORLD_H
+
+#include <Box2D/Common/b2Math.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Common/b2StackAllocator.h>
+#include <Box2D/Dynamics/b2ContactManager.h>
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+
+struct b2AABB;
+struct b2BodyDef;
+struct b2JointDef;
+struct b2TimeStep;
+class b2Body;
+class b2Fixture;
+class b2Joint;
+
+/// The world class manages all physics entities, dynamic simulation,
+/// and asynchronous queries. The world also contains efficient memory
+/// management facilities.
+class b2World
+{
+public:
+	/// Construct a world object.
+	/// @param gravity the world gravity vector.
+	/// @param doSleep improve performance by not simulating inactive bodies.
+	b2World(const b2Vec2& gravity, bool doSleep);
+
+	/// Destruct the world. All physics entities are destroyed and all heap memory is released.
+	~b2World();
+
+	/// Register a destruction listener. The listener is owned by you and must
+	/// remain in scope.
+	void SetDestructionListener(b2DestructionListener* listener);
+
+	/// Register a contact filter to provide specific control over collision.
+	/// Otherwise the default filter is used (b2_defaultFilter). The listener is
+	/// owned by you and must remain in scope. 
+	void SetContactFilter(b2ContactFilter* filter);
+
+	/// Register a contact event listener. The listener is owned by you and must
+	/// remain in scope.
+	void SetContactListener(b2ContactListener* listener);
+
+	/// Register a routine for debug drawing. The debug draw functions are called
+	/// inside with b2World::DrawDebugData method. The debug draw object is owned
+	/// by you and must remain in scope.
+	void SetDebugDraw(b2DebugDraw* debugDraw);
+
+	/// Create a rigid body given a definition. No reference to the definition
+	/// is retained.
+	/// @warning This function is locked during callbacks.
+	b2Body* CreateBody(const b2BodyDef* def);
+
+	/// Destroy a rigid body given a definition. No reference to the definition
+	/// is retained. This function is locked during callbacks.
+	/// @warning This automatically deletes all associated shapes and joints.
+	/// @warning This function is locked during callbacks.
+	void DestroyBody(b2Body* body);
+
+	/// Create a joint to constrain bodies together. No reference to the definition
+	/// is retained. This may cause the connected bodies to cease colliding.
+	/// @warning This function is locked during callbacks.
+	b2Joint* CreateJoint(const b2JointDef* def);
+
+	/// Destroy a joint. This may cause the connected bodies to begin colliding.
+	/// @warning This function is locked during callbacks.
+	void DestroyJoint(b2Joint* joint);
+
+	/// Take a time step. This performs collision detection, integration,
+	/// and constraint solution.
+	/// @param timeStep the amount of time to simulate, this should not vary.
+	/// @param velocityIterations for the velocity constraint solver.
+	/// @param positionIterations for the position constraint solver.
+	void Step(	float32 timeStep,
+				int32 velocityIterations,
+				int32 positionIterations);
+
+	/// Call this after you are done with time steps to clear the forces. You normally
+	/// call this after each call to Step, unless you are performing sub-steps. By default,
+	/// forces will be automatically cleared, so you don't need to call this function.
+	/// @see SetAutoClearForces
+	void ClearForces();
+
+	/// Call this to draw shapes and other debug draw data.
+	void DrawDebugData();
+
+	/// Query the world for all fixtures that potentially overlap the
+	/// provided AABB.
+	/// @param callback a user implemented callback class.
+	/// @param aabb the query box.
+	void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast the world for all fixtures in the path of the ray. Your callback
+	/// controls whether you get the closest point, any point, or n-points.
+	/// The ray-cast ignores shapes that contain the starting point.
+	/// @param callback a user implemented callback class.
+	/// @param point1 the ray starting point
+	/// @param point2 the ray ending point
+	void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;
+
+	/// Get the world body list. With the returned body, use b2Body::GetNext to get
+	/// the next body in the world list. A NULL body indicates the end of the list.
+	/// @return the head of the world body list.
+	b2Body* GetBodyList();
+
+	/// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
+	/// the next joint in the world list. A NULL joint indicates the end of the list.
+	/// @return the head of the world joint list.
+	b2Joint* GetJointList();
+
+	/// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
+	/// the next contact in the world list. A NULL contact indicates the end of the list.
+	/// @return the head of the world contact list.
+	/// @warning contacts are 
+	b2Contact* GetContactList();
+
+	/// Enable/disable warm starting. For testing.
+	void SetWarmStarting(bool flag) { m_warmStarting = flag; }
+
+	/// Enable/disable continuous physics. For testing.
+	void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
+
+	/// Get the number of broad-phase proxies.
+	int32 GetProxyCount() const;
+
+	/// Get the number of bodies.
+	int32 GetBodyCount() const;
+
+	/// Get the number of joints.
+	int32 GetJointCount() const;
+
+	/// Get the number of contacts (each may have 0 or more contact points).
+	int32 GetContactCount() const;
+
+	/// Change the global gravity vector.
+	void SetGravity(const b2Vec2& gravity);
+	
+	/// Get the global gravity vector.
+	b2Vec2 GetGravity() const;
+
+	/// Is the world locked (in the middle of a time step).
+	bool IsLocked() const;
+
+	/// Set flag to control automatic clearing of forces after each time step.
+	void SetAutoClearForces(bool flag);
+
+	/// Get the flag that controls automatic clearing of forces after each time step.
+	bool GetAutoClearForces() const;
+
+private:
+
+	// m_flags
+	enum
+	{
+		e_newFixture	= 0x0001,
+		e_locked		= 0x0002,
+		e_clearForces	= 0x0004,
+	};
+
+	friend class b2Body;
+	friend class b2ContactManager;
+	friend class b2Controller;
+
+	void Solve(const b2TimeStep& step);
+	void SolveTOI();
+	void SolveTOI(b2Body* body);
+
+	void DrawJoint(b2Joint* joint);
+	void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);
+
+	b2BlockAllocator m_blockAllocator;
+	b2StackAllocator m_stackAllocator;
+
+	int32 m_flags;
+
+	b2ContactManager m_contactManager;
+
+	b2Body* m_bodyList;
+	b2Joint* m_jointList;
+
+	int32 m_bodyCount;
+	int32 m_jointCount;
+
+	b2Vec2 m_gravity;
+	bool m_allowSleep;
+
+	b2Body* m_groundBody;
+
+	b2DestructionListener* m_destructionListener;
+	b2DebugDraw* m_debugDraw;
+
+	// This is used to compute the time step ratio to
+	// support a variable time step.
+	float32 m_inv_dt0;
+
+	// This is for debugging the solver.
+	bool m_warmStarting;
+
+	// This is for debugging the solver.
+	bool m_continuousPhysics;
+};
+
+inline b2Body* b2World::GetBodyList()
+{
+	return m_bodyList;
+}
+
+inline b2Joint* b2World::GetJointList()
+{
+	return m_jointList;
+}
+
+inline b2Contact* b2World::GetContactList()
+{
+	return m_contactManager.m_contactList;
+}
+
+inline int32 b2World::GetBodyCount() const
+{
+	return m_bodyCount;
+}
+
+inline int32 b2World::GetJointCount() const
+{
+	return m_jointCount;
+}
+
+inline int32 b2World::GetContactCount() const
+{
+	return m_contactManager.m_contactCount;
+}
+
+inline void b2World::SetGravity(const b2Vec2& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline b2Vec2 b2World::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline bool b2World::IsLocked() const
+{
+	return (m_flags & e_locked) == e_locked;
+}
+
+inline void b2World::SetAutoClearForces(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_clearForces;
+	}
+	else
+	{
+		m_flags &= ~e_clearForces;
+	}
+}
+
+/// Get the flag that controls automatic clearing of forces after each time step.
+inline bool b2World::GetAutoClearForces() const
+{
+	return (m_flags & e_clearForces) == e_clearForces;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -16,24 +16,22 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#include "b2WorldCallbacks.h"
-#include "../Collision/Shapes/b2Shape.h"
-
-b2ContactFilter b2_defaultFilter;
+#include <Box2D/Dynamics/b2WorldCallbacks.h>
+#include <Box2D/Dynamics/b2Fixture.h>
 
 // Return true if contact calculations should be performed between these two shapes.
 // If you implement your own collision filter you may want to build from this implementation.
-bool b2ContactFilter::ShouldCollide(b2Shape* shape1, b2Shape* shape2)
+bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
 {
-	const b2FilterData& filter1 = shape1->GetFilterData();
-	const b2FilterData& filter2 = shape2->GetFilterData();
+	const b2Filter& filterA = fixtureA->GetFilterData();
+	const b2Filter& filterB = fixtureB->GetFilterData();
 
-	if (filter1.groupIndex == filter2.groupIndex && filter1.groupIndex != 0)
+	if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
 	{
-		return filter1.groupIndex > 0;
+		return filterA.groupIndex > 0;
 	}
 
-	bool collide = (filter1.maskBits & filter2.categoryBits) != 0 && (filter1.categoryBits & filter2.maskBits) != 0;
+	bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
 	return collide;
 }
 

src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
@@ -19,18 +19,19 @@
 #ifndef B2_WORLD_CALLBACKS_H
 #define B2_WORLD_CALLBACKS_H
 
-#include "../Common/b2Settings.h"
+#include <Box2D/Common/b2Settings.h>
 
 struct b2Vec2;
-struct b2XForm;
-class b2Shape;
+struct b2Transform;
+class b2Fixture;
 class b2Body;
 class b2Joint;
 class b2Contact;
 struct b2ContactPoint;
 struct b2ContactResult;
+struct b2Manifold;
 
-/// Joints and shapes are destroyed when their associated
+/// Joints and fixtures are destroyed when their associated
 /// body is destroyed. Implement this listener so that you
 /// may nullify references to these joints and shapes.
 class b2DestructionListener
@@ -42,24 +43,11 @@ public:
 	/// to the destruction of one of its attached bodies.
 	virtual void SayGoodbye(b2Joint* joint) = 0;
 
-	/// Called when any shape is about to be destroyed due
+	/// Called when any fixture is about to be destroyed due
 	/// to the destruction of its parent body.
-	virtual void SayGoodbye(b2Shape* shape) = 0;
+	virtual void SayGoodbye(b2Fixture* fixture) = 0;
 };
 
-
-/// This is called when a body's shape passes outside of the world boundary.
-class b2BoundaryListener
-{
-public:
-	virtual ~b2BoundaryListener() {}
-
-	/// This is called for each body that leaves the world boundary.
-	/// @warning you can't modify the world inside this callback.
-	virtual void Violation(b2Body* body) = 0;
-};
-
-
 /// Implement this class to provide collision filtering. In other words, you can implement
 /// this class if you want finer control over contact creation.
 class b2ContactFilter
@@ -69,13 +57,19 @@ public:
 
 	/// Return true if contact calculations should be performed between these two shapes.
 	/// @warning for performance reasons this is only called when the AABBs begin to overlap.
-	virtual bool ShouldCollide(b2Shape* shape1, b2Shape* shape2);
+	virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB);
 };
 
-/// The default contact filter.
-extern b2ContactFilter b2_defaultFilter;
+/// Contact impulses for reporting. Impulses are used instead of forces because
+/// sub-step forces may approach infinity for rigid body collisions. These
+/// match up one-to-one with the contact points in b2Manifold.
+struct b2ContactImpulse
+{
+	float32 normalImpulses[b2_maxManifoldPoints];
+	float32 tangentImpulses[b2_maxManifoldPoints];
+};
 
-/// Implement this class to get collision results. You can use these results for
+/// Implement this class to get contact information. You can use these results for
 /// things like sounds and game logic. You can also get contact results by
 /// traversing the contact lists after the time step. However, you might miss
 /// some contacts because continuous physics leads to sub-stepping.
@@ -83,27 +77,79 @@ extern b2ContactFilter b2_defaultFilter;
 /// single time step.
 /// You should strive to make your callbacks efficient because there may be
 /// many callbacks per time step.
-/// @warning The contact separation is the last computed value.
 /// @warning You cannot create/destroy Box2D entities inside these callbacks.
 class b2ContactListener
 {
 public:
 	virtual ~b2ContactListener() {}
 
-	/// Called when a contact point is added. This includes the geometry
-	/// and the forces.
-	virtual void Add(const b2ContactPoint* point) { }
+	/// Called when two fixtures begin to touch.
+	virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); }
 
-	/// Called when a contact point persists. This includes the geometry
-	/// and the forces.
-	virtual void Persist(const b2ContactPoint* point) { }
+	/// Called when two fixtures cease to touch.
+	virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); }
 
-	/// Called when a contact point is removed. This includes the last
-	/// computed geometry and forces.
-	virtual void Remove(const b2ContactPoint* point) { }
+	/// This is called after a contact is updated. This allows you to inspect a
+	/// contact before it goes to the solver. If you are careful, you can modify the
+	/// contact manifold (e.g. disable contact).
+	/// A copy of the old manifold is provided so that you can detect changes.
+	/// Note: this is called only for awake bodies.
+	/// Note: this is called even when the number of contact points is zero.
+	/// Note: this is not called for sensors.
+	/// Note: if you set the number of contact points to zero, you will not
+	/// get an EndContact callback. However, you may get a BeginContact callback
+	/// the next step.
+	virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
+	{
+		B2_NOT_USED(contact);
+		B2_NOT_USED(oldManifold);
+	}
 
-	/// Called after a contact point is solved.
-	virtual void Result(const b2ContactResult* point) { }
+	/// This lets you inspect a contact after the solver is finished. This is useful
+	/// for inspecting impulses.
+	/// Note: the contact manifold does not include time of impact impulses, which can be
+	/// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
+	/// in a separate data structure.
+	/// Note: this is only called for contacts that are touching, solid, and awake.
+	virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse)
+	{
+		B2_NOT_USED(contact);
+		B2_NOT_USED(impulse);
+	}
+};
+
+/// Callback class for AABB queries.
+/// See b2World::Query
+class b2QueryCallback
+{
+public:
+	virtual ~b2QueryCallback() {}
+
+	/// Called for each fixture found in the query AABB.
+	/// @return false to terminate the query.
+	virtual bool ReportFixture(b2Fixture* fixture) = 0;
+};
+
+/// Callback class for ray casts.
+/// See b2World::RayCast
+class b2RayCastCallback
+{
+public:
+	virtual ~b2RayCastCallback() {}
+
+	/// Called for each fixture found in the query. You control how the ray cast
+	/// proceeds by returning a float:
+	/// return -1: ignore this fixture and continue
+	/// return 0: terminate the ray cast
+	/// return fraction: clip the ray to this point
+	/// return 1: don't clip the ray and continue
+	/// @param fixture the fixture hit by the ray
+	/// @param point the point of initial intersection
+	/// @param normal the normal vector at the point of intersection
+	/// @return -1 to filter, 0 to terminate, fraction to clip the ray for
+	/// closest hit, 1 to continue
+	virtual float32 ReportFixture(	b2Fixture* fixture, const b2Vec2& point,
+									const b2Vec2& normal, float32 fraction) = 0;
 };
 
 /// Color for debug drawing. Each value has the range [0,1].
@@ -111,6 +157,7 @@ struct b2Color
 {
 	b2Color() {}
 	b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
+	void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }
 	float32 r, g, b;
 };
 
@@ -127,11 +174,9 @@ public:
 	{
 		e_shapeBit				= 0x0001, ///< draw shapes
 		e_jointBit				= 0x0002, ///< draw joint connections
-		e_coreShapeBit			= 0x0004, ///< draw core (TOI) shapes
-		e_aabbBit				= 0x0008, ///< draw axis aligned bounding boxes
-		e_obbBit				= 0x0010, ///< draw oriented bounding boxes
-		e_pairBit				= 0x0020, ///< draw broad-phase pairs
-		e_centerOfMassBit		= 0x0040, ///< draw center of mass frame
+		e_aabbBit				= 0x0004, ///< draw axis aligned bounding boxes
+		e_pairBit				= 0x0008, ///< draw broad-phase pairs
+		e_centerOfMassBit		= 0x0010, ///< draw center of mass frame
 	};
 
 	/// Set the drawing flags.
@@ -163,7 +208,7 @@ public:
 
 	/// Draw a transform. Choose your own length scale.
 	/// @param xf a transform.
-	virtual void DrawXForm(const b2XForm& xf) = 0;
+	virtual void DrawTransform(const b2Transform& xf) = 0;
 
 protected:
 	uint32 m_drawFlags;

src/modules/physics/box2d/Contact.h

@@ -27,7 +27,7 @@
 #include "World.h"
 
 // Box2D
-#include "Include/Box2D.h"
+#include <Box2D/Box2D.h>
 
 namespace love
 {

src/modules/physics/box2d/Include/Box2D.h

@@ -1,52 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 BOX2D_H
-#define BOX2D_H
-
-/**
-\mainpage Box2D API Documentation
-
-\section intro_sec Getting Started
-
-For tutorials please see http://www.box2d.org/manual.html
-
-For discussion please visit http://www.box2d.org/forum
-*/
-
-// These include files constitute the main Box2D API
-
-#include "../Source/Common/b2Settings.h"
-
-#include "../Source/Collision/Shapes/b2CircleShape.h"
-#include "../Source/Collision/Shapes/b2PolygonShape.h"
-#include "../Source/Collision/b2BroadPhase.h"
-#include "../Source/Dynamics/b2WorldCallbacks.h"
-#include "../Source/Dynamics/b2World.h"
-#include "../Source/Dynamics/b2Body.h"
-
-#include "../Source/Dynamics/Contacts/b2Contact.h"
-
-#include "../Source/Dynamics/Joints/b2DistanceJoint.h"
-#include "../Source/Dynamics/Joints/b2MouseJoint.h"
-#include "../Source/Dynamics/Joints/b2PrismaticJoint.h"
-#include "../Source/Dynamics/Joints/b2RevoluteJoint.h"
-#include "../Source/Dynamics/Joints/b2PulleyJoint.h"
-#include "../Source/Dynamics/Joints/b2GearJoint.h"
-
-#endif

src/modules/physics/box2d/Joint.h

@@ -26,7 +26,7 @@
 #include <physics/Joint.h>
 
 // Box2D
-#include "Include/Box2D.h"
+#include <Box2D/Box2D.h>
 
 namespace love
 {

src/modules/physics/box2d/Shape.h

@@ -27,7 +27,7 @@
 #include <common/Reference.h>
 
 // Box2D
-#include "Include/Box2D.h"
+#include <Box2D/Box2D.h>
 
 namespace love
 {

src/modules/physics/box2d/Source/Collision/Shapes/b2CircleShape.cpp

@@ -1,120 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2CircleShape.h"
-
-b2CircleShape::b2CircleShape(const b2ShapeDef* def)
-: b2Shape(def)
-{
-	b2Assert(def->type == e_circleShape);
-	const b2CircleDef* circleDef = (const b2CircleDef*)def;
-
-	m_type = e_circleShape;
-	m_localPosition = circleDef->localPosition;
-	m_radius = circleDef->radius;
-}
-
-void b2CircleShape::UpdateSweepRadius(const b2Vec2& center)
-{
-	// Update the sweep radius (maximum radius) as measured from
-	// a local center point.
-	b2Vec2 d = m_localPosition - center;
-	m_sweepRadius = d.Length() + m_radius - b2_toiSlop;
-}
-
-bool b2CircleShape::TestPoint(const b2XForm& transform, const b2Vec2& p) const
-{
-	b2Vec2 center = transform.position + b2Mul(transform.R, m_localPosition);
-	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::TestSegment(const b2XForm& transform,
-								float32* lambda,
-								b2Vec2* normal,
-								const b2Segment& segment,
-								float32 maxLambda) const
-{
-	b2Vec2 position = transform.position + b2Mul(transform.R, m_localPosition);
-	b2Vec2 s = segment.p1 - position;
-	float32 b = b2Dot(s, s) - m_radius * m_radius;
-
-	// Does the segment start inside the circle?
-	if (b < 0.0f)
-	{
-		return false;
-	}
-
-	// Solve quadratic equation.
-	b2Vec2 r = segment.p2 - segment.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_FLT_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 <= maxLambda * rr)
-	{
-		a /= rr;
-		*lambda = a;
-		*normal = s + a * r;
-		normal->Normalize();
-		return true;
-	}
-
-	return false;
-}
-
-void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2XForm& transform) const
-{
-	b2Vec2 p = transform.position + b2Mul(transform.R, m_localPosition);
-	aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
-	aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
-}
-
-void b2CircleShape::ComputeSweptAABB(b2AABB* aabb, const b2XForm& transform1, const b2XForm& transform2) const
-{
-	b2Vec2 p1 = transform1.position + b2Mul(transform1.R, m_localPosition);
-	b2Vec2 p2 = transform2.position + b2Mul(transform2.R, m_localPosition);
-	b2Vec2 lower = b2Min(p1, p2);
-	b2Vec2 upper = b2Max(p1, p2);
-
-	aabb->lowerBound.Set(lower.x - m_radius, lower.y - m_radius);
-	aabb->upperBound.Set(upper.x + m_radius, upper.y + m_radius);
-}
-
-void b2CircleShape::ComputeMass(b2MassData* massData) const
-{
-	massData->mass = m_density * b2_pi * m_radius * m_radius;
-	massData->center = m_localPosition;
-
-	// inertia about the local origin
-	massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_localPosition, m_localPosition));
-}

src/modules/physics/box2d/Source/Collision/Shapes/b2CircleShape.h

@@ -1,92 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Shape.h"
-
-/// This structure is used to build circle shapes.
-struct b2CircleDef : public b2ShapeDef
-{
-	b2CircleDef()
-	{
-		type = e_circleShape;
-		localPosition.SetZero();
-		radius = 1.0f;
-	}
-
-	b2Vec2 localPosition;
-	float32 radius;
-};
-
-/// A circle shape.
-class b2CircleShape : public b2Shape
-{
-public:
-	/// @see b2Shape::TestPoint
-	bool TestPoint(const b2XForm& transform, const b2Vec2& p) const;
-
-	/// @see b2Shape::TestSegment
-	bool TestSegment(	const b2XForm& transform,
-						float32* lambda,
-						b2Vec2* normal,
-						const b2Segment& segment,
-						float32 maxLambda) const;
-
-	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2XForm& transform) const;
-
-	/// @see b2Shape::ComputeSweptAABB
-	void ComputeSweptAABB(	b2AABB* aabb,
-							const b2XForm& transform1,
-							const b2XForm& transform2) const;
-
-	/// @see b2Shape::ComputeMass
-	void ComputeMass(b2MassData* massData) const;
-
-	/// Get the local position of this circle in its parent body.
-	const b2Vec2& GetLocalPosition() const;
-
-	/// Get the radius of this circle.
-	float32 GetRadius() const;
-
-private:
-
-	friend class b2Shape;
-
-	b2CircleShape(const b2ShapeDef* def);
-
-	void UpdateSweepRadius(const b2Vec2& center);
-
-	// Local position in parent body
-	b2Vec2 m_localPosition;
-	float32 m_radius;
-};
-
-inline const b2Vec2& b2CircleShape::GetLocalPosition() const
-{
-	return m_localPosition;
-}
-
-inline float32 b2CircleShape::GetRadius() const
-{
-	return m_radius;
-}
-
-#endif

src/modules/physics/box2d/Source/Collision/Shapes/b2PolygonShape.cpp

@@ -1,449 +0,0 @@
-
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2PolygonShape.h"
-
-void b2PolygonDef::SetAsBox(float32 hx, float32 hy)
-{
-	vertexCount = 4;
-	vertices[0].Set(-hx, -hy);
-	vertices[1].Set( hx, -hy);
-	vertices[2].Set( hx,  hy);
-	vertices[3].Set(-hx,  hy);
-}
-
-void b2PolygonDef::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
-{
-	SetAsBox(hx, hy);
-	b2XForm xf;
-	xf.position = center;
-	xf.R.Set(angle);
-
-	for (int32 i = 0; i < vertexCount; ++i)
-	{
-		vertices[i] = b2Mul(xf, vertices[i]);
-	}
-}
-
-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_FLT_EPSILON);
-	c *= 1.0f / area;
-	return c;
-}
-
-// http://www.geometrictools.com/Documentation/MinimumAreaRectangle.pdf
-static void ComputeOBB(b2OBB* obb, const b2Vec2* vs, int32 count)
-{
-	b2Assert(count <= b2_maxPolygonVertices);
-	b2Vec2 p[b2_maxPolygonVertices + 1];
-	for (int32 i = 0; i < count; ++i)
-	{
-		p[i] = vs[i];
-	}
-	p[count] = p[0];
-
-	float32 minArea = B2_FLT_MAX;
-	
-	for (int32 i = 1; i <= count; ++i)
-	{
-		b2Vec2 root = p[i-1];
-		b2Vec2 ux = p[i] - root;
-		float32 length = ux.Normalize();
-		b2Assert(length > B2_FLT_EPSILON);
-		b2Vec2 uy(-ux.y, ux.x);
-		b2Vec2 lower(B2_FLT_MAX, B2_FLT_MAX);
-		b2Vec2 upper(-B2_FLT_MAX, -B2_FLT_MAX);
-
-		for (int32 j = 0; j < count; ++j)
-		{
-			b2Vec2 d = p[j] - root;
-			b2Vec2 r;
-			r.x = b2Dot(ux, d);
-			r.y = b2Dot(uy, d);
-			lower = b2Min(lower, r);
-			upper = b2Max(upper, r);
-		}
-
-		float32 area = (upper.x - lower.x) * (upper.y - lower.y);
-		if (area < 0.95f * minArea)
-		{
-			minArea = area;
-			obb->R.col1 = ux;
-			obb->R.col2 = uy;
-			b2Vec2 center = 0.5f * (lower + upper);
-			obb->center = root + b2Mul(obb->R, center);
-			obb->extents = 0.5f * (upper - lower);
-		}
-	}
-
-	b2Assert(minArea < B2_FLT_MAX);
-}
-
-b2PolygonShape::b2PolygonShape(const b2ShapeDef* def)
-	 : b2Shape(def)
-{
-	b2Assert(def->type == e_polygonShape);
-	m_type = e_polygonShape;
-	const b2PolygonDef* poly = (const b2PolygonDef*)def;
-
-	// Get the vertices transformed into the body frame.
-	m_vertexCount = poly->vertexCount;
-	b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolygonVertices);
-
-	// Copy vertices.
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		m_vertices[i] = poly->vertices[i];
-	}
-
-	// Compute normals. Ensure the edges have non-zero length.
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		int32 i1 = i;
-		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
-		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
-		b2Assert(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON);
-		m_normals[i] = b2Cross(edge, 1.0f);
-		m_normals[i].Normalize();
-	}
-
-#ifdef _DEBUG
-	// Ensure the polygon is convex.
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		for (int32 j = 0; j < m_vertexCount; ++j)
-		{
-			// Don't check vertices on the current edge.
-			if (j == i || j == (i + 1) % m_vertexCount)
-			{
-				continue;
-			}
-			
-			// Your polygon is non-convex (it has an indentation).
-			// Or your polygon is too skinny.
-			float32 s = b2Dot(m_normals[i], m_vertices[j] - m_vertices[i]);
-			b2Assert(s < -b2_linearSlop);
-		}
-	}
-
-	// Ensure the polygon is counter-clockwise.
-	for (int32 i = 1; i < m_vertexCount; ++i)
-	{
-		float32 cross = b2Cross(m_normals[i-1], m_normals[i]);
-
-		// Keep asinf happy.
-		cross = b2Clamp(cross, -1.0f, 1.0f);
-
-		// You have consecutive edges that are almost parallel on your polygon.
-		float32 angle = asinf(cross);
-		b2Assert(angle > b2_angularSlop);
-	}
-#endif
-
-	// Compute the polygon centroid.
-	m_centroid = ComputeCentroid(poly->vertices, poly->vertexCount);
-
-	// Compute the oriented bounding box.
-	ComputeOBB(&m_obb, m_vertices, m_vertexCount);
-
-	// Create core polygon shape by shifting edges inward.
-	// Also compute the min/max radius for CCD.
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		int32 i1 = i - 1 >= 0 ? i - 1 : m_vertexCount - 1;
-		int32 i2 = i;
-
-		b2Vec2 n1 = m_normals[i1];
-		b2Vec2 n2 = m_normals[i2];
-		b2Vec2 v = m_vertices[i] - m_centroid;;
-
-		b2Vec2 d;
-		d.x = b2Dot(n1, v) - b2_toiSlop;
-		d.y = b2Dot(n2, v) - b2_toiSlop;
-
-		// Shifting the edge inward by b2_toiSlop should
-		// not cause the plane to pass the centroid.
-
-		// Your shape has a radius/extent less than b2_toiSlop.
-		b2Assert(d.x >= 0.0f);
-		b2Assert(d.y >= 0.0f);
-		b2Mat22 A;
-		A.col1.x = n1.x; A.col2.x = n1.y;
-		A.col1.y = n2.x; A.col2.y = n2.y;
-		m_coreVertices[i] = A.Solve(d) + m_centroid;
-	}
-}
-
-void b2PolygonShape::UpdateSweepRadius(const b2Vec2& center)
-{
-	// Update the sweep radius (maximum radius) as measured from
-	// a local center point.
-	m_sweepRadius = 0.0f;
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		b2Vec2 d = m_coreVertices[i] - center;
-		m_sweepRadius = b2Max(m_sweepRadius, d.Length());
-	}
-}
-
-bool b2PolygonShape::TestPoint(const b2XForm& xf, const b2Vec2& p) const
-{
-	b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
-
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
-		if (dot > 0.0f)
-		{
-			return false;
-		}
-	}
-
-	return true;
-}
-
-bool b2PolygonShape::TestSegment(
-	const b2XForm& xf,
-	float32* lambda,
-	b2Vec2* normal,
-	const b2Segment& segment,
-	float32 maxLambda) const
-{
-	float32 lower = 0.0f, upper = maxLambda;
-
-	b2Vec2 p1 = b2MulT(xf.R, segment.p1 - xf.position);
-	b2Vec2 p2 = b2MulT(xf.R, segment.p2 - xf.position);
-	b2Vec2 d = p2 - p1;
-	int32 index = -1;
-
-	for (int32 i = 0; i < m_vertexCount; ++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);
-
-		// 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;
-		}
-
-		if (upper < lower)
-		{
-			return false;
-		}
-	}
-
-	b2Assert(0.0f <= lower && lower <= maxLambda);
-
-	if (index >= 0)
-	{
-		*lambda = lower;
-		*normal = b2Mul(xf.R, m_normals[index]);
-		return true;
-	}
-
-	return false;
-}
-
-void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2XForm& xf) const
-{
-	b2Mat22 R = b2Mul(xf.R, m_obb.R);
-	b2Mat22 absR = b2Abs(R);
-	b2Vec2 h = b2Mul(absR, m_obb.extents);
-	b2Vec2 position = xf.position + b2Mul(xf.R, m_obb.center);
-	aabb->lowerBound = position - h;
-	aabb->upperBound = position + h;
-}
-
-void b2PolygonShape::ComputeSweptAABB(b2AABB* aabb,
-					  const b2XForm& transform1,
-					  const b2XForm& transform2) const
-{
-	b2AABB aabb1, aabb2;
-	ComputeAABB(&aabb1, transform1);
-	ComputeAABB(&aabb2, transform2);
-	aabb->lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
-	aabb->upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
-}
-
-void b2PolygonShape::ComputeMass(b2MassData* massData) 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_vertexCount >= 3);
-
-	b2Vec2 center; center.Set(0.0f, 0.0f);
-	float32 area = 0.0f;
-	float32 I = 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 < m_vertexCount; ++i)
-	{
-		pRef += m_vertices[i];
-	}
-	pRef *= 1.0f / count;
-#endif
-
-	const float32 k_inv3 = 1.0f / 3.0f;
-
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		// Triangle vertices.
-		b2Vec2 p1 = pRef;
-		b2Vec2 p2 = m_vertices[i];
-		b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
-
-		b2Vec2 e1 = p2 - p1;
-		b2Vec2 e2 = p3 - p1;
-
-		float32 D = b2Cross(e1, e2);
-
-		float32 triangleArea = 0.5f * D;
-		area += triangleArea;
-
-		// Area weighted centroid
-		center += triangleArea * k_inv3 * (p1 + p2 + p3);
-
-		float32 px = p1.x, py = p1.y;
-		float32 ex1 = e1.x, ey1 = e1.y;
-		float32 ex2 = e2.x, ey2 = e2.y;
-
-		float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
-		float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
-
-		I += D * (intx2 + inty2);
-	}
-
-	// Total mass
-	massData->mass = m_density * area;
-
-	// Center of mass
-	b2Assert(area > B2_FLT_EPSILON);
-	center *= 1.0f / area;
-	massData->center = center;
-
-	// Inertia tensor relative to the local origin.
-	massData->I = m_density * I;
-}
-
-b2Vec2 b2PolygonShape::Centroid(const b2XForm& xf) const
-{
-	return b2Mul(xf, m_centroid);
-}
-
-b2Vec2 b2PolygonShape::Support(const b2XForm& xf, const b2Vec2& d) const
-{
-	b2Vec2 dLocal = b2MulT(xf.R, d);
-
-	int32 bestIndex = 0;
-	float32 bestValue = b2Dot(m_coreVertices[0], dLocal);
-	for (int32 i = 1; i < m_vertexCount; ++i)
-	{
-		float32 value = b2Dot(m_coreVertices[i], dLocal);
-		if (value > bestValue)
-		{
-			bestIndex = i;
-			bestValue = value;
-		}
-	}
-
-	return b2Mul(xf, m_coreVertices[bestIndex]);
-}

src/modules/physics/box2d/Source/Collision/Shapes/b2PolygonShape.h

@@ -1,163 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Shape.h"
-
-/// Convex polygon. The vertices must be in CCW order for a right-handed
-/// coordinate system with the z-axis coming out of the screen.
-struct b2PolygonDef : public b2ShapeDef
-{
-	b2PolygonDef()
-	{
-		type = e_polygonShape;
-		vertexCount = 0;
-	}
-
-	/// Build vertices to represent an axis-aligned box.
-	/// @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);
-
-	/// The polygon vertices in local coordinates.
-	b2Vec2 vertices[b2_maxPolygonVertices];
-
-	/// The number of polygon vertices.
-	int32 vertexCount;
-};
-
-
-/// A convex polygon.
-class b2PolygonShape : public b2Shape
-{
-public:
-	/// @see b2Shape::TestPoint
-	bool TestPoint(const b2XForm& transform, const b2Vec2& p) const;
-
-	/// @see b2Shape::TestSegment
-	bool TestSegment(	const b2XForm& transform,
-		float32* lambda,
-		b2Vec2* normal,
-		const b2Segment& segment,
-		float32 maxLambda) const;
-
-	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2XForm& transform) const;
-
-	/// @see b2Shape::ComputeSweptAABB
-	void ComputeSweptAABB(	b2AABB* aabb,
-		const b2XForm& transform1,
-		const b2XForm& transform2) const;
-
-	/// @see b2Shape::ComputeMass
-	void ComputeMass(b2MassData* massData) const;
-
-	/// Get the oriented bounding box relative to the parent body.
-	const b2OBB& GetOBB() const;
-
-	/// Get local centroid relative to the parent body.
-	const b2Vec2& GetCentroid() const;
-
-	/// Get the vertex count.
-	int32 GetVertexCount() const;
-
-	/// Get the vertices in local coordinates.
-	const b2Vec2* GetVertices() const;
-
-	/// Get the core vertices in local coordinates. These vertices
-	/// represent a smaller polygon that is used for time of impact
-	/// computations.
-	const b2Vec2* GetCoreVertices() const;
-
-	/// Get the edge normal vectors. There is one for each vertex.
-	const b2Vec2* GetNormals() const;
-
-	/// Get the first vertex and apply the supplied transform.
-	b2Vec2 GetFirstVertex(const b2XForm& xf) const;
-
-	/// Get the centroid and apply the supplied transform.
-	b2Vec2 Centroid(const b2XForm& xf) const;
-
-	/// Get the support point in the given world direction.
-	/// Use the supplied transform.
-	b2Vec2 Support(const b2XForm& xf, const b2Vec2& d) const;
-
-private:
-
-	friend class b2Shape;
-
-	b2PolygonShape(const b2ShapeDef* def);
-
-	void UpdateSweepRadius(const b2Vec2& center);
-
-	// Local position of the polygon centroid.
-	b2Vec2 m_centroid;
-
-	b2OBB m_obb;
-
-	b2Vec2 m_vertices[b2_maxPolygonVertices];
-	b2Vec2 m_normals[b2_maxPolygonVertices];
-	b2Vec2 m_coreVertices[b2_maxPolygonVertices];
-	int32 m_vertexCount;
-};
-
-inline b2Vec2 b2PolygonShape::GetFirstVertex(const b2XForm& xf) const
-{
-	return b2Mul(xf, m_coreVertices[0]);
-}
-
-inline const b2OBB& b2PolygonShape::GetOBB() const
-{
-	return m_obb;
-}
-
-inline const b2Vec2& b2PolygonShape::GetCentroid() const
-{
-	return m_centroid;
-}
-
-inline int32 b2PolygonShape::GetVertexCount() const
-{
-	return m_vertexCount;
-}
-
-inline const b2Vec2* b2PolygonShape::GetVertices() const
-{
-	return m_vertices;
-}
-
-inline const b2Vec2* b2PolygonShape::GetCoreVertices() const
-{
-	return m_coreVertices;
-}
-
-inline const b2Vec2* b2PolygonShape::GetNormals() const
-{
-	return m_normals;
-}
-
-#endif

src/modules/physics/box2d/Source/Collision/Shapes/b2Shape.cpp

@@ -1,167 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Shape.h"
-#include "b2CircleShape.h"
-#include "b2PolygonShape.h"
-#include "../b2Collision.h"
-#include "../b2BroadPhase.h"
-#include "../../Common/b2BlockAllocator.h"
-
-#include <new>
-
-b2Shape* b2Shape::Create(const b2ShapeDef* def, b2BlockAllocator* allocator)
-{
-	switch (def->type)
-	{
-	case e_circleShape:
-		{
-			void* mem = allocator->Allocate(sizeof(b2CircleShape));
-			return new (mem) b2CircleShape(def);
-		}
-
-	case e_polygonShape:
-		{
-			void* mem = allocator->Allocate(sizeof(b2PolygonShape));
-			return new (mem) b2PolygonShape(def);
-		}
-
-	default:
-		b2Assert(false);
-		return NULL;
-	}
-}
-
-void b2Shape::Destroy(b2Shape* s, b2BlockAllocator* allocator)
-{
-	switch (s->GetType())
-	{
-	case e_circleShape:
-		s->~b2Shape();
-		allocator->Free(s, sizeof(b2CircleShape));
-		break;
-
-	case e_polygonShape:
-		s->~b2Shape();
-		allocator->Free(s, sizeof(b2PolygonShape));
-		break;
-
-	default:
-		b2Assert(false);
-	}
-}
-
-b2Shape::b2Shape(const b2ShapeDef* def)
-{
-	m_userData = def->userData;
-	m_friction = def->friction;
-	m_restitution = def->restitution;
-	m_density = def->density;
-	m_body = NULL;
-	m_sweepRadius = 0.0f;
-
-	m_next = NULL;
-
-	m_proxyId = b2_nullProxy;
-
-	m_filter = def->filter;
-
-	m_isSensor = def->isSensor;
-}
-
-b2Shape::~b2Shape()
-{
-	b2Assert(m_proxyId == b2_nullProxy);
-}
-
-void b2Shape::CreateProxy(b2BroadPhase* broadPhase, const b2XForm& transform)
-{
-	b2Assert(m_proxyId == b2_nullProxy);
-
-	b2AABB aabb;
-	ComputeAABB(&aabb, transform);
-
-	bool inRange = broadPhase->InRange(aabb);
-
-	// You are creating a shape outside the world box.
-	b2Assert(inRange);
-
-	if (inRange)
-	{
-		m_proxyId = broadPhase->CreateProxy(aabb, this);
-	}
-	else
-	{
-		m_proxyId = b2_nullProxy;
-	}
-}
-
-void b2Shape::DestroyProxy(b2BroadPhase* broadPhase)
-{
-	if (m_proxyId != b2_nullProxy)
-	{
-		broadPhase->DestroyProxy(m_proxyId);
-		m_proxyId = b2_nullProxy;
-	}
-}
-
-bool b2Shape::Synchronize(b2BroadPhase* broadPhase, const b2XForm& transform1, const b2XForm& transform2)
-{
-	if (m_proxyId == b2_nullProxy)
-	{	
-		return false;
-	}
-
-	// Compute an AABB that covers the swept shape (may miss some rotation effect).
-	b2AABB aabb;
-	ComputeSweptAABB(&aabb, transform1, transform2);
-
-	if (broadPhase->InRange(aabb))
-	{
-		broadPhase->MoveProxy(m_proxyId, aabb);
-		return true;
-	}
-	else
-	{
-		return false;
-	}
-}
-
-void b2Shape::RefilterProxy(b2BroadPhase* broadPhase, const b2XForm& transform)
-{
-	if (m_proxyId == b2_nullProxy)
-	{	
-		return;
-	}
-
-	broadPhase->DestroyProxy(m_proxyId);
-
-	b2AABB aabb;
-	ComputeAABB(&aabb, transform);
-
-	bool inRange = broadPhase->InRange(aabb);
-
-	if (inRange)
-	{
-		m_proxyId = broadPhase->CreateProxy(aabb, this);
-	}
-	else
-	{
-		m_proxyId = b2_nullProxy;
-	}
-}

src/modules/physics/box2d/Source/Collision/Shapes/b2Shape.h

@@ -1,286 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "../../Common/b2Math.h"
-#include "../b2Collision.h"
-
-class b2BlockAllocator;
-class b2Body;
-class b2BroadPhase;
-
-/// 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.
-	float32 I;
-};
-
-/// This holds contact filtering data.
-struct b2FilterData
-{
-	/// The collision category bits. Normally you would just set one bit.
-	uint16 categoryBits;
-
-	/// The collision mask bits. This states the categories that this
-	/// shape would accept for collision.
-	uint16 maskBits;
-
-	/// Collision groups allow a certain group of objects to never collide (negative)
-	/// or always collide (positive). Zero means no collision group. Non-zero group
-	/// filtering always wins against the mask bits.
-	int16 groupIndex;
-};
-
-/// The various collision shape types supported by Box2D.
-enum b2ShapeType
-{
-	e_unknownShape = -1,
-	e_circleShape,
-	e_polygonShape,
-	e_shapeTypeCount,
-};
-
-/// A shape definition is used to construct a shape. This class defines an
-/// abstract shape definition. You can reuse shape definitions safely.
-struct b2ShapeDef
-{
-	/// The constructor sets the default shape definition values.
-	b2ShapeDef()
-	{
-		type = e_unknownShape;
-		userData = NULL;
-		friction = 0.2f;
-		restitution = 0.0f;
-		density = 0.0f;
-		filter.categoryBits = 0x0001;
-		filter.maskBits = 0xFFFF;
-		filter.groupIndex = 0;
-		isSensor = false;
-	}
-
-	virtual ~b2ShapeDef() {}
-
-	/// Holds the shape type for down-casting.
-	b2ShapeType type;
-
-	/// Use this to store application specify shape data.
-	void* userData;
-
-	/// The shape's friction coefficient, usually in the range [0,1].
-	float32 friction;
-
-	/// The shape's restitution (elasticity) usually in the range [0,1].
-	float32 restitution;
-
-	/// The shape's density, usually in kg/m^2.
-	float32 density;
-
-	/// A sensor shape collects contact information but never generates a collision
-	/// response.
-	bool isSensor;
-
-	/// Contact filtering data.
-	b2FilterData filter;
-};
-
-/// A shape is used for collision detection. Shapes are created in b2World.
-/// You can use shape for collision detection before they are attached to the world.
-/// @warning you cannot reuse shapes.
-class b2Shape
-{
-public:
-	/// Get the type of this shape. You can use this to down cast to the concrete shape.
-	/// @return the shape type.
-	b2ShapeType GetType() const;
-
-	/// Is this shape a sensor (non-solid)?
-	/// @return the true if the shape is a sensor.
-	bool IsSensor() const;
-
-	/// Set the contact filtering data. You must call b2World::Refilter to correct
-	/// existing contacts/non-contacts.
-	void SetFilterData(const b2FilterData& filter);
-
-	/// Get the contact filtering data.
-	const b2FilterData& GetFilterData() const;
-
-	/// Get the parent body of this shape. This is NULL if the shape is not attached.
-	/// @return the parent body.
-	b2Body* GetBody();
-
-	/// Get the next shape in the parent body's shape list.
-	/// @return the next shape.
-	b2Shape* GetNext();
-
-	/// Get the user data that was assigned in the shape definition. Use this to
-	/// store your application specific data.
-	void* GetUserData();
-
-	/// Set the user data. Use this to store your application specific data.
-	void SetUserData(void* data);
-
-	/// 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 b2XForm& xf, const b2Vec2& p) const = 0;
-
-	/// Perform a ray cast against this shape.
-	/// @param xf the shape world transform.
-	/// @param lambda returns the hit fraction. You can use this to compute the contact point
-	/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.
-	/// @param normal returns the normal at the contact point. If there is no intersection, the normal
-	/// is not set.
-	/// @param segment defines the begin and end point of the ray cast.
-	/// @param maxLambda a number typically in the range [0,1].
-	/// @return true if there was an intersection.
-	virtual bool TestSegment(	const b2XForm& xf,
-								float32* lambda,
-								b2Vec2* normal,
-								const b2Segment& segment,
-								float32 maxLambda) const = 0;
-
-	/// Given a transform, compute the associated axis aligned bounding box for this shape.
-	/// @param aabb returns the axis aligned box.
-	/// @param xf the world transform of the shape.
-	virtual void ComputeAABB(b2AABB* aabb, const b2XForm& xf) const = 0;
-
-	/// Given two transforms, compute the associated swept axis aligned bounding box for this shape.
-	/// @param aabb returns the axis aligned box.
-	/// @param xf1 the starting shape world transform.
-	/// @param xf2 the ending shape world transform.
-	virtual void ComputeSweptAABB(	b2AABB* aabb,
-									const b2XForm& xf1,
-									const b2XForm& xf2) const = 0;
-
-	/// Compute the mass properties of this shape using its dimensions and density.
-	/// The inertia tensor is computed about the local origin, not the centroid.
-	/// @param massData returns the mass data for this shape.
-	virtual void ComputeMass(b2MassData* massData) const = 0;
-
-	/// Get the maximum radius about the parent body's center of mass.
-	float32 GetSweepRadius() const;
-
-	/// Get the coefficient of friction.
-	float32 GetFriction() const;
-
-	/// Get the coefficient of restitution.
-	float32 GetRestitution() const;
-
-	float32 m_density;
-	float32 m_friction;
-	float32 m_restitution;
-	bool m_isSensor;
-
-protected:
-
-	friend class b2Body;
-	friend class b2World;
-
-	static b2Shape* Create(const b2ShapeDef* def, b2BlockAllocator* allocator);
-	static void Destroy(b2Shape* shape, b2BlockAllocator* allocator);
-
-	b2Shape(const b2ShapeDef* def);
-	virtual ~b2Shape();
-
-	void CreateProxy(b2BroadPhase* broadPhase, const b2XForm& xf);
-	void DestroyProxy(b2BroadPhase* broadPhase);
-	bool Synchronize(b2BroadPhase* broadPhase, const b2XForm& xf1, const b2XForm& xf2);
-	void RefilterProxy(b2BroadPhase* broadPhase, const b2XForm& xf);
-
-	virtual void UpdateSweepRadius(const b2Vec2& center) = 0;
-
-	b2ShapeType m_type;
-	b2Shape* m_next;
-	b2Body* m_body;
-
-	// Sweep radius relative to the parent body's center of mass.
-	float32 m_sweepRadius;
-
-	uint16 m_proxyId;
-	b2FilterData m_filter;
-
-	
-
-	void* m_userData;
-};
-
-inline b2ShapeType b2Shape::GetType() const
-{
-	return m_type;
-}
-
-inline bool b2Shape::IsSensor() const
-{
-	return m_isSensor;
-}
-
-inline void b2Shape::SetFilterData(const b2FilterData& filter)
-{
-	m_filter = filter;
-}
-
-inline const b2FilterData& b2Shape::GetFilterData() const
-{
-	return m_filter;
-}
-
-inline void* b2Shape::GetUserData()
-{
-	return m_userData;
-}
-
-inline void b2Shape::SetUserData(void* data)
-{
-	m_userData = data;
-}
-
-inline b2Body* b2Shape::GetBody()
-{
-	return m_body;
-}
-
-inline b2Shape* b2Shape::GetNext()
-{
-	return m_next;
-}
-
-inline float32 b2Shape::GetSweepRadius() const
-{
-	return m_sweepRadius;
-}
-
-inline float32 b2Shape::GetFriction() const
-{
-	return m_friction;
-}
-
-inline float32 b2Shape::GetRestitution() const
-{
-	return m_restitution;
-}
-
-#endif

src/modules/physics/box2d/Source/Collision/b2BroadPhase.cpp

@@ -1,668 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2BroadPhase.h"
-#include <algorithm>
-#include <string.h>
-
-// Notes:
-// - we use bound arrays instead of linked lists for cache coherence.
-// - we use quantized integral values for fast compares.
-// - we use short indices rather than pointers to save memory.
-// - we use a stabbing count for fast overlap queries (less than order N).
-// - we also use a time stamp on each proxy to speed up the registration of
-//   overlap query results.
-// - where possible, we compare bound indices instead of values to reduce
-//   cache misses (TODO_ERIN).
-// - no broadphase is perfect and neither is this one: it is not great for huge
-//   worlds (use a multi-SAP instead), it is not great for large objects.
-
-bool b2BroadPhase::s_validate = false;
-
-struct b2BoundValues
-{
-	uint16 lowerValues[2];
-	uint16 upperValues[2];
-};
-
-static int32 BinarySearch(b2Bound* bounds, int32 count, uint16 value)
-{
-	int32 low = 0;
-	int32 high = count - 1;
-	while (low <= high)
-	{
-		int32 mid = (low + high) >> 1;
-		if (bounds[mid].value > value)
-		{
-			high = mid - 1;
-		}
-		else if (bounds[mid].value < value)
-		{
-			low = mid + 1;
-		}
-		else
-		{
-			return (uint16)mid;
-		}
-	}
-	
-	return low;
-}
-
-b2BroadPhase::b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback)
-{
-	m_pairManager.Initialize(this, callback);
-
-	b2Assert(worldAABB.IsValid());
-	m_worldAABB = worldAABB;
-	m_proxyCount = 0;
-
-	b2Vec2 d = worldAABB.upperBound - worldAABB.lowerBound;
-	m_quantizationFactor.x = float32(B2BROADPHASE_MAX) / d.x;
-	m_quantizationFactor.y = float32(B2BROADPHASE_MAX) / d.y;
-
-	for (uint16 i = 0; i < b2_maxProxies - 1; ++i)
-	{
-		m_proxyPool[i].SetNext(i + 1);
-		m_proxyPool[i].timeStamp = 0;
-		m_proxyPool[i].overlapCount = b2_invalid;
-		m_proxyPool[i].userData = NULL;
-	}
-	m_proxyPool[b2_maxProxies-1].SetNext(b2_nullProxy);
-	m_proxyPool[b2_maxProxies-1].timeStamp = 0;
-	m_proxyPool[b2_maxProxies-1].overlapCount = b2_invalid;
-	m_proxyPool[b2_maxProxies-1].userData = NULL;
-	m_freeProxy = 0;
-
-	m_timeStamp = 1;
-	m_queryResultCount = 0;
-}
-
-b2BroadPhase::~b2BroadPhase()
-{
-}
-
-// This one is only used for validation.
-bool b2BroadPhase::TestOverlap(b2Proxy* p1, b2Proxy* p2)
-{
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-
-		b2Assert(p1->lowerBounds[axis] < 2 * m_proxyCount);
-		b2Assert(p1->upperBounds[axis] < 2 * m_proxyCount);
-		b2Assert(p2->lowerBounds[axis] < 2 * m_proxyCount);
-		b2Assert(p2->upperBounds[axis] < 2 * m_proxyCount);
-
-		if (bounds[p1->lowerBounds[axis]].value > bounds[p2->upperBounds[axis]].value)
-			return false;
-
-		if (bounds[p1->upperBounds[axis]].value < bounds[p2->lowerBounds[axis]].value)
-			return false;
-	}
-
-	return true;
-}
-
-bool b2BroadPhase::TestOverlap(const b2BoundValues& b, b2Proxy* p)
-{
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-
-		b2Assert(p->lowerBounds[axis] < 2 * m_proxyCount);
-		b2Assert(p->upperBounds[axis] < 2 * m_proxyCount);
-
-		if (b.lowerValues[axis] > bounds[p->upperBounds[axis]].value)
-			return false;
-
-		if (b.upperValues[axis] < bounds[p->lowerBounds[axis]].value)
-			return false;
-	}
-
-	return true;
-}
-
-void b2BroadPhase::ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb)
-{
-	b2Assert(aabb.upperBound.x > aabb.lowerBound.x);
-	b2Assert(aabb.upperBound.y > aabb.lowerBound.y);
-
-	b2Vec2 minVertex = b2Clamp(aabb.lowerBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
-	b2Vec2 maxVertex = b2Clamp(aabb.upperBound, m_worldAABB.lowerBound, m_worldAABB.upperBound);
-
-	// Bump lower bounds downs and upper bounds up. This ensures correct sorting of
-	// lower/upper bounds that would have equal values.
-	// TODO_ERIN implement fast float to uint16 conversion.
-	lowerValues[0] = (uint16)(m_quantizationFactor.x * (minVertex.x - m_worldAABB.lowerBound.x)) & (B2BROADPHASE_MAX - 1);
-	upperValues[0] = (uint16)(m_quantizationFactor.x * (maxVertex.x - m_worldAABB.lowerBound.x)) | 1;
-
-	lowerValues[1] = (uint16)(m_quantizationFactor.y * (minVertex.y - m_worldAABB.lowerBound.y)) & (B2BROADPHASE_MAX - 1);
-	upperValues[1] = (uint16)(m_quantizationFactor.y * (maxVertex.y - m_worldAABB.lowerBound.y)) | 1;
-}
-
-void b2BroadPhase::IncrementTimeStamp()
-{
-	if (m_timeStamp == B2BROADPHASE_MAX)
-	{
-		for (uint16 i = 0; i < b2_maxProxies; ++i)
-		{
-			m_proxyPool[i].timeStamp = 0;
-		}
-		m_timeStamp = 1;
-	}
-	else
-	{
-		++m_timeStamp;
-	}
-}
-
-void b2BroadPhase::IncrementOverlapCount(int32 proxyId)
-{
-	b2Proxy* proxy = m_proxyPool + proxyId;
-	if (proxy->timeStamp < m_timeStamp)
-	{
-		proxy->timeStamp = m_timeStamp;
-		proxy->overlapCount = 1;
-	}
-	else
-	{
-		proxy->overlapCount = 2;
-		b2Assert(m_queryResultCount < b2_maxProxies);
-		m_queryResults[m_queryResultCount] = (uint16)proxyId;
-		++m_queryResultCount;
-	}
-}
-
-void b2BroadPhase::Query(int32* lowerQueryOut, int32* upperQueryOut,
-					   uint16 lowerValue, uint16 upperValue,
-					   b2Bound* bounds, int32 boundCount, int32 axis)
-{
-	int32 lowerQuery = BinarySearch(bounds, boundCount, lowerValue);
-	int32 upperQuery = BinarySearch(bounds, boundCount, upperValue);
-
-	// Easy case: lowerQuery <= lowerIndex(i) < upperQuery
-	// Solution: search query range for min bounds.
-	for (int32 i = lowerQuery; i < upperQuery; ++i)
-	{
-		if (bounds[i].IsLower())
-		{
-			IncrementOverlapCount(bounds[i].proxyId);
-		}
-	}
-
-	// Hard case: lowerIndex(i) < lowerQuery < upperIndex(i)
-	// Solution: use the stabbing count to search down the bound array.
-	if (lowerQuery > 0)
-	{
-		int32 i = lowerQuery - 1;
-		int32 s = bounds[i].stabbingCount;
-
-		// Find the s overlaps.
-		while (s)
-		{
-			b2Assert(i >= 0);
-
-			if (bounds[i].IsLower())
-			{
-				b2Proxy* proxy = m_proxyPool + bounds[i].proxyId;
-				if (lowerQuery <= proxy->upperBounds[axis])
-				{
-					IncrementOverlapCount(bounds[i].proxyId);
-					--s;
-				}
-			}
-			--i;
-		}
-	}
-
-	*lowerQueryOut = lowerQuery;
-	*upperQueryOut = upperQuery;
-}
-
-uint16 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
-{
-	b2Assert(m_proxyCount < b2_maxProxies);
-	b2Assert(m_freeProxy != b2_nullProxy);
-
-	uint16 proxyId = m_freeProxy;
-	b2Proxy* proxy = m_proxyPool + proxyId;
-	m_freeProxy = proxy->GetNext();
-
-	proxy->overlapCount = 0;
-	proxy->userData = userData;
-
-	int32 boundCount = 2 * m_proxyCount;
-
-	uint16 lowerValues[2], upperValues[2];
-	ComputeBounds(lowerValues, upperValues, aabb);
-
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-		int32 lowerIndex, upperIndex;
-		Query(&lowerIndex, &upperIndex, lowerValues[axis], upperValues[axis], bounds, boundCount, axis);
-
-		memmove(bounds + upperIndex + 2, bounds + upperIndex, (boundCount - upperIndex) * sizeof(b2Bound));
-		memmove(bounds + lowerIndex + 1, bounds + lowerIndex, (upperIndex - lowerIndex) * sizeof(b2Bound));
-
-		// The upper index has increased because of the lower bound insertion.
-		++upperIndex;
-
-		// Copy in the new bounds.
-		bounds[lowerIndex].value = lowerValues[axis];
-		bounds[lowerIndex].proxyId = proxyId;
-		bounds[upperIndex].value = upperValues[axis];
-		bounds[upperIndex].proxyId = proxyId;
-
-		bounds[lowerIndex].stabbingCount = lowerIndex == 0 ? 0 : bounds[lowerIndex-1].stabbingCount;
-		bounds[upperIndex].stabbingCount = bounds[upperIndex-1].stabbingCount;
-
-		// Adjust the stabbing count between the new bounds.
-		for (int32 index = lowerIndex; index < upperIndex; ++index)
-		{
-			++bounds[index].stabbingCount;
-		}
-
-		// Adjust the all the affected bound indices.
-		for (int32 index = lowerIndex; index < boundCount + 2; ++index)
-		{
-			b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
-			if (bounds[index].IsLower())
-			{
-				proxy->lowerBounds[axis] = (uint16)index;
-			}
-			else
-			{
-				proxy->upperBounds[axis] = (uint16)index;
-			}
-		}
-	}
-
-	++m_proxyCount;
-
-	b2Assert(m_queryResultCount < b2_maxProxies);
-
-	// Create pairs if the AABB is in range.
-	for (int32 i = 0; i < m_queryResultCount; ++i)
-	{
-		b2Assert(m_queryResults[i] < b2_maxProxies);
-		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
-
-		m_pairManager.AddBufferedPair(proxyId, m_queryResults[i]);
-	}
-
-	m_pairManager.Commit();
-
-	if (s_validate)
-	{
-		Validate();
-	}
-
-	// Prepare for next query.
-	m_queryResultCount = 0;
-	IncrementTimeStamp();
-
-	return proxyId;
-}
-
-void b2BroadPhase::DestroyProxy(int32 proxyId)
-{
-	b2Assert(0 < m_proxyCount && m_proxyCount <= b2_maxProxies);
-	b2Proxy* proxy = m_proxyPool + proxyId;
-	b2Assert(proxy->IsValid());
-
-	int32 boundCount = 2 * m_proxyCount;
-
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-
-		int32 lowerIndex = proxy->lowerBounds[axis];
-		int32 upperIndex = proxy->upperBounds[axis];
-		uint16 lowerValue = bounds[lowerIndex].value;
-		uint16 upperValue = bounds[upperIndex].value;
-
-		memmove(bounds + lowerIndex, bounds + lowerIndex + 1, (upperIndex - lowerIndex - 1) * sizeof(b2Bound));
-		memmove(bounds + upperIndex-1, bounds + upperIndex + 1, (boundCount - upperIndex - 1) * sizeof(b2Bound));
-
-		// Fix bound indices.
-		for (int32 index = lowerIndex; index < boundCount - 2; ++index)
-		{
-			b2Proxy* proxy = m_proxyPool + bounds[index].proxyId;
-			if (bounds[index].IsLower())
-			{
-				proxy->lowerBounds[axis] = (uint16)index;
-			}
-			else
-			{
-				proxy->upperBounds[axis] = (uint16)index;
-			}
-		}
-
-		// Fix stabbing count.
-		for (int32 index = lowerIndex; index < upperIndex - 1; ++index)
-		{
-			--bounds[index].stabbingCount;
-		}
-
-		// Query for pairs to be removed. lowerIndex and upperIndex are not needed.
-		Query(&lowerIndex, &upperIndex, lowerValue, upperValue, bounds, boundCount - 2, axis);
-	}
-
-	b2Assert(m_queryResultCount < b2_maxProxies);
-
-	for (int32 i = 0; i < m_queryResultCount; ++i)
-	{
-		b2Assert(m_proxyPool[m_queryResults[i]].IsValid());
-		m_pairManager.RemoveBufferedPair(proxyId, m_queryResults[i]);
-	}
-
-	m_pairManager.Commit();
-
-	// Prepare for next query.
-	m_queryResultCount = 0;
-	IncrementTimeStamp();
-
-	// Return the proxy to the pool.
-	proxy->userData = NULL;
-	proxy->overlapCount = b2_invalid;
-	proxy->lowerBounds[0] = b2_invalid;
-	proxy->lowerBounds[1] = b2_invalid;
-	proxy->upperBounds[0] = b2_invalid;
-	proxy->upperBounds[1] = b2_invalid;
-
-	proxy->SetNext(m_freeProxy);
-	m_freeProxy = (uint16)proxyId;
-	--m_proxyCount;
-
-	if (s_validate)
-	{
-		Validate();
-	}
-}
-
-void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb)
-{
-	if (proxyId == b2_nullProxy || b2_maxProxies <= proxyId)
-	{
-		b2Assert(false);
-		return;
-	}
-
-	if (aabb.IsValid() == false)
-	{
-		b2Assert(false);
-		return;
-	}
-
-	int32 boundCount = 2 * m_proxyCount;
-
-	b2Proxy* proxy = m_proxyPool + proxyId;
-
-	// Get new bound values
-	b2BoundValues newValues;
-	ComputeBounds(newValues.lowerValues, newValues.upperValues, aabb);
-
-	// Get old bound values
-	b2BoundValues oldValues;
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		oldValues.lowerValues[axis] = m_bounds[axis][proxy->lowerBounds[axis]].value;
-		oldValues.upperValues[axis] = m_bounds[axis][proxy->upperBounds[axis]].value;
-	}
-
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-
-		int32 lowerIndex = proxy->lowerBounds[axis];
-		int32 upperIndex = proxy->upperBounds[axis];
-
-		uint16 lowerValue = newValues.lowerValues[axis];
-		uint16 upperValue = newValues.upperValues[axis];
-
-		int32 deltaLower = lowerValue - bounds[lowerIndex].value;
-		int32 deltaUpper = upperValue - bounds[upperIndex].value;
-
-		bounds[lowerIndex].value = lowerValue;
-		bounds[upperIndex].value = upperValue;
-
-		//
-		// Expanding adds overlaps
-		//
-
-		// Should we move the lower bound down?
-		if (deltaLower < 0)
-		{
-			int32 index = lowerIndex;
-			while (index > 0 && lowerValue < bounds[index-1].value)
-			{
-				b2Bound* bound = bounds + index;
-				b2Bound* prevBound = bound - 1;
-
-				int32 prevProxyId = prevBound->proxyId;
-				b2Proxy* prevProxy = m_proxyPool + prevBound->proxyId;
-
-				++prevBound->stabbingCount;
-
-				if (prevBound->IsUpper() == true)
-				{
-					if (TestOverlap(newValues, prevProxy))
-					{
-						m_pairManager.AddBufferedPair(proxyId, prevProxyId);
-					}
-
-					++prevProxy->upperBounds[axis];
-					++bound->stabbingCount;
-				}
-				else
-				{
-					++prevProxy->lowerBounds[axis];
-					--bound->stabbingCount;
-				}
-
-				--proxy->lowerBounds[axis];
-				b2Swap(*bound, *prevBound);
-				--index;
-			}
-		}
-
-		// Should we move the upper bound up?
-		if (deltaUpper > 0)
-		{
-			int32 index = upperIndex;
-			while (index < boundCount-1 && bounds[index+1].value <= upperValue)
-			{
-				b2Bound* bound = bounds + index;
-				b2Bound* nextBound = bound + 1;
-				int32 nextProxyId = nextBound->proxyId;
-				b2Proxy* nextProxy = m_proxyPool + nextProxyId;
-
-				++nextBound->stabbingCount;
-
-				if (nextBound->IsLower() == true)
-				{
-					if (TestOverlap(newValues, nextProxy))
-					{
-						m_pairManager.AddBufferedPair(proxyId, nextProxyId);
-					}
-
-					--nextProxy->lowerBounds[axis];
-					++bound->stabbingCount;
-				}
-				else
-				{
-					--nextProxy->upperBounds[axis];
-					--bound->stabbingCount;
-				}
-
-				++proxy->upperBounds[axis];
-				b2Swap(*bound, *nextBound);
-				++index;
-			}
-		}
-
-		//
-		// Shrinking removes overlaps
-		//
-
-		// Should we move the lower bound up?
-		if (deltaLower > 0)
-		{
-			int32 index = lowerIndex;
-			while (index < boundCount-1 && bounds[index+1].value <= lowerValue)
-			{
-				b2Bound* bound = bounds + index;
-				b2Bound* nextBound = bound + 1;
-
-				int32 nextProxyId = nextBound->proxyId;
-				b2Proxy* nextProxy = m_proxyPool + nextProxyId;
-
-				--nextBound->stabbingCount;
-
-				if (nextBound->IsUpper())
-				{
-					if (TestOverlap(oldValues, nextProxy))
-					{
-						m_pairManager.RemoveBufferedPair(proxyId, nextProxyId);
-					}
-
-					--nextProxy->upperBounds[axis];
-					--bound->stabbingCount;
-				}
-				else
-				{
-					--nextProxy->lowerBounds[axis];
-					++bound->stabbingCount;
-				}
-
-				++proxy->lowerBounds[axis];
-				b2Swap(*bound, *nextBound);
-				++index;
-			}
-		}
-
-		// Should we move the upper bound down?
-		if (deltaUpper < 0)
-		{
-			int32 index = upperIndex;
-			while (index > 0 && upperValue < bounds[index-1].value)
-			{
-				b2Bound* bound = bounds + index;
-				b2Bound* prevBound = bound - 1;
-
-				int32 prevProxyId = prevBound->proxyId;
-				b2Proxy* prevProxy = m_proxyPool + prevProxyId;
-
-				--prevBound->stabbingCount;
-
-				if (prevBound->IsLower() == true)
-				{
-					if (TestOverlap(oldValues, prevProxy))
-					{
-						m_pairManager.RemoveBufferedPair(proxyId, prevProxyId);
-					}
-
-					++prevProxy->lowerBounds[axis];
-					--bound->stabbingCount;
-				}
-				else
-				{
-					++prevProxy->upperBounds[axis];
-					++bound->stabbingCount;
-				}
-
-				--proxy->upperBounds[axis];
-				b2Swap(*bound, *prevBound);
-				--index;
-			}
-		}
-	}
-
-	if (s_validate)
-	{
-		Validate();
-	}
-}
-
-void b2BroadPhase::Commit()
-{
-	m_pairManager.Commit();
-}
-
-int32 b2BroadPhase::Query(const b2AABB& aabb, void** userData, int32 maxCount)
-{
-	uint16 lowerValues[2];
-	uint16 upperValues[2];
-	ComputeBounds(lowerValues, upperValues, aabb);
-
-	int32 lowerIndex, upperIndex;
-
-	Query(&lowerIndex, &upperIndex, lowerValues[0], upperValues[0], m_bounds[0], 2*m_proxyCount, 0);
-	Query(&lowerIndex, &upperIndex, lowerValues[1], upperValues[1], m_bounds[1], 2*m_proxyCount, 1);
-
-	b2Assert(m_queryResultCount < b2_maxProxies);
-
-	int32 count = 0;
-	for (int32 i = 0; i < m_queryResultCount && count < maxCount; ++i, ++count)
-	{
-		b2Assert(m_queryResults[i] < b2_maxProxies);
-		b2Proxy* proxy = m_proxyPool + m_queryResults[i];
-		b2Assert(proxy->IsValid());
-		userData[i] = proxy->userData;
-	}
-
-	// Prepare for next query.
-	m_queryResultCount = 0;
-	IncrementTimeStamp();
-
-	return count;
-}
-
-void b2BroadPhase::Validate()
-{
-	for (int32 axis = 0; axis < 2; ++axis)
-	{
-		b2Bound* bounds = m_bounds[axis];
-
-		int32 boundCount = 2 * m_proxyCount;
-		uint16 stabbingCount = 0;
-
-		for (int32 i = 0; i < boundCount; ++i)
-		{
-			b2Bound* bound = bounds + i;
-			b2Assert(i == 0 || bounds[i-1].value <= bound->value);
-			b2Assert(bound->proxyId != b2_nullProxy);
-			b2Assert(m_proxyPool[bound->proxyId].IsValid());
-
-			if (bound->IsLower() == true)
-			{
-				b2Assert(m_proxyPool[bound->proxyId].lowerBounds[axis] == i);
-				++stabbingCount;
-			}
-			else
-			{
-				b2Assert(m_proxyPool[bound->proxyId].upperBounds[axis] == i);
-				--stabbingCount;
-			}
-
-			b2Assert(bound->stabbingCount == stabbingCount);
-		}
-	}
-}

src/modules/physics/box2d/Source/Collision/b2BroadPhase.h

@@ -1,146 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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
-
-/*
-This broad phase uses the Sweep and Prune algorithm as described in:
-Collision Detection in Interactive 3D Environments by Gino van den Bergen
-Also, some ideas, such as using integral values for fast compares comes from
-Bullet (http:/www.bulletphysics.com).
-*/
-
-#include "../Common/b2Settings.h"
-#include "b2Collision.h"
-#include "b2PairManager.h"
-#include <climits>
-
-#ifdef TARGET_FLOAT32_IS_FIXED
-#define	B2BROADPHASE_MAX	(USHRT_MAX/2)
-#else
-#define	B2BROADPHASE_MAX	USHRT_MAX
-
-#endif
-
-const uint16 b2_invalid = B2BROADPHASE_MAX;
-const uint16 b2_nullEdge = B2BROADPHASE_MAX;
-struct b2BoundValues;
-
-struct b2Bound
-{
-	bool IsLower() const { return (value & 1) == 0; }
-	bool IsUpper() const { return (value & 1) == 1; }
-
-	uint16 value;
-	uint16 proxyId;
-	uint16 stabbingCount;
-};
-
-struct b2Proxy
-{
-	uint16 GetNext() const { return lowerBounds[0]; }
-	void SetNext(uint16 next) { lowerBounds[0] = next; }
-	bool IsValid() const { return overlapCount != b2_invalid; }
-
-	uint16 lowerBounds[2], upperBounds[2];
-	uint16 overlapCount;
-	uint16 timeStamp;
-	void* userData;
-};
-
-class b2BroadPhase
-{
-public:
-	b2BroadPhase(const b2AABB& worldAABB, b2PairCallback* callback);
-	~b2BroadPhase();
-
-	// Use this to see if your proxy is in range. If it is not in range,
-	// it should be destroyed. Otherwise you may get O(m^2) pairs, where m
-	// is the number of proxies that are out of range.
-	bool InRange(const b2AABB& aabb) const;
-
-	// Create and destroy proxies. These call Flush first.
-	uint16 CreateProxy(const b2AABB& aabb, void* userData);
-	void DestroyProxy(int32 proxyId);
-
-	// Call MoveProxy as many times as you like, then when you are done
-	// call Commit to finalized the proxy pairs (for your time step).
-	void MoveProxy(int32 proxyId, const b2AABB& aabb);
-	void Commit();
-
-	// Get a single proxy. Returns NULL if the id is invalid.
-	b2Proxy* GetProxy(int32 proxyId);
-
-	// Query an AABB for overlapping proxies, returns the user data and
-	// the count, up to the supplied maximum count.
-	int32 Query(const b2AABB& aabb, void** userData, int32 maxCount);
-
-	void Validate();
-	void ValidatePairs();
-
-private:
-	void ComputeBounds(uint16* lowerValues, uint16* upperValues, const b2AABB& aabb);
-
-	bool TestOverlap(b2Proxy* p1, b2Proxy* p2);
-	bool TestOverlap(const b2BoundValues& b, b2Proxy* p);
-
-	void Query(int32* lowerIndex, int32* upperIndex, uint16 lowerValue, uint16 upperValue,
-				b2Bound* bounds, int32 boundCount, int32 axis);
-	void IncrementOverlapCount(int32 proxyId);
-	void IncrementTimeStamp();
-
-public:
-	friend class b2PairManager;
-
-	b2PairManager m_pairManager;
-
-	b2Proxy m_proxyPool[b2_maxProxies];
-	uint16 m_freeProxy;
-
-	b2Bound m_bounds[2][2*b2_maxProxies];
-
-	uint16 m_queryResults[b2_maxProxies];
-	int32 m_queryResultCount;
-
-	b2AABB m_worldAABB;
-	b2Vec2 m_quantizationFactor;
-	int32 m_proxyCount;
-	uint16 m_timeStamp;
-
-	static bool s_validate;
-};
-
-
-inline bool b2BroadPhase::InRange(const b2AABB& aabb) const
-{
-	b2Vec2 d = b2Max(aabb.lowerBound - m_worldAABB.upperBound, m_worldAABB.lowerBound - aabb.upperBound);
-	return b2Max(d.x, d.y) < 0.0f;
-}
-
-inline b2Proxy* b2BroadPhase::GetProxy(int32 proxyId)
-{
-	if (proxyId == b2_nullProxy || m_proxyPool[proxyId].IsValid() == false)
-	{
-		return NULL;
-	}
-
-	return m_proxyPool + proxyId;
-}
-
-#endif

src/modules/physics/box2d/Source/Collision/b2CollideCircle.cpp

@@ -1,168 +0,0 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Collision.h"
-#include "Shapes/b2CircleShape.h"
-#include "Shapes/b2PolygonShape.h"
-
-void b2CollideCircles(
-	b2Manifold* manifold,
-	const b2CircleShape* circle1, const b2XForm& xf1,
-	const b2CircleShape* circle2, const b2XForm& xf2)
-{
-	manifold->pointCount = 0;
-
-	b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
-	b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
-
-	b2Vec2 d = p2 - p1;
-	float32 distSqr = b2Dot(d, d);
-	float32 r1 = circle1->GetRadius();
-	float32 r2 = circle2->GetRadius();
-	float32 radiusSum = r1 + r2;
-	if (distSqr > radiusSum * radiusSum)
-	{
-		return;
-	}
-
-	float32 separation;
-	if (distSqr < B2_FLT_EPSILON)
-	{
-		separation = -radiusSum;
-		manifold->normal.Set(0.0f, 1.0f);
-	}
-	else
-	{
-		float32 dist = b2Sqrt(distSqr);
-		separation = dist - radiusSum;
-		float32 a = 1.0f / dist;
-		manifold->normal.x = a * d.x;
-		manifold->normal.y = a * d.y;
-	}
-
-	manifold->pointCount = 1;
-	manifold->points[0].id.key = 0;
-	manifold->points[0].separation = separation;
-
-	p1 += r1 * manifold->normal;
-	p2 -= r2 * manifold->normal;
-
-	b2Vec2 p = 0.5f * (p1 + p2);
-
-	manifold->points[0].localPoint1 = b2MulT(xf1, p);
-	manifold->points[0].localPoint2 = b2MulT(xf2, p);
-}
-
-void b2CollidePolygonAndCircle(
-	b2Manifold* manifold,
-	const b2PolygonShape* polygon, const b2XForm& xf1,
-	const b2CircleShape* circle, const b2XForm& xf2)
-{
-	manifold->pointCount = 0;
-
-	// Compute circle position in the frame of the polygon.
-	b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
-	b2Vec2 cLocal = b2MulT(xf1, c);
-
-	// Find the min separating edge.
-	int32 normalIndex = 0;
-	float32 separation = -B2_FLT_MAX;
-	float32 radius = circle->GetRadius();
-	int32 vertexCount = polygon->GetVertexCount();
-	const b2Vec2* vertices = polygon->GetVertices();
-	const b2Vec2* normals = polygon->GetNormals();
-
-	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;
-		}
-	}
-
-	// If the center is inside the polygon ...
-	if (separation < B2_FLT_EPSILON)
-	{
-		manifold->pointCount = 1;
-		manifold->normal = b2Mul(xf1.R, normals[normalIndex]);
-		manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
-		manifold->points[0].id.features.incidentVertex = b2_nullFeature;
-		manifold->points[0].id.features.referenceEdge = 0;
-		manifold->points[0].id.features.flip = 0;
-		b2Vec2 position = c - radius * manifold->normal;
-		manifold->points[0].localPoint1 = b2MulT(xf1, position);
-		manifold->points[0].localPoint2 = b2MulT(xf2, position);
-		manifold->points[0].separation = separation - radius;
-		return;
-	}
-
-	// Project the circle center onto the edge segment.
-	int32 vertIndex1 = normalIndex;
-	int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
-	b2Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
-
-	float32 length = e.Normalize();
-	b2Assert(length > B2_FLT_EPSILON);
-
-	// Project the center onto the edge.
-	float32 u = b2Dot(cLocal - vertices[vertIndex1], e);
-	b2Vec2 p;
-	if (u <= 0.0f)
-	{
-		p = vertices[vertIndex1];
-		manifold->points[0].id.features.incidentEdge = b2_nullFeature;
-		manifold->points[0].id.features.incidentVertex = (uint8)vertIndex1;
-	}
-	else if (u >= length)
-	{
-		p = vertices[vertIndex2];
-		manifold->points[0].id.features.incidentEdge = b2_nullFeature;
-		manifold->points[0].id.features.incidentVertex = (uint8)vertIndex2;
-	}
-	else
-	{
-		p = vertices[vertIndex1] + u * e;
-		manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
-		manifold->points[0].id.features.incidentVertex = 0;
-	}
-
-	b2Vec2 d = cLocal - p;
-	float32 dist = d.Normalize();
-	if (dist > radius)
-	{
-		return;
-	}
-
-	manifold->pointCount = 1;
-	manifold->normal = b2Mul(xf1.R, d);
-	b2Vec2 position = c - radius * manifold->normal;
-	manifold->points[0].localPoint1 = b2MulT(xf1, position);
-	manifold->points[0].localPoint2 = b2MulT(xf2, position);
-	manifold->points[0].separation = dist - radius;
-	manifold->points[0].id.features.referenceEdge = 0;
-	manifold->points[0].id.features.flip = 0;
-}

src/modules/physics/box2d/Source/Collision/b2Collision.cpp

@@ -1,72 +0,0 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Collision.h"
-
-// Collision Detection in Interactive 3D Environments by Gino van den Bergen
-// From Section 3.4.1
-// x = mu1 * p1 + mu2 * p2
-// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
-// mu1 = 1 - mu2;
-// x = (1 - mu2) * p1 + mu2 * p2
-//   = p1 + mu2 * (p2 - p1)
-// x = s + a * r (s := start, r := end - start)
-// s + a * r = p1 + mu2 * d (d := p2 - p1)
-// -a * r + mu2 * d = b (b := s - p1)
-// [-r d] * [a; mu2] = b
-// Cramer's rule:
-// denom = det[-r d]
-// a = det[b d] / denom
-// mu2 = det[-r b] / denom
-bool b2Segment::TestSegment(float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const
-{
-	b2Vec2 s = segment.p1;
-	b2Vec2 r = segment.p2 - s;
-	b2Vec2 d = p2 - p1;
-	b2Vec2 n = b2Cross(d, 1.0f);
-
-	const float32 k_slop = 100.0f * B2_FLT_EPSILON;
-	float32 denom = -b2Dot(r, n);
-
-	// Cull back facing collision and ignore parallel segments.
-	if (denom > k_slop)
-	{
-		// Does the segment intersect the infinite line associated with this segment?
-		b2Vec2 b = s - p1;
-		float32 a = b2Dot(b, n);
-
-		if (0.0f <= a && a <= maxLambda * denom)
-		{
-			float32 mu2 = -r.x * b.y + r.y * b.x;
-
-			// Does the segment intersect this segment?
-			if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
-			{
-				a /= denom;
-				n.Normalize();
-				*lambda = a;
-				*normal = n;
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
-

src/modules/physics/box2d/Source/Collision/b2Collision.h

@@ -1,154 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "../Common/b2Math.h"
-#include <climits>
-
-/// @file
-/// Structures and functions used for computing contact points, distance
-/// queries, and TOI queries.
-
-class b2Shape;
-class b2CircleShape;
-class b2PolygonShape;
-
-const uint8 b2_nullFeature = UCHAR_MAX;
-
-/// Contact ids to facilitate warm starting.
-union b2ContactID
-{
-	/// The features that intersect to form the contact point
-	struct Features
-	{
-		uint8 referenceEdge;	///< The edge that defines the outward contact normal.
-		uint8 incidentEdge;		///< The edge most anti-parallel to the reference edge.
-		uint8 incidentVertex;	///< The vertex (0 or 1) on the incident edge that was clipped.
-		uint8 flip;				///< A value of 1 indicates that the reference edge is on shape2.
-	} features;
-	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 point is stored in local coordinates because CCD
-/// requires sub-stepping in which the separation is stale.
-struct b2ManifoldPoint
-{
-	b2Vec2 localPoint1;		///< local position of the contact point in body1
-	b2Vec2 localPoint2;		///< local position of the contact point in body2
-	float32 separation;		///< the separation of the shapes along the normal vector
-	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.
-struct b2Manifold
-{
-	b2ManifoldPoint points[b2_maxManifoldPoints];	///< the points of contact
-	b2Vec2 normal;	///< the shared unit normal vector
-	int32 pointCount;	///< the number of manifold points
-};
-
-/// A line segment.
-struct b2Segment
-{
-	/// Ray cast against this segment with another segment.
-	bool TestSegment(float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const;
-
-	b2Vec2 p1;	///< the starting point
-	b2Vec2 p2;	///< the ending point
-};
-
-/// An axis aligned bounding box.
-struct b2AABB
-{
-	/// Verify that the bounds are sorted.
-	bool IsValid() const;
-
-	b2Vec2 lowerBound;	///< the lower vertex
-	b2Vec2 upperBound;	///< the upper vertex
-};
-
-/// An oriented bounding box.
-struct b2OBB
-{
-	b2Mat22 R;			///< the rotation matrix
-	b2Vec2 center;		///< the local centroid
-	b2Vec2 extents;		///< the half-widths
-};
-
-/// Compute the collision manifold between two circles.
-void b2CollideCircles(b2Manifold* manifold,
-					  const b2CircleShape* circle1, const b2XForm& xf1,
-					  const b2CircleShape* circle2, const b2XForm& xf2);
-
-/// Compute the collision manifold between a polygon and a circle.
-void b2CollidePolygonAndCircle(b2Manifold* manifold,
-							   const b2PolygonShape* polygon, const b2XForm& xf1,
-							   const b2CircleShape* circle, const b2XForm& xf2);
-
-/// Compute the collision manifold between two circles.
-void b2CollidePolygons(b2Manifold* manifold,
-					   const b2PolygonShape* polygon1, const b2XForm& xf1,
-					   const b2PolygonShape* polygon2, const b2XForm& xf2);
-
-/// Compute the distance between two shapes and the closest points.
-/// @return the distance between the shapes or zero if they are overlapped/touching.
-float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
-				   const b2Shape* shape1, const b2XForm& xf1,
-				   const b2Shape* shape2, const b2XForm& xf2);
-
-/// Compute the time when two shapes begin to touch or touch at a closer distance.
-/// @warning the sweeps must have the same time interval.
-/// @return the fraction between [0,1] in which the shapes first touch.
-/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
-float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
-					   const b2Shape* shape2, const b2Sweep& sweep2);
-
-
-// ---------------- 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

src/modules/physics/box2d/Source/Collision/b2Distance.cpp

@@ -1,364 +0,0 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Collision.h"
-#include "Shapes/b2CircleShape.h"
-#include "Shapes/b2PolygonShape.h"
-
-int32 g_GJK_Iterations = 0;
-
-// GJK using Voronoi regions (Christer Ericson) and region selection
-// optimizations (Casey Muratori).
-
-// The origin is either in the region of points[1] or in the edge region. The origin is
-// not in region of points[0] because that is the old point.
-static int32 ProcessTwo(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
-{
-	// If in point[1] region
-	b2Vec2 r = -points[1];
-	b2Vec2 d = points[0] - points[1];
-	float32 length = d.Normalize();
-	float32 lambda = b2Dot(r, d);
-	if (lambda <= 0.0f || length < B2_FLT_EPSILON)
-	{
-		// The simplex is reduced to a point.
-		*x1 = p1s[1];
-		*x2 = p2s[1];
-		p1s[0] = p1s[1];
-		p2s[0] = p2s[1];
-		points[0] = points[1];
-		return 1;
-	}
-
-	// Else in edge region
-	lambda /= length;
-	*x1 = p1s[1] + lambda * (p1s[0] - p1s[1]);
-	*x2 = p2s[1] + lambda * (p2s[0] - p2s[1]);
-	return 2;
-}
-
-// Possible regions:
-// - points[2]
-// - edge points[0]-points[2]
-// - edge points[1]-points[2]
-// - inside the triangle
-static int32 ProcessThree(b2Vec2* x1, b2Vec2* x2, b2Vec2* p1s, b2Vec2* p2s, b2Vec2* points)
-{
-	b2Vec2 a = points[0];
-	b2Vec2 b = points[1];
-	b2Vec2 c = points[2];
-
-	b2Vec2 ab = b - a;
-	b2Vec2 ac = c - a;
-	b2Vec2 bc = c - b;
-
-	float32 sn = -b2Dot(a, ab), sd = b2Dot(b, ab);
-	float32 tn = -b2Dot(a, ac), td = b2Dot(c, ac);
-	float32 un = -b2Dot(b, bc), ud = b2Dot(c, bc);
-
-	// In vertex c region?
-	if (td <= 0.0f && ud <= 0.0f)
-	{
-		// Single point
-		*x1 = p1s[2];
-		*x2 = p2s[2];
-		p1s[0] = p1s[2];
-		p2s[0] = p2s[2];
-		points[0] = points[2];
-		return 1;
-	}
-
-	// Should not be in vertex a or b region.
-	B2_NOT_USED(sd);
-	B2_NOT_USED(sn);
-	b2Assert(sn > 0.0f || tn > 0.0f);
-	b2Assert(sd > 0.0f || un > 0.0f);
-
-	float32 n = b2Cross(ab, ac);
-
-#ifdef TARGET_FLOAT32_IS_FIXED
-	n = (n < 0.0)? -1.0 : ((n > 0.0)? 1.0 : 0.0);
-#endif
-
-	// Should not be in edge ab region.
-	float32 vc = n * b2Cross(a, b);
-	b2Assert(vc > 0.0f || sn > 0.0f || sd > 0.0f);
-
-	// In edge bc region?
-	float32 va = n * b2Cross(b, c);
-	if (va <= 0.0f && un >= 0.0f && ud >= 0.0f && (un+ud) > 0.0f)
-	{
-		b2Assert(un + ud > 0.0f);
-		float32 lambda = un / (un + ud);
-		*x1 = p1s[1] + lambda * (p1s[2] - p1s[1]);
-		*x2 = p2s[1] + lambda * (p2s[2] - p2s[1]);
-		p1s[0] = p1s[2];
-		p2s[0] = p2s[2];
-		points[0] = points[2];
-		return 2;
-	}
-
-	// In edge ac region?
-	float32 vb = n * b2Cross(c, a);
-	if (vb <= 0.0f && tn >= 0.0f && td >= 0.0f && (tn+td) > 0.0f)
-	{
-		b2Assert(tn + td > 0.0f);
-		float32 lambda = tn / (tn + td);
-		*x1 = p1s[0] + lambda * (p1s[2] - p1s[0]);
-		*x2 = p2s[0] + lambda * (p2s[2] - p2s[0]);
-		p1s[1] = p1s[2];
-		p2s[1] = p2s[2];
-		points[1] = points[2];
-		return 2;
-	}
-
-	// Inside the triangle, compute barycentric coordinates
-	float32 denom = va + vb + vc;
-	b2Assert(denom > 0.0f);
-	denom = 1.0f / denom;
-
-#ifdef TARGET_FLOAT32_IS_FIXED
-	*x1 = denom * (va * p1s[0] + vb * p1s[1] + vc * p1s[2]);
-	*x2 = denom * (va * p2s[0] + vb * p2s[1] + vc * p2s[2]);
-#else
-	float32 u = va * denom;
-	float32 v = vb * denom;
-	float32 w = 1.0f - u - v;
-	*x1 = u * p1s[0] + v * p1s[1] + w * p1s[2];
-	*x2 = u * p2s[0] + v * p2s[1] + w * p2s[2];
-#endif
-	return 3;
-}
-
-static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
-{
-	const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
-	for (int32 i = 0; i < pointCount; ++i)
-	{
-		b2Vec2 d = b2Abs(w - points[i]);
-		b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
-		
-		if (d.x < k_tolerance * (m.x + 1.0f) &&
-			d.y < k_tolerance * (m.y + 1.0f))
-		{
-			return true;
-		}
-	}
-
-	return false;
-}
-
-template <typename T1, typename T2>
-float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
-				   const T1* shape1, const b2XForm& xf1,
-				   const T2* shape2, const b2XForm& xf2)
-{
-	b2Vec2 p1s[3], p2s[3];
-	b2Vec2 points[3];
-	int32 pointCount = 0;
-
-	*x1 = shape1->GetFirstVertex(xf1);
-	*x2 = shape2->GetFirstVertex(xf2);
-
-	float32 vSqr = 0.0f;
-	const int32 maxIterations = 20;
-	for (int32 iter = 0; iter < maxIterations; ++iter)
-	{
-		b2Vec2 v = *x2 - *x1;
-		b2Vec2 w1 = shape1->Support(xf1, v);
-		b2Vec2 w2 = shape2->Support(xf2, -v);
-
-		vSqr = b2Dot(v, v);
-		b2Vec2 w = w2 - w1;
-		float32 vw = b2Dot(v, w);
-		if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
-		{
-			if (pointCount == 0)
-			{
-				*x1 = w1;
-				*x2 = w2;
-			}
-			g_GJK_Iterations = iter;
-			return b2Sqrt(vSqr);
-		}
-
-		switch (pointCount)
-		{
-		case 0:
-			p1s[0] = w1;
-			p2s[0] = w2;
-			points[0] = w;
-			*x1 = p1s[0];
-			*x2 = p2s[0];
-			++pointCount;
-			break;
-
-		case 1:
-			p1s[1] = w1;
-			p2s[1] = w2;
-			points[1] = w;
-			pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
-			break;
-
-		case 2:
-			p1s[2] = w1;
-			p2s[2] = w2;
-			points[2] = w;
-			pointCount = ProcessThree(x1, x2, p1s, p2s, points);
-			break;
-		}
-
-		// If we have three points, then the origin is in the corresponding triangle.
-		if (pointCount == 3)
-		{
-			g_GJK_Iterations = iter;
-			return 0.0f;
-		}
-
-		float32 maxSqr = -B2_FLT_MAX;
-		for (int32 i = 0; i < pointCount; ++i)
-		{
-			maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
-		}
-
-#ifdef TARGET_FLOAT32_IS_FIXED
-		if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
-#else
-		if (pointCount == 3 || vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
-#endif
-		{
-			g_GJK_Iterations = iter;
-			v = *x2 - *x1;
-			vSqr = b2Dot(v, v);
-			return b2Sqrt(vSqr);
-		}
-	}
-
-	g_GJK_Iterations = maxIterations;
-	return b2Sqrt(vSqr);
-}
-
-static float32 DistanceCC(
-	b2Vec2* x1, b2Vec2* x2,
-	const b2CircleShape* circle1, const b2XForm& xf1,
-	const b2CircleShape* circle2, const b2XForm& xf2)
-{
-	b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
-	b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
-
-	b2Vec2 d = p2 - p1;
-	float32 dSqr = b2Dot(d, d);
-	float32 r1 = circle1->GetRadius() - b2_toiSlop;
-	float32 r2 = circle2->GetRadius() - b2_toiSlop;
-	float32 r = r1 + r2;
-	if (dSqr > r * r)
-	{
-		float32 dLen = d.Normalize();
-		float32 distance = dLen - r;
-		*x1 = p1 + r1 * d;
-		*x2 = p2 - r2 * d;
-		return distance;
-	}
-	else if (dSqr > B2_FLT_EPSILON * B2_FLT_EPSILON)
-	{
-		d.Normalize();
-		*x1 = p1 + r1 * d;
-		*x2 = *x1;
-		return 0.0f;
-	}
-
-	*x1 = p1;
-	*x2 = *x1;
-	return 0.0f;
-}
-
-// This is used for polygon-vs-circle distance.
-struct Point
-{
-	b2Vec2 Support(const b2XForm&, const b2Vec2&) const
-	{
-		return p;
-	}
-
-	b2Vec2 GetFirstVertex(const b2XForm&) const
-	{
-		return p;
-	}
-	
-	b2Vec2 p;
-};
-
-// GJK is more robust with polygon-vs-point than polygon-vs-circle.
-// So we convert polygon-vs-circle to polygon-vs-point.
-static float32 DistancePC(
-	b2Vec2* x1, b2Vec2* x2,
-	const b2PolygonShape* polygon, const b2XForm& xf1,
-	const b2CircleShape* circle, const b2XForm& xf2)
-{
-	Point point;
-	point.p = b2Mul(xf2, circle->GetLocalPosition());
-
-	float32 distance = DistanceGeneric(x1, x2, polygon, xf1, &point, b2XForm_identity);
-
-	float32 r = circle->GetRadius() - b2_toiSlop;
-
-	if (distance > r)
-	{
-		distance -= r;
-		b2Vec2 d = *x2 - *x1;
-		d.Normalize();
-		*x2 -= r * d;
-	}
-	else
-	{
-		distance = 0.0f;
-		*x2 = *x1;
-	}
-
-	return distance;
-}
-
-float32 b2Distance(b2Vec2* x1, b2Vec2* x2,
-				   const b2Shape* shape1, const b2XForm& xf1,
-				   const b2Shape* shape2, const b2XForm& xf2)
-{
-	b2ShapeType type1 = shape1->GetType();
-	b2ShapeType type2 = shape2->GetType();
-
-	if (type1 == e_circleShape && type2 == e_circleShape)
-	{
-		return DistanceCC(x1, x2, (b2CircleShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
-	}
-	
-	if (type1 == e_polygonShape && type2 == e_circleShape)
-	{
-		return DistancePC(x1, x2, (b2PolygonShape*)shape1, xf1, (b2CircleShape*)shape2, xf2);
-	}
-
-	if (type1 == e_circleShape && type2 == e_polygonShape)
-	{
-		return DistancePC(x2, x1, (b2PolygonShape*)shape2, xf2, (b2CircleShape*)shape1, xf1);
-	}
-
-	if (type1 == e_polygonShape && type2 == e_polygonShape)
-	{
-		return DistanceGeneric(x1, x2, (b2PolygonShape*)shape1, xf1, (b2PolygonShape*)shape2, xf2);
-	}
-
-	return 0.0f;
-}

src/modules/physics/box2d/Source/Collision/b2PairManager.cpp

@@ -1,396 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2PairManager.h"
-#include "b2BroadPhase.h"
-
-#include <algorithm>
-
-// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
-// This assumes proxyId1 and proxyId2 are 16-bit.
-inline uint32 Hash(uint32 proxyId1, uint32 proxyId2)
-{
-	uint32 key = (proxyId2 << 16) | proxyId1;
-	key = ~key + (key << 15);
-	key = key ^ (key >> 12);
-	key = key + (key << 2);
-	key = key ^ (key >> 4);
-	key = key * 2057;
-	key = key ^ (key >> 16);
-	return key;
-}
-
-inline bool Equals(const b2Pair& pair, int32 proxyId1, int32 proxyId2)
-{
-	return pair.proxyId1 == proxyId1 && pair.proxyId2 == proxyId2;
-}
-
-inline bool Equals(const b2BufferedPair& pair1, const b2BufferedPair& pair2)
-{
-	return pair1.proxyId1 == pair2.proxyId1 && pair1.proxyId2 == pair2.proxyId2;
-}
-
-// For sorting.
-inline bool operator < (const b2BufferedPair& pair1, const b2BufferedPair& pair2)
-{
-	if (pair1.proxyId1 < pair2.proxyId1)
-	{
-		return true;
-	}
-
-	if (pair1.proxyId1 == pair2.proxyId1)
-	{
-		return pair1.proxyId2 < pair2.proxyId2;
-	}
-
-	return false;
-}
-
-
-b2PairManager::b2PairManager()
-{
-	b2Assert(b2IsPowerOfTwo(b2_tableCapacity) == true);
-	b2Assert(b2_tableCapacity >= b2_maxPairs);
-	for (int32 i = 0; i < b2_tableCapacity; ++i)
-	{
-		m_hashTable[i] = b2_nullPair;
-	}
-	m_freePair = 0;
-	for (int32 i = 0; i < b2_maxPairs; ++i)
-	{
-		m_pairs[i].proxyId1 = b2_nullProxy;
-		m_pairs[i].proxyId2 = b2_nullProxy;
-		m_pairs[i].userData = NULL;
-		m_pairs[i].status = 0;
-		m_pairs[i].next = uint16(i + 1);
-	}
-	m_pairs[b2_maxPairs-1].next = b2_nullPair;
-	m_pairCount = 0;
-	m_pairBufferCount = 0;
-}
-
-void b2PairManager::Initialize(b2BroadPhase* broadPhase, b2PairCallback* callback)
-{
-	m_broadPhase = broadPhase;
-	m_callback = callback;
-}
-
-b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2, uint32 hash)
-{
-	int32 index = m_hashTable[hash];
-
-	while (index != b2_nullPair && Equals(m_pairs[index], proxyId1, proxyId2) == false)
-	{
-		index = m_pairs[index].next;
-	}
-
-	if (index == b2_nullPair)
-	{
-		return NULL;
-	}
-
-	b2Assert(index < b2_maxPairs);
-
-	return m_pairs + index;
-}
-
-b2Pair* b2PairManager::Find(int32 proxyId1, int32 proxyId2)
-{
-	if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
-
-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
-
-	return Find(proxyId1, proxyId2, hash);
-}
-
-// Returns existing pair or creates a new one.
-b2Pair* b2PairManager::AddPair(int32 proxyId1, int32 proxyId2)
-{
-	if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
-
-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
-
-	b2Pair* pair = Find(proxyId1, proxyId2, hash);
-	if (pair != NULL)
-	{
-		return pair;
-	}
-
-	b2Assert(m_pairCount < b2_maxPairs && m_freePair != b2_nullPair);
-
-	uint16 pairIndex = m_freePair;
-	pair = m_pairs + pairIndex;
-	m_freePair = pair->next;
-
-	pair->proxyId1 = (uint16)proxyId1;
-	pair->proxyId2 = (uint16)proxyId2;
-	pair->status = 0;
-	pair->userData = NULL;
-	pair->next = m_hashTable[hash];
-
-	m_hashTable[hash] = pairIndex;
-
-	++m_pairCount;
-
-	return pair;
-}
-
-// Removes a pair. The pair must exist.
-void* b2PairManager::RemovePair(int32 proxyId1, int32 proxyId2)
-{
-	b2Assert(m_pairCount > 0);
-
-	if (proxyId1 > proxyId2) b2Swap(proxyId1, proxyId2);
-
-	int32 hash = Hash(proxyId1, proxyId2) & b2_tableMask;
-
-	uint16* node = &m_hashTable[hash];
-	while (*node != b2_nullPair)
-	{
-		if (Equals(m_pairs[*node], proxyId1, proxyId2))
-		{
-			uint16 index = *node;
-			*node = m_pairs[*node].next;
-			
-			b2Pair* pair = m_pairs + index;
-			void* userData = pair->userData;
-
-			// Scrub
-			pair->next = m_freePair;
-			pair->proxyId1 = b2_nullProxy;
-			pair->proxyId2 = b2_nullProxy;
-			pair->userData = NULL;
-			pair->status = 0;
-
-			m_freePair = index;
-			--m_pairCount;
-			return userData;
-		}
-		else
-		{
-			node = &m_pairs[*node].next;
-		}
-	}
-
-	b2Assert(false);
-	return NULL;
-}
-
-/*
-As proxies are created and moved, many pairs are created and destroyed. Even worse, the same
-pair may be added and removed multiple times in a single time step of the physics engine. To reduce
-traffic in the pair manager, we try to avoid destroying pairs in the pair manager until the
-end of the physics step. This is done by buffering all the RemovePair requests. AddPair
-requests are processed immediately because we need the hash table entry for quick lookup.
-
-All user user callbacks are delayed until the buffered pairs are confirmed in Commit.
-This is very important because the user callbacks may be very expensive and client logic
-may be harmed if pairs are added and removed within the same time step.
-
-Buffer a pair for addition.
-We may add a pair that is not in the pair manager or pair buffer.
-We may add a pair that is already in the pair manager and pair buffer.
-If the added pair is not a new pair, then it must be in the pair buffer (because RemovePair was called).
-*/
-void b2PairManager::AddBufferedPair(int32 id1, int32 id2)
-{
-	b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
-	b2Assert(m_pairBufferCount < b2_maxPairs);
-
-	b2Pair* pair = AddPair(id1, id2);
-
-	// If this pair is not in the pair buffer ...
-	if (pair->IsBuffered() == false)
-	{
-		// This must be a newly added pair.
-		b2Assert(pair->IsFinal() == false);
-
-		// Add it to the pair buffer.
-		pair->SetBuffered();
-		m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
-		m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
-		++m_pairBufferCount;
-
-		b2Assert(m_pairBufferCount <= m_pairCount);
-	}
-
-	// Confirm this pair for the subsequent call to Commit.
-	pair->ClearRemoved();
-
-	if (b2BroadPhase::s_validate)
-	{
-		ValidateBuffer();
-	}
-}
-
-// Buffer a pair for removal.
-void b2PairManager::RemoveBufferedPair(int32 id1, int32 id2)
-{
-	b2Assert(id1 != b2_nullProxy && id2 != b2_nullProxy);
-	b2Assert(m_pairBufferCount < b2_maxPairs);
-
-	b2Pair* pair = Find(id1, id2);
-
-	if (pair == NULL)
-	{
-		// The pair never existed. This is legal (due to collision filtering).
-		return;
-	}
-
-	// If this pair is not in the pair buffer ...
-	if (pair->IsBuffered() == false)
-	{
-		// This must be an old pair.
-		b2Assert(pair->IsFinal() == true);
-
-		pair->SetBuffered();
-		m_pairBuffer[m_pairBufferCount].proxyId1 = pair->proxyId1;
-		m_pairBuffer[m_pairBufferCount].proxyId2 = pair->proxyId2;
-		++m_pairBufferCount;
-
-		b2Assert(m_pairBufferCount <= m_pairCount);
-	}
-
-	pair->SetRemoved();
-
-	if (b2BroadPhase::s_validate)
-	{
-		ValidateBuffer();
-	}
-}
-
-void b2PairManager::Commit()
-{
-	int32 removeCount = 0;
-
-	b2Proxy* proxies = m_broadPhase->m_proxyPool;
-
-	for (int32 i = 0; i < m_pairBufferCount; ++i)
-	{
-		b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
-		b2Assert(pair->IsBuffered());
-		pair->ClearBuffered();
-
-		b2Assert(pair->proxyId1 < b2_maxProxies && pair->proxyId2 < b2_maxProxies);
-
-		b2Proxy* proxy1 = proxies + pair->proxyId1;
-		b2Proxy* proxy2 = proxies + pair->proxyId2;
-
-		b2Assert(proxy1->IsValid());
-		b2Assert(proxy2->IsValid());
-
-		if (pair->IsRemoved())
-		{
-			// It is possible a pair was added then removed before a commit. Therefore,
-			// we should be careful not to tell the user the pair was removed when the
-			// the user didn't receive a matching add.
-			if (pair->IsFinal() == true)
-			{
-				m_callback->PairRemoved(proxy1->userData, proxy2->userData, pair->userData);
-			}
-
-			// Store the ids so we can actually remove the pair below.
-			m_pairBuffer[removeCount].proxyId1 = pair->proxyId1;
-			m_pairBuffer[removeCount].proxyId2 = pair->proxyId2;
-			++removeCount;
-		}
-		else
-		{
-			b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);
-
-			if (pair->IsFinal() == false)
-			{
-				pair->userData = m_callback->PairAdded(proxy1->userData, proxy2->userData);
-				pair->SetFinal();
-			}
-		}
-	}
-
-	for (int32 i = 0; i < removeCount; ++i)
-	{
-		RemovePair(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
-	}
-
-	m_pairBufferCount = 0;
-
-	if (b2BroadPhase::s_validate)
-	{
-		ValidateTable();
-	}
-}
-
-void b2PairManager::ValidateBuffer()
-{
-#ifdef _DEBUG
-	b2Assert(m_pairBufferCount <= m_pairCount);
-
-	std::sort(m_pairBuffer, m_pairBuffer + m_pairBufferCount);
-
-	for (int32 i = 0; i < m_pairBufferCount; ++i)
-	{
-		if (i > 0)
-		{
-			b2Assert(Equals(m_pairBuffer[i], m_pairBuffer[i-1]) == false);
-		}
-
-		b2Pair* pair = Find(m_pairBuffer[i].proxyId1, m_pairBuffer[i].proxyId2);
-		b2Assert(pair->IsBuffered());
-
-		b2Assert(pair->proxyId1 != pair->proxyId2);
-		b2Assert(pair->proxyId1 < b2_maxProxies);
-		b2Assert(pair->proxyId2 < b2_maxProxies);
-
-		b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
-		b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;
-
-		b2Assert(proxy1->IsValid() == true);
-		b2Assert(proxy2->IsValid() == true);
-	}
-#endif
-}
-
-void b2PairManager::ValidateTable()
-{
-#ifdef _DEBUG
-	for (int32 i = 0; i < b2_tableCapacity; ++i)
-	{
-		uint16 index = m_hashTable[i];
-		while (index != b2_nullPair)
-		{
-			b2Pair* pair = m_pairs + index;
-			b2Assert(pair->IsBuffered() == false);
-			b2Assert(pair->IsFinal() == true);
-			b2Assert(pair->IsRemoved() == false);
-
-			b2Assert(pair->proxyId1 != pair->proxyId2);
-			b2Assert(pair->proxyId1 < b2_maxProxies);
-			b2Assert(pair->proxyId2 < b2_maxProxies);
-
-			b2Proxy* proxy1 = m_broadPhase->m_proxyPool + pair->proxyId1;
-			b2Proxy* proxy2 = m_broadPhase->m_proxyPool + pair->proxyId2;
-
-			b2Assert(proxy1->IsValid() == true);
-			b2Assert(proxy2->IsValid() == true);
-
-			b2Assert(m_broadPhase->TestOverlap(proxy1, proxy2) == true);
-
-			index = pair->next;
-		}
-	}
-#endif
-}

src/modules/physics/box2d/Source/Collision/b2PairManager.h

@@ -1,121 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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.
-*/
-
-// The pair manager is used by the broad-phase to quickly add/remove/find pairs
-// of overlapping proxies. It is based closely on code provided by Pierre Terdiman.
-// http://www.codercorner.com/IncrementalSAP.txt
-
-#ifndef B2_PAIR_MANAGER_H
-#define B2_PAIR_MANAGER_H
-
-#include "../Common/b2Settings.h"
-#include "../Common/b2Math.h"
-
-#include <climits>
-
-class b2BroadPhase;
-struct b2Proxy;
-
-const uint16 b2_nullPair = USHRT_MAX;
-const uint16 b2_nullProxy = USHRT_MAX;
-const int32 b2_tableCapacity = b2_maxPairs;	// must be a power of two
-const int32 b2_tableMask = b2_tableCapacity - 1;
-
-struct b2Pair
-{
-	enum
-	{
-		e_pairBuffered	= 0x0001,
-		e_pairRemoved	= 0x0002,
-		e_pairFinal		= 0x0004,
-	};
-
-	void SetBuffered()		{ status |= e_pairBuffered; }
-	void ClearBuffered()	{ status &= ~e_pairBuffered; }
-	bool IsBuffered()		{ return (status & e_pairBuffered) == e_pairBuffered; }
-
-	void SetRemoved()		{ status |= e_pairRemoved; }
-	void ClearRemoved()		{ status &= ~e_pairRemoved; }
-	bool IsRemoved()		{ return (status & e_pairRemoved) == e_pairRemoved; }
-
-	void SetFinal()		{ status |= e_pairFinal; }
-	bool IsFinal()		{ return (status & e_pairFinal) == e_pairFinal; }
-
-	void* userData;
-	uint16 proxyId1;
-	uint16 proxyId2;
-	uint16 next;
-	uint16 status;
-};
-
-struct b2BufferedPair
-{
-	uint16 proxyId1;
-	uint16 proxyId2;
-};
-
-class b2PairCallback
-{
-public:
-	virtual ~b2PairCallback() {}
-
-	// This should return the new pair user data. It is ok if the
-	// user data is null.
-	virtual void* PairAdded(void* proxyUserData1, void* proxyUserData2) = 0;
-
-	// This should free the pair's user data. In extreme circumstances, it is possible
-	// this will be called with null pairUserData because the pair never existed.
-	virtual void PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData) = 0;
-};
-
-class b2PairManager
-{
-public:
-	b2PairManager();
-
-	void Initialize(b2BroadPhase* broadPhase, b2PairCallback* callback);
-
-	void AddBufferedPair(int32 proxyId1, int32 proxyId2);
-	void RemoveBufferedPair(int32 proxyId1, int32 proxyId2);
-
-	void Commit();
-
-private:
-	b2Pair* Find(int32 proxyId1, int32 proxyId2);
-	b2Pair* Find(int32 proxyId1, int32 proxyId2, uint32 hashValue);
-
-	b2Pair* AddPair(int32 proxyId1, int32 proxyId2);
-	void* RemovePair(int32 proxyId1, int32 proxyId2);
-
-	void ValidateBuffer();
-	void ValidateTable();
-
-public:
-	b2BroadPhase *m_broadPhase;
-	b2PairCallback *m_callback;
-	b2Pair m_pairs[b2_maxPairs];
-	uint16 m_freePair;
-	int32 m_pairCount;
-
-	b2BufferedPair m_pairBuffer[b2_maxPairs];
-	int32 m_pairBufferCount;
-
-	uint16 m_hashTable[b2_tableCapacity];
-};
-
-#endif

src/modules/physics/box2d/Source/Collision/b2TimeOfImpact.cpp

@@ -1,112 +0,0 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Collision.h"
-#include "Shapes/b2Shape.h"
-
-// This algorithm uses conservative advancement to compute the time of
-// impact (TOI) of two shapes.
-// Refs: Bullet, Young Kim
-float32 b2TimeOfImpact(const b2Shape* shape1, const b2Sweep& sweep1,
-					   const b2Shape* shape2, const b2Sweep& sweep2)
-{
-	float32 r1 = shape1->GetSweepRadius();
-	float32 r2 = shape2->GetSweepRadius();
-
-	b2Assert(sweep1.t0 == sweep2.t0);
-	b2Assert(1.0f - sweep1.t0 > B2_FLT_EPSILON);
-
-	float32 t0 = sweep1.t0;
-	b2Vec2 v1 = sweep1.c - sweep1.c0;
-	b2Vec2 v2 = sweep2.c - sweep2.c0;
-	float32 omega1 = sweep1.a - sweep1.a0;
-	float32 omega2 = sweep2.a - sweep2.a0;
-
-	float32 alpha = 0.0f;
-
-	b2Vec2 p1, p2;
-	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
-	int32 iter = 0;
-	b2Vec2 normal = b2Vec2_zero;
-	float32 distance = 0.0f;
-	float32 targetDistance = 0.0f;
-	for(;;)
-	{
-		float32 t = (1.0f - alpha) * t0 + alpha;
-		b2XForm xf1, xf2;
-		sweep1.GetXForm(&xf1, t);
-		sweep2.GetXForm(&xf2, t);
-
-		// Get the distance between shapes.
-		distance = b2Distance(&p1, &p2, shape1, xf1, shape2, xf2);
-
-		if (iter == 0)
-		{
-			// Compute a reasonable target distance to give some breathing room
-			// for conservative advancement.
-			if (distance > 2.0f * b2_toiSlop)
-			{
-				targetDistance = 1.5f * b2_toiSlop;
-			}
-			else
-			{
-				targetDistance = b2Max(0.05f * b2_toiSlop, distance - 0.5f * b2_toiSlop);
-			}
-		}
-
-		if (distance - targetDistance < 0.05f * b2_toiSlop || iter == k_maxIterations)
-		{
-			break;
-		}
-
-		normal = p2 - p1;
-		normal.Normalize();
-
-		// Compute upper bound on remaining movement.
-		float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
-		if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
-		{
-			alpha = 1.0f;
-			break;
-		}
-
-		// Get the conservative time increment. Don't advance all the way.
-		float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
-		//float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
-		float32 newAlpha = alpha + dAlpha;
-
-		// The shapes may be moving apart or a safe distance apart.
-		if (newAlpha < 0.0f || 1.0f < newAlpha)
-		{
-			alpha = 1.0f;
-			break;
-		}
-
-		// Ensure significant advancement.
-		if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
-		{
-			break;
-		}
-
-		alpha = newAlpha;
-
-		++iter;
-	}
-
-	return alpha;
-}

src/modules/physics/box2d/Source/Common/Fixed.h

@@ -1,477 +0,0 @@
-/*
-Copyright (c) 2006 Henry Strickland & Ryan Seto
-              2007-2008 Tobias Weyand (modifications and extensions)
-
-Permission is hereby granted, free of charge, to any person obtaining a
-copy of this software and associated documentation files (the "Software"),
-to deal in the Software without restriction, including without limitation
-the rights to use, copy, modify, merge, publish, distribute, sublicense,
-and/or sell copies of the Software, and to permit persons to whom the
-Software is furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included
-in all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
-THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
-OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
-ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
-OTHER DEALINGS IN THE SOFTWARE.
-
-        (* http://www.opensource.org/licenses/mit-license.php *)
-*/
-
-#ifndef _FIXED_H_
-#define _FIXED_H_
-
-#include <stdio.h>
-
-#ifdef TARGET_IS_NDS
-
-#include "nds.h"
-
-#endif
-
-#define FIXED_BP        16
-#define FIXED_MAX       ((1<<(32-FIXED_BP-1))-1)
-#define FIXED_MIN       (-(1<<(32-FIXED_BP-1)))
-#define FIXED_EPSILON   (Fixed(0.00007f))
-
-#define G_1_DIV_PI		20861
-
-class Fixed {
-
-	private:
-	
-		int	g; // the guts
-	
-		const static int BP= FIXED_BP;  // how many low bits are right of Binary Point
-		const static int BP2= BP*2;  // how many low bits are right of Binary Point
-		const static int BPhalf= BP/2;  // how many low bits are right of Binary Point
-	
-		double STEP();  // smallest step we can represent
-	
-		// for private construction via guts
-		enum FixedRaw { RAW };
-		Fixed(FixedRaw, int guts);
-	
-	public:
-	
-		Fixed();
-		Fixed(const Fixed &a);
-		Fixed(float a);
-		Fixed(double a);
-		Fixed(int a);
-		Fixed(long a);
-	
-		Fixed& operator =(const Fixed a);
-		Fixed& operator =(float a);
-		Fixed& operator =(double a);
-		Fixed& operator =(int a);
-		Fixed& operator =(long a);
-	
-		operator float();
-		operator double();
-		operator int();
-		operator long();
-		operator unsigned short();
-	
-		operator float() const;
-	
-		Fixed operator +() const;
-		Fixed operator -() const;
-
-		Fixed operator +(const Fixed a) const;
-		Fixed operator -(const Fixed a) const;
-#if 1
-		// more acurate, using long long
-		Fixed operator *(const Fixed a) const;
-#else
-		// faster, but with only half as many bits right of binary point
-		Fixed operator *(const Fixed a) const;
-#endif
-		Fixed operator /(const Fixed a) const;
-
-		Fixed operator *(unsigned short a) const;
-		Fixed operator *(int a) const;
-	
-		Fixed operator +(float a) const;
-		Fixed operator -(float a) const;
-		Fixed operator *(float a) const;
-		Fixed operator /(float a) const;
-	
-		Fixed operator +(double a) const;
-		Fixed operator -(double a) const;
-		Fixed operator *(double a) const;
-		Fixed operator /(double a) const;
-	
-		Fixed operator >>(int a) const;
-		Fixed operator <<(int a) const;
-	
-		Fixed& operator +=(Fixed a);
-		Fixed& operator -=(Fixed a);
-		Fixed& operator *=(Fixed a);
-		Fixed& operator /=(Fixed a);
-	
-		Fixed& operator +=(int a);
-		Fixed& operator -=(int a);
-		Fixed& operator *=(int a);
-		Fixed& operator /=(int a);
-	
-		Fixed& operator +=(long a);
-		Fixed& operator -=(long a);
-		Fixed& operator *=(long a);
-		Fixed& operator /=(long a);
-	
-		Fixed& operator +=(float a);
-		Fixed& operator -=(float a);
-		Fixed& operator *=(float a);
-		Fixed& operator /=(float a);
-	
-		Fixed& operator +=(double a);
-		Fixed& operator -=(double a);
-		Fixed& operator *=(double a);
-		Fixed& operator /=(double a);
-	
-		bool operator ==(const Fixed a) const;
-		bool operator !=(const Fixed a) const;
-		bool operator <=(const Fixed a) const;
-		bool operator >=(const Fixed a) const;
-		bool operator  <(const Fixed a) const;
-		bool operator  >(const Fixed a) const;
-	
-		bool operator ==(float a) const;
-		bool operator !=(float a) const;
-		bool operator <=(float a) const;
-		bool operator >=(float a) const;
-		bool operator  <(float a) const;
-		bool operator  >(float a) const;
-	
-		bool operator ==(double a) const;
-		bool operator !=(double a) const;
-		bool operator <=(double a) const;
-		bool operator >=(double a) const;
-		bool operator  <(double a) const;
-		bool operator  >(double a) const;
-	
-		bool operator  >(int a) const;
-		bool operator  <(int a) const;
-		bool operator  >=(int a) const;
-		bool operator  <=(int a) const;
-	
-		Fixed abs();
-		Fixed sqrt();
-#ifdef TARGET_IS_NDS
-		Fixed cosf();
-		Fixed sinf();
-		Fixed tanf();
-#endif
-};
-
-//
-// Implementation
-//
-
-inline double Fixed::STEP() { return 1.0 / (1<<BP); }  // smallest step we can represent
-
-// for private construction via guts
-inline Fixed::Fixed(FixedRaw, int guts) : g(guts) {}
-
-inline Fixed::Fixed() : g(0) {}
-inline Fixed::Fixed(const Fixed &a) : g( a.g ) {}
-inline Fixed::Fixed(float a) : g( int(a * (float)(1<<BP)) ) {}
-inline Fixed::Fixed(double a) : g( int(a * (double)(1<<BP) ) ) {}
-inline Fixed::Fixed(int a) : g( a << BP ) {}
-inline Fixed::Fixed(long a) : g( a << BP ) {}
-
-inline Fixed& Fixed::operator =(const Fixed a) { g= a.g; return *this; }
-inline Fixed& Fixed::operator =(float a) { g= Fixed(a).g; return *this; }
-inline Fixed& Fixed::operator =(double a) { g= Fixed(a).g; return *this; }
-inline Fixed& Fixed::operator =(int a) { g= Fixed(a).g; return *this; }
-inline Fixed& Fixed::operator =(long a) { g= Fixed(a).g; return *this; }
-
-inline Fixed::operator float() { return g * (float)STEP(); }
-inline Fixed::operator double() { return g * (double)STEP(); }
-inline Fixed::operator int() { return g>>BP; }
-inline Fixed::operator long() { return g>>BP; }
-//#pragma warning(disable: 4244) //HARDWIRE added pragma to prevent VS2005 compilation error
-inline Fixed::operator unsigned short() { return g>>BP; }
-inline Fixed::operator float() const { return g / (float)(1<<BP); }
-
-inline Fixed Fixed::operator +() const { return Fixed(RAW,g); }
-inline Fixed Fixed::operator -() const { return Fixed(RAW,-g); }
-
-inline Fixed Fixed::operator +(const Fixed a) const { return Fixed(RAW, g + a.g); }
-inline Fixed Fixed::operator -(const Fixed a) const { return Fixed(RAW, g - a.g); }
-
-#if 1
-// more acurate, using long long
-inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW,  (int)( ((long long)g * (long long)a.g ) >> BP)); }
-
-#elif 0
-
-// check for overflow and figure out where.  Must specify -rdynamic in linker
-#include <execinfo.h>
-#include <signal.h>
-#include <exception>
-
-inline Fixed Fixed::operator *(const Fixed a) const {
-	long long x =  ((long long)g * (long long)a.g );
-	if(x > 0x7fffffffffffLL || x < -0x7fffffffffffLL) {
-		printf("overflow");
-		void *array[2];
-		int nSize = backtrace(array, 2);
-		char **symbols = backtrace_symbols(array, nSize);
-		for(int i=0; i<nSize; i++) {
-			printf(" %s", symbols[i]);
-		}
-		printf("\n");
-	}
-	return Fixed(RAW, (int)(x>>BP)); 
-}
-
-#else
-// faster, but with only half as many bits right of binary point
-inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (g>>BPhalf) * (a.g>>BPhalf) ); }
-#endif
-
-
-#ifdef TARGET_IS_NDS
-// Division using the DS's maths coprocessor
-inline Fixed Fixed::operator /(const Fixed a) const
-{
-	//printf("%d %d\n", (long long)g << BP, a.g);
-	return Fixed(RAW, int( div64((long long)g << BP, a.g) ) );
-}
-#else
-inline Fixed Fixed::operator /(const Fixed a) const
-{
-	return Fixed(RAW, int( (((long long)g << BP2) / (long long)(a.g)) >> BP) );
-	//return Fixed(RAW, int( (((long long)g << BP) / (long long)(a.g)) ) );
-}
-#endif
-
-inline Fixed Fixed::operator *(unsigned short a) const { return operator*(Fixed(a)); }
-inline Fixed Fixed::operator *(int a) const { return operator*(Fixed(a)); }
-
-inline Fixed Fixed::operator +(float a) const { return Fixed(RAW, g + Fixed(a).g); }
-inline Fixed Fixed::operator -(float a) const { return Fixed(RAW, g - Fixed(a).g); }
-inline Fixed Fixed::operator *(float a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
-//inline Fixed Fixed::operator /(float a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
-inline Fixed Fixed::operator /(float a) const { return operator/(Fixed(a)); }
-
-inline Fixed Fixed::operator +(double a) const { return Fixed(RAW, g + Fixed(a).g); }
-inline Fixed Fixed::operator -(double a) const { return Fixed(RAW, g - Fixed(a).g); }
-inline Fixed Fixed::operator *(double a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
-//inline Fixed Fixed::operator /(double a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
-inline Fixed Fixed::operator /(double a) const { return operator/(Fixed(a)); }
-
-inline Fixed Fixed::operator >>(int a) const { return Fixed(RAW, g >> a); }
-inline Fixed Fixed::operator <<(int a) const { return Fixed(RAW, g << a); }
-
-inline Fixed& Fixed::operator +=(Fixed a) { return *this = *this + a; }
-inline Fixed& Fixed::operator -=(Fixed a) { return *this = *this - a; }
-inline Fixed& Fixed::operator *=(Fixed a) { return *this = *this * a; }
-//inline Fixed& Fixed::operator /=(Fixed a) { return *this = *this / a; }
-inline Fixed& Fixed::operator /=(Fixed a) { return *this = operator/(a); }
-
-inline Fixed& Fixed::operator +=(int a) { return *this = *this + (Fixed)a; }
-inline Fixed& Fixed::operator -=(int a) { return *this = *this - (Fixed)a; }
-inline Fixed& Fixed::operator *=(int a) { return *this = *this * (Fixed)a; }
-//inline Fixed& Fixed::operator /=(int a) { return *this = *this / (Fixed)a; }
-inline Fixed& Fixed::operator /=(int a) { return *this = operator/((Fixed)a); }
-
-inline Fixed& Fixed::operator +=(long a) { return *this = *this + (Fixed)a; }
-inline Fixed& Fixed::operator -=(long a) { return *this = *this - (Fixed)a; }
-inline Fixed& Fixed::operator *=(long a) { return *this = *this * (Fixed)a; }
-//inline Fixed& Fixed::operator /=(long a) { return *this = *this / (Fixed)a; }
-inline Fixed& Fixed::operator /=(long a) { return *this = operator/((Fixed)a); }
-
-inline Fixed& Fixed::operator +=(float a) { return *this = *this + a; }
-inline Fixed& Fixed::operator -=(float a) { return *this = *this - a; }
-inline Fixed& Fixed::operator *=(float a) { return *this = *this * a; }
-//inline Fixed& Fixed::operator /=(float a) { return *this = *this / a; }
-inline Fixed& Fixed::operator /=(float a) { return *this = operator/(a); }
-
-inline Fixed& Fixed::operator +=(double a) { return *this = *this + a; }
-inline Fixed& Fixed::operator -=(double a) { return *this = *this - a; }
-inline Fixed& Fixed::operator *=(double a) { return *this = *this * a; }
-//inline Fixed& Fixed::operator /=(double a) { return *this = *this / a; }
-inline Fixed& Fixed::operator /=(double a) { return *this = operator/(a); }
-
-inline Fixed operator +(int a, const Fixed b) { return Fixed(a)+b; }
-inline Fixed operator -(int a, const Fixed b) { return Fixed(a)-b; }
-inline Fixed operator *(int a, const Fixed b) { return Fixed(a)*b; }
-inline Fixed operator /(int a, const Fixed b) { return Fixed(a)/b; };
-
-inline Fixed operator +(float a, const Fixed b) { return Fixed(a)+b; }
-inline Fixed operator -(float a, const Fixed b) { return Fixed(a)-b; }
-inline Fixed operator *(float a, const Fixed b) { return Fixed(a)*b; }
-inline Fixed operator /(float a, const Fixed b) { return Fixed(a)/b; }
-
-inline bool Fixed::operator ==(const Fixed a) const { return g == a.g; }
-inline bool Fixed::operator !=(const Fixed a) const { return g != a.g; }
-inline bool Fixed::operator <=(const Fixed a) const { return g <= a.g; }
-inline bool Fixed::operator >=(const Fixed a) const { return g >= a.g; }
-inline bool Fixed::operator  <(const Fixed a) const { return g  < a.g; }
-inline bool Fixed::operator  >(const Fixed a) const { return g  > a.g; }
-
-inline bool Fixed::operator ==(float a) const { return g == Fixed(a).g; }
-inline bool Fixed::operator !=(float a) const { return g != Fixed(a).g; }
-inline bool Fixed::operator <=(float a) const { return g <= Fixed(a).g; }
-inline bool Fixed::operator >=(float a) const { return g >= Fixed(a).g; }
-inline bool Fixed::operator  <(float a) const { return g  < Fixed(a).g; }
-inline bool Fixed::operator  >(float a) const { return g  > Fixed(a).g; }
-
-inline bool Fixed::operator ==(double a) const { return g == Fixed(a).g; }
-inline bool Fixed::operator !=(double a) const { return g != Fixed(a).g; }
-inline bool Fixed::operator <=(double a) const { return g <= Fixed(a).g; }
-inline bool Fixed::operator >=(double a) const { return g >= Fixed(a).g; }
-inline bool Fixed::operator  <(double a) const { return g  < Fixed(a).g; }
-inline bool Fixed::operator  >(double a) const { return g  > Fixed(a).g; }
-
-inline bool Fixed::operator  >(int a) const { return g > Fixed(a).g; }
-inline bool Fixed::operator  <(int a) const { return g < Fixed(a).g; }
-inline bool Fixed::operator  >=(int a) const{ return g >= Fixed(a).g; };
-inline bool Fixed::operator  <=(int a) const{ return g <= Fixed(a).g; };
-
-inline bool operator ==(float a, const Fixed b) { return Fixed(a) == b; }
-inline bool operator !=(float a, const Fixed b) { return Fixed(a) != b; }
-inline bool operator <=(float a, const Fixed b) { return Fixed(a) <= b; }
-inline bool operator >=(float a, const Fixed b) { return Fixed(a) >= b; }
-inline bool operator  <(float a, const Fixed b) { return Fixed(a)  < b; }
-inline bool operator  >(float a, const Fixed b) { return Fixed(a)  > b; }
-
-inline Fixed operator +(double a, const Fixed b) { return Fixed(a)+b; }
-inline Fixed operator -(double a, const Fixed b) { return Fixed(a)-b; }
-inline Fixed operator *(double a, const Fixed b) { return Fixed(a)*b; }
-inline Fixed operator /(double a, const Fixed b) { return Fixed(a)/b; }
-
-inline bool operator ==(double a, const Fixed b) { return Fixed(a) == b; }
-inline bool operator !=(double a, const Fixed b) { return Fixed(a) != b; }
-inline bool operator <=(double a, const Fixed b) { return Fixed(a) <= b; }
-inline bool operator >=(double a, const Fixed b) { return Fixed(a) >= b; }
-inline bool operator  <(double a, const Fixed b) { return Fixed(a)  < b; }
-inline bool operator  >(double a, const Fixed b) { return Fixed(a)  > b; }
-
-inline bool operator ==(int a, const Fixed b) { return Fixed(a) == b; }
-inline bool operator !=(int a, const Fixed b) { return Fixed(a) != b; }
-inline bool operator <=(int a, const Fixed b) { return Fixed(a) <= b; }
-inline bool operator >=(int a, const Fixed b) { return Fixed(a) >= b; }
-inline bool operator  <(int a, const Fixed b) { return Fixed(a)  < b; }
-inline bool operator  >(int a, const Fixed b) { return Fixed(a)  > b; }
-
-inline int& operator +=(int& a, const Fixed b) { a = (Fixed)a + b; return a; }
-inline int& operator -=(int& a, const Fixed b) { a = (Fixed)a - b; return a; }
-inline int& operator *=(int& a, const Fixed b) { a = (Fixed)a * b; return a; }
-inline int& operator /=(int& a, const Fixed b) { a = (Fixed)a / b; return a; }
-
-inline long& operator +=(long& a, const Fixed b) { a = (Fixed)a + b; return a; }
-inline long& operator -=(long& a, const Fixed b) { a = (Fixed)a - b; return a; }
-inline long& operator *=(long& a, const Fixed b) { a = (Fixed)a * b; return a; }
-inline long& operator /=(long& a, const Fixed b) { a = (Fixed)a / b; return a; }
-
-inline float& operator +=(float& a, const Fixed b) { a = a + b; return a; }
-inline float& operator -=(float& a, const Fixed b) { a = a - b; return a; }
-inline float& operator *=(float& a, const Fixed b) { a = a * b; return a; }
-inline float& operator /=(float& a, const Fixed b) { a = a / b; return a; }
-
-inline double& operator +=(double& a, const Fixed b) { a = a + b; return a; }
-inline double& operator -=(double& a, const Fixed b) { a = a - b; return a; }
-inline double& operator *=(double& a, const Fixed b) { a = a * b; return a; }
-inline double& operator /=(double& a, const Fixed b) { a = a / b; return a; }
-
-inline Fixed Fixed::abs() { return (g>0) ? Fixed(RAW, g) : Fixed(RAW, -g); }
-inline Fixed abs(Fixed f) { return f.abs(); }
-
-//inline Fixed atan2(Fixed a, Fixed b) { return atan2f((float) a, (float) b); }
-inline Fixed atan2(Fixed y, Fixed x)
-{
-	Fixed abs_y = y.abs() + FIXED_EPSILON;	// avoid 0/0
-	Fixed r, angle;
-
-	if(x >= 0.0f) {
-		r = (x - abs_y) / (x + abs_y);
-		angle = 3.1415926/4.0;
-	} else {
-		r = (x + abs_y) / (abs_y - x);
-		angle = 3.0*3.1415926/4.0;
-	}
-	angle += Fixed(0.1963) * (r * r * r) - Fixed(0.9817) * r;
-	return (y < 0) ? -angle : angle;
-}
-
-#if TARGET_IS_NDS
-
-static inline long nds_sqrt64(long long a)
-{
-	SQRT_CR = SQRT_64;
-	while(SQRT_CR & SQRT_BUSY);
-	SQRT_PARAM64 = a;
-	while(SQRT_CR & SQRT_BUSY);
-
-	return SQRT_RESULT32;
-}
-
-static inline int32 div6464(int64 num, int64 den)
-{
-	DIV_CR = DIV_64_64;
-	while(DIV_CR & DIV_BUSY);
-	DIV_NUMERATOR64 = num;
-	DIV_DENOMINATOR64 = den;
-	while(DIV_CR & DIV_BUSY);
-
-	return (DIV_RESULT32);
-}
-
-inline Fixed Fixed::sqrt()
-{
-	return Fixed(RAW, nds_sqrt64(((long long)(g))<<BP));
-}
-#else
-inline Fixed Fixed::sqrt()
-{
-	long long m, root = 0, left = (long long)g<<FIXED_BP;
-	for ( m = (long long)1<<( (sizeof(long long)<<3) - 2); m; m >>= 2 )
-	{
-		if ( ( left & -m ) > root ) 
-			left -= ( root += m ), root += m;
-		root >>= 1;
-	}
-	return Fixed(RAW, root);
-}
-#endif
-
-inline Fixed sqrt(Fixed a) { return a.sqrt(); }
-inline Fixed sqrtf(Fixed a) { return a.sqrt(); }
-
-#endif
-
-#ifdef TARGET_IS_NDS
-// Use the libnds lookup tables for trigonometry functions
-inline Fixed Fixed::cosf() {
-	int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
-	if(idx < 0)
-		idx += 512;
-	return Fixed(RAW, COS_bin[idx] << 4);
-}
-inline Fixed cosf(Fixed x) { return x.cosf(); }
-inline Fixed Fixed::sinf() {
-	int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
-	if(idx < 0)
-			idx += 512;
-	return Fixed(RAW, SIN_bin[idx] << 4);
-}
-inline Fixed sinf(Fixed x) { return x.sinf(); }
-inline Fixed Fixed::tanf() {
-	int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
-	if(idx < 0)
-				idx += 512;
-	return Fixed(RAW, TAN_bin[idx] << 4);
-}
-inline Fixed tanf(Fixed x) { return x.tanf(); }
-
-
-#endif

src/modules/physics/box2d/Source/Common/b2Math.cpp

@@ -1,54 +0,0 @@
-/*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Math.h"
-
-const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
-const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f);
-const b2XForm b2XForm_identity(b2Vec2_zero, b2Mat22_identity);
-
-void b2Sweep::GetXForm(b2XForm* xf, float32 t) const
-{
-	// center = p + R * localCenter
-	if (1.0f - t0 > B2_FLT_EPSILON)
-	{
-		float32 alpha = (t - t0) / (1.0f - t0);
-		xf->position = (1.0f - alpha) * c0 + alpha * c;
-		float32 angle = (1.0f - alpha) * a0 + alpha * a;
-		xf->R.Set(angle);
-	}
-	else
-	{
-		xf->position = c;
-		xf->R.Set(a);
-	}
-
-	// Shift to origin
-	xf->position -= b2Mul(xf->R, localCenter);
-}
-
-void b2Sweep::Advance(float32 t)
-{
-	if (t0 < t && 1.0f - t0 > B2_FLT_EPSILON)
-	{
-		float32 alpha = (t - t0) / (1.0f - t0);
-		c0 = (1.0f - alpha) * c0 + alpha * c;
-		a0 = (1.0f - alpha) * a0 + alpha * a;
-		t0 = t;
-	}
-}

src/modules/physics/box2d/Source/Common/jtypes.h

@@ -1,139 +0,0 @@
-/*---------------------------------------------------------------------------------
-	$Id: jtypes.h,v 1.17 2007/07/18 05:20:45 wntrmute Exp $
-
-	jtypes.h -- Common types (and a few useful macros)
-
-	Copyright (C) 2005
-		Michael Noland (joat)
-		Jason Rogers (dovoto)
-		Dave Murphy (WinterMute)
-		Chris Double (doublec)
-
-	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 NDS_JTYPES_INCLUDE
-#define NDS_JTYPES_INCLUDE
-//---------------------------------------------------------------------------------
-
-
-#define PACKED __attribute__ ((packed))
-#define packed_struct struct PACKED
-
-//---------------------------------------------------------------------------------
-// libgba compatible section macros
-//---------------------------------------------------------------------------------
-#define ITCM_CODE	__attribute__((section(".itcm"), long_call))
-
-#define DTCM_DATA	__attribute__((section(".dtcm")))
-#define DTCM_BSS	__attribute__((section(".sbss")))
-#define ALIGN(m)	__attribute__((aligned (m)))
-
-#define PACKED __attribute__ ((packed))
-#define packed_struct struct PACKED
-
-//---------------------------------------------------------------------------------
-// These are linked to the bin2o macro in the Makefile
-//---------------------------------------------------------------------------------
-#define GETRAW(name)      (name)
-#define GETRAWSIZE(name)  ((int)name##_size)
-#define GETRAWEND(name)  ((int)name##_end)
-
-#ifndef TRUE
-#define TRUE 1
-#define FALSE 0
-#endif
-
-#define BIT(n) (1 << (n))
-
-// define libnds types in terms of stdint
-#include <stdint.h>
-
-typedef uint8_t		uint8;
-typedef uint16_t	uint16;
-typedef uint32_t	uint32;
-typedef uint64_t	uint64;
-
-typedef int8_t		int8;
-typedef int16_t		int16;
-typedef int32_t		int32;
-typedef int64_t		int64;
-
-//typedef float		float32;
-typedef double		float64;
-
-typedef volatile uint8_t	vuint8;
-typedef volatile uint16_t	vuint16;
-typedef volatile uint32_t	vuint32;
-typedef volatile uint64_t	vuint64;
-
-typedef volatile int8_t		vint8;
-typedef volatile int16_t	vint16;
-typedef volatile int32_t	vint32;
-typedef volatile int64_t	vint64;
-
-typedef volatile float          vfloat32;
-typedef volatile float64        vfloat64;
-
-typedef uint8_t		byte;
-
-typedef uint8_t		u8;
-typedef uint16_t	u16;
-typedef uint32_t	u32;
-typedef uint64_t	u64;
-
-typedef int8_t		s8;
-typedef int16_t		s16;
-typedef int32_t		s32;
-typedef int64_t		s64;
-
-typedef volatile u8          vu8;
-typedef volatile u16         vu16;
-typedef volatile u32         vu32;
-typedef volatile u64         vu64;
-
-typedef volatile s8           vs8;
-typedef volatile s16          vs16;
-typedef volatile s32          vs32;
-typedef volatile s64          vs64;
-
-typedef struct touchPosition {
-	int16	x;
-	int16	y;
-	int16	px;
-	int16	py;
-	int16	z1;
-	int16	z2;
-} touchPosition;
-
-
-#ifndef __cplusplus
-/** C++ compatible bool for C
-
-*/
-typedef enum { false, true } bool;
-#endif
-
-// Handy function pointer typedefs
-typedef void ( * IntFn)(void);
-typedef void (* VoidFunctionPointer)(void);
-typedef void (* fp)(void);
-
-//---------------------------------------------------------------------------------
-#endif
-//---------------------------------------------------------------------------------

src/modules/physics/box2d/Source/Dynamics/Contacts/b2CircleContact.cpp

@@ -1,122 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2CircleContact.h"
-#include "../b2Body.h"
-#include "../b2WorldCallbacks.h"
-#include "../../Common/b2BlockAllocator.h"
-
-#include <new>
-#include <string.h>
-
-b2Contact* b2CircleContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2CircleContact));
-	return new (mem) b2CircleContact(shape1, shape2);
-}
-
-void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2CircleContact*)contact)->~b2CircleContact();
-	allocator->Free(contact, sizeof(b2CircleContact));
-}
-
-b2CircleContact::b2CircleContact(b2Shape* s1, b2Shape* s2)
-: b2Contact(s1, s2)
-{
-	b2Assert(m_shape1->GetType() == e_circleShape);
-	b2Assert(m_shape2->GetType() == e_circleShape);
-	m_manifold.pointCount = 0;
-	m_manifold.points[0].normalImpulse = 0.0f;
-	m_manifold.points[0].tangentImpulse = 0.0f;
-}
-
-void b2CircleContact::Evaluate(b2ContactListener* listener)
-{
-	b2Body* b1 = m_shape1->GetBody();
-	b2Body* b2 = m_shape2->GetBody();
-
-	b2Manifold m0;
-	memcpy(&m0, &m_manifold, sizeof(b2Manifold));
-
-	b2CollideCircles(&m_manifold, (b2CircleShape*)m_shape1, b1->GetXForm(), (b2CircleShape*)m_shape2, b2->GetXForm());
-
-	b2ContactPoint cp;
-	cp.shape1 = m_shape1;
-	cp.shape2 = m_shape2;
-	cp.friction = m_friction;
-	cp.restitution = m_restitution;
-
-	if (m_manifold.pointCount > 0)
-	{
-		m_manifoldCount = 1;
-		b2ManifoldPoint* mp = m_manifold.points + 0;
-
-		if (m0.pointCount == 0)
-		{
-			mp->normalImpulse = 0.0f;
-			mp->tangentImpulse = 0.0f;
-
-			if (listener)
-			{
-				cp.position = b1->GetWorldPoint(mp->localPoint1);
-				b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-				b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-				cp.velocity = v2 - v1;
-				cp.normal = m_manifold.normal;
-				cp.separation = mp->separation;
-				cp.id = mp->id;
-				listener->Add(&cp);
-			}
-		}
-		else
-		{
-			b2ManifoldPoint* mp0 = m0.points + 0;
-			mp->normalImpulse = mp0->normalImpulse;
-			mp->tangentImpulse = mp0->tangentImpulse;
-
-			if (listener)
-			{
-				cp.position = b1->GetWorldPoint(mp->localPoint1);
-				b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-				b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-				cp.velocity = v2 - v1;
-				cp.normal = m_manifold.normal;
-				cp.separation = mp->separation;
-				cp.id = mp->id;
-				listener->Persist(&cp);
-			}
-		}
-	}
-	else
-	{
-		m_manifoldCount = 0;
-		if (m0.pointCount > 0 && listener)
-		{
-			b2ManifoldPoint* mp0 = m0.points + 0;
-			cp.position = b1->GetWorldPoint(mp0->localPoint1);
-			b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
-			b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
-			cp.velocity = v2 - v1;
-			cp.normal = m0.normal;
-			cp.separation = mp0->separation;
-			cp.id = mp0->id;
-			listener->Remove(&cp);
-		}
-	}
-}

src/modules/physics/box2d/Source/Dynamics/Contacts/b2Contact.cpp

@@ -1,172 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2Contact.h"
-#include "b2CircleContact.h"
-#include "b2PolyAndCircleContact.h"
-#include "b2PolyContact.h"
-#include "b2ContactSolver.h"
-#include "../../Collision/b2Collision.h"
-#include "../../Collision/Shapes/b2Shape.h"
-#include "../../Common/b2BlockAllocator.h"
-#include "../../Dynamics/b2World.h"
-#include "../../Dynamics/b2Body.h"
-
-b2ContactRegister b2Contact::s_registers[e_shapeTypeCount][e_shapeTypeCount];
-bool b2Contact::s_initialized = false;
-
-void b2Contact::InitializeRegisters()
-{
-	AddType(b2CircleContact::Create, b2CircleContact::Destroy, e_circleShape, e_circleShape);
-	AddType(b2PolyAndCircleContact::Create, b2PolyAndCircleContact::Destroy, e_polygonShape, e_circleShape);
-	AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, e_polygonShape, e_polygonShape);
-}
-
-void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
-					  b2ShapeType type1, b2ShapeType type2)
-{
-	b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
-	b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
-	
-	s_registers[type1][type2].createFcn = createFcn;
-	s_registers[type1][type2].destroyFcn = destoryFcn;
-	s_registers[type1][type2].primary = true;
-
-	if (type1 != type2)
-	{
-		s_registers[type2][type1].createFcn = createFcn;
-		s_registers[type2][type1].destroyFcn = destoryFcn;
-		s_registers[type2][type1].primary = false;
-	}
-}
-
-b2Contact* b2Contact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
-{
-	if (s_initialized == false)
-	{
-		InitializeRegisters();
-		s_initialized = true;
-	}
-
-	b2ShapeType type1 = shape1->GetType();
-	b2ShapeType type2 = shape2->GetType();
-
-	b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
-	b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
-	
-	b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
-	if (createFcn)
-	{
-		if (s_registers[type1][type2].primary)
-		{
-			return createFcn(shape1, shape2, allocator);
-		}
-		else
-		{
-			b2Contact* c = createFcn(shape2, shape1, allocator);
-			for (int32 i = 0; i < c->GetManifoldCount(); ++i)
-			{
-				b2Manifold* m = c->GetManifolds() + i;
-				m->normal = -m->normal;
-			}
-			return c;
-		}
-	}
-	else
-	{
-		return NULL;
-	}
-}
-
-void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	b2Assert(s_initialized == true);
-
-	if (contact->GetManifoldCount() > 0)
-	{
-		contact->GetShape1()->GetBody()->WakeUp();
-		contact->GetShape2()->GetBody()->WakeUp();
-	}
-
-	b2ShapeType type1 = contact->GetShape1()->GetType();
-	b2ShapeType type2 = contact->GetShape2()->GetType();
-
-	b2Assert(e_unknownShape < type1 && type1 < e_shapeTypeCount);
-	b2Assert(e_unknownShape < type2 && type2 < e_shapeTypeCount);
-
-	b2ContactDestroyFcn* destroyFcn = s_registers[type1][type2].destroyFcn;
-	destroyFcn(contact, allocator);
-}
-
-b2Contact::b2Contact(b2Shape* s1, b2Shape* s2)
-{
-	m_flags = 0;
-
-	if (s1->IsSensor() || s2->IsSensor())
-	{
-		m_flags |= e_nonSolidFlag;
-	}
-
-	m_shape1 = s1;
-	m_shape2 = s2;
-
-	m_manifoldCount = 0;
-
-	m_friction = b2MixFriction(m_shape1->GetFriction(), m_shape2->GetFriction());
-	m_restitution = b2MixRestitution(m_shape1->GetRestitution(), m_shape2->GetRestitution());
-	m_prev = NULL;
-	m_next = NULL;
-
-	m_node1.contact = NULL;
-	m_node1.prev = NULL;
-	m_node1.next = NULL;
-	m_node1.other = NULL;
-
-	m_node2.contact = NULL;
-	m_node2.prev = NULL;
-	m_node2.next = NULL;
-	m_node2.other = NULL;
-}
-
-void b2Contact::Update(b2ContactListener* listener)
-{
-	int32 oldCount = GetManifoldCount();
-
-	Evaluate(listener);
-
-	int32 newCount = GetManifoldCount();
-
-	b2Body* body1 = m_shape1->GetBody();
-	b2Body* body2 = m_shape2->GetBody();
-
-	if (newCount == 0 && oldCount > 0)
-	{
-		body1->WakeUp();
-		body2->WakeUp();
-	}
-
-	// Slow contacts don't generate TOI events.
-	if (body1->IsStatic() || body1->IsBullet() || body2->IsStatic() || body2->IsBullet())
-	{
-		m_flags &= ~e_slowFlag;
-	}
-	else
-	{
-		m_flags |= e_slowFlag;
-	}
-}

src/modules/physics/box2d/Source/Dynamics/Contacts/b2Contact.h

@@ -1,183 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 CONTACT_H
-#define CONTACT_H
-
-#include "../../Common/b2Math.h"
-#include "../../Collision/b2Collision.h"
-#include "../../Collision/Shapes/b2Shape.h"
-
-class b2Body;
-class b2Contact;
-class b2World;
-class b2BlockAllocator;
-class b2StackAllocator;
-class b2ContactListener;
-
-typedef b2Contact* b2ContactCreateFcn(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
-typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
-
-struct b2ContactRegister
-{
-	b2ContactCreateFcn* createFcn;
-	b2ContactDestroyFcn* destroyFcn;
-	bool primary;
-};
-
-/// A contact edge is used to connect bodies and contacts together
-/// in a contact graph where each body is a node and each contact
-/// is an edge. A contact edge belongs to a doubly linked list
-/// maintained in each attached body. Each contact has two contact
-/// nodes, one for each attached body.
-struct b2ContactEdge
-{
-	b2Body* other;			///< provides quick access to the other body attached.
-	b2Contact* contact;		///< the contact
-	b2ContactEdge* prev;	///< the previous contact edge in the body's contact list
-	b2ContactEdge* next;	///< the next contact edge in the body's contact list
-};
-
-/// This structure is used to report contact points.
-struct b2ContactPoint
-{
-	b2Shape* shape1;		///< the first shape
-	b2Shape* shape2;		///< the second shape
-	b2Vec2 position;		///< position in world coordinates
-	b2Vec2 velocity;		///< velocity of point on body2 relative to point on body1 (pre-solver)
-	b2Vec2 normal;			///< points from shape1 to shape2
-	float32 separation;		///< the separation is negative when shapes are touching
-	float32 friction;		///< the combined friction coefficient
-	float32 restitution;	///< the combined restitution coefficient
-	b2ContactID id;			///< the contact id identifies the features in contact
-};
-
-/// This structure is used to report contact point results.
-struct b2ContactResult
-{
-	b2Shape* shape1;		///< the first shape
-	b2Shape* shape2;		///< the second shape
-	b2Vec2 position;		///< position in world coordinates
-	b2Vec2 normal;			///< points from shape1 to shape2
-	float32 normalImpulse;	///< the normal impulse applied to body2
-	float32 tangentImpulse;	///< the tangent impulse applied to body2
-	b2ContactID id;			///< the contact id identifies the features in contact
-};
-
-/// The class manages contact between two shapes. A contact exists for each overlapping
-/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
-/// that has no contact points.
-class b2Contact
-{
-public:
-
-	/// Get the manifold array.
-	virtual b2Manifold* GetManifolds() = 0;
-
-	/// Get the number of manifolds. This is 0 or 1 between convex shapes.
-	/// This may be greater than 1 for convex-vs-concave shapes. Each
-	/// manifold holds up to two contact points with a shared contact normal.
-	int32 GetManifoldCount() const;
-
-	/// Is this contact solid?
-	/// @return true if this contact should generate a response.
-	bool IsSolid() const;
-
-	/// Get the next contact in the world's contact list.
-	b2Contact* GetNext();
-
-	/// Get the first shape in this contact.
-	b2Shape* GetShape1();
-
-	/// Get the second shape in this contact.
-	b2Shape* GetShape2();
-
-	//--------------- Internals Below -------------------
-public:
-
-	// m_flags
-	enum
-	{
-		e_nonSolidFlag	= 0x0001,
-		e_slowFlag		= 0x0002,
-		e_islandFlag	= 0x0004,
-		e_toiFlag		= 0x0008,
-	};
-
-	static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
-						b2ShapeType type1, b2ShapeType type2);
-	static void InitializeRegisters();
-	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
-	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
-
-	b2Contact() : m_shape1(NULL), m_shape2(NULL) {}
-	b2Contact(b2Shape* shape1, b2Shape* shape2);
-	virtual ~b2Contact() {}
-
-	void Update(b2ContactListener* listener);
-	virtual void Evaluate(b2ContactListener* listener) = 0;
-	static b2ContactRegister s_registers[e_shapeTypeCount][e_shapeTypeCount];
-	static bool s_initialized;
-
-	uint32 m_flags;
-	int32 m_manifoldCount;
-
-	// World pool and list pointers.
-	b2Contact* m_prev;
-	b2Contact* m_next;
-
-	// Nodes for connecting bodies.
-	b2ContactEdge m_node1;
-	b2ContactEdge m_node2;
-
-	b2Shape* m_shape1;
-	b2Shape* m_shape2;
-
-	// Combined friction
-	float32 m_friction;
-	float32 m_restitution;
-
-	float32 m_toi;
-};
-
-inline int32 b2Contact::GetManifoldCount() const
-{
-	return m_manifoldCount;
-}
-
-inline bool b2Contact::IsSolid() const
-{
-	return (m_flags & e_nonSolidFlag) == 0;
-}
-
-inline b2Contact* b2Contact::GetNext()
-{
-	return m_next;
-}
-
-inline b2Shape* b2Contact::GetShape1()
-{
-	return m_shape1;
-}
-
-inline b2Shape* b2Contact::GetShape2()
-{
-	return m_shape2;
-}
-
-#endif

src/modules/physics/box2d/Source/Dynamics/Contacts/b2ContactSolver.cpp

@@ -1,360 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2ContactSolver.h"
-#include "b2Contact.h"
-#include "../b2Body.h"
-#include "../b2World.h"
-#include "../../Common/b2StackAllocator.h"
-
-b2ContactSolver::b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator)
-{
-	m_step = step;
-	m_allocator = allocator;
-
-	m_constraintCount = 0;
-	for (int32 i = 0; i < contactCount; ++i)
-	{
-		b2Assert(contacts[i]->IsSolid());
-		m_constraintCount += contacts[i]->GetManifoldCount();
-	}
-
-	m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
-
-	int32 count = 0;
-	for (int32 i = 0; i < contactCount; ++i)
-	{
-		b2Contact* contact = contacts[i];
-
-		b2Body* b1 = contact->m_shape1->GetBody();
-		b2Body* b2 = contact->m_shape2->GetBody();
-		int32 manifoldCount = contact->GetManifoldCount();
-		b2Manifold* manifolds = contact->GetManifolds();
-		float32 friction = contact->m_friction;
-		float32 restitution = contact->m_restitution;
-
-		b2Vec2 v1 = b1->m_linearVelocity;
-		b2Vec2 v2 = b2->m_linearVelocity;
-		float32 w1 = b1->m_angularVelocity;
-		float32 w2 = b2->m_angularVelocity;
-
-		for (int32 j = 0; j < manifoldCount; ++j)
-		{
-			b2Manifold* manifold = manifolds + j;
-
-			b2Assert(manifold->pointCount > 0);
-
-			const b2Vec2 normal = manifold->normal;
-
-			b2Assert(count < m_constraintCount);
-			b2ContactConstraint* c = m_constraints + count;
-			c->body1 = b1;
-			c->body2 = b2;
-			c->manifold = manifold;
-			c->normal = normal;
-			c->pointCount = manifold->pointCount;
-			c->friction = friction;
-			c->restitution = restitution;
-
-			for (int32 k = 0; k < c->pointCount; ++k)
-			{
-				b2ManifoldPoint* cp = manifold->points + k;
-				b2ContactConstraintPoint* ccp = c->points + k;
-
-				ccp->normalImpulse = cp->normalImpulse;
-				ccp->tangentImpulse = cp->tangentImpulse;
-				ccp->separation = cp->separation;
-				ccp->positionImpulse = 0.0f;
-
-				ccp->localAnchor1 = cp->localPoint1;
-				ccp->localAnchor2 = cp->localPoint2;
-				ccp->r1 = b2Mul(b1->GetXForm().R, cp->localPoint1 - b1->GetLocalCenter());
-				ccp->r2 = b2Mul(b2->GetXForm().R, cp->localPoint2 - b2->GetLocalCenter());
-
-				float32 r1Sqr = b2Dot(ccp->r1, ccp->r1);
-				float32 r2Sqr = b2Dot(ccp->r2, ccp->r2);
-				float32 rn1 = b2Dot(ccp->r1, normal);
-				float32 rn2 = b2Dot(ccp->r2, normal);
-
-				float32 kNormal = b1->m_invMass + b2->m_invMass;
-				kNormal += b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_invI * (r2Sqr - rn2 * rn2);
-
-				b2Assert(kNormal > B2_FLT_EPSILON);
-				ccp->normalMass = 1.0f / kNormal;
-
-				float32 kEqualized = b1->m_mass * b1->m_invMass + b2->m_mass * b2->m_invMass;
-				kEqualized += b1->m_mass * b1->m_invI * (r1Sqr - rn1 * rn1) + b2->m_mass * b2->m_invI * (r2Sqr - rn2 * rn2);
-
-				b2Assert(kEqualized > B2_FLT_EPSILON);
-				ccp->equalizedMass = 1.0f / kEqualized;
-
-				b2Vec2 tangent = b2Cross(normal, 1.0f);
-
-				float32 rt1 = b2Dot(ccp->r1, tangent);
-				float32 rt2 = b2Dot(ccp->r2, tangent);
-				float32 kTangent = b1->m_invMass + b2->m_invMass;
-				kTangent += b1->m_invI * (r1Sqr - rt1 * rt1) + b2->m_invI * (r2Sqr - rt2 * rt2);
-
-				b2Assert(kTangent > B2_FLT_EPSILON);
-				ccp->tangentMass = 1.0f /  kTangent;
-
-				// Setup a velocity bias for restitution.
-				ccp->velocityBias = 0.0f;
-				if (ccp->separation > 0.0f)
-				{
-					ccp->velocityBias = -60.0f * ccp->separation; // TODO_ERIN b2TimeStep
-				}
-
-				float32 vRel = b2Dot(c->normal, v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1));
-				if (vRel < -b2_velocityThreshold)
-				{
-					ccp->velocityBias += -c->restitution * vRel;
-				}
-			}
-
-			++count;
-		}
-	}
-
-	b2Assert(count == m_constraintCount);
-}
-
-b2ContactSolver::~b2ContactSolver()
-{
-	m_allocator->Free(m_constraints);
-}
-
-void b2ContactSolver::InitVelocityConstraints(const b2TimeStep& step)
-{
-	// Warm start.
-	for (int32 i = 0; i < m_constraintCount; ++i)
-	{
-		b2ContactConstraint* c = m_constraints + i;
-
-		b2Body* b1 = c->body1;
-		b2Body* b2 = c->body2;
-		float32 invMass1 = b1->m_invMass;
-		float32 invI1 = b1->m_invI;
-		float32 invMass2 = b2->m_invMass;
-		float32 invI2 = b2->m_invI;
-		b2Vec2 normal = c->normal;
-		b2Vec2 tangent = b2Cross(normal, 1.0f);
-
-		if (step.warmStarting)
-		{
-			for (int32 j = 0; j < c->pointCount; ++j)
-			{
-				b2ContactConstraintPoint* ccp = c->points + j;
-				ccp->normalImpulse *= step.dtRatio;
-				ccp->tangentImpulse *= step.dtRatio;
-				b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
-				b1->m_angularVelocity -= invI1 * b2Cross(ccp->r1, P);
-				b1->m_linearVelocity -= invMass1 * P;
-				b2->m_angularVelocity += invI2 * b2Cross(ccp->r2, P);
-				b2->m_linearVelocity += invMass2 * P;
-			}
-		}
-		else
-		{
-			for (int32 j = 0; j < c->pointCount; ++j)
-			{
-				b2ContactConstraintPoint* ccp = c->points + j;
-				ccp->normalImpulse = 0.0f;
-				ccp->tangentImpulse = 0.0f;
-			}
-		}
-	}
-}
-
-void b2ContactSolver::SolveVelocityConstraints()
-{
-	for (int32 i = 0; i < m_constraintCount; ++i)
-	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Body* b1 = c->body1;
-		b2Body* b2 = c->body2;
-		float32 w1 = b1->m_angularVelocity;
-		float32 w2 = b2->m_angularVelocity;
-		b2Vec2 v1 = b1->m_linearVelocity;
-		b2Vec2 v2 = b2->m_linearVelocity;
-		float32 invMass1 = b1->m_invMass;
-		float32 invI1 = b1->m_invI;
-		float32 invMass2 = b2->m_invMass;
-		float32 invI2 = b2->m_invI;
-		b2Vec2 normal = c->normal;
-		b2Vec2 tangent = b2Cross(normal, 1.0f);
-		float32 friction = c->friction;
-//#define DEFERRED_UPDATE
-#ifdef DEFERRED_UPDATE
-		b2Vec2 b1_linearVelocity = b1->m_linearVelocity;
-		float32 b1_angularVelocity = b1->m_angularVelocity;
-		b2Vec2 b2_linearVelocity = b2->m_linearVelocity;
-		float32 b2_angularVelocity = b2->m_angularVelocity;
-#endif
-		// Solve normal constraints
-		for (int32 j = 0; j < c->pointCount; ++j)
-		{
-			b2ContactConstraintPoint* ccp = c->points + j;
-
-			// Relative velocity at contact
-			b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
-
-			// Compute normal impulse
-			float32 vn = b2Dot(dv, normal);
-			float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
-
-			// b2Clamp the accumulated impulse
-			float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
-			lambda = newImpulse - ccp->normalImpulse;
-
-			// Apply contact impulse
-			b2Vec2 P = lambda * normal;
-#ifdef DEFERRED_UPDATE
-			b1_linearVelocity -= invMass1 * P;
-			b1_angularVelocity -= invI1 * b2Cross(r1, P);
-
-			b2_linearVelocity += invMass2 * P;
-			b2_angularVelocity += invI2 * b2Cross(r2, P);
-#else
-			v1 -= invMass1 * P;
-			w1 -= invI1 * b2Cross(ccp->r1, P);
-
-			v2 += invMass2 * P;
-			w2 += invI2 * b2Cross(ccp->r2, P);
-#endif
-			ccp->normalImpulse = newImpulse;
-		}
-
-#ifdef DEFERRED_UPDATE
-		b1->m_linearVelocity = b1_linearVelocity;
-		b1->m_angularVelocity = b1_angularVelocity;
-		b2->m_linearVelocity = b2_linearVelocity;
-		b2->m_angularVelocity = b2_angularVelocity;
-#endif
-		// Solve tangent constraints
-		for (int32 j = 0; j < c->pointCount; ++j)
-		{
-			b2ContactConstraintPoint* ccp = c->points + j;
-
-			// Relative velocity at contact
-			b2Vec2 dv = v2 + b2Cross(w2, ccp->r2) - v1 - b2Cross(w1, ccp->r1);
-
-			// Compute tangent force
-			float32 vt = b2Dot(dv, tangent);
-			float32 lambda = ccp->tangentMass * (-vt);
-
-			// b2Clamp the accumulated force
-			float32 maxFriction = friction * ccp->normalImpulse;
-			float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
-			lambda = newImpulse - ccp->tangentImpulse;
-
-			// Apply contact impulse
-			b2Vec2 P = lambda * tangent;
-
-			v1 -= invMass1 * P;
-			w1 -= invI1 * b2Cross(ccp->r1, P);
-
-			v2 += invMass2 * P;
-			w2 += invI2 * b2Cross(ccp->r2, P);
-
-			ccp->tangentImpulse = newImpulse;
-		}
-
-		b1->m_linearVelocity = v1;
-		b1->m_angularVelocity = w1;
-		b2->m_linearVelocity = v2;
-		b2->m_angularVelocity = w2;
-	}
-}
-
-void b2ContactSolver::FinalizeVelocityConstraints()
-{
-	for (int32 i = 0; i < m_constraintCount; ++i)
-	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Manifold* m = c->manifold;
-
-		for (int32 j = 0; j < c->pointCount; ++j)
-		{
-			m->points[j].normalImpulse = c->points[j].normalImpulse;
-			m->points[j].tangentImpulse = c->points[j].tangentImpulse;
-		}
-	}
-}
-
-bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
-{
-	float32 minSeparation = 0.0f;
-
-	for (int32 i = 0; i < m_constraintCount; ++i)
-	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Body* b1 = c->body1;
-		b2Body* b2 = c->body2;
-		float32 invMass1 = b1->m_mass * b1->m_invMass;
-		float32 invI1 = b1->m_mass * b1->m_invI;
-		float32 invMass2 = b2->m_mass * b2->m_invMass;
-		float32 invI2 = b2->m_mass * b2->m_invI;
-		
-		b2Vec2 normal = c->normal;
-
-		// Solver normal constraints
-		for (int32 j = 0; j < c->pointCount; ++j)
-		{
-			b2ContactConstraintPoint* ccp = c->points + j;
-
-			b2Vec2 r1 = b2Mul(b1->GetXForm().R, ccp->localAnchor1 - b1->GetLocalCenter());
-			b2Vec2 r2 = b2Mul(b2->GetXForm().R, ccp->localAnchor2 - b2->GetLocalCenter());
-
-			b2Vec2 p1 = b1->m_sweep.c + r1;
-			b2Vec2 p2 = b2->m_sweep.c + r2;
-			b2Vec2 dp = p2 - p1;
-
-			// Approximate the current separation.
-			float32 separation = b2Dot(dp, normal) + ccp->separation;
-
-			// Track max constraint error.
-			minSeparation = b2Min(minSeparation, separation);
-
-			// Prevent large corrections and allow slop.
-			float32 C = baumgarte * b2Clamp(separation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-
-			// Compute normal impulse
-			float32 dImpulse = -ccp->equalizedMass * C;
-
-			// b2Clamp the accumulated impulse
-			float32 impulse0 = ccp->positionImpulse;
-			ccp->positionImpulse = b2Max(impulse0 + dImpulse, 0.0f);
-			dImpulse = ccp->positionImpulse - impulse0;
-
-			b2Vec2 impulse = dImpulse * normal;
-
-			b1->m_sweep.c -= invMass1 * impulse;
-			b1->m_sweep.a -= invI1 * b2Cross(r1, impulse);
-			b1->SynchronizeTransform();
-
-			b2->m_sweep.c += invMass2 * impulse;
-			b2->m_sweep.a += invI2 * b2Cross(r2, impulse);
-			b2->SynchronizeTransform();
-		}
-	}
-
-	// We can't expect minSpeparation >= -b2_linearSlop because we don't
-	// push the separation above -b2_linearSlop.
-	return minSeparation >= -1.5f * b2_linearSlop;
-}

src/modules/physics/box2d/Source/Dynamics/Contacts/b2PolyAndCircleContact.cpp

@@ -1,158 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2PolyAndCircleContact.h"
-#include "../b2Body.h"
-#include "../b2WorldCallbacks.h"
-#include "../../Common/b2BlockAllocator.h"
-
-#include <new>
-#include <string.h>
-
-b2Contact* b2PolyAndCircleContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2PolyAndCircleContact));
-	return new (mem) b2PolyAndCircleContact(shape1, shape2);
-}
-
-void b2PolyAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2PolyAndCircleContact*)contact)->~b2PolyAndCircleContact();
-	allocator->Free(contact, sizeof(b2PolyAndCircleContact));
-}
-
-b2PolyAndCircleContact::b2PolyAndCircleContact(b2Shape* s1, b2Shape* s2)
-: b2Contact(s1, s2)
-{
-	b2Assert(m_shape1->GetType() == e_polygonShape);
-	b2Assert(m_shape2->GetType() == e_circleShape);
-	m_manifold.pointCount = 0;
-	m_manifold.points[0].normalImpulse = 0.0f;
-	m_manifold.points[0].tangentImpulse = 0.0f;
-}
-
-void b2PolyAndCircleContact::Evaluate(b2ContactListener* listener)
-{
-	b2Body* b1 = m_shape1->GetBody();
-	b2Body* b2 = m_shape2->GetBody();
-
-	b2Manifold m0;
-	memcpy(&m0, &m_manifold, sizeof(b2Manifold));
-
-	b2CollidePolygonAndCircle(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2CircleShape*)m_shape2, b2->GetXForm());
-
-	bool persisted[b2_maxManifoldPoints] = {false, false};
-
-	b2ContactPoint cp;
-	cp.shape1 = m_shape1;
-	cp.shape2 = m_shape2;
-	cp.friction = m_friction;
-	cp.restitution = m_restitution;
-
-	// Match contact ids to facilitate warm starting.
-	if (m_manifold.pointCount > 0)
-	{
-		// Match old contact ids to new contact ids and copy the
-		// stored impulses to warm start the solver.
-		for (int32 i = 0; i < m_manifold.pointCount; ++i)
-		{
-			b2ManifoldPoint* mp = m_manifold.points + i;
-			mp->normalImpulse = 0.0f;
-			mp->tangentImpulse = 0.0f;
-			bool found = false;
-			b2ContactID id = mp->id;
-
-			for (int32 j = 0; j < m0.pointCount; ++j)
-			{
-				if (persisted[j] == true)
-				{
-					continue;
-				}
-
-				b2ManifoldPoint* mp0 = m0.points + j;
-
-				if (mp0->id.key == id.key)
-				{
-					persisted[j] = true;
-					mp->normalImpulse = mp0->normalImpulse;
-					mp->tangentImpulse = mp0->tangentImpulse;
-
-					// A persistent point.
-					found = true;
-
-					// Report persistent point.
-					if (listener != NULL)
-					{
-						cp.position = b1->GetWorldPoint(mp->localPoint1);
-						b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-						b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-						cp.velocity = v2 - v1;
-						cp.normal = m_manifold.normal;
-						cp.separation = mp->separation;
-						cp.id = id;
-						listener->Persist(&cp);
-					}
-					break;
-				}
-			}
-
-			// Report added point.
-			if (found == false && listener != NULL)
-			{
-				cp.position = b1->GetWorldPoint(mp->localPoint1);
-				b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-				b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-				cp.velocity = v2 - v1;
-				cp.normal = m_manifold.normal;
-				cp.separation = mp->separation;
-				cp.id = id;
-				listener->Add(&cp);
-			}
-		}
-
-		m_manifoldCount = 1;
-	}
-	else
-	{
-		m_manifoldCount = 0;
-	}
-
-	if (listener == NULL)
-	{
-		return;
-	}
-
-	// Report removed points.
-	for (int32 i = 0; i < m0.pointCount; ++i)
-	{
-		if (persisted[i])
-		{
-			continue;
-		}
-
-		b2ManifoldPoint* mp0 = m0.points + i;
-		cp.position = b1->GetWorldPoint(mp0->localPoint1);
-		b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
-		b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
-		cp.velocity = v2 - v1;
-		cp.normal = m0.normal;
-		cp.separation = mp0->separation;
-		cp.id = mp0->id;
-		listener->Remove(&cp);
-	}
-}

src/modules/physics/box2d/Source/Dynamics/Contacts/b2PolyContact.cpp

@@ -1,157 +0,0 @@
-/*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
-*
-* 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 "b2PolyContact.h"
-#include "../b2Body.h"
-#include "../b2WorldCallbacks.h"
-#include "../../Common/b2BlockAllocator.h"
-
-#include <memory>
-#include <new>
-#include <string.h>
-
-b2Contact* b2PolygonContact::Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator)
-{
-	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
-	return new (mem) b2PolygonContact(shape1, shape2);
-}
-
-void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
-{
-	((b2PolygonContact*)contact)->~b2PolygonContact();
-	allocator->Free(contact, sizeof(b2PolygonContact));
-}
-
-b2PolygonContact::b2PolygonContact(b2Shape* s1, b2Shape* s2)
-	: b2Contact(s1, s2)
-{
-	b2Assert(m_shape1->GetType() == e_polygonShape);
-	b2Assert(m_shape2->GetType() == e_polygonShape);
-	m_manifold.pointCount = 0;
-}
-
-void b2PolygonContact::Evaluate(b2ContactListener* listener)
-{
-	b2Body* b1 = m_shape1->GetBody();
-	b2Body* b2 = m_shape2->GetBody();
-
-	b2Manifold m0;
-	memcpy(&m0, &m_manifold, sizeof(b2Manifold));
-
-	b2CollidePolygons(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2PolygonShape*)m_shape2, b2->GetXForm());
-
-	bool persisted[b2_maxManifoldPoints] = {false, false};
-
-	b2ContactPoint cp;
-	cp.shape1 = m_shape1;
-	cp.shape2 = m_shape2;
-	cp.friction = m_friction;
-	cp.restitution = m_restitution;
-
-	// Match contact ids to facilitate warm starting.
-	if (m_manifold.pointCount > 0)
-	{
-		// Match old contact ids to new contact ids and copy the
-		// stored impulses to warm start the solver.
-		for (int32 i = 0; i < m_manifold.pointCount; ++i)
-		{
-			b2ManifoldPoint* mp = m_manifold.points + i;
-			mp->normalImpulse = 0.0f;
-			mp->tangentImpulse = 0.0f;
-			bool found = false;
-			b2ContactID id = mp->id;
-
-			for (int32 j = 0; j < m0.pointCount; ++j)
-			{
-				if (persisted[j] == true)
-				{
-					continue;
-				}
-
-				b2ManifoldPoint* mp0 = m0.points + j;
-
-				if (mp0->id.key == id.key)
-				{
-					persisted[j] = true;
-					mp->normalImpulse = mp0->normalImpulse;
-					mp->tangentImpulse = mp0->tangentImpulse;
-
-					// A persistent point.
-					found = true;
-
-					// Report persistent point.
-					if (listener != NULL)
-					{
-						cp.position = b1->GetWorldPoint(mp->localPoint1);
-						b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-						b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-						cp.velocity = v2 - v1;
-						cp.normal = m_manifold.normal;
-						cp.separation = mp->separation;
-						cp.id = id;
-						listener->Persist(&cp);
-					}
-					break;
-				}
-			}
-
-			// Report added point.
-			if (found == false && listener != NULL)
-			{
-				cp.position = b1->GetWorldPoint(mp->localPoint1);
-				b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
-				b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
-				cp.velocity = v2 - v1;
-				cp.normal = m_manifold.normal;
-				cp.separation = mp->separation;
-				cp.id = id;
-				listener->Add(&cp);
-			}
-		}
-
-		m_manifoldCount = 1;
-	}
-	else
-	{
-		m_manifoldCount = 0;
-	}
-
-	if (listener == NULL)
-	{
-		return;
-	}
-
-	// Report removed points.
-	for (int32 i = 0; i < m0.pointCount; ++i)
-	{
-		if (persisted[i])
-		{
-			continue;
-		}
-
-		b2ManifoldPoint* mp0 = m0.points + i;
-		cp.position = b1->GetWorldPoint(mp0->localPoint1);
-		b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1);
-		b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2);
-		cp.velocity = v2 - v1;
-		cp.normal = m0.normal;
-		cp.separation = mp0->separation;
-		cp.id = mp0->id;
-		listener->Remove(&cp);
-	}
-}