Initial Mercurial commit.

src/modules/physics/box2d/Body.cpp

@@ -0,0 +1,286 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "Body.h"
+
+#include <common/math.h>
+
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Body::Body(World * world, b2Vec2 p, float m, float i)
+		: world(world)
+	{
+		world->retain();
+		b2BodyDef def;
+		def.position = world->scaleDown(p);
+		def.massData.mass = m;
+		def.massData.I = i;
+		body = world->world->CreateBody(&def);
+	}
+
+	Body::~Body()
+	{
+		world->world->DestroyBody(body);
+		world->release();
+		body = 0;
+	}
+
+	float Body::getX()
+	{
+		return world->scaleUp(body->GetPosition().x);
+	}
+
+	float Body::getY()
+	{
+		return world->scaleUp(body->GetPosition().y);
+	}
+
+	int Body::getPosition(lua_State * L)
+	{
+		return pushVector(L, world->scaleUp(body->GetPosition()));
+	}
+
+	int Body::getVelocity(lua_State * L)
+	{
+		return pushVector(L, world->scaleUp(body->GetLinearVelocity()));
+	}
+
+	float Body::getAngle()
+	{
+		return body->GetAngle();
+	}
+
+	int Body::getWorldCenter(lua_State * L)
+	{
+		return pushVector(L, world->scaleUp(body->GetWorldCenter()));
+	}
+
+	int Body::getLocalCenter(lua_State * L)
+	{
+		return pushVector(L, world->scaleUp(body->GetLocalCenter()));
+	}
+
+	float Body::getSpin() const
+	{
+		return body->GetAngularVelocity();
+	}
+
+	float Body::getMass() const
+	{
+		return body->GetMass();
+	}
+
+	float Body::getInertia() const
+	{
+		return body->GetInertia();
+	}
+
+	float Body::getAngularDamping() const
+	{
+		return body->m_angularDamping;
+	}
+
+	float Body::getDamping() const
+	{
+		return body->m_linearDamping;
+	}
+
+	void Body::applyImpulse(float jx, float jy)
+	{
+		body->ApplyImpulse(b2Vec2(jx, jy), world->scaleDown(body->GetWorldCenter()));
+	}
+
+	void Body::applyImpulse(float jx, float jy, float rx, float ry)
+	{
+		body->ApplyImpulse(b2Vec2(jx, jy), world->scaleDown(b2Vec2(rx, ry)));
+	}
+
+	void Body::applyTorque(float t)
+	{
+		body->ApplyTorque(t);
+	}
+
+	void Body::applyForce(float fx, float fy, float rx, float ry)
+	{
+		body->ApplyForce(b2Vec2(fx, fy), world->scaleDown(b2Vec2(rx, ry)));
+	}
+
+	void Body::applyForce(float fx, float fy)
+	{
+		body->ApplyForce(b2Vec2(fx, fy), world->scaleDown(body->GetWorldCenter()));
+	}
+
+	void Body::setX(float x)
+	{
+		body->SetXForm(world->scaleDown(b2Vec2(x, getY())), getAngle());
+	}
+
+	void Body::setY(float y)
+	{
+		body->SetXForm(world->scaleDown(b2Vec2(getX(), y)), getAngle());
+	}
+
+	void Body::setVelocity(float x, float y)
+	{
+		body->SetLinearVelocity(world->scaleDown(b2Vec2(x, y)));
+	}
+
+	void Body::setAngle(float d)
+	{
+		body->SetXForm(body->GetPosition(), d);
+	}
+
+	void Body::setSpin(float r)
+	{
+		body->SetAngularVelocity(r);
+	}
+
+	void Body::setPosition(float x, float y)
+	{
+		body->SetXForm(world->scaleDown(b2Vec2(x, y)), body->GetAngle());
+	}
+
+	void Body::setAngularDamping(float d)
+	{
+		body->m_angularDamping = d;
+	}
+
+	void Body::setDamping(float d)
+	{
+		body->m_linearDamping = d;
+	}
+
+	void Body::setMassFromShapes()
+	{
+		body->SetMassFromShapes();
+	}
+
+	void Body::setMass(float x, float y, float m, float i)
+	{
+		b2MassData massData;
+		massData.center = world->scaleDown(b2Vec2(x, y));
+		massData.mass = m;
+		massData.I = i;
+		body->SetMass(&massData);
+	}
+
+	int Body::getWorldPoint(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetWorldPoint(v)));
+	}
+
+	int Body::getWorldVector(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetWorldVector(v)));
+	}
+
+	int Body::getLocalPoint(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetLocalPoint(v)));
+	}
+
+	int Body::getLocalVector(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetLocalVector(v)));
+	}
+
+	int Body::getVelocityWorldPoint(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetLinearVelocityFromWorldPoint(v)));
+	}
+
+	int Body::getVelocityLocalPoint(lua_State * L)
+	{
+		b2Vec2 v = world->scaleDown(getVector(L));
+		return pushVector(L, world->scaleUp(body->GetLinearVelocityFromLocalPoint(v)));
+	}
+
+	bool Body::isBullet() const
+	{
+		return body->IsBullet();
+	}
+
+	void Body::setBullet(bool bullet)
+	{
+		return body->SetBullet(bullet);
+	}
+
+	bool Body::isStatic() const
+	{
+		return body->IsStatic();
+	}
+
+	bool Body::isDynamic() const
+	{
+		return body->IsDynamic();
+	}
+
+	bool Body::isFrozen() const
+	{
+		return body->IsFrozen();
+	}
+
+	bool Body::isSleeping() const
+	{
+		return body->IsSleeping();
+	}
+
+	void Body::setAllowSleep(bool allow)
+	{
+		body->AllowSleeping(true);
+	}
+
+	void Body::setSleep(bool sleep)
+	{
+		if(sleep)
+			body->PutToSleep();
+		else
+			body->WakeUp();
+	}
+
+	b2Vec2 Body::getVector(lua_State * L)
+	{
+		love::luax_assert_argc(L, 2, 2);
+		b2Vec2 v((float)lua_tonumber(L, 1), (float)lua_tonumber(L, 2));
+		lua_pop(L, 2);
+		return v;
+	}
+
+	int Body::pushVector(lua_State * L, const b2Vec2 & v)
+	{
+		lua_pushnumber(L, v.x);
+		lua_pushnumber(L, v.y);
+		return 2;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/Body.h

@@ -0,0 +1,349 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_BODY_H
+#define LOVE_PHYSICS_BOX2D_BODY_H
+
+// LOVE
+#include <common/runtime.h>
+#include <common/Object.h>
+
+
+// Box2D
+#include "Include/Box2D.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	// Forward declarations.
+	class World;
+
+	/**
+	* A Body is an entity which has position and orientation
+	* in world space. A Body does have collision geometry 
+	* by itself, but depend on an arbitrary number of child Shape objects 
+	* which together constitute the final geometry for the Body.
+	**/
+	class Body : public Object
+	{
+		// Friends.
+		friend class Joint;
+		friend class DistanceJoint;
+		friend class MouseJoint;
+		friend class CircleShape;
+		friend class PolygonShape;
+		friend class Shape;
+
+	private:
+
+		// We need a shared_ptr to the parent World, 
+		// because World can not be destroyed as long as
+		// bodies exists in it.
+		//
+		// This ensures that a World only can be destroyed
+		// once all bodies have been destroyed too.
+		World * world;
+		
+	public:
+
+		// The Box2D body. (Should not be public?)
+		b2Body * body;
+
+		/**
+		* Create a Body at position p.
+		**/ 
+		Body(World * world, b2Vec2 p, float m, float i);
+
+		~Body();
+
+		/**
+		* Gets the current x-position of the Body.
+		**/
+		float getX();
+
+		/**
+		* Gets the current y-position of the Body.
+		**/
+		float getY();
+
+		/**
+		* Gets the current angle (deg) of the Body.
+		**/
+		float getAngle();
+
+		/**
+		* Gets the current position of the Body.
+		* @returns The current x-position.
+		* @returns The current y-position.
+		**/
+		int getPosition(lua_State * L);
+
+		/**
+		* Gets the velocity in the current center of mass.
+		* @returns The x-component of the velocity.
+		* @returns The y-component of the velocity.
+		**/
+		int getVelocity(lua_State * L);
+
+		/**
+		* The current center of mass for the Body in world
+		* coordinates.
+		* @returns The x-component of the point.
+		* @returns The y-component of the point.
+		**/
+		int getWorldCenter(lua_State * L);
+
+		/**
+		* The current center of mass for the Body in local
+		* coordinates.
+		* @returns The x-component of the point.
+		* @returns The y-component of the point.
+		**/
+		int getLocalCenter(lua_State * L);
+
+		/**
+		* Get the current Body spin. (Angular velocity).
+		**/
+		float getSpin() const;
+
+		/**
+		* Gets the Body's mass.
+		**/
+		float getMass() const;
+
+		/**
+		* Gets the Body's intertia.
+		**/
+		float getInertia() const;
+
+		/**
+		* Gets the Body's angular damping.
+		**/
+		float getAngularDamping() const;
+
+		/**
+		* Gets the Body's linear damping.
+		**/
+		float getDamping() const;
+
+		/**
+		* Apply an impulse (jx, jy) with offset (0, 0).
+		**/
+		void applyImpulse(float jx, float jy);
+
+		/**
+		* Apply an impulse (jx, jy) with offset (rx, ry).
+		**/
+		void applyImpulse(float jx, float jy, float rx, float ry);
+
+		/**
+		* Apply torque (t).
+		**/
+		void applyTorque(float t);
+
+		/**
+		* Apply force (fx, fy) with offset (0, 0).
+		**/
+		void applyForce(float fx, float fy);
+
+		/**
+		* Apply force (fx, fy) with offset (rx, ry).
+		**/
+		void applyForce(float fx, float fy, float rx, float ry);
+	
+		/**
+		* Sets the x-position of the Body.
+		**/
+		void setX(float x);
+
+		/**
+		* Sets the Y-position of the Body.
+		**/
+		void setY(float y);
+
+		/**
+		* Sets the current velocity of the Body.
+		**/
+		void setVelocity(float x, float y);
+
+		/**
+		* Sets the angle of the Body.
+		**/
+		void setAngle(float d);
+
+		/**
+		* Sets the current spin of the Body. 
+		**/
+		void setSpin(float r);
+
+		/**
+		* Sets the current position of the Body.
+		**/ 
+		void setPosition(float x, float y);
+
+		/**
+		* Sets the mass from the currently attatched shapes.
+		**/ 
+		void setMassFromShapes();
+
+		/**
+		* Sets mass properties.
+		* @param x The x-coordinate for the local center of mass.
+		* @param y The y-coordinate for the local center of mass.
+		* @param m The mass.
+		* @param i The inertia.
+		**/
+		void setMass(float x, float y, float m, float i);
+
+		/**
+		* Sets the Body's angular damping.
+		**/
+		void setAngularDamping(float d);
+
+		/**
+		* Sets the Body's linear damping.
+		**/
+		void setDamping(float d);
+
+		/**
+		* Transforms a point (x, y) from local coordinates
+		* to world coordinates.
+		* @param x The x-coordinate of the local point.
+		* @param y The y-coordinate of the local point.
+		* @returns The x-coordinate of the point in world coordinates.
+		* @returns The y-coordinate of the point in world coordinates.
+		**/
+		int getWorldPoint(lua_State * L);
+
+		/**
+		* Transforms a vector (x, y) from local coordinates
+		* to world coordinates.
+		* @param x The x-coordinate of the local vector.
+		* @param y The y-coordinate of the local vector.
+		* @returns The x-coordinate of the vector in world coordinates.
+		* @returns The y-coordinate of the vector in world coordinates.
+		**/
+		int getWorldVector(lua_State * L);
+
+		/**
+		* Transforms a point (x, y) from world coordinates
+		* to local coordinates.
+		* @param x The x-coordinate of the world point.
+		* @param y The y-coordinate of the world point.
+		* @returns The x-coordinate of the point in local coordinates.
+		* @returns The y-coordinate of the point in local coordinates.
+		**/
+		int getLocalPoint(lua_State * L);
+
+		/**
+		* Transforms a vector (x, y) from world coordinates
+		* to local coordinates.
+		* @param x The x-coordinate of the world vector.
+		* @param y The y-coordinate of the world vector.
+		* @returns The x-coordinate of the vector in local coordinates.
+		* @returns The y-coordinate of the vector in local coordinates.
+		**/
+		int getLocalVector(lua_State * L);
+
+		/**
+		* Gets the velocity on the Body for the given world point.
+		* @param x The x-coordinate of the world point.
+		* @param y The y-coordinate of the world point.
+		* @returns The x-component of the velocity vector.
+		* @returns The y-component of the velocity vector.
+		**/
+		int getVelocityWorldPoint(lua_State * L);
+
+		/**
+		* Gets the velocity on the Body for the given local point.
+		* @param x The x-coordinate of the local point.
+		* @param y The y-coordinate of the local point.
+		* @returns The x-component of the velocity vector.
+		* @returns The y-component of the velocity vector.
+		**/
+		int getVelocityLocalPoint(lua_State * L);
+
+		/**
+		* Returns true if the Body is a bullet, false otherwise.
+		**/
+		bool isBullet() const;
+
+		/**
+		* Set whether this Body should be treated as a bullet.
+		* Bullets require more processing power than normal shapes.
+		**/
+		void setBullet(bool bullet);
+
+		/**
+		* Checks whether a Body is static or not, i.e. terrain
+		* or not.
+		**/
+		bool isStatic() const;
+
+		/**
+		* The opposite of isStatic.
+		**/
+		bool isDynamic() const;
+
+		/**
+		* Checks whether a Body is frozen or not.
+		* A Body will freeze if hits the world bounding box.
+		**/
+		bool isFrozen() const;
+
+		/**
+		* Checks whether a Body is sleeping or nor. A Body
+		* will fall to sleep if nothing happens to it for while.
+		**/
+		bool isSleeping() const;
+
+		/**
+		* Controls whether this Body should be allowed to sleep.
+		**/ 
+		void setAllowSleep(bool allow);
+
+		/**
+		* Controls the Body's sleep.
+		* @param sleep True to put to sleep, false to wake up.
+		**/
+		void setSleep(bool sleep);
+
+	private:
+
+		/**
+		* Gets a 2d vector from the arguments on the stack.
+		**/
+		b2Vec2 getVector(lua_State * L);
+
+		/**
+		* Pushed the x- and y-components of a vector on 
+		* the stack.
+		**/
+		int pushVector(lua_State * L, const b2Vec2 & v);
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_BODY_H

src/modules/physics/box2d/CircleShape.cpp

@@ -0,0 +1,56 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "CircleShape.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	CircleShape::CircleShape(Body * body, b2CircleDef * def)
+		: Shape(body), radius(def->radius)
+	{
+		def->localPosition = body->world->scaleDown(def->localPosition);
+		def->radius = body->world->scaleDown(def->radius);
+		radius = def->radius;
+		shape = body->body->CreateShape(def);
+		shape->SetUserData((void*)data);
+	}
+
+	CircleShape::~CircleShape()
+	{
+		body->body->DestroyShape(shape);
+		shape = 0;
+	}
+
+	float CircleShape::getRadius() const
+	{
+		return body->world->scaleUp(radius);
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/CircleShape.h

@@ -0,0 +1,72 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_CIRCLE_SHAPE_H
+#define LOVE_PHYSICS_BOX2D_CIRCLE_SHAPE_H
+
+// Module
+#include "Shape.h"
+
+#include <vector>
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* A CircleShape represent a Circle which can 
+	* be used for collision detection and physics.
+	* 
+	* The CircleShape is much faster than the PolygonShape, 
+	* and should generally be used where possible.
+	**/
+	class CircleShape : public Shape
+	{
+	private:
+		// The radius of the circle. We need to store this because
+		// Box2D has no built-in method for getting the radius.
+		float radius;
+	public:
+		
+		/**
+		* Create a new CircleShape from the parent body and a 
+		* Box2D CircleShape definition.
+		* @param body The parent body.
+		* @param def The CircleShape definition.
+		**/
+		CircleShape(Body * body, b2CircleDef * def);
+
+		virtual ~CircleShape();
+
+		/**
+		* Gets the radius for the circle.
+		**/
+		float getRadius() const;
+
+		// There is no support for setting the radius.
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_CIRCLE_SHAPE_H

src/modules/physics/box2d/Contact.cpp

@@ -0,0 +1,82 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "Contact.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Contact::Contact(World * world, const b2ContactPoint * point)
+		: point(*point), world(world)
+	{
+		world->retain();
+	}
+
+	Contact::~Contact()
+	{
+		world->release();
+	}
+
+	int Contact::getPosition(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0, 0);
+		lua_pushnumber(L, world->scaleUp(point.position.x));
+		lua_pushnumber(L, world->scaleUp(point.position.y));
+		return 2;
+	}
+
+	int Contact::getVelocity(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0, 0);
+		lua_pushnumber(L, world->scaleUp(point.velocity.x));
+		lua_pushnumber(L, world->scaleUp(point.velocity.y));
+		return 2;
+	}
+
+	int Contact::getNormal(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0, 0);
+		lua_pushnumber(L, world->scaleUp(point.normal.x));
+		lua_pushnumber(L, world->scaleUp(point.normal.y));
+		return 2;
+	}
+
+	float Contact::getSeparation() const
+	{
+		return world->scaleUp(point.separation);
+	}
+
+	float Contact::getFriction() const
+	{
+		return point.friction;
+	}
+
+	float Contact::getRestitution() const
+	{
+		return point.restitution;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/Contact.h

@@ -0,0 +1,113 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_CONTACT_H
+#define LOVE_PHYSICS_BOX2D_CONTACT_H
+
+// LOVE
+#include <common/Object.h>
+#include <common/runtime.h>
+
+// Box2D
+#include "Include/Box2D.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	class World;
+
+	/**
+	* A Contact represents a collision point between
+	* two shapes.
+	**/
+	class Contact : public Object
+	{
+		// Friends.
+		friend class World;
+
+	private:
+
+		// The Box2D contact point.
+		b2ContactPoint point;
+
+		// The parent world. Needed for scaling.
+		World * world;
+
+	public:
+
+		/**
+		* Creates a new Contact by copying a Box2D contact
+		* point. It does not store the pointer, but copy the
+		* data pointed to.
+		* @param point Pointer to the Box2D contact.
+		**/
+		Contact(World * world, const b2ContactPoint * point);
+
+		~Contact();
+
+		/**
+		* Gets the position of the Contact.
+		* @return The position along the x-axis.
+		* @return The position along the y-axis.
+		**/
+		int getPosition(lua_State * L);
+
+		/**
+		* Gets the linear impact velocity.
+		* @return The velocity along the x-axis.
+		* @return The velocity along the y-axis.
+		**/
+		int getVelocity(lua_State * L);
+
+		/**
+		* Gets the collision normal.
+		* @return The x-component of the normal.
+		* @return The y-component of the normal.
+		**/
+		int getNormal(lua_State * L);
+
+		/**
+		* How far apart the shapes are. If they are intersecting
+		* this value is negative.
+		**/
+		float getSeparation() const;
+
+		/**
+		* The mixed friction between the two shapes at
+		* the point of impact.
+		**/
+		float getFriction() const;
+
+		/**
+		* The mixed restitution of the two shapes
+		* at the point of impact.
+		**/
+		float getRestitution() const;
+
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_CONTACT_H

src/modules/physics/box2d/DistanceJoint.cpp

@@ -0,0 +1,80 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "DistanceJoint.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	DistanceJoint::DistanceJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2)
+		: Joint(body1, body2)
+	{
+		b2DistanceJointDef def;
+		def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x1,y1)), world->scaleDown(b2Vec2(x2,y2)));
+		joint = (b2DistanceJoint*)createJoint(&def);
+	}
+
+	DistanceJoint::~DistanceJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+
+	void DistanceJoint::setLength(float length)
+	{
+		joint->m_length = world->scaleDown(length);
+	}
+
+	float DistanceJoint::getLength() const
+	{
+		return world->scaleUp(joint->m_length);
+	}
+
+	void DistanceJoint::setFrequency(float hz)
+	{
+		joint->m_frequencyHz = hz;
+	}
+
+	float DistanceJoint::getFrequency() const
+	{
+		return joint->m_frequencyHz;
+	}
+
+	void DistanceJoint::setDamping(float d)
+	{
+		joint->m_dampingRatio = d;
+	}
+
+	float DistanceJoint::getDamping() const
+	{
+		return joint->m_dampingRatio;
+	}
+
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/DistanceJoint.h

@@ -0,0 +1,89 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_DISTANCE_JOINT_H
+#define LOVE_PHYSICS_BOX2D_DISTANCE_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* The DistanceJoint keeps Bodies at a fixed distance 
+	* from eachother.
+	**/
+	class DistanceJoint : public Joint
+	{
+	private:
+		// The Box2D DistanceJoint object.
+		b2DistanceJoint * joint;
+	public:
+
+		/**
+		* Creates a DistanceJoint connecting body1 to body2. 
+		**/
+		DistanceJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2);
+		
+		virtual ~DistanceJoint();
+
+		/**
+		* Sets the equilibrium distance between the two bodies.
+		**/
+		void setLength(float length);
+
+		/**
+		* Gets the equilibrium distance between the two bodies.
+		**/
+		float getLength() const;
+
+		/**
+		* Sets the response speed.
+		**/
+		void setFrequency(float hz);
+
+		/**
+		* Gets the response speed.
+		**/
+		float getFrequency() const;
+
+		/**
+		* Sets the damping ratio.
+		* 0 = no damping, 1 = critical damping.
+		**/
+		void setDamping(float d);
+
+		/**
+		* Gets the damping ratio.
+		* 0 = no damping, 1 = critical damping.
+		**/
+		float getDamping() const;
+
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_DISTANCE_JOINT_H

src/modules/physics/box2d/GearJoint.cpp

@@ -0,0 +1,64 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "GearJoint.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	GearJoint::GearJoint(Joint * joint1, Joint * joint2, float ratio)
+		: Joint(joint1->body2, joint2->body2)
+	{
+		b2GearJointDef def;
+		def.joint1 = joint1->joint;
+		def.joint2 = joint2->joint;
+		def.body1 = joint1->body2->body;
+		def.body2 = joint2->body2->body;
+		def.ratio = ratio;
+		
+		joint = (b2GearJoint*)createJoint(&def);
+	}
+
+	GearJoint::~GearJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+	
+	void GearJoint::setRatio(float ratio)
+	{
+		joint->m_ratio = ratio;
+	}
+	
+	float GearJoint::getRatio() const
+	{
+		return joint->m_ratio;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/GearJoint.h

@@ -0,0 +1,75 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_GEAR_JOINT_H
+#define LOVE_PHYSICS_BOX2D_GEAR_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* A gear joint is used to connect two joints together. Either joint
+	* can be a revolute or prismatic joint. You specify a gear ratio
+	* to bind the motions together:
+	* coordinate1 + ratio * coordinate2 = constant
+	* 
+	* The ratio can be negative or positive. If one joint is a revolute joint
+	* and the other joint is a prismatic joint, then the ratio will have units
+	* of length or units of 1/length.
+	* Warning: The revolute and prismatic joints must be attached to
+	* fixed bodies (which must be body1 on those joints).
+	**/
+	class GearJoint : public Joint
+	{
+	private:
+		// The Box2D GearJoint object.
+		b2GearJoint * joint;
+	public:
+
+		/**
+		* Creates a GearJoint connecting joint1 to joint2. 
+		**/
+		GearJoint(Joint * joint1, Joint * joint2, float ratio);
+		
+		virtual ~GearJoint();
+		
+		/**
+		* Sets the ratio.
+		**/
+		void setRatio(float ratio);
+		
+		/**
+		* Gets the ratio.
+		**/
+		float getRatio() const;
+		
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_GEAR_JOINT_H

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

@@ -0,0 +1,52 @@
+/*
+* 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.cpp

@@ -0,0 +1,125 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "Joint.h"
+
+// STD
+#include <bitset>
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Joint::Joint(Body * body1)
+		: body1(body1), body2(0), world(body1->world)
+	{
+		body1->retain();
+	}
+
+	Joint::Joint(Body * body1, Body * body2)
+		: body1(body1), body2(body2), world(body1->world)
+	{
+		body1->retain();
+		body2->retain();
+	}
+
+	Joint::~Joint()
+	{
+		if(body1 != 0)
+			body1->release();
+		if(body2 != 0)
+			body2->release();
+
+		joint = 0;
+	}
+
+	int Joint::getType() const
+	{
+		switch(joint->GetType())
+		{
+		case e_revoluteJoint: 
+			return love::JOINT_REVOLUTE;
+		case e_prismaticJoint: 
+			return love::JOINT_PRISMATIC;
+		case e_distanceJoint: 
+			return love::JOINT_DISTANCE;
+		case e_pulleyJoint: 
+			return love::JOINT_PULLEY;
+		case e_mouseJoint: 
+			return love::JOINT_MOUSE;
+		case e_gearJoint: 
+			return love::JOINT_GEAR;
+		default:
+			return -1;
+		}
+	}
+
+	int Joint::getAnchors(lua_State * L)
+	{
+		lua_pushnumber(L, world->scaleUp(joint->GetAnchor1().x));
+		lua_pushnumber(L, world->scaleUp(joint->GetAnchor1().y));
+		lua_pushnumber(L, world->scaleUp(joint->GetAnchor2().x));
+		lua_pushnumber(L, world->scaleUp(joint->GetAnchor2().y));
+		return 4;
+	}
+
+	int Joint::getReactionForce(lua_State * L)
+	{
+		b2Vec2 v = joint->GetReactionForce();
+		lua_pushnumber(L, v.x);
+		lua_pushnumber(L, v.y);
+		return 2;
+	}
+
+	float Joint::getReactionTorque()
+	{
+		return joint->GetReactionTorque();
+	}
+
+	void Joint::setCollideConnected(bool collide)
+	{
+		joint->m_collideConnected = collide;
+	}
+
+	bool Joint::getCollideConnected() const
+	{
+		return joint->m_collideConnected;
+	}
+
+	b2Joint * Joint::createJoint(b2JointDef * def)
+	{
+		joint = world->world->CreateJoint(def);
+		return joint;
+	}
+
+	void Joint::destroyJoint(b2Joint * joint)
+	{
+		world->world->DestroyJoint(joint);
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/Joint.h

@@ -0,0 +1,140 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_JOINT_H
+#define LOVE_PHYSICS_BOX2D_JOINT_H
+
+// LOVE
+#include <common/runtime.h>
+#include <common/Object.h>
+#include <common/constants.h>
+
+// Box2D
+#include "Include/Box2D.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	// Forward declarations.
+	class Body;
+	class World;
+
+	/**
+	* A Joint acts as positioning constraints on Bodies. 
+	* A Joint can be used to prevent Bodies from going to 
+	* far apart, or coming too close together.
+	**/
+	class Joint : public Object
+	{
+		friend class GearJoint;
+		
+	private:
+		
+		// A Joint must be destroyed *before* the bodies it acts upon, 
+		// and the world they reside in. We therefore need refs
+		// parents and associations to prevent wrong destruction order.
+		Body * body1, * body2;
+		
+
+		// The Box2D joint object.
+		b2Joint * joint;
+
+	protected:
+		World * world;
+
+	public:
+
+		/**
+		* This constructor will connect one end of the joint to body1, 
+		* and the other one to the default ground body.
+		* 
+		* This constructor is mainly used by MouseJoint.
+		**/
+		Joint(Body * body1);
+
+		/**
+		* Create a joint between body1 and body2.
+		**/
+		Joint(Body * body1, Body * body2);
+
+		virtual ~Joint();
+
+		/**
+		* Gets the type of joint.
+		**/
+		int getType() const;
+
+		/**
+		* Gets the anchor positions of the Joint in world
+		* coordinates. This is useful for debugdrawing the joint.
+		**/
+		int getAnchors(lua_State * L);
+
+		/**
+		* Gets the reaction force on body2 at the joint anchor.
+		**/
+		int getReactionForce(lua_State * L);
+
+		/**
+		* Gets the reaction torque on body2.
+		**/
+		float getReactionTorque();
+
+		/**
+		* Sets whether connected bodies should collide
+		* or not. Default is false.
+		**/
+		void setCollideConnected(bool collide);
+
+		/**
+		* Gets whether connected bodies should collide
+		* or not.
+		**/
+		bool getCollideConnected() const;
+
+	protected:
+
+		/**
+		* Joints require pointers to a Box2D joint objects at
+		* different polymorphic levels, which is why these function 
+		* were created.
+		**/
+
+		/**
+		* Creates a Joint, and ensures that the parent class
+		* gets a copy of the pointer.
+		**/
+		b2Joint * createJoint(b2JointDef * def);
+
+		/**
+		* Destroys the joint. This function was created just to 
+		* get some cinsistency.
+		**/
+		void destroyJoint(b2Joint * joint);
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_JOINT_H

src/modules/physics/box2d/MouseJoint.cpp

@@ -0,0 +1,75 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "MouseJoint.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	MouseJoint::MouseJoint(Body * body1, float x, float y)
+		: Joint(body1)
+	{
+		b2MouseJointDef def;
+		
+		def.body1 = body1->world->world->GetGroundBody();
+		def.body2 = body1->body;
+		def.maxForce = 1000.0f * body1->body->GetMass();
+		def.target = body1->world->scaleDown(b2Vec2(x,y));
+		joint = (b2MouseJoint*)createJoint(&def);
+	}
+
+	MouseJoint::~MouseJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+
+	void MouseJoint::setTarget(float x, float y)
+	{
+		joint->SetTarget(world->scaleDown(b2Vec2(x, y)));
+	}
+
+	int MouseJoint::getTarget(lua_State * L)
+	{
+		lua_pushnumber(L, world->scaleUp(joint->m_target.x));
+		lua_pushnumber(L, world->scaleUp(joint->m_target.y));
+		return 2;
+	}
+
+	void MouseJoint::setMaxForce(float force)
+	{
+		joint->m_maxForce = force;
+	}
+
+	float MouseJoint::getMaxForce() const
+	{
+		return joint->m_maxForce;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/MouseJoint.h

@@ -0,0 +1,84 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_MOUSE_JOINT_H
+#define LOVE_PHYSICS_BOX2D_MOUSE_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* The MouseJoint is a joint type that 
+	* is suitable for controlling objects with the mouse.
+	*
+	* One end is anchored in the dynamic body, and the other id
+	* anchor to a static ground body. The anchor offset can then be
+	* moved to the current mouse position.
+	**/
+	class MouseJoint : public Joint
+	{
+	private:
+		// The Box2D MouseJoint object.
+		b2MouseJoint * joint;
+	public:
+
+		/**
+		* Creates a MouseJoint which connects body1 to the target point.
+		**/
+		MouseJoint(Body * body1, float x, float y);
+		
+		virtual ~MouseJoint();
+
+		/**
+		* Sets the target of anchor2. (You usually want
+		* to set this to the current mouse.)
+		**/
+		void setTarget(float x, float y);
+
+		/**
+		* Gets the current anchor2 target.
+		**/
+		int getTarget(lua_State * L);
+
+		/**
+		* Sets the maximum constraint force that can be exerted
+		* to move the candidate body.
+		**/
+		void setMaxForce(float force);
+
+		/**
+		* Gets the maximum constraint force that can be exerted
+		* to move the candidate body.
+		**/
+		float getMaxForce() const;
+	};
+
+} // box2d
+} // physics
+} // love
+
+
+#endif // LOVE_PHYSICS_BOX2D_MOUSE_JOINT_H

src/modules/physics/box2d/Physics.cpp

@@ -0,0 +1,182 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "Physics.h"
+
+// Convex Hull Scan
+#include "graham/GrahamScanConvexHull.h"
+
+// LOVE
+#include <common/math.h>
+#include "wrap_Body.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	
+	const char * Physics::getName() const
+	{
+		return "love.physics.box2d";
+	}
+
+	World * Physics::newWorld(float lx, float ly, float ux, float uy, float gx, float gy, bool sleep)
+	{
+		b2AABB aabb;
+		aabb.lowerBound.Set(lx, ly);
+		aabb.upperBound.Set(ux, uy);
+		return new World(aabb, b2Vec2(gx, gy), sleep);
+	}
+
+	World * Physics::newWorld(float w, float h)
+	{
+		return newWorld(-w, -h, w, h, 0, 0, true);
+	}
+
+	Body * Physics::newBody(World * world, float x, float y, float mass, float i)
+	{
+		return new Body(world, b2Vec2(x, y), mass, i);
+	}
+
+	Body * Physics::newBody(World * world, float x, float y)
+	{
+		return new Body(world, b2Vec2(x, y), 1, 1);
+	}
+
+	Body * Physics::newBody(World * world)
+	{
+		return new Body(world, b2Vec2(0, 0), 1, 1);
+	}
+
+	CircleShape * Physics::newCircleShape(Body * body, float radius)
+	{
+		return newCircleShape(body, 0, 0, radius);
+	}
+
+	CircleShape * Physics::newCircleShape(Body * body, float x, float y, float radius)
+	{
+		b2CircleDef def;
+		def.density = 1.0f;
+		def.localPosition.Set(x, y);
+		def.friction = 0.5f;
+		def.restitution = 0.1f;
+		def.radius = radius;
+		return new CircleShape(body, &def);
+	}
+
+	PolygonShape * Physics::newRectangleShape(Body * body, float w, float h)
+	{
+		return newRectangleShape(body, 0, 0, w, h, 0);
+	}
+
+	PolygonShape * Physics::newRectangleShape(Body * body, float x, float y, float w, float h)
+	{
+		return newRectangleShape(body, x, y, w, h, 0);
+	}
+
+	PolygonShape * Physics::newRectangleShape(Body * body, float x, float y, float w, float h, float angle)
+	{
+		b2PolygonDef def;
+		def.friction = 0.5f;
+		def.restitution = 0.1f;
+		def.density = 1.0f;
+		def.SetAsBox(w/2.0f, h/2.0f, b2Vec2(x, y), angle);
+		return new PolygonShape(body, &def);
+	}
+
+	int Physics::newPolygonShape(lua_State * L)
+	{
+		int argc = lua_gettop(L);
+		int vcount = (int)(argc-1)/2;
+		// 1 body + 3 vertices
+		love::luax_assert_argc(L, 1 + (2 * 3));
+
+		Body * b = luax_checkbody(L, 1);
+
+		b2PolygonDef def;
+		def.friction = 0.5f;
+		def.restitution = 0.1f;
+		def.density = 1.0f;
+
+		std::vector<point2d> points(def.vertexCount);
+		std::vector<point2d> convex_hull;
+
+		for(int i = 0;i<vcount;i++)
+		{
+			float x = (float)lua_tonumber(L, -2);
+			float y = (float)lua_tonumber(L, -1);
+			point2d tmp(x, y);
+			points.push_back(tmp);
+			lua_pop(L, 2);
+		}
+
+		// Compute convex hull.
+		GrahamScanConvexHull()(points, convex_hull);
+
+		def.vertexCount = (int32)convex_hull.size();
+
+		if(def.vertexCount < 3)
+			return luaL_error(L, "Polygon degenerated to less than three points.");
+
+		for(int i = 0;i<def.vertexCount;i++)
+			def.vertices[def.vertexCount-i-1].Set((float)convex_hull[i].x, (float)convex_hull[i].y);
+
+		PolygonShape * p = new PolygonShape(b, &def);
+
+		luax_newtype(L, "PolygonShape", LOVE_PHYSICS_POLYGON_SHAPE_BITS, (void*)p);
+
+		return 1;
+	}
+
+	DistanceJoint * Physics::newDistanceJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2)
+	{
+		return new DistanceJoint(body1, body2, x1, y1, x2, y2);
+	}
+
+	MouseJoint * Physics::newMouseJoint(Body * body, float x, float y)
+	{
+		return new MouseJoint(body, x, y);
+	}
+
+	RevoluteJoint * Physics::newRevoluteJoint(Body * body1, Body * body2, float x, float y)
+	{
+		return new RevoluteJoint(body1, body2, x, y);
+	}
+
+	PrismaticJoint * Physics::newPrismaticJoint(Body * body1, Body * body2, float x, float y, float ax, float ay)
+	{
+		return new PrismaticJoint(body1, body2, x, y, ax, ay);
+	}
+	
+	PulleyJoint * Physics::newPulleyJoint(Body * body1, Body * body2, b2Vec2 groundAnchor1, b2Vec2 groundAnchor2, b2Vec2 anchor1, b2Vec2 anchor2, float ratio)
+	{
+		return new PulleyJoint(body1, body2, groundAnchor1, groundAnchor2, anchor1, anchor2, ratio);
+	}
+
+	GearJoint * Physics::newGearJoint(Joint * joint1, Joint * joint2, float ratio)
+	{
+		return new GearJoint(joint1, joint2, ratio);
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/Physics.h

@@ -0,0 +1,205 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_PHYSICS_H
+#define LOVE_PHYSICS_BOX2D_PHYSICS_H
+
+// LOVE
+#include <common/Module.h>
+#include "World.h"
+#include "Contact.h"
+#include "Body.h"
+#include "Shape.h"
+#include "CircleShape.h"
+#include "PolygonShape.h"
+#include "Joint.h"
+#include "MouseJoint.h"
+#include "DistanceJoint.h"
+#include "PrismaticJoint.h"
+#include "RevoluteJoint.h"
+#include "PulleyJoint.h"
+#include "GearJoint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	class Physics : public Module
+	{
+	public:
+
+		// Implements Module.
+		const char * getName() const;
+
+		/**
+		* Creates a new World.
+		* @param lx Lower bound on the x-axis.
+		* @param ly Lower bound on the y-axis.
+		* @param ux Upper bound on the x-axis.
+		* @param uy Upper bound on the y-axis.
+		* @param gx Gravity along x-axis.
+		* @param gy Gravity along y-axis.
+		* @param sleep Whether the World allows sleep.
+		**/
+		World * newWorld(float lx, float ly, float ux, float uy, float gx, float gy, bool sleep);
+
+		/**
+		* Creates a new World with with size (w,h).
+		* @param w The width of the world.
+		* @param h The height of the world.
+		**/
+		World * newWorld(float w, float h);
+
+		/**
+		* Creates a new Body at the specified position.
+		* @param world The world to create the Body in.
+		* @param x The position along the x-axis.
+		* @param x The position along the y-axis.
+		* @param m The mass.
+		* @param i The rotational inertia.
+		**/
+		Body * newBody(World * world, float x, float y, float m, float i);
+
+		/**
+		* Creates a new Body at the specified position.
+		* @param world The world to create the Body in.
+		* @param x The position along the x-axis.
+		* @param x The position along the y-axis.
+		**/
+		Body * newBody(World * world, float x, float y);
+
+		/**
+		* Creates a new Body at (0, 0)
+		* @param world The world to create the Body in.
+		**/
+		Body * newBody(World * world);
+
+		/**
+		* Creates a new CircleShape at the Body origin.
+		* @param body The Body to create the Shape on.
+		* @param radius The radius of the circle.
+		**/
+		CircleShape * newCircleShape(Body * body, float radius);
+
+		/**
+		* Creates a new CircleShape at (x,y) in local coorinates.
+		* @param body The Body to create the Shape on.
+		* @param x The offset along the x-axis.
+		* @param y The offset along the y-axis.
+		* @param radius The radius of the circle.
+		**/
+		CircleShape * newCircleShape(Body * body, float x, float y, float radius);
+
+		/**
+		* Shorthand for creating rectangluar PolygonShapes. The rectangle
+		* will be created at the local origin.
+		* @param w The width of the rectangle.
+		* @param h The height of the rectangle.
+		**/
+		PolygonShape * newRectangleShape(Body * body, float w, float h);
+
+		/**
+		* Shorthand for creating rectangluar PolygonShapes. The rectangle
+		* will be created at (x,y) in local coordinates.
+		* @param x The offset along the x-axis.
+		* @param y The offset along the y-axis.
+		* @param w The width of the rectangle.
+		* @param h The height of the rectangle.
+		**/
+		PolygonShape * newRectangleShape(Body * body, float x, float y, float w, float h);
+
+		/**
+		* Shorthand for creating rectangluar PolygonShapes. The rectangle
+		* will be created at (x,y) in local coordinates.
+		* @param x The offset along the x-axis.
+		* @param y The offset along the y-axis.
+		* @param w The width of the rectangle.
+		* @param h The height of the rectangle.
+		* @param angle The angle of the rectangle. (deg)
+		**/
+		PolygonShape * newRectangleShape(Body * body, float x, float y, float w, float h, float angle);
+
+		/**
+		* Creates a new PolygonShape.
+		* @param body The body to create this shape on.
+		* @param ... A variable number of vertices.
+		**/
+		int newPolygonShape(lua_State * L);
+
+		/**
+		* Creates a new DistanceJoint connecting body1 with body2.
+		* @param x1 Anchor1 along the x-axis. (World coordinates)
+		* @param y1 Anchor1 along the y-axis. (World coordinates)
+		* @param x2 Anchor2 along the x-axis. (World coordinates)
+		* @param y2 Anchor2 along the y-axis. (World coordinates)
+		**/
+		DistanceJoint * newDistanceJoint(Body * body1, Body * body2, float x1, float y1, float x2, float y2);
+
+		/**
+		* Creates a new MouseJoint connecting the body with an arbitrary point.
+		* @param x Anchor along the x-axis. (World coordinates)
+		* @param y Anchor along the y-axis. (World coordinates)
+		**/
+		MouseJoint * newMouseJoint(Body * body, float x, float y);
+
+		/**
+		* Creates a new RevoluteJoint connecting body1 with body2.
+		* @param x Anchor along the x-axis. (World coordinates)
+		* @param y Anchor along the y-axis. (World coordinates)
+		**/
+		RevoluteJoint * newRevoluteJoint(Body * body1, Body * body2, float x, float y);
+
+		/**
+		* Creates a new PrismaticJoint connecting body1 with body2.
+		* @param x World-anchor along the x-axis.
+		* @param y World-anchor along the y-axis.
+		* @param ax The x-component of the world-axis.
+		* @param ay The y-component of the world-axis.
+		**/
+		PrismaticJoint * newPrismaticJoint(Body * body1, Body * body2, float x, float y, float ax, float ay);
+		
+		/**
+		* Creates a new PulleyJoint connecting body1 with body2.
+		* @param groundAnchor1 World ground-anchor for body1.
+		* @param groundAnchor2 World ground-anchor for body2.
+		* @param anchor1 World anchor on body1.
+		* @param anchor2 World anchor on body2.
+		* @param ratio The pulley ratio.
+		**/
+		PulleyJoint * newPulleyJoint(Body * body1, Body * body2, b2Vec2 groundAnchor1, b2Vec2 groundAnchor2, b2Vec2 anchor1, b2Vec2 anchor2, float ratio);
+
+		/**
+		* Creates a new GearJoint connecting joint1 with joint2.
+		* @param joint1 The first joint.
+		* @param joint2 The second joint.
+		* @param ratio The gear ratio.
+		**/
+		GearJoint * newGearJoint(Joint * joint1, Joint * joint2, float ratio);
+
+
+	}; // Physics
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_PHYSICS_H

src/modules/physics/box2d/PolygonShape.cpp

@@ -0,0 +1,66 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "PolygonShape.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	PolygonShape::PolygonShape(Body * body, b2PolygonDef * def)
+		: Shape(body)
+	{
+		for(int i = 0; i<def->vertexCount; i++)
+			def->vertices[i] = body->world->scaleDown(def->vertices[i]);
+
+		shape = body->body->CreateShape(def);
+		shape->SetUserData((void*)data);
+	}
+
+	PolygonShape::~PolygonShape()
+	{
+		body->body->DestroyShape(shape);
+		shape = 0;
+	}
+
+	int PolygonShape::getPoints(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0);
+		b2PolygonShape * p = (b2PolygonShape *)shape;
+		const b2Vec2 * vertices = p->GetVertices();
+		int count = p->GetVertexCount();
+		for(int i = 0;i<count; i++)
+		{
+			b2Vec2 v = body->world->scaleUp(body->body->GetWorldPoint(vertices[i]));
+			lua_pushnumber(L, v.x);
+			lua_pushnumber(L, v.y);
+		}
+		return count*2;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/PolygonShape.h

@@ -0,0 +1,67 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_POLYGON_SHAPE_H
+#define LOVE_PHYSICS_BOX2D_POLYGON_SHAPE_H
+
+// Module
+#include "Shape.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* You should know what a Polygon is. :)
+	* 
+	* This class is needed so that we can easily get
+	* the transformed points in Lua. By calling shape:getPoints(), 
+	* the result can be passed directly to love.graphics.polygon().
+	**/
+	class PolygonShape : public Shape
+	{
+	public:
+
+		/**
+		* Create a new PolygonShape from the parent Body and
+		* a Box2D polygon definition.
+		* @param body The parent Body. 
+		* @param def The polygon definition.
+		**/
+		PolygonShape(Body * body, b2PolygonDef * def);
+
+		virtual ~PolygonShape();
+
+		/**
+		* Returns the transformed points of the polygon.
+		* This function is useful for debug drawing and such.
+		*
+		* The result can be directly passed into love.graphics.polygon().
+		**/
+		int getPoints(lua_State * L);
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_POLYGON_SHAPE_H

src/modules/physics/box2d/PrismaticJoint.cpp

@@ -0,0 +1,141 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "PrismaticJoint.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	PrismaticJoint::PrismaticJoint(Body * body1, Body * body2, float x, float y, float ax, float ay)
+		: Joint(body1, body2)
+	{
+		b2PrismaticJointDef def;
+		
+		def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y)), b2Vec2(ax,ay));
+		def.lowerTranslation = 0.0f;
+		def.upperTranslation = 100.0f;
+		def.enableLimit = true;
+		joint = (b2PrismaticJoint*)createJoint(&def);
+	}
+
+	PrismaticJoint::~PrismaticJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+
+	float PrismaticJoint::getTranslation() const
+	{
+		return world->scaleDown(joint->GetJointTranslation());
+	}
+
+	float PrismaticJoint::getSpeed() const
+	{
+		return world->scaleDown(joint->GetJointSpeed());
+	}
+
+	void PrismaticJoint::setMotorEnabled(bool motor)
+	{
+		return joint->EnableMotor(true);
+	}
+
+	bool PrismaticJoint::isMotorEnabled() const
+	{
+		return joint->IsMotorEnabled();
+	}
+
+	void PrismaticJoint::setMaxMotorForce(float force)
+	{
+		joint->SetMaxMotorForce(force);
+	}
+
+	float PrismaticJoint::getMaxMotorForce() const
+	{
+		return joint->GetMotorForce();
+	}
+
+	void PrismaticJoint::setMotorSpeed(float speed)
+	{
+		joint->SetMotorSpeed(speed);
+	}
+
+	float PrismaticJoint::getMotorSpeed() const
+	{
+		return joint->GetMotorSpeed();
+	}
+
+	float PrismaticJoint::getMotorForce() const
+	{
+		return joint->GetMotorForce();
+	}
+
+	void PrismaticJoint::setLimitsEnabled(bool limit)
+	{
+		joint->EnableLimit(limit);
+	}
+
+	bool PrismaticJoint::isLimitsEnabled() const
+	{
+		return joint->IsLimitEnabled();
+	}
+
+	void PrismaticJoint::setUpperLimit(float limit)
+	{
+		joint->SetLimits(joint->GetLowerLimit(), world->scaleDown(limit));
+	}
+
+	void PrismaticJoint::setLowerLimit(float limit)
+	{
+		joint->SetLimits(world->scaleDown(limit), joint->GetUpperLimit());
+	}
+
+	void PrismaticJoint::setLimits(float lower, float upper)
+	{
+		joint->SetLimits(world->scaleDown(lower), world->scaleDown(upper));
+	}
+
+	float PrismaticJoint::getLowerLimit() const
+	{
+		return world->scaleUp(joint->GetLowerLimit());
+	}
+
+	float PrismaticJoint::getUpperLimit() const
+	{
+		return world->scaleUp(joint->GetUpperLimit());
+	}
+
+	int PrismaticJoint::getLimits(lua_State * L)
+	{
+		lua_pushnumber(L, world->scaleUp(joint->GetLowerLimit()));
+		lua_pushnumber(L, world->scaleUp(joint->GetUpperLimit()));
+		return 2;
+	}
+
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/PrismaticJoint.h

@@ -0,0 +1,146 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_PRISMATIC_JOINT_H
+#define LOVE_PHYSICS_BOX2D_PRISMATIC_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* PrismaticJoints allow shapes to move in relation to eachother
+	* along a defined axis.
+	**/
+	class PrismaticJoint : public Joint
+	{
+	private:
+
+		// The Box2D prismatic joint object.
+		b2PrismaticJoint * joint;
+
+	public:
+
+		/**
+		* Creates a new PrismaticJoint connecting body1 and body2.
+		**/
+		PrismaticJoint(Body * body1, Body * body2, float x, float y, float ax, float ay);
+
+		virtual ~PrismaticJoint();
+
+		/**
+		* Get the current joint translation, usually in meters.
+		**/
+		float getTranslation() const;
+
+		/**
+		* Get the current joint translation speed, usually in meters per second.
+		**/
+		float getSpeed() const;
+
+		/**
+		* Enable/disable the joint motor.
+		**/
+		void setMotorEnabled(bool motor);
+
+		/**
+		* Checks whether the motor is enabled.
+		**/
+		bool isMotorEnabled() const;
+
+		/**
+		* Set the maximum motor force, usually in N.
+		**/
+		void setMaxMotorForce(float force);
+
+		/**
+		* Get the current motor force, usually in N.
+		**/
+		float getMaxMotorForce() const;
+
+		/**
+		* Set the motor speed, usually in meters per second.
+		**/
+		void setMotorSpeed(float speed);
+
+		/**
+		* Get the motor speed, usually in meters per second.
+		**/
+		float getMotorSpeed() const;
+
+		/**
+		* Get the current motor force, usually in N.
+		**/
+		float getMotorForce() const;
+
+		/**
+		* Enable/disable the joint limit.
+		**/
+		void setLimitsEnabled(bool limit);
+
+		/**
+		* Checks whether limits are enabled.
+		**/
+		bool isLimitsEnabled() const;
+
+		/**
+		* Sets the upper limit, usually in meters.
+		**/
+		void setUpperLimit(float limit);
+
+		/**
+		* Sets the lower limit, usually in meters.
+		**/
+		void setLowerLimit(float limit);
+
+		/**
+		* Sets the limits, usually in meters.
+		**/
+		void setLimits(float lower, float upper);
+
+		/**
+		* Gets the lower limit, usually in meters.
+		**/
+		float getLowerLimit() const;
+
+		/**
+		* Gets the upper limit, usually in meters.
+		**/
+		float getUpperLimit() const;
+		
+		/**
+		* Gets the limits, usually in meters.
+		* @returns The upper limit.
+		* @returns The lower limit.
+		**/
+		int getLimits(lua_State * L);
+		
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_PRISMATIC_JOINT_H

src/modules/physics/box2d/PulleyJoint.cpp

@@ -0,0 +1,116 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "PulleyJoint.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	PulleyJoint::PulleyJoint(Body * body1, Body * body2, b2Vec2 groundAnchor1, b2Vec2 groundAnchor2, b2Vec2 anchor1, b2Vec2 anchor2, float ratio)
+		: Joint(body1, body2)
+	{
+		b2PulleyJointDef def;
+		def.Initialize(body1->body, body2->body, world->scaleDown(groundAnchor1), world->scaleDown(groundAnchor2), \
+			 world->scaleDown(anchor1), world->scaleDown(anchor2), ratio);
+		
+		joint = (b2PulleyJoint*)createJoint(&def);
+	}
+
+	PulleyJoint::~PulleyJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+
+	int PulleyJoint::getGroundAnchors(lua_State * L)
+	{
+		lua_pushnumber(L, world->scaleUp(joint->GetGroundAnchor1().x));
+		lua_pushnumber(L, world->scaleUp(joint->GetGroundAnchor1().y));
+		lua_pushnumber(L, world->scaleUp(joint->GetGroundAnchor2().x));
+		lua_pushnumber(L, world->scaleUp(joint->GetGroundAnchor2().y));
+		return 4;
+	}
+	
+	void PulleyJoint::setMaxLengths(float maxlength1, float maxlength2)
+	{	
+		// Apply Box2D's maximum lengths too. They know better.
+		
+		if (maxlength1 > 0)
+		{
+			joint->m_maxLength1 = b2Min(world->scaleDown(maxlength1), joint->m_constant - joint->m_ratio * b2_minPulleyLength);
+		}
+		
+		if (maxlength2 > 0)
+		{
+			joint->m_maxLength2 = b2Min(world->scaleDown(maxlength2), (joint->m_constant - b2_minPulleyLength) / joint->m_ratio);
+		}
+	}
+	
+	int PulleyJoint::getMaxLengths(lua_State * L)
+	{
+		lua_pushnumber(L, world->scaleUp(joint->m_maxLength1));
+		lua_pushnumber(L, world->scaleUp(joint->m_maxLength2));
+		return 2;
+	}
+	
+	void PulleyJoint::setConstant(float constant)
+	{
+		joint->m_constant = world->scaleDown(constant);
+		
+		// Update the max lengths, as does Box2D
+		joint->m_maxLength1 = joint->m_constant - joint->m_ratio * b2_minPulleyLength;
+		joint->m_maxLength2 = (joint->m_constant - b2_minPulleyLength) / joint->m_ratio;
+	}
+	
+	float PulleyJoint::getConstant() const
+	{
+		return world->scaleUp(joint->m_constant);
+	}
+	
+	float PulleyJoint::getLength1() const
+	{
+		return world->scaleUp(joint->GetLength1());
+	}
+		
+	float PulleyJoint::getLength2() const
+	{
+		return world->scaleUp(joint->GetLength2());
+	}
+	
+	void PulleyJoint::setRatio(float ratio)
+	{
+		joint->m_ratio = ratio;
+	}
+	
+	float PulleyJoint::getRatio() const
+	{
+		return joint->m_ratio;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/PulleyJoint.h

@@ -0,0 +1,108 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_PULLEY_JOINT_H
+#define LOVE_PHYSICS_BOX2D_PULLEY_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* The PulleyJoint The pulley connects two bodies to ground and 
+	* to each other. As one body goes up, the other goes down. The 
+	* total length of the pulley rope is conserved according to the 
+	* initial configuration: length1 + ratio * length2 <= constant.
+	**/
+	class PulleyJoint : public Joint
+	{
+	private:
+		// The Box2D DistanceJoint object.
+		b2PulleyJoint * joint;
+	public:
+
+		/**
+		* Creates a PulleyJoint connecting body1 to body2. 
+		**/
+		PulleyJoint(Body * body1, Body * body2, b2Vec2 groundAnchor1, b2Vec2 groundAnchor2, b2Vec2 anchor1, b2Vec2 anchor2, float ratio);
+		
+		virtual ~PulleyJoint();
+		
+		/**
+		* Gets the ground anchors position in world
+		* coordinates.
+		**/
+		int getGroundAnchors(lua_State * L);
+		
+		/**
+		* Sets the max rope lengths (a value of zero keeps it
+		* unchanged). Beware that Box2D also imposes a maximum,
+		* so the smallest of these is actually set.
+		**/
+		void setMaxLengths(float maxlength1, float maxlength2);
+		
+		/**
+		* Gets the max rope lengths.
+		**/
+		int getMaxLengths(lua_State * L);
+				
+		/**
+		* Sets the total length of the rope and updates the 
+		* MaxLengths values accordingly.
+		**/
+		void setConstant(float length);
+		
+		/**
+		* Gets the total length of the rope.
+		**/
+		float getConstant() const;
+		
+		/**
+		* Gets the current length of the segment attached to body1.
+		**/
+		float getLength1() const;
+	
+		/**
+		* Gets the current length of the segment attached to body2.
+		**/
+		float getLength2() const;
+		
+		/**
+		* Sets the pulley ratio.
+		**/
+		void setRatio(float ratio);
+		
+		/**
+		* Gets the pulley ratio.
+		**/
+		float getRatio() const;
+		
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_PULLEY_JOINT_H

src/modules/physics/box2d/RevoluteJoint.cpp

@@ -0,0 +1,138 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "RevoluteJoint.h"
+
+#include <common/math.h>
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	RevoluteJoint::RevoluteJoint(Body * body1, Body * body2, float x, float y)
+		: Joint(body1, body2)
+	{	
+		b2RevoluteJointDef def;
+		def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y)));
+		joint = (b2RevoluteJoint*)createJoint(&def);
+	}
+
+	RevoluteJoint::~RevoluteJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+
+	float RevoluteJoint::getAngle() const
+	{
+		return joint->GetJointAngle();
+	}
+
+	float RevoluteJoint::getSpeed() const
+	{
+		return joint->GetJointSpeed();
+	}
+
+	void RevoluteJoint::setMotorEnabled(bool motor)
+	{
+		return joint->EnableMotor(motor);
+	}
+
+	bool RevoluteJoint::isMotorEnabled() const
+	{
+		return joint->IsMotorEnabled();
+	}
+
+	void RevoluteJoint::setMaxMotorTorque(float torque)
+	{
+		joint->SetMaxMotorTorque(torque);
+	}
+
+	float RevoluteJoint::getMaxMotorTorque() const
+	{
+		return joint->GetMotorTorque();
+	}
+
+	void RevoluteJoint::setMotorSpeed(float speed)
+	{
+		joint->SetMotorSpeed(speed);
+	}
+
+	float RevoluteJoint::getMotorSpeed() const
+	{
+		return joint->GetMotorSpeed();
+	}
+
+	float RevoluteJoint::getMotorTorque() const
+	{
+		return joint->GetMotorTorque();
+	}
+
+	void RevoluteJoint::setLimitsEnabled(bool limit)
+	{
+		joint->EnableLimit(limit);
+	}
+
+	bool RevoluteJoint::isLimitsEnabled() const
+	{
+		return joint->IsLimitEnabled();
+	}
+
+	void RevoluteJoint::setUpperLimit(float limit)
+	{
+		joint->SetLimits(joint->GetLowerLimit(), limit);
+	}
+
+	void RevoluteJoint::setLowerLimit(float limit)
+	{
+		joint->SetLimits(limit, joint->GetUpperLimit());
+	}
+
+	void RevoluteJoint::setLimits(float lower, float upper)
+	{
+		joint->SetLimits(lower, upper);
+	}
+
+	float RevoluteJoint::getLowerLimit() const
+	{
+		return joint->GetLowerLimit();
+	}
+
+	float RevoluteJoint::getUpperLimit() const
+	{
+		return joint->GetUpperLimit();
+	}
+
+	int RevoluteJoint::getLimits(lua_State * L)
+	{
+		lua_pushnumber(L, joint->GetLowerLimit());
+		lua_pushnumber(L, joint->GetUpperLimit());
+		return 2;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/RevoluteJoint.h

@@ -0,0 +1,146 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_REVOLUTE_JOINT_H
+#define LOVE_PHYSICS_BOX2D_REVOLUTE_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* A RevoluteJoint allows two bodies relative rotation
+	* around a single point.
+	**/
+	class RevoluteJoint : public Joint
+	{
+	private:
+
+		// The Box2D revolute joint object.
+		b2RevoluteJoint * joint;
+
+	public:
+
+		/**
+		* Creates a new RevoluteJoint connecting body1 and body2.
+		**/
+		RevoluteJoint(Body * body1, Body * body2, float x, float y);
+		
+		virtual ~RevoluteJoint();
+
+		/**
+		* Get the current joint angle in degrees.
+		**/
+		float getAngle() const;
+
+		/**
+		* Get the current joint angle speed in degrees per second.
+		**/
+		float getSpeed() const;
+		
+		/**
+		* Enable/disable the joint motor.
+		**/
+		void setMotorEnabled(bool motor);
+
+		/**
+		* Checks whether the motor is enabled.
+		**/
+		bool isMotorEnabled() const;
+
+		/**
+		* Set the maximum motor torque, usually in N-m.
+		**/
+		void setMaxMotorTorque(float torque);
+
+		/**
+		* Gets the maximum motor torque, usually in N-m.
+		**/
+		float getMaxMotorTorque() const;
+
+		/**
+		* Sets the motor speed in degrees per second.
+		**/
+		void setMotorSpeed(float speed);
+
+		/**
+		* Gets the motor speed in degrees per second.
+		**/
+		float getMotorSpeed() const;
+
+		/**
+		* Get the current motor torque, usually in N-m.
+		**/
+		float getMotorTorque() const;
+
+		/**
+		* Enable/disable the joint limit.
+		**/
+		void setLimitsEnabled(bool limit);
+
+		/**
+		* Checks whether limits are enabled.
+		**/
+		bool isLimitsEnabled() const;
+
+		/**
+		* Sets the upper limit in degrees.
+		**/
+		void setUpperLimit(float limit);
+
+		/**
+		* Sets the lower limit in degrees.
+		**/
+		void setLowerLimit(float limit);
+
+		/**
+		* Sets the limits in degrees.
+		**/
+		void setLimits(float lower, float upper);
+
+		/**
+		* Gets the lower limit in degrees.
+		**/
+		float getLowerLimit() const;
+
+		/**
+		* Gets the upper limit in degrees.
+		**/
+		float getUpperLimit() const;
+		
+		/**
+		* Gets the limits in degrees.
+		* @returns The lower limit.
+		* @returns The upper limit.
+		**/
+		int getLimits(lua_State * L);
+
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_REVOLUTE_JOINT_H

src/modules/physics/box2d/Shape.cpp

@@ -0,0 +1,259 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "Shape.h"
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+// STD
+#include <bitset>
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Shape::Shape(Body * body)
+		: body(body)
+	{
+		body->retain();
+		data = new shapeudata();
+		data->ref = 0;
+	}
+
+	Shape::~Shape()
+	{
+		if(data->ref != 0)
+			delete data->ref;
+
+		delete data;
+		data = 0;
+	}
+
+	int Shape::getType() const
+	{
+		switch(shape->GetType())
+		{
+		case e_circleShape:
+			return SHAPE_CIRCLE;
+		case e_polygonShape:
+			return SHAPE_POLYGON;
+		default:
+			return -1;
+		}
+	}
+
+	void Shape::setFriction(float friction)
+	{
+		shape->m_friction = friction;
+	}
+
+	void Shape::setRestitution(float restitution)
+	{
+		shape->m_restitution = restitution;
+	}
+
+	void Shape::setDensity(float density)
+	{
+		shape->m_density = density;
+	}
+
+	void Shape::setSensor(bool sensor)
+	{
+		shape->m_isSensor = sensor;
+	}
+
+	float Shape::getFriction() const
+	{
+		return shape->GetFriction();
+	}
+
+	float Shape::getRestituion() const
+	{
+		return shape->GetRestitution();
+	}
+
+	float Shape::getDensity() const
+	{
+		return shape->m_density;
+	}
+
+	bool Shape::isSensor() const
+	{
+		return shape->IsSensor();
+	}
+
+	Body * Shape::getBody() const
+	{
+		return body;
+	}
+
+	bool Shape::testPoint(float x, float y) const
+	{
+		return shape->TestPoint(shape->GetBody()->GetXForm(), body->world->scaleDown(b2Vec2(x, y)));
+	}
+
+	int Shape::testSegment(lua_State * L)
+	{
+		love::luax_assert_argc(L, 4, 4);
+
+		b2Segment s;
+
+		s.p1.x = (float)lua_tonumber(L, 1);
+		s.p1.y = (float)lua_tonumber(L, 2);
+		s.p2.x = (float)lua_tonumber(L, 3);
+		s.p2.y = (float)lua_tonumber(L, 4);
+
+		s.p1 = body->world->scaleDown(s.p1);
+		s.p2 = body->world->scaleDown(s.p2);
+
+		float lambda;
+		b2Vec2 normal;
+		
+		if(shape->TestSegment(shape->GetBody()->GetXForm(), &lambda, &normal, s, 1.0f))
+		{
+			lua_pushnumber(L, lambda);
+			normal = body->world->scaleUp(normal);
+			lua_pushnumber(L, normal.x);
+			lua_pushnumber(L, normal.y);
+			return 3;
+		}
+
+		return 0;
+	}
+
+	int Shape::setCategory(lua_State * L)
+	{
+		b2FilterData f = shape->GetFilterData();
+		f.categoryBits = (uint16)getBits(L);
+		shape->SetFilterData(f);
+		shape->GetBody()->GetWorld()->Refilter(shape);
+		return 0;
+	}
+
+	int Shape::setMask(lua_State * L)
+	{
+		b2FilterData f = shape->GetFilterData();
+		f.maskBits = ~(uint16)getBits(L);
+		shape->SetFilterData(f);
+		shape->GetBody()->GetWorld()->Refilter(shape);
+		return 0;
+	}
+
+	int Shape::getCategory(lua_State * L)
+	{
+		return pushBits(L, shape->GetFilterData().categoryBits);
+	}
+
+	int Shape::getMask(lua_State * L)
+	{
+		return pushBits(L, ~(shape->GetFilterData().maskBits));
+	}
+
+	int Shape::setData(lua_State * L)
+	{
+		love::luax_assert_argc(L, 1, 1);
+		
+		if(data->ref != 0)
+		{
+			delete data->ref;
+			data->ref = 0;
+		}
+
+		data->ref = new Reference(L);
+		return 0;
+	}
+
+	int Shape::getData(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0, 0);
+		if(data->ref != 0)
+			data->ref->push();
+		else
+			lua_pushnil(L);
+
+		return 1;
+	}
+
+	int Shape::getBoundingBox(lua_State * L)
+	{
+		love::luax_assert_argc(L, 0, 0);
+		b2AABB bb;
+		shape->ComputeAABB(&bb, shape->GetBody()->GetXForm());
+		bb = body->world->scaleUp(bb);
+
+		// Top left.
+		lua_pushnumber(L, bb.lowerBound.x);
+		lua_pushnumber(L, bb.upperBound.y);
+
+		// Bottom left.
+		lua_pushnumber(L, bb.lowerBound.x);
+		lua_pushnumber(L, bb.lowerBound.y);
+
+		// Bottom right.
+		lua_pushnumber(L, bb.upperBound.x);
+		lua_pushnumber(L, bb.lowerBound.y);
+
+		// Top right.
+		lua_pushnumber(L, bb.upperBound.x);
+		lua_pushnumber(L, bb.upperBound.y);
+
+		return 8;
+	}
+
+	uint16 Shape::getBits(lua_State * L)
+	{
+		// Get number of args.
+		int argc = lua_gettop(L);
+
+		// The new bitset.
+		std::bitset<16> b;
+
+		for(int i = 1;i<=argc;i++)
+		{
+			size_t bpos = (size_t)(lua_tointeger(L, i)-1);
+			if(bpos < 0 || bpos > 16) 
+				return luaL_error(L, "Values must be in range 1-16.");
+			b.set(bpos, true);
+		}
+		
+		return (uint16)b.to_ulong();
+	}
+
+	int Shape::pushBits(lua_State * L, uint16 bits)
+	{
+		// Create a bitset.
+		std::bitset<16> b((unsigned long)bits);
+
+		// Push all set bits.
+		for(int i = 0;i<16;i++)
+			if(b.test(i))
+				lua_pushinteger(L, i+1);
+
+		// Count number of set bits.
+		return (int)b.count();
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/Shape.h

@@ -0,0 +1,235 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_SHAPE_H
+#define LOVE_PHYSICS_BOX2D_SHAPE_H
+
+// LOVE
+#include <common/Object.h>
+#include <common/Reference.h>
+#include <common/constants.h>
+
+// Box2D
+#include "Include/Box2D.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	// Forward declarations.
+	class Body;
+
+	/**
+	* This struct is stored in a void pointer
+	* in the Box2D Shape class. For now, all we
+	* need is a Lua reference to arbitrary data,
+	* but we might need more later.
+	**/
+	struct shapeudata
+	{
+		// Reference to arbitrary data.
+		Reference * ref;
+	};
+
+	/**
+	* A Shape is geometry attached to a Body.
+	* A Body has position and orientation, and 
+	* a Shape's geometry will be affected by the parent
+	* body's transformation.
+	**/
+	class Shape : public Object
+	{
+	protected:
+		
+		// A pointer to the parent Body. If the Body
+		// is destroyed, all child shapes are destroyed as well. 
+		// This ensures that all child shapes are always destroyed
+		// *before* the parent Body.
+		Body * body;
+
+		// The Box2D shape.
+		b2Shape * shape;
+
+		// Our shape data, to be stored in
+		// the Box2D shape.
+		shapeudata * data;
+
+	public:
+
+		/**
+		* Creates a Shape attatched to the specified
+		* Body. 
+		**/
+		Shape(Body * body);
+
+		virtual ~Shape();
+
+		/**
+		* Gets the type of Shape. Useful for 
+		* debug drawing.
+		**/
+		int getType() const;
+
+		/**
+		* Sets the friction of the Shape.
+		* @param friction The new friction.
+		**/
+		void setFriction(float friction);
+
+		/**
+		* Sets the restitution for the Shape.
+		* @param restitution The restitution.
+		**/
+		void setRestitution(float restitution);
+
+		/**
+		* Sets the density of the Shape.
+		* @param density The density of the Shape.
+		**/
+		void setDensity(float density);
+
+		/**
+		* Set whether this shape should be a sensor or not.
+		* @param sensor True if sensor, false if not.
+		**/
+		void setSensor(bool sensor);
+
+		/**
+		* Gets the friction of the Shape.
+		* @returns The friction.
+		**/
+		float getFriction() const;
+
+		/**
+		* Gets the restitution of the Shape.
+		* @return The restitution of the Shape.
+		**/
+		float getRestituion() const;
+
+		/**
+		* Gets the density of the Shape.
+		* @return The density.
+		**/
+		float getDensity() const;
+
+		/**
+		* Checks whether this Shape acts as a sensor.
+		* @return True if sensor, false otherwise.
+		**/
+		bool isSensor() const;
+
+		/**
+		* Get the body attatched to this Shape.
+		* @return The parent Body.
+		**/
+		Body * getBody() const;
+
+		/**
+		* Checks if a point is inside the Shape.
+		* @param x The x-component of the Shape.
+		* @param y The y-component of the Shape.
+		**/
+		bool testPoint(float x, float y) const;
+
+		/**
+		* Tests whether a line segment intersects a Shape.
+		**/
+		int testSegment(lua_State * L);
+
+		/**
+		* With this function, you can easily set the categories
+		* a Shape is a member of.
+		* 
+		* The function accepts 1-16 arguments. Each argument must 
+		* be a number from 1-16, and must be unique in the list.
+		**/
+		int setCategory(lua_State * L);
+
+		/**
+		* Returns the categories this Shape is
+		* a member of. So if the Shape is a member
+		* of 5 categories, this function returns five
+		* values.
+		**/ 
+		int getCategory(lua_State * L);
+
+		/**
+		* Sets which categories this Shape should *not*
+		* collide with. 
+		* 
+		* The function works the same was as setCategory, but
+		* each entry here represents a 0 in the bit string, not
+		* a 1 like setCategory.
+		**/
+		int setMask(lua_State * L);
+
+		/**
+		* Returns the current masked categories.
+		* If there are 5 masked categories, then this
+		* function will have 5 return values.
+		**/
+		int getMask(lua_State * L);
+
+		/**
+		* This function stores an in-C reference to
+		* arbitrary Lua data in the Box2D shape object.
+		* 
+		* The data set here will be passed to the collision
+		* handler when collisions occur.
+		**/
+		int setData(lua_State * L);
+
+		/**
+		* Gets the data set with setData. If no
+		* data is set, nil is returned.
+		**/
+		int getData(lua_State * L);
+
+		/**
+		* Gets the bounding box for this Shape. 
+		* The function returns eight values which can be
+		* passed directly to love.graphics.polygon.
+		**/ 
+		int getBoundingBox(lua_State * L);
+
+	private:
+
+		/**
+		* Gets a 16-integer from the current stack top.
+		* @param The 16-bit integer.
+		**/
+		uint16 getBits(lua_State * L);
+
+		/**
+		* Push each set bit in a 16-bit integer as
+		* a list of integer indices. That is, if the bits are 
+		* set to 0000 0000 0011 0001, then this function will
+		* push (1, 5, 6) on the stack.
+		**/
+		int pushBits(lua_State * L, uint16 bits);
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_SHAPE_H

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

@@ -0,0 +1,120 @@
+/*
+* 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

@@ -0,0 +1,92 @@
+/*
+* 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

@@ -0,0 +1,449 @@
+
+/*
+* 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

@@ -0,0 +1,163 @@
+/*
+* 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

@@ -0,0 +1,167 @@
+/*
+* 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

@@ -0,0 +1,286 @@
+/*
+* 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

@@ -0,0 +1,668 @@
+/*
+* 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

@@ -0,0 +1,146 @@
+/*
+* 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

@@ -0,0 +1,168 @@
+/*
+* 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/b2CollidePoly.cpp

@@ -0,0 +1,352 @@
+/*
+* 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 "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;
+}
+
+// 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)
+{
+	int32 count1 = poly1->GetVertexCount();
+	const b2Vec2* vertices1 = poly1->GetVertices();
+	const b2Vec2* normals1 = poly1->GetNormals();
+
+	int32 count2 = poly2->GetVertexCount();
+	const b2Vec2* vertices2 = poly2->GetVertices();
+
+	b2Assert(0 <= edge1 && edge1 < count1);
+
+	// Convert normal from poly1's frame into poly2's frame.
+	b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
+	b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
+
+	// Find support vertex on poly2 for -normal.
+	int32 index = 0;
+	float32 minDot = B2_FLT_MAX;
+
+	for (int32 i = 0; i < count2; ++i)
+	{
+		float32 dot = b2Dot(vertices2[i], normal1);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
+	b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
+	float32 separation = b2Dot(v2 - v1, normal1World);
+	return separation;
+}
+
+// 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)
+{
+	int32 count1 = poly1->GetVertexCount();
+	const b2Vec2* normals1 = poly1->GetNormals();
+
+	// Vector pointing from the centroid of poly1 to the centroid of poly2.
+	b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
+	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;
+	for (int32 i = 0; i < count1; ++i)
+	{
+		float32 dot = b2Dot(normals1[i], dLocal1);
+		if (dot > maxDot)
+		{
+			maxDot = dot;
+			edge = i;
+		}
+	}
+
+	// Get the separation for the edge normal.
+	float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+	if (s > 0.0f)
+	{
+		return s;
+	}
+
+	// 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;
+	}
+
+	// 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;
+	}
+
+	// Find the best edge and the search direction.
+	int32 bestEdge;
+	float32 bestSeparation;
+	int32 increment;
+	if (sPrev > s && sPrev > sNext)
+	{
+		increment = -1;
+		bestEdge = prevEdge;
+		bestSeparation = sPrev;
+	}
+	else if (sNext > s)
+	{
+		increment = 1;
+		bestEdge = nextEdge;
+		bestSeparation = sNext;
+	}
+	else
+	{
+		*edgeIndex = edge;
+		return s;
+	}
+
+	// Perform a local search for the best edge normal.
+	for ( ; ; )
+	{
+		if (increment == -1)
+			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+		else
+			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+		s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+		if (s > 0.0f)
+		{
+			return s;
+		}
+
+		if (s > bestSeparation)
+		{
+			bestEdge = edge;
+			bestSeparation = s;
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	*edgeIndex = bestEdge;
+	return bestSeparation;
+}
+
+static void FindIncidentEdge(ClipVertex c[2],
+							 const b2PolygonShape* poly1, const b2XForm& xf1, int32 edge1,
+							 const b2PolygonShape* poly2, const b2XForm& xf2)
+{
+	int32 count1 = poly1->GetVertexCount();
+	const b2Vec2* normals1 = poly1->GetNormals();
+
+	int32 count2 = poly2->GetVertexCount();
+	const b2Vec2* vertices2 = poly2->GetVertices();
+	const b2Vec2* normals2 = poly2->GetNormals();
+
+	b2Assert(0 <= edge1 && edge1 < count1);
+
+	// Get the normal of the reference edge in poly2's frame.
+	b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
+
+	// Find the incident edge on poly2.
+	int32 index = 0;
+	float32 minDot = B2_FLT_MAX;
+	for (int32 i = 0; i < count2; ++i)
+	{
+		float32 dot = b2Dot(normal1, normals2[i]);
+		if (dot < minDot)
+		{
+			minDot = dot;
+			index = i;
+		}
+	}
+
+	// Build the clip vertices for the incident edge.
+	int32 i1 = index;
+	int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
+
+	c[0].v = b2Mul(xf2, vertices2[i1]);
+	c[0].id.features.referenceEdge = (uint8)edge1;
+	c[0].id.features.incidentEdge = (uint8)i1;
+	c[0].id.features.incidentVertex = 0;
+
+	c[1].v = b2Mul(xf2, vertices2[i2]);
+	c[1].id.features.referenceEdge = (uint8)edge1;
+	c[1].id.features.incidentEdge = (uint8)i2;
+	c[1].id.features.incidentVertex = 1;
+}
+
+// Find edge normal of max separation on A - return if separating axis is found
+// Find edge normal of max separation on B - return if separation axis is found
+// Choose reference edge as min(minA, minB)
+// Find incident edge
+// Clip
+
+// The normal points from 1 to 2
+void b2CollidePolygons(b2Manifold* manifold,
+					  const b2PolygonShape* polyA, const b2XForm& xfA,
+					  const b2PolygonShape* polyB, const b2XForm& xfB)
+{
+	manifold->pointCount = 0;
+
+	int32 edgeA = 0;
+	float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+	if (separationA > 0.0f)
+		return;
+
+	int32 edgeB = 0;
+	float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+	if (separationB > 0.0f)
+		return;
+
+	const b2PolygonShape* poly1;	// reference poly
+	const b2PolygonShape* poly2;	// incident poly
+	b2XForm 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;
+		poly2 = polyA;
+		xf1 = xfB;
+		xf2 = xfA;
+		edge1 = edgeB;
+		flip = 1;
+	}
+	else
+	{
+		poly1 = polyA;
+		poly2 = polyB;
+		xf1 = xfA;
+		xf2 = xfB;
+		edge1 = edgeA;
+		flip = 0;
+	}
+
+	ClipVertex incidentEdge[2];
+	FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
+
+	int32 count1 = poly1->GetVertexCount();
+	const b2Vec2* vertices1 = poly1->GetVertices();
+
+	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);
+	
+	v11 = b2Mul(xf1, v11);
+	v12 = b2Mul(xf1, v12);
+
+	float32 frontOffset = b2Dot(frontNormal, v11);
+	float32 sideOffset1 = -b2Dot(sideNormal, v11);
+	float32 sideOffset2 = b2Dot(sideNormal, v12);
+
+	// Clip incident edge against extruded edge1 side edges.
+	ClipVertex clipPoints1[2];
+	ClipVertex clipPoints2[2];
+	int np;
+
+	// Clip to box side 1
+	np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
+
+	if (np < 2)
+		return;
+
+	// Clip to negative box side 1
+	np = ClipSegmentToLine(clipPoints2, clipPoints1,  sideNormal, sideOffset2);
+
+	if (np < 2)
+		return;
+
+	// Now clipPoints2 contains the clipped points.
+	manifold->normal = flip ? -frontNormal : frontNormal;
+
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
+
+		if (separation <= 0.0f)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			cp->separation = separation;
+			cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
+			cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
+			cp->id = clipPoints2[i].id;
+			cp->id.features.flip = flip;
+			++pointCount;
+		}
+	}
+
+	manifold->pointCount = pointCount;
+}

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

@@ -0,0 +1,72 @@
+/*
+* 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

@@ -0,0 +1,154 @@
+/*
+* 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

@@ -0,0 +1,364 @@
+/*
+* 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

@@ -0,0 +1,396 @@
+/*
+* 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

@@ -0,0 +1,121 @@
+/*
+* 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

@@ -0,0 +1,112 @@
+/*
+* 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

@@ -0,0 +1,477 @@
+/*
+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/b2BlockAllocator.cpp

@@ -0,0 +1,206 @@
+/*
+* 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 "b2BlockAllocator.h"
+#include <cstdlib>
+#include <memory>
+#include <climits>
+#include <string.h>
+
+int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
+{
+	16,		// 0
+	32,		// 1
+	64,		// 2
+	96,		// 3
+	128,	// 4
+	160,	// 5
+	192,	// 6
+	224,	// 7
+	256,	// 8
+	320,	// 9
+	384,	// 10
+	448,	// 11
+	512,	// 12
+	640,	// 13
+};
+uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
+bool b2BlockAllocator::s_blockSizeLookupInitialized;
+
+struct b2Chunk
+{
+	int32 blockSize;
+	b2Block* blocks;
+};
+
+struct b2Block
+{
+	b2Block* next;
+};
+
+b2BlockAllocator::b2BlockAllocator()
+{
+	b2Assert(b2_blockSizes < UCHAR_MAX);
+
+	m_chunkSpace = b2_chunkArrayIncrement;
+	m_chunkCount = 0;
+	m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+	
+	memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+	memset(m_freeLists, 0, sizeof(m_freeLists));
+
+	if (s_blockSizeLookupInitialized == false)
+	{
+		int32 j = 0;
+		for (int32 i = 1; i <= b2_maxBlockSize; ++i)
+		{
+			b2Assert(j < b2_blockSizes);
+			if (i <= s_blockSizes[j])
+			{
+				s_blockSizeLookup[i] = (uint8)j;
+			}
+			else
+			{
+				++j;
+				s_blockSizeLookup[i] = (uint8)j;
+			}
+		}
+
+		s_blockSizeLookupInitialized = true;
+	}
+}
+
+b2BlockAllocator::~b2BlockAllocator()
+{
+	for (int32 i = 0; i < m_chunkCount; ++i)
+	{
+		b2Free(m_chunks[i].blocks);
+	}
+
+	b2Free(m_chunks);
+}
+
+void* b2BlockAllocator::Allocate(int32 size)
+{
+	if (size == 0)
+		return NULL;
+
+	b2Assert(0 < size && size <= b2_maxBlockSize);
+
+	int32 index = s_blockSizeLookup[size];
+	b2Assert(0 <= index && index < b2_blockSizes);
+
+	if (m_freeLists[index])
+	{
+		b2Block* block = m_freeLists[index];
+		m_freeLists[index] = block->next;
+		return block;
+	}
+	else
+	{
+		if (m_chunkCount == m_chunkSpace)
+		{
+			b2Chunk* oldChunks = m_chunks;
+			m_chunkSpace += b2_chunkArrayIncrement;
+			m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
+			memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
+			memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
+			b2Free(oldChunks);
+		}
+
+		b2Chunk* chunk = m_chunks + m_chunkCount;
+		chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
+#if defined(_DEBUG)
+		memset(chunk->blocks, 0xcd, b2_chunkSize);
+#endif
+		int32 blockSize = s_blockSizes[index];
+		chunk->blockSize = blockSize;
+		int32 blockCount = b2_chunkSize / blockSize;
+		b2Assert(blockCount * blockSize <= b2_chunkSize);
+		for (int32 i = 0; i < blockCount - 1; ++i)
+		{
+			b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
+			b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
+			block->next = next;
+		}
+		b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
+		last->next = NULL;
+
+		m_freeLists[index] = chunk->blocks->next;
+		++m_chunkCount;
+
+		return chunk->blocks;
+	}
+}
+
+void b2BlockAllocator::Free(void* p, int32 size)
+{
+	if (size == 0)
+	{
+		return;
+	}
+
+	b2Assert(0 < size && size <= b2_maxBlockSize);
+
+	int32 index = s_blockSizeLookup[size];
+	b2Assert(0 <= index && index < b2_blockSizes);
+
+#ifdef _DEBUG
+	// 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);
+		}
+		else
+		{
+			if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
+			{
+				found = true;
+			}
+		}
+	}
+
+	b2Assert(found);
+
+	memset(p, 0xfd, blockSize);
+#endif
+
+	b2Block* block = (b2Block*)p;
+	block->next = m_freeLists[index];
+	m_freeLists[index] = block;
+}
+
+void b2BlockAllocator::Clear()
+{
+	for (int32 i = 0; i < m_chunkCount; ++i)
+	{
+		b2Free(m_chunks[i].blocks);
+	}
+
+	m_chunkCount = 0;
+	memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
+
+	memset(m_freeLists, 0, sizeof(m_freeLists));
+}

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

@@ -0,0 +1,59 @@
+/*
+* 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_BLOCK_ALLOCATOR_H
+#define B2_BLOCK_ALLOCATOR_H
+
+#include "b2Settings.h"
+
+const int32 b2_chunkSize = 4096;
+const int32 b2_maxBlockSize = 640;
+const int32 b2_blockSizes = 14;
+const int32 b2_chunkArrayIncrement = 128;
+
+struct b2Block;
+struct b2Chunk;
+
+// This is a small object allocator used for allocating small
+// objects that persist for more than one time step.
+// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
+class b2BlockAllocator
+{
+public:
+	b2BlockAllocator();
+	~b2BlockAllocator();
+
+	void* Allocate(int32 size);
+	void Free(void* p, int32 size);
+
+	void Clear();
+
+private:
+
+	b2Chunk* m_chunks;
+	int32 m_chunkCount;
+	int32 m_chunkSpace;
+
+	b2Block* m_freeLists[b2_blockSizes];
+
+	static int32 s_blockSizes[b2_blockSizes];
+	static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];
+	static bool s_blockSizeLookupInitialized;
+};
+
+#endif

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

@@ -0,0 +1,54 @@
+/*
+* 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/b2Math.h

@@ -0,0 +1,636 @@
+/*
+* 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_MATH_H
+#define B2_MATH_H
+
+#include "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
+
+/// 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
+}
+
+/// This is a approximate yet fast inverse square-root.
+inline float32 b2InvSqrt(float32 x)
+{
+	union
+	{
+		float32 x;
+		int32 i;
+	} convert;
+
+	convert.x = x;
+	float32 xhalf = 0.5f * x;
+	convert.i = 0x5f3759df - (convert.i >> 1);
+	x = convert.x;
+	x = x * (1.5f - xhalf * x * x);
+	return 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).
+	b2Vec2() {}
+
+	/// Construct using coordinates.
+	b2Vec2(float32 x, float32 y) : x(x), y(y) {}
+
+	/// Set this vector to all zeros.
+	void SetZero() { x = 0.0f; y = 0.0f; }
+
+	/// Set this vector to some specified coordinates.
+	void Set(float32 x_, float32 y_) { x = x_; y = y_; }
+
+	/// Negate this vector.
+	b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
+	
+	/// Add a vector to this vector.
+	void operator += (const b2Vec2& v)
+	{
+		x += v.x; y += v.y;
+	}
+	
+	/// Subtract a vector from this vector.
+	void operator -= (const b2Vec2& v)
+	{
+		x -= v.x; y -= v.y;
+	}
+
+	/// Multiply this vector by a scalar.
+	void operator *= (float32 a)
+	{
+		x *= a; y *= a;
+	}
+
+	/// 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
+	/// b2Vec2::Length (if possible).
+	float32 LengthSquared() const
+	{
+		return x * x + y * y;
+	}
+
+	/// 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)
+		{
+			return 0.0f;
+		}
+		float32 invLength = 1.0f / length;
+		x *= invLength;
+		y *= invLength;
+
+		return length;
+	}
+#endif
+
+	/// Does this vector contain finite coordinates?
+	bool IsValid() const
+	{
+		return b2IsValid(x) && b2IsValid(y);
+	}
+
+	float32 x, y;
+};
+
+/// A 2-by-2 matrix. Stored in column-major order.
+struct b2Mat22
+{
+	/// The default constructor does nothing (for performance).
+	b2Mat22() {}
+
+	/// Construct this matrix using columns.
+	b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
+	{
+		col1 = c1;
+		col2 = c2;
+	}
+
+	/// Construct this matrix using scalars.
+	b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
+	{
+		col1.x = a11; col1.y = a21;
+		col2.x = a12; col2.y = a22;
+	}
+
+	/// Construct this matrix using an angle. This matrix becomes
+	/// an orthonormal rotation matrix.
+	explicit b2Mat22(float32 angle)
+	{
+		float32 c = cosf(angle), s = sinf(angle);
+		col1.x = c; col2.x = -s;
+		col1.y = s; col2.y = c;
+	}
+
+	/// Initialize this matrix using columns.
+	void Set(const b2Vec2& c1, const b2Vec2& c2)
+	{
+		col1 = c1;
+		col2 = c2;
+	}
+
+	/// Initialize this matrix using an angle. This matrix becomes
+	/// an orthonormal rotation matrix.
+	void Set(float32 angle)
+	{
+		float32 c = cosf(angle), s = sinf(angle);
+		col1.x = c; col2.x = -s;
+		col1.y = s; col2.y = c;
+	}
+
+	/// Set this to the identity matrix.
+	void SetIdentity()
+	{
+		col1.x = 1.0f; col2.x = 0.0f;
+		col1.y = 0.0f; col2.y = 1.0f;
+	}
+
+	/// Set this matrix to all zeros.
+	void SetZero()
+	{
+		col1.x = 0.0f; col2.x = 0.0f;
+		col1.y = 0.0f; col2.y = 0.0f;
+	}
+
+	/// Extract the angle from this matrix (assumed to be
+	/// a rotation matrix).
+	float32 GetAngle() const
+	{
+		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
+	{
+		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;
+		B.col1.x =  det * d;	B.col2.x = -det * b;
+		B.col1.y = -det * c;	B.col2.y =  det * a;
+		return B;
+	}
+
+	/// Solve A * x = b, where b is a column vector. This is more efficient
+	/// than computing the inverse in one-shot cases.
+	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;
+		b2Assert(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
+{
+	/// The default constructor does nothing (for performance).
+	b2XForm() {}
+
+	/// Initialize using a position vector and a rotation matrix.
+	b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
+
+	/// Set this to the identity transform.
+	void SetIdentity()
+	{
+		position.SetZero();
+		R.SetIdentity();
+	}
+
+	b2Vec2 position;
+	b2Mat22 R;
+};
+
+/// This describes the motion of a body/shape for TOI computation.
+/// Shapes are defined with respect to the body origin, which may
+/// no coincide with the center of mass. However, to support dynamics
+/// we must interpolate the center of mass position.
+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;
+
+	/// Advance the sweep forward, yielding a new initial state.
+	/// @param t the new initial time.
+	void Advance(float32 t);
+
+	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;
+
+/// Peform the dot product on two vectors.
+inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x * b.x + a.y * b.y;
+}
+
+/// Perform the cross product on two vectors. In 2D this produces a scalar.
+inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x * b.y - a.y * b.x;
+}
+
+/// Perform the cross product on a vector and a scalar. In 2D this produces
+/// a vector.
+inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
+{
+	b2Vec2 v; v.Set(s * a.y, -s * a.x);
+	return v;
+}
+
+/// 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;
+}
+
+/// 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;
+}
+
+/// 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;
+}
+
+/// 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;
+}
+
+/// 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;
+}
+
+inline b2Vec2 operator * (float32 s, const b2Vec2& a)
+{
+	b2Vec2 v; v.Set(s * a.x, s * a.y);
+	return v;
+}
+
+inline bool operator == (const b2Vec2& a, const b2Vec2& b)
+{
+	return a.x == b.x && a.y == b.y;
+}
+
+inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
+{
+	b2Vec2 c = a - b;
+	return c.Length();
+}
+
+inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
+{
+	b2Vec2 c = a - b;
+	return b2Dot(c, c);
+}
+
+inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
+{
+	b2Mat22 C;
+	C.Set(A.col1 + B.col1, A.col2 + B.col2);
+	return C;
+}
+
+// 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;
+}
+
+// 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;
+}
+
+inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
+{
+	return T.position + b2Mul(T.R, v);
+}
+
+inline b2Vec2 b2MulT(const b2XForm& 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;
+}
+
+inline b2Mat22 b2Abs(const b2Mat22& A)
+{
+	b2Mat22 B;
+	B.Set(b2Abs(A.col1), b2Abs(A.col2));
+	return B;
+}
+
+template <typename T>
+inline T b2Min(T a, T b)
+{
+	return a < b ? a : 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;
+}
+
+template <typename T>
+inline T b2Max(T a, T b)
+{
+	return a > b ? a : 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;
+}
+
+template <typename T>
+inline T b2Clamp(T a, T low, T high)
+{
+	return b2Max(low, b2Min(a, high));
+}
+
+inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
+{
+	return b2Max(low, b2Min(a, high));
+}
+
+template<typename T> inline void b2Swap(T& a, T& b)
+{
+	T tmp = a;
+	a = 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
+/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
+/// largest power of 2. For a 32-bit value:"
+inline uint32 b2NextPowerOfTwo(uint32 x)
+{
+	x |= (x >> 1);
+	x |= (x >> 2);
+	x |= (x >> 4);
+	x |= (x >> 8);
+	x |= (x >> 16);
+	return x + 1;
+}
+
+inline bool b2IsPowerOfTwo(uint32 x)
+{
+	bool result = x > 0 && (x & (x - 1)) == 0;
+	return result;
+}
+
+#endif

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

@@ -0,0 +1,51 @@
+/*
+* 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 "b2Settings.h"
+#include <cstdlib>
+
+b2Version b2_version = {2, 0, 1};
+
+int32 b2_byteCount = 0;
+
+
+
+// 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);
+}

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

@@ -0,0 +1,176 @@
+/*
+* 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_SETTINGS_H
+#define B2_SETTINGS_H
+
+#include <assert.h>
+#include <cmath>
+
+#define B2_NOT_USED(x) x
+#define b2Assert(A) assert(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;
+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;
+
+/// @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
+
+/// 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
+
+/// 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
+
+/// 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;
+
+/// Maximum number of contacts to be handled to solve a TOI island.
+const int32 b2_maxTOIContactsPerIsland = 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
+
+/// The maximum linear position correction used when solving constraints. This helps to
+/// prevent overshoot.
+const float32 b2_maxLinearCorrection = 0.2f;	// 20 cm
+
+/// 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
+
+/// 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
+
+/// 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
+
+/// 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;
+
+// Sleep
+
+/// The time that a body must be still before it will go to sleep.
+const float32 b2_timeToSleep = 0.5f;									// half a second
+
+/// A body cannot sleep if its linear velocity is above this tolerance.
+const float32 b2_linearSleepTolerance = 0.01f;		// 1 cm/s
+
+/// A body cannot sleep if its angular velocity is above this tolerance.
+const float32 b2_angularSleepTolerance = 2.0f / 180.0f;		// 2 degrees/s
+
+// 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);
+
+/// If you implement b2Alloc, you should also implement this function.
+void b2Free(void* mem);
+
+/// Version numbering scheme.
+/// See http://en.wikipedia.org/wiki/Software_versioning
+struct b2Version
+{
+	int32 major;		///< significant changes
+	int32 minor;		///< incremental changes
+	int32 revision;		///< bug fixes
+};
+
+/// Current version.
+extern b2Version b2_version;
+
+/// Friction mixing law. Feel free to customize this.
+inline float32 b2MixFriction(float32 friction1, float32 friction2)
+{
+	return sqrtf(friction1 * friction2);
+}
+
+/// Restitution mixing law. Feel free to customize this.
+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
+{
+	return restitution1 > restitution2 ? restitution1 : restitution2;
+}
+
+#endif

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

@@ -0,0 +1,83 @@
+/*
+* 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 "b2StackAllocator.h"
+#include "b2Math.h"
+
+b2StackAllocator::b2StackAllocator()
+{
+	m_index = 0;
+	m_allocation = 0;
+	m_maxAllocation = 0;
+	m_entryCount = 0;
+}
+
+b2StackAllocator::~b2StackAllocator()
+{
+	b2Assert(m_index == 0);
+	b2Assert(m_entryCount == 0);
+}
+
+void* b2StackAllocator::Allocate(int32 size)
+{
+	b2Assert(m_entryCount < b2_maxStackEntries);
+
+	b2StackEntry* entry = m_entries + m_entryCount;
+	entry->size = size;
+	if (m_index + size > b2_stackSize)
+	{
+		entry->data = (char*)b2Alloc(size);
+		entry->usedMalloc = true;
+	}
+	else
+	{
+		entry->data = m_data + m_index;
+		entry->usedMalloc = false;
+		m_index += size;
+	}
+
+	m_allocation += size;
+	m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
+	++m_entryCount;
+
+	return entry->data;
+}
+
+void b2StackAllocator::Free(void* p)
+{
+	b2Assert(m_entryCount > 0);
+	b2StackEntry* entry = m_entries + m_entryCount - 1;
+	b2Assert(p == entry->data);
+	if (entry->usedMalloc)
+	{
+		b2Free(p);
+	}
+	else
+	{
+		m_index -= entry->size;
+	}
+	m_allocation -= entry->size;
+	--m_entryCount;
+
+	p = NULL;
+}
+
+int32 b2StackAllocator::GetMaxAllocation() const
+{
+	return m_maxAllocation;
+}

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

@@ -0,0 +1,60 @@
+/*
+* 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_STACK_ALLOCATOR_H
+#define B2_STACK_ALLOCATOR_H
+
+#include "b2Settings.h"
+
+const int32 b2_stackSize = 100 * 1024;	// 100k
+const int32 b2_maxStackEntries = 32;
+
+struct b2StackEntry
+{
+	char* data;
+	int32 size;
+	bool usedMalloc;
+};
+
+// This is a stack allocator used for fast per step allocations.
+// You must nest allocate/free pairs. The code will assert
+// if you try to interleave multiple allocate/free pairs.
+class b2StackAllocator
+{
+public:
+	b2StackAllocator();
+	~b2StackAllocator();
+
+	void* Allocate(int32 size);
+	void Free(void* p);
+
+	int32 GetMaxAllocation() const;
+
+private:
+
+	char m_data[b2_stackSize];
+	int32 m_index;
+
+	int32 m_allocation;
+	int32 m_maxAllocation;
+
+	b2StackEntry m_entries[b2_maxStackEntries];
+	int32 m_entryCount;
+};
+
+#endif

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

@@ -0,0 +1,139 @@
+/*---------------------------------------------------------------------------------
+	$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

@@ -0,0 +1,122 @@
+/*
+* 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/b2CircleContact.h

@@ -0,0 +1,46 @@
+/*
+* 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 CIRCLE_CONTACT_H
+#define CIRCLE_CONTACT_H
+
+#include "../../Common/b2Math.h"
+#include "../../Collision/b2Collision.h"
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2CircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2CircleContact(b2Shape* shape1, b2Shape* shape2);
+	~b2CircleContact() {}
+
+	void Evaluate(b2ContactListener* listener);
+	b2Manifold* GetManifolds()
+	{
+		return &m_manifold;
+	}
+
+	b2Manifold m_manifold;
+};
+
+#endif

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

@@ -0,0 +1,172 @@
+/*
+* 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

@@ -0,0 +1,183 @@
+/*
+* 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

@@ -0,0 +1,360 @@
+/*
+* 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/b2ContactSolver.h

@@ -0,0 +1,77 @@
+/*
+* 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_SOLVER_H
+#define CONTACT_SOLVER_H
+
+#include "../../Common/b2Math.h"
+#include "../../Collision/b2Collision.h"
+#include "../b2World.h"
+
+class b2Contact;
+class b2Body;
+class b2Island;
+class b2StackAllocator;
+
+struct b2ContactConstraintPoint
+{
+	b2Vec2 localAnchor1;
+	b2Vec2 localAnchor2;
+	b2Vec2 r1;
+	b2Vec2 r2;
+	float32 normalImpulse;
+	float32 tangentImpulse;
+	float32 positionImpulse;
+	float32 normalMass;
+	float32 tangentMass;
+	float32 equalizedMass;
+	float32 separation;
+	float32 velocityBias;
+};
+
+struct b2ContactConstraint
+{
+	b2ContactConstraintPoint points[b2_maxManifoldPoints];
+	b2Vec2 normal;
+	b2Manifold* manifold;
+	b2Body* body1;
+	b2Body* body2;
+	float32 friction;
+	float32 restitution;
+	int32 pointCount;
+};
+
+class b2ContactSolver
+{
+public:
+	b2ContactSolver(const b2TimeStep& step, b2Contact** contacts, int32 contactCount, b2StackAllocator* allocator);
+	~b2ContactSolver();
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints();
+	void FinalizeVelocityConstraints();
+
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2TimeStep m_step;
+	b2StackAllocator* m_allocator;
+	b2ContactConstraint* m_constraints;
+	int m_constraintCount;
+};
+
+#endif

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

@@ -0,0 +1,33 @@
+/*
+* 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_NULL_CONTACT_H
+#define B2_NULL_CONTACT_H
+
+#include "../../Common/b2Math.h"
+#include "b2Contact.h"
+
+class b2NullContact : public b2Contact
+{
+public:
+	b2NullContact() {}
+	void Evaluate(b2ContactListener*) {}
+	b2Manifold* GetManifolds() { return NULL; }
+};
+
+#endif

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

@@ -0,0 +1,158 @@
+/*
+* 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/b2PolyAndCircleContact.h

@@ -0,0 +1,44 @@
+/*
+* 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 POLY_AND_CIRCLE_CONTACT_H
+#define POLY_AND_CIRCLE_CONTACT_H
+
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolyAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolyAndCircleContact(b2Shape* shape1, b2Shape* shape2);
+	~b2PolyAndCircleContact() {}
+
+	void Evaluate(b2ContactListener* listener);
+	b2Manifold* GetManifolds()
+	{
+		return &m_manifold;
+	}
+
+	b2Manifold m_manifold;
+};
+
+#endif

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

@@ -0,0 +1,157 @@
+/*
+* 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);
+	}
+}

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

@@ -0,0 +1,44 @@
+/*
+* 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 POLYCONTACT_H
+#define POLYCONTACT_H
+
+#include "b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Shape* shape1, b2Shape* shape2, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolygonContact(b2Shape* shape1, b2Shape* shape2);
+	~b2PolygonContact() {}
+
+	void Evaluate(b2ContactListener* listener);
+	b2Manifold* GetManifolds()
+	{
+		return &m_manifold;
+	}
+
+	b2Manifold m_manifold;
+};
+
+#endif

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

@@ -0,0 +1,206 @@
+/*
+* 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 "b2DistanceJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// 1-D constrained system
+// m (v2 - v1) = lambda
+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
+// x2 = x1 + h * v2
+
+// 1-D mass-damper-spring system
+// m (v2 - v1) + h * d * v2 + h * k * 
+
+// C = norm(p2 - p1) - L
+// u = (p2 - p1) / norm(p2 - p1)
+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// J = [-u -cross(r1, u) u cross(r2, u)]
+// K = J * invM * JT
+//   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
+
+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);
+	b2Vec2 d = anchor2 - anchor1;
+	length = d.Length();
+}
+
+
+b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	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;
+
+	// 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());
+	m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	// Handle singularity.
+	float32 length = m_u.Length();
+	if (length > b2_linearSlop)
+	{
+		m_u *= 1.0f / length;
+	}
+	else
+	{
+		m_u.Set(0.0f, 0.0f);
+	}
+
+	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;
+
+	if (m_frequencyHz > 0.0f)
+	{
+		float32 C = length - m_length;
+
+		// Frequency
+		float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+		// Damping coefficient
+		float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+
+		// Spring stiffness
+		float32 k = m_mass * omega * omega;
+
+		// magic formulas
+		m_gamma = 1.0f / (step.dt * (d + step.dt * k));
+		m_bias = C * step.dt * k * m_gamma;
+
+		m_mass = 1.0f / (invMass + m_gamma);
+	}
+
+	if (step.warmStarting)
+	{
+		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);
+		b2->m_linearVelocity += b2->m_invMass * P;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+	}
+	else
+	{
+		m_impulse = 0.0f;
+	}
+}
+
+void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// Cdot = dot(u, v + cross(w, r))
+	b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+	b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+	float32 Cdot = b2Dot(m_u, v2 - v1);
+
+	float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+	m_impulse += impulse;
+
+	b2Vec2 P = impulse * m_u;
+	b1->m_linearVelocity -= b1->m_invMass * P;
+	b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+	b2->m_linearVelocity += b2->m_invMass * P;
+	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+}
+
+bool b2DistanceJoint::SolvePositionConstraints()
+{
+	if (m_frequencyHz > 0.0f)
+	{
+		return true;
+	}
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	float32 length = d.Normalize();
+	float32 C = length - m_length;
+	C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+
+	float32 impulse = -m_mass * C;
+	m_u = d;
+	b2Vec2 P = impulse * m_u;
+
+	b1->m_sweep.c -= b1->m_invMass * P;
+	b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
+	b2->m_sweep.c += b2->m_invMass * P;
+	b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	return b2Abs(C) < b2_linearSlop;
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2DistanceJoint::GetReactionForce() const
+{
+	b2Vec2 F = (m_inv_dt * m_impulse) * m_u;
+	return F;
+}
+
+float32 b2DistanceJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}

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

@@ -0,0 +1,96 @@
+/*
+* 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_DISTANCE_JOINT_H
+#define B2_DISTANCE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Distance joint definition. This requires defining an
+/// anchor point on both bodies and the non-zero length of the
+/// distance joint. The definition uses local anchor points
+/// so that the initial configuration can violate the constraint
+/// slightly. This helps when saving and loading a game.
+/// @warning Do not use a zero or short length.
+struct b2DistanceJointDef : public b2JointDef
+{
+	b2DistanceJointDef()
+	{
+		type = e_distanceJoint;
+		localAnchor1.Set(0.0f, 0.0f);
+		localAnchor2.Set(0.0f, 0.0f);
+		length = 1.0f;
+		frequencyHz = 0.0f;
+		dampingRatio = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, and length using the world
+	/// anchors.
+	void Initialize(b2Body* body1, b2Body* body2,
+					const b2Vec2& anchor1, const b2Vec2& anchor2);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The equilibrium length between the anchor points.
+	float32 length;
+
+	/// The response speed.
+	float32 frequencyHz;
+
+	/// The damping ratio. 0 = no damping, 1 = critical damping.
+	float32 dampingRatio;
+};
+
+/// A distance joint constrains two points on two bodies
+/// to remain at a fixed distance from each other. You can view
+/// this as a massless, rigid rod.
+class b2DistanceJoint : public b2Joint
+{
+public:
+
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	//--------------- Internals Below -------------------
+
+	b2DistanceJoint(const b2DistanceJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_u;
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+	float32 m_gamma;
+	float32 m_bias;
+	float32 m_impulse;
+	float32 m_mass;		// effective mass for the constraint.
+	float32 m_length;
+};
+
+#endif

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

@@ -0,0 +1,253 @@
+/*
+* 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 "b2GearJoint.h"
+#include "b2RevoluteJoint.h"
+#include "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// C = C0 - (cordinate1 + ratio * coordinate2) = 0
+// Cdot = -(Cdot1 + ratio * Cdot2)
+// J = -[J1 ratio * J2]
+// K = J * invM * JT
+//   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
+//
+// Revolute:
+// coordinate = rotation
+// Cdot = angularVelocity
+// J = [0 0 1]
+// K = J * invM * JT = invI
+//
+// Prismatic:
+// coordinate = dot(p - pg, ug)
+// Cdot = dot(v + cross(w, r), ug)
+// J = [ug cross(r, ug)]
+// K = J * invM * JT = invMass + invI * cross(r, ug)^2
+
+b2GearJoint::b2GearJoint(const b2GearJointDef* def)
+: b2Joint(def)
+{
+	b2JointType type1 = def->joint1->GetType();
+	b2JointType type2 = def->joint2->GetType();
+
+	b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
+	b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
+	b2Assert(def->joint1->GetBody1()->IsStatic());
+	b2Assert(def->joint2->GetBody1()->IsStatic());
+
+	m_revolute1 = NULL;
+	m_prismatic1 = NULL;
+	m_revolute2 = NULL;
+	m_prismatic2 = NULL;
+
+	float32 coordinate1, coordinate2;
+
+	m_ground1 = def->joint1->GetBody1();
+	m_body1 = def->joint1->GetBody2();
+	if (type1 == e_revoluteJoint)
+	{
+		m_revolute1 = (b2RevoluteJoint*)def->joint1;
+		m_groundAnchor1 = m_revolute1->m_localAnchor1;
+		m_localAnchor1 = m_revolute1->m_localAnchor2;
+		coordinate1 = m_revolute1->GetJointAngle();
+	}
+	else
+	{
+		m_prismatic1 = (b2PrismaticJoint*)def->joint1;
+		m_groundAnchor1 = m_prismatic1->m_localAnchor1;
+		m_localAnchor1 = m_prismatic1->m_localAnchor2;
+		coordinate1 = m_prismatic1->GetJointTranslation();
+	}
+
+	m_ground2 = def->joint2->GetBody1();
+	m_body2 = def->joint2->GetBody2();
+	if (type2 == e_revoluteJoint)
+	{
+		m_revolute2 = (b2RevoluteJoint*)def->joint2;
+		m_groundAnchor2 = m_revolute2->m_localAnchor1;
+		m_localAnchor2 = m_revolute2->m_localAnchor2;
+		coordinate2 = m_revolute2->GetJointAngle();
+	}
+	else
+	{
+		m_prismatic2 = (b2PrismaticJoint*)def->joint2;
+		m_groundAnchor2 = m_prismatic2->m_localAnchor1;
+		m_localAnchor2 = m_prismatic2->m_localAnchor2;
+		coordinate2 = m_prismatic2->GetJointTranslation();
+	}
+
+	m_ratio = def->ratio;
+
+	m_constant = coordinate1 + m_ratio * coordinate2;
+
+	m_force = 0.0f;
+}
+
+void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* g1 = m_ground1;
+	b2Body* g2 = m_ground2;
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 K = 0.0f;
+	m_J.SetZero();
+
+	if (m_revolute1)
+	{
+		m_J.angular1 = -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());
+		float32 crug = b2Cross(r, ug);
+		m_J.linear1 = -ug;
+		m_J.angular1 = -crug;
+		K += b1->m_invMass + b1->m_invI * crug * crug;
+	}
+
+	if (m_revolute2)
+	{
+		m_J.angular2 = -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());
+		float32 crug = b2Cross(r, ug);
+		m_J.linear2 = -m_ratio * ug;
+		m_J.angular2 = -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;
+
+	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;
+	}
+	else
+	{
+		m_force = 0.0f;
+	}
+}
+
+void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	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 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;
+}
+
+bool b2GearJoint::SolvePositionConstraints()
+{
+	float32 linearError = 0.0f;
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 coordinate1, coordinate2;
+	if (m_revolute1)
+	{
+		coordinate1 = m_revolute1->GetJointAngle();
+	}
+	else
+	{
+		coordinate1 = m_prismatic1->GetJointTranslation();
+	}
+
+	if (m_revolute2)
+	{
+		coordinate2 = m_revolute2->GetJointAngle();
+	}
+	else
+	{
+		coordinate2 = m_prismatic2->GetJointTranslation();
+	}
+
+	float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
+
+	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->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2GearJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2GearJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2GearJoint::GetReactionForce() const
+{
+	// TODO_ERIN not tested
+	b2Vec2 F = B2FORCE_SCALE(m_force) * m_J.linear2;
+	return F;
+}
+
+float32 b2GearJoint::GetReactionTorque() 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;
+}
+
+float32 b2GearJoint::GetRatio() const
+{
+	return m_ratio;
+}
+
+

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

@@ -0,0 +1,109 @@
+/*
+* 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_GEAR_JOINT_H
+#define B2_GEAR_JOINT_H
+
+#include "b2Joint.h"
+
+class b2RevoluteJoint;
+class b2PrismaticJoint;
+
+/// Gear joint definition. This definition requires two existing
+/// revolute or prismatic joints (any combination will work).
+/// The provided joints must attach a dynamic body to a static body.
+struct b2GearJointDef : public b2JointDef
+{
+	b2GearJointDef()
+	{
+		type = e_gearJoint;
+		joint1 = NULL;
+		joint2 = NULL;
+		ratio = 1.0f;
+	}
+
+	/// The first revolute/prismatic joint attached to the gear joint.
+	b2Joint* joint1;
+
+	/// The second revolute/prismatic joint attached to the gear joint.
+	b2Joint* joint2;
+
+	/// The gear ratio.
+	/// @see b2GearJoint for explanation.
+	float32 ratio;
+};
+
+/// A gear joint is used to connect two joints together. Either joint
+/// can be a revolute or prismatic joint. You specify a gear ratio
+/// to bind the motions together:
+/// coordinate1 + ratio * coordinate2 = constant
+/// The ratio can be negative or positive. If one joint is a revolute joint
+/// and the other joint is a prismatic joint, then the ratio will have units
+/// of length or units of 1/length.
+/// @warning The revolute and prismatic joints must be attached to
+/// fixed bodies (which must be body1 on those joints).
+class b2GearJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the gear ratio.
+	float32 GetRatio() const;
+
+	//--------------- Internals Below -------------------
+
+	b2GearJoint(const b2GearJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Body* m_ground1;
+	b2Body* m_ground2;
+
+	// One of these is NULL.
+	b2RevoluteJoint* m_revolute1;
+	b2PrismaticJoint* m_prismatic1;
+
+	// One of these is NULL.
+	b2RevoluteJoint* m_revolute2;
+	b2PrismaticJoint* m_prismatic2;
+
+	b2Vec2 m_groundAnchor1;
+	b2Vec2 m_groundAnchor2;
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+
+	b2Jacobian m_J;
+
+	float32 m_constant;
+	float32 m_ratio;
+
+	// Effective mass
+	float32 m_mass;
+
+	// Impulse for accumulation/warm starting.
+	float32 m_force;
+};
+
+#endif

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

@@ -0,0 +1,134 @@
+/*
+* 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 "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 <new>
+
+b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
+{
+	b2Joint* joint = NULL;
+
+	switch (def->type)
+	{
+	case e_distanceJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
+			joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
+		}
+		break;
+
+	case e_mouseJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2MouseJoint));
+			joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
+		}
+		break;
+
+	case e_prismaticJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
+			joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
+		}
+		break;
+
+	case e_revoluteJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
+			joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
+		}
+		break;
+
+	case e_pulleyJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
+			joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
+		}
+		break;
+
+	case e_gearJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2GearJoint));
+			joint = new (mem) b2GearJoint((b2GearJointDef*)def);
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	return joint;
+}
+
+void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
+{
+	joint->~b2Joint();
+	switch (joint->m_type)
+	{
+	case e_distanceJoint:
+		allocator->Free(joint, sizeof(b2DistanceJoint));
+		break;
+
+	case e_mouseJoint:
+		allocator->Free(joint, sizeof(b2MouseJoint));
+		break;
+
+	case e_prismaticJoint:
+		allocator->Free(joint, sizeof(b2PrismaticJoint));
+		break;
+
+	case e_revoluteJoint:
+		allocator->Free(joint, sizeof(b2RevoluteJoint));
+		break;
+
+	case e_pulleyJoint:
+		allocator->Free(joint, sizeof(b2PulleyJoint));
+		break;
+
+	case e_gearJoint:
+		allocator->Free(joint, sizeof(b2GearJoint));
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+	m_type = def->type;
+	m_prev = NULL;
+	m_next = NULL;
+	m_body1 = def->body1;
+	m_body2 = def->body2;
+	m_collideConnected = def->collideConnected;
+	m_islandFlag = false;
+	m_userData = def->userData;
+}

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

@@ -0,0 +1,221 @@
+/*
+* 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 JOINT_H
+#define JOINT_H
+
+#include "../../Common/b2Math.h"
+
+class b2Body;
+class b2Joint;
+struct b2TimeStep;
+class b2BlockAllocator;
+
+enum b2JointType
+{
+	e_unknownJoint,
+	e_revoluteJoint,
+	e_prismaticJoint,
+	e_distanceJoint,
+	e_pulleyJoint,
+	e_mouseJoint,
+	e_gearJoint
+};
+
+enum b2LimitState
+{
+	e_inactiveLimit,
+	e_atLowerLimit,
+	e_atUpperLimit,
+	e_equalLimits
+};
+
+struct b2Jacobian
+{
+	b2Vec2 linear1;
+	float32 angular1;
+	b2Vec2 linear2;
+	float32 angular2;
+
+	void SetZero();
+	void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+	float32 Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+};
+
+/// A joint edge is used to connect bodies and joints together
+/// in a joint graph where each body is a node and each joint
+/// is an edge. A joint edge belongs to a doubly linked list
+/// maintained in each attached body. Each joint has two joint
+/// nodes, one for each attached body.
+struct b2JointEdge
+{
+	b2Body* other;			///< provides quick access to the other body attached.
+	b2Joint* joint;			///< the joint
+	b2JointEdge* prev;		///< the previous joint edge in the body's joint list
+	b2JointEdge* next;		///< the next joint edge in the body's joint list
+};
+
+/// Joint definitions are used to construct joints.
+struct b2JointDef
+{
+	b2JointDef()
+	{
+		type = e_unknownJoint;
+		userData = NULL;
+		body1 = NULL;
+		body2 = NULL;
+		collideConnected = false;
+	}
+
+	/// The joint type is set automatically for concrete joint types.
+	b2JointType type;
+
+	/// Use this to attach application specific data to your joints.
+	void* userData;
+
+	/// The first attached body.
+	b2Body* body1;
+
+	/// The second attached body.
+	b2Body* body2;
+
+	/// Set this flag to true if the attached bodies should collide.
+	bool collideConnected;
+};
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+class b2Joint
+{
+public:
+
+	/// Get the type of the concrete joint.
+	b2JointType GetType() const;
+
+	/// Get the first body attached to this joint.
+	b2Body* GetBody1();
+
+	/// Get the second body attached to this joint.
+	b2Body* GetBody2();
+
+	/// Get the anchor point on body1 in world coordinates.
+	virtual b2Vec2 GetAnchor1() const = 0;
+
+	/// Get the anchor point on body2 in world coordinates.
+	virtual b2Vec2 GetAnchor2() const = 0;
+
+	/// Get the reaction force on body2 at the joint anchor.
+	virtual b2Vec2 GetReactionForce() const = 0;
+
+	/// Get the reaction torque on body2.
+	virtual float32 GetReactionTorque() const = 0;
+
+	/// Get the next joint the world joint list.
+	b2Joint* GetNext();
+
+	/// Get the user data pointer.
+	void* GetUserData();
+
+	/// Set the user data pointer.
+	void SetUserData(void* data);
+
+	//--------------- Internals Below -------------------
+protected:
+	friend class b2World;
+	friend class b2Body;
+	friend class b2Island;
+
+	static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
+	static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
+
+	b2Joint(const b2JointDef* def);
+	virtual ~b2Joint() {}
+
+	virtual void InitVelocityConstraints(const b2TimeStep& step) = 0;
+	virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
+
+	// This returns true if the position errors are within tolerance.
+	virtual void InitPositionConstraints() {}
+	virtual bool SolvePositionConstraints() = 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;
+
+	bool m_islandFlag;
+	
+
+	void* m_userData;
+public:
+	bool m_collideConnected;
+};
+
+inline void b2Jacobian::SetZero()
+{
+	linear1.SetZero(); angular1 = 0.0f;
+	linear2.SetZero(); angular2 = 0.0f;
+}
+
+inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+	linear1 = x1; angular1 = a1;
+	linear2 = x2; angular2 = 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;
+}
+
+inline b2JointType b2Joint::GetType() const
+{
+	return m_type;
+}
+
+inline b2Body* b2Joint::GetBody1()
+{
+	return m_body1;
+}
+
+inline b2Body* b2Joint::GetBody2()
+{
+	return m_body2;
+}
+
+inline b2Joint* b2Joint::GetNext()
+{
+	return m_next;
+}
+
+inline void* b2Joint::GetUserData()
+{
+	return m_userData;
+}
+
+inline void b2Joint::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+#endif

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

@@ -0,0 +1,146 @@
+/*
+* 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 "b2MouseJoint.h"
+#include "../b2Body.h"
+#include "../b2World.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)
+{
+	m_target = def->target;
+	m_localAnchor = b2MulT(m_body2->GetXForm(), m_target);
+
+	m_maxForce = B2FORCE_INV_SCALE(def->maxForce);
+	m_impulse.SetZero();
+
+	float32 mass = m_body2->m_mass;
+
+	// Frequency
+	float32 omega = 2.0f * b2_pi * def->frequencyHz;
+
+	// Damping coefficient
+	float32 d = 2.0f * mass * def->dampingRatio * omega;
+
+	// Spring stiffness
+	float32 k = (def->timeStep * mass) * (omega * omega);
+
+	// magic formulas
+	b2Assert(d + k > B2_FLT_EPSILON);
+	m_gamma = 1.0f / (d + k);
+	m_beta = k / (d + k);
+}
+
+void b2MouseJoint::SetTarget(const b2Vec2& target)
+{
+	if (m_body2->IsSleeping())
+	{
+		m_body2->WakeUp();
+	}
+	m_target = target;
+}
+
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_body2;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r = b2Mul(b->GetXForm().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.Invert();
+
+	m_C = b->m_sweep.c + r - m_target;
+
+	// Cheat with some damping
+	b->m_angularVelocity *= 0.98f;
+
+	// Warm starting.
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * m_impulse;
+	b->m_linearVelocity += invMass * P;
+	b->m_angularVelocity += invI * b2Cross(r, P);
+}
+
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_body2;
+
+	b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
+
+	// Cdot = v + cross(w, r)
+	b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+	b2Vec2 force = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C + B2FORCE_SCALE(step.dt) * (m_gamma * m_impulse));
+
+	b2Vec2 oldForce = m_impulse;
+	m_impulse += force;
+	float32 forceMagnitude = m_impulse.Length();
+	if (forceMagnitude > m_maxForce)
+	{
+		m_impulse *= m_maxForce / forceMagnitude;
+	}
+	force = m_impulse - oldForce;
+
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * force;
+	b->m_linearVelocity += b->m_invMass * P;
+	b->m_angularVelocity += b->m_invI * b2Cross(r, P);
+}
+
+b2Vec2 b2MouseJoint::GetAnchor1() const
+{
+	return m_target;
+}
+
+b2Vec2 b2MouseJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor);
+}
+
+b2Vec2 b2MouseJoint::GetReactionForce() const
+{
+	return B2FORCE_SCALE(float32(1.0))*m_impulse;
+}
+
+float32 b2MouseJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}

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

@@ -0,0 +1,102 @@
+/*
+* 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_MOUSE_JOINT_H
+#define B2_MOUSE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Mouse joint definition. This requires a world target point,
+/// tuning parameters, and the time step.
+struct b2MouseJointDef : public b2JointDef
+{
+	b2MouseJointDef()
+	{
+		type = e_mouseJoint;
+		target.Set(0.0f, 0.0f);
+		maxForce = 0.0f;
+		frequencyHz = 5.0f;
+		dampingRatio = 0.7f;
+		timeStep = 1.0f / 60.0f;
+	}
+
+	/// The initial world target point. This is assumed
+	/// to coincide with the body anchor initially.
+	b2Vec2 target;
+
+	/// The maximum constraint force that can be exerted
+	/// to move the candidate body. Usually you will express
+	/// as some multiple of the weight (multiplier * mass * gravity).
+	float32 maxForce;
+
+	/// The response speed.
+	float32 frequencyHz;
+
+	/// 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.
+class b2MouseJoint : public b2Joint
+{
+public:
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchor1() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchor2() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetReactionForce() const;
+
+	/// Implements b2Joint.
+	float32 GetReactionTorque() const;
+
+	/// Use this to update the target point.
+	void SetTarget(const b2Vec2& target);
+
+	//--------------- Internals Below -------------------
+
+	b2MouseJoint(const b2MouseJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints()
+	{
+		return true;
+	}
+
+	b2Vec2 m_localAnchor;
+	b2Vec2 m_target;
+	b2Vec2 m_impulse;
+
+	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
+};
+
+#endif

src/modules/physics/box2d/Source/Dynamics/Joints/b2PrismaticJoint.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 "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(ay1, d)
+// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
+// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+
+// 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)]
+
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	body1 = b1;
+	body2 = b2;
+	localAnchor1 = body1->GetLocalPoint(anchor);
+	localAnchor2 = body2->GetLocalPoint(anchor);
+	localAxis1 = body1->GetLocalVector(axis);
+	referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	m_localXAxis1 = def->localAxis1;
+	m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+	m_refAngle = def->referenceAngle;
+
+	m_linearJacobian.SetZero();
+	m_linearMass = 0.0f;
+	m_force = 0.0f;
+
+	m_angularMass = 0.0f;
+	m_torque = 0.0f;
+
+	m_motorJacobian.SetZero();
+	m_motorMass = 0.0;
+	m_motorForce = 0.0f;
+	m_limitForce = 0.0f;
+	m_limitPositionImpulse = 0.0f;
+
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+	m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	// Compute the effective masses.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	// Compute point to line constraint effective mass.
+	// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+	b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+	b2Vec2 e = b2->m_sweep.c + r2 - b1->m_sweep.c;	// e = d + r1
+
+	m_linearJacobian.Set(-ay1, -b2Cross(e, ay1), ay1, b2Cross(r2, ay1));
+	m_linearMass =	invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 +
+					invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
+	b2Assert(m_linearMass > B2_FLT_EPSILON);
+	m_linearMass = 1.0f / m_linearMass;
+
+	// Compute angular constraint effective mass.
+	m_angularMass = invI1 + invI2;
+	if (m_angularMass > B2_FLT_EPSILON)
+	{
+		m_angularMass = 1.0f / m_angularMass;
+	}
+
+	// Compute motor and limit terms.
+	if (m_enableLimit || m_enableMotor)
+	{
+		// The motor and limit share a Jacobian and effective mass.
+		b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+		m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
+		m_motorMass =	invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
+						invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
+		b2Assert(m_motorMass > B2_FLT_EPSILON);
+		m_motorMass = 1.0f / m_motorMass;
+
+		if (m_enableLimit)
+		{
+			b2Vec2 d = e - r1;	// p2 - p1
+			float32 jointTranslation = b2Dot(ax1, 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_limitForce = 0.0f;
+				}
+				m_limitState = e_atLowerLimit;
+			}
+			else if (jointTranslation >= m_upperTranslation)
+			{
+				if (m_limitState != e_atUpperLimit)
+				{
+					m_limitForce = 0.0f;
+				}
+				m_limitState = e_atUpperLimit;
+			}
+			else
+			{
+				m_limitState = e_inactiveLimit;
+				m_limitForce = 0.0f;
+			}
+		}
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorForce = 0.0f;
+	}
+
+	if (m_enableLimit == false)
+	{
+		m_limitForce = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
+		b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
+		float32 L1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
+		float32 L2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
+
+		b1->m_linearVelocity += invMass1 * P1;
+		b1->m_angularVelocity += invI1 * L1;
+
+		b2->m_linearVelocity += invMass2 * P2;
+		b2->m_angularVelocity += invI2 * L2;
+	}
+	else
+	{
+		m_force = 0.0f;
+		m_torque = 0.0f;
+		m_limitForce = 0.0f;
+		m_motorForce = 0.0f;
+	}
+
+	m_limitPositionImpulse = 0.0f;
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	// Solve linear constraint.
+	float32 linearCdot = m_linearJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+	float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_linearMass * linearCdot;
+	m_force += force;
+
+	float32 P = B2FORCE_SCALE(step.dt) * force;
+	b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
+	b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
+
+	b2->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
+	b2->m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
+
+	// Solve angular constraint.
+	float32 angularCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+	float32 torque = -B2FORCE_INV_SCALE(step.inv_dt) * m_angularMass * angularCdot;
+	m_torque += torque;
+
+	float32 L = B2FORCE_SCALE(step.dt) * torque;
+	b1->m_angularVelocity -= invI1 * L;
+	b2->m_angularVelocity += invI2 * L;
+
+	// Solve linear motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 motorCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity) - m_motorSpeed;
+		float32 motorForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * motorCdot;
+		float32 oldMotorForce = m_motorForce;
+		m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
+		motorForce = m_motorForce - oldMotorForce;
+
+		float32 P = B2FORCE_SCALE(step.dt) * motorForce;
+		b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+		b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+		b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+		b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+	}
+
+	// Solve linear limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 limitCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+		float32 limitForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * limitCdot;
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_limitForce += limitForce;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+
+		float32 P = B2FORCE_SCALE(step.dt) * limitForce;
+
+		b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+		b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+		b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+		b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+	}
+}
+
+bool b2PrismaticJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+
+	// Solve linear (point-to-line) constraint.
+	float32 linearC = b2Dot(ay1, d);
+	// Prevent overly large corrections.
+	linearC = b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+	float32 linearImpulse = -m_linearMass * linearC;
+
+	b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
+	b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
+	//b1->SynchronizeTransform(); // updated by angular constraint
+	b2->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
+	b2->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
+	//b2->SynchronizeTransform(); // updated by angular constraint
+
+	float32 positionError = b2Abs(linearC);
+
+	// Solve angular constraint.
+	float32 angularC = b2->m_sweep.a - b1->m_sweep.a - m_refAngle;
+	// Prevent overly large corrections.
+	angularC = b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+	float32 angularImpulse = -m_angularMass * angularC;
+
+	b1->m_sweep.a -= b1->m_invI * angularImpulse;
+	b2->m_sweep.a += b2->m_invI * angularImpulse;
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	float32 angularError = b2Abs(angularC);
+
+	// Solve linear limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+		b2Vec2 d = p2 - p1;
+		b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+
+		float32 translation = b2Dot(ax1, d);
+		float32 limitImpulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			// Prevent large angular corrections
+			float32 limitC = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			positionError = b2Max(positionError, b2Abs(angularC));
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 limitC = translation - m_lowerTranslation;
+			positionError = b2Max(positionError, -limitC);
+
+			// Prevent large linear corrections and allow some slop.
+			limitC = b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 limitC = translation - m_upperTranslation;
+			positionError = b2Max(positionError, limitC);
+
+			// Prevent large linear corrections and allow some slop.
+			limitC = b2Clamp(limitC - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+
+		b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
+		b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
+		b2->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
+		b2->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce() const
+{
+	b2Vec2 ax1 = b2Mul(m_body1->GetXForm().R, m_localXAxis1);
+	b2Vec2 ay1 = b2Mul(m_body1->GetXForm().R, m_localYAxis1);
+
+	return B2FORCE_SCALE(float32(1.0))*(m_limitForce * ax1 + m_force * ay1);
+}
+
+float32 b2PrismaticJoint::GetReactionTorque() const
+{
+	return B2FORCE_SCALE(m_torque);
+}
+
+float32 b2PrismaticJoint::GetJointTranslation() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	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_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().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_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_lowerTranslation = lower;
+	m_upperTranslation = upper;
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+	m_enableMotor = flag;
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+{
+	m_motorSpeed = speed;
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+{
+	m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
+}
+
+float32 b2PrismaticJoint::GetMotorForce() const
+{
+	return m_motorForce;
+}
+
+
+

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

@@ -0,0 +1,176 @@
+/*
+* 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_PRISMATIC_JOINT_H
+#define B2_PRISMATIC_JOINT_H
+
+#include "b2Joint.h"
+
+/// Prismatic 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 b2PrismaticJointDef : public b2JointDef
+{
+	b2PrismaticJointDef()
+	{
+		type = e_prismaticJoint;
+		localAnchor1.SetZero();
+		localAnchor2.SetZero();
+		localAxis1.Set(1.0f, 0.0f);
+		referenceAngle = 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* body1, b2Body* body2, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The local translation axis in body1.
+	b2Vec2 localAxis1;
+
+	/// The constrained angle between the bodies: body2_angle - body1_angle.
+	float32 referenceAngle;
+
+	/// 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 prismatic joint. This joint provides one degree of freedom: translation
+/// along an axis fixed in body1. Relative rotation is prevented. 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 b2PrismaticJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() 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 the maximum motor force, usually in N.
+	void SetMaxMotorForce(float32 force);
+
+	/// Get the current motor force, usually in N.
+	float32 GetMotorForce() const;
+
+	//--------------- Internals Below -------------------
+
+	b2PrismaticJoint(const b2PrismaticJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_localXAxis1;
+	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;
+
+	b2Jacobian m_motorJacobian;
+	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
+	float32 m_motorForce;
+	float32 m_limitForce;
+	float32 m_limitPositionImpulse;
+
+	float32 m_lowerTranslation;
+	float32 m_upperTranslation;
+	float32 m_maxMotorForce;
+	float32 m_motorSpeed;
+	
+	bool m_enableLimit;
+	bool m_enableMotor;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2PrismaticJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

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

@@ -0,0 +1,430 @@
+/*
+* 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 "b2PulleyJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Pulley:
+// length1 = norm(p1 - s1)
+// length2 = norm(p2 - s2)
+// C0 = (length1 + ratio * length2)_initial
+// C = C0 - (length1 + ratio * length2) >= 0
+// u1 = (p1 - s1) / norm(p1 - s1)
+// u2 = (p2 - s2) / norm(p2 - s2)
+// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
+// J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
+// K = J * invM * JT
+//   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
+//
+// Limit:
+// C = maxLength - length
+// u = (p - s) / norm(p - s)
+// Cdot = -dot(u, v + cross(w, r))
+// K = invMass + invI * cross(r, u)^2
+// 0 <= impulse
+
+void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
+				const b2Vec2& ga1, const b2Vec2& ga2,
+				const b2Vec2& anchor1, const b2Vec2& anchor2,
+				float32 r)
+{
+	body1 = b1;
+	body2 = b2;
+	groundAnchor1 = ga1;
+	groundAnchor2 = ga2;
+	localAnchor1 = body1->GetLocalPoint(anchor1);
+	localAnchor2 = body2->GetLocalPoint(anchor2);
+	b2Vec2 d1 = anchor1 - ga1;
+	length1 = d1.Length();
+	b2Vec2 d2 = anchor2 - ga2;
+	length2 = d2.Length();
+	ratio = r;
+	b2Assert(ratio > B2_FLT_EPSILON);
+	float32 C = length1 + ratio * length2;
+	maxLength1 = C - ratio * b2_minPulleyLength;
+	maxLength2 = (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;
+
+	b2Assert(def->ratio != 0.0f);
+	m_ratio = def->ratio;
+
+	m_constant = def->length1 + m_ratio * def->length2;
+
+	m_maxLength1 = b2Min(def->maxLength1, m_constant - m_ratio * b2_minPulleyLength);
+	m_maxLength2 = b2Min(def->maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
+
+	m_force = 0.0f;
+	m_limitForce1 = 0.0f;
+	m_limitForce2 = 0.0f;
+}
+
+void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().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;
+
+	// Get the pulley axes.
+	m_u1 = p1 - s1;
+	m_u2 = p2 - s2;
+
+	float32 length1 = m_u1.Length();
+	float32 length2 = m_u2.Length();
+
+	if (length1 > b2_linearSlop)
+	{
+		m_u1 *= 1.0f / length1;
+	}
+	else
+	{
+		m_u1.SetZero();
+	}
+
+	if (length2 > b2_linearSlop)
+	{
+		m_u2 *= 1.0f / length2;
+	}
+	else
+	{
+		m_u2.SetZero();
+	}
+
+	float32 C = m_constant - length1 - m_ratio * length2;
+	if (C > 0.0f)
+	{
+		m_state = e_inactiveLimit;
+		m_force = 0.0f;
+	}
+	else
+	{
+		m_state = e_atUpperLimit;
+		m_positionImpulse = 0.0f;
+	}
+
+	if (length1 < m_maxLength1)
+	{
+		m_limitState1 = e_inactiveLimit;
+		m_limitForce1 = 0.0f;
+	}
+	else
+	{
+		m_limitState1 = e_atUpperLimit;
+		m_limitPositionImpulse1 = 0.0f;
+	}
+
+	if (length2 < m_maxLength2)
+	{
+		m_limitState2 = e_inactiveLimit;
+		m_limitForce2 = 0.0f;
+	}
+	else
+	{
+		m_limitState2 = e_atUpperLimit;
+		m_limitPositionImpulse2 = 0.0f;
+	}
+
+	// Compute effective mass.
+	float32 cr1u1 = b2Cross(r1, m_u1);
+	float32 cr2u2 = b2Cross(r2, m_u2);
+
+	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);
+	m_limitMass1 = 1.0f / m_limitMass1;
+	m_limitMass2 = 1.0f / m_limitMass2;
+	m_pulleyMass = 1.0f / m_pulleyMass;
+
+	if (step.warmStarting)
+	{
+		// 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;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+	else
+	{
+		m_force = 0.0f;
+		m_limitForce1 = 0.0f;
+		m_limitForce2 = 0.0f;
+	}
+}
+
+void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	if (m_state == e_atUpperLimit)
+	{
+		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+		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;
+
+		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * 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;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+
+	if (m_limitState1 == e_atUpperLimit)
+	{
+		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;
+
+		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+	}
+
+	if (m_limitState2 == e_atUpperLimit)
+	{
+		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;
+
+		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+}
+
+bool b2PulleyJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
+	b2Vec2 s2 = m_ground->GetXForm().position + 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 p1 = b1->m_sweep.c + r1;
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+
+		// Get the pulley axes.
+		m_u1 = p1 - s1;
+		m_u2 = p2 - s2;
+
+		float32 length1 = m_u1.Length();
+		float32 length2 = m_u2.Length();
+
+		if (length1 > b2_linearSlop)
+		{
+			m_u1 *= 1.0f / length1;
+		}
+		else
+		{
+			m_u1.SetZero();
+		}
+
+		if (length2 > b2_linearSlop)
+		{
+			m_u2 *= 1.0f / length2;
+		}
+		else
+		{
+			m_u2.SetZero();
+		}
+
+		float32 C = m_constant - length1 - m_ratio * length2;
+		linearError = b2Max(linearError, -C);
+
+		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;
+
+		b1->m_sweep.c += b1->m_invMass * P1;
+		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+		b2->m_sweep.c += b2->m_invMass * P2;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	if (m_limitState1 == e_atUpperLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+
+		m_u1 = p1 - s1;
+		float32 length1 = m_u1.Length();
+
+		if (length1 > b2_linearSlop)
+		{
+			m_u1 *= 1.0f / length1;
+		}
+		else
+		{
+			m_u1.SetZero();
+		}
+
+		float32 C = m_maxLength1 - length1;
+		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;
+		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+
+		b1->SynchronizeTransform();
+	}
+
+	if (m_limitState2 == e_atUpperLimit)
+	{
+		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+
+		m_u2 = p2 - s2;
+		float32 length2 = m_u2.Length();
+
+		if (length2 > b2_linearSlop)
+		{
+			m_u2 *= 1.0f / length2;
+		}
+		else
+		{
+			m_u2.SetZero();
+		}
+
+		float32 C = m_maxLength2 - length2;
+		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;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+		b2->SynchronizeTransform();
+	}
+
+	return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PulleyJoint::GetReactionForce() const
+{
+	b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
+	return F;
+}
+
+float32 b2PulleyJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor1() const
+{
+	return m_ground->GetXForm().position + m_groundAnchor1;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor2() const
+{
+	return m_ground->GetXForm().position + m_groundAnchor2;
+}
+
+float32 b2PulleyJoint::GetLength1() const
+{
+	b2Vec2 p = m_body1->GetWorldPoint(m_localAnchor1);
+	b2Vec2 s = m_ground->GetXForm().position + 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 d = p - s;
+	return d.Length();
+}
+
+float32 b2PulleyJoint::GetRatio() const
+{
+	return m_ratio;
+}

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

@@ -0,0 +1,153 @@
+/*
+* 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_PULLEY_JOINT_H
+#define B2_PULLEY_JOINT_H
+
+#include "b2Joint.h"
+
+const float32 b2_minPulleyLength = 2.0f;
+
+/// Pulley joint definition. This requires two ground anchors,
+/// two dynamic body anchor points, max lengths for each side,
+/// and a pulley ratio.
+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;
+		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,
+					float32 ratio);
+
+	/// The first ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchor1;
+
+	/// The second ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchor2;
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The a reference length for the segment attached to body1.
+	float32 length1;
+
+	/// The maximum length of the segment attached to body1.
+	float32 maxLength1;
+
+	/// The a reference length for the segment attached to body2.
+	float32 length2;
+
+	/// The maximum length of the segment attached to body2.
+	float32 maxLength2;
+
+	/// The pulley ratio, used to simulate a block-and-tackle.
+	float32 ratio;
+};
+
+/// The pulley joint is connected to two bodies and two fixed ground points.
+/// The pulley supports a ratio such that:
+/// length1 + ratio * length2 <= constant
+/// Yes, the force transmitted is scaled by the ratio.
+/// The pulley also enforces a maximum length limit on both sides. This is
+/// useful to prevent one side of the pulley hitting the top.
+class b2PulleyJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the first ground anchor.
+	b2Vec2 GetGroundAnchor1() const;
+
+	/// Get the second ground anchor.
+	b2Vec2 GetGroundAnchor2() const;
+
+	/// Get the current length of the segment attached to body1.
+	float32 GetLength1() const;
+
+	/// Get the current length of the segment attached to body2.
+	float32 GetLength2() const;
+
+	/// Get the pulley ratio.
+	float32 GetRatio() const;
+
+	//--------------- Internals Below -------------------
+
+	b2PulleyJoint(const b2PulleyJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Body* m_ground;
+	b2Vec2 m_groundAnchor1;
+	b2Vec2 m_groundAnchor2;
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+
+	b2Vec2 m_u1;
+	b2Vec2 m_u2;
+	
+	float32 m_constant;
+	float32 m_ratio;
+	
+	float32 m_maxLength1;
+	float32 m_maxLength2;
+
+	// Effective masses
+	float32 m_pulleyMass;
+	float32 m_limitMass1;
+	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;
+
+	b2LimitState m_state;
+	b2LimitState m_limitState1;
+	b2LimitState m_limitState2;
+};
+
+#endif

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

@@ -0,0 +1,399 @@
+/*
+* 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 "b2RevoluteJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+#include "../b2Island.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)
+{
+	body1 = b1;
+	body2 = b2;
+	localAnchor1 = body1->GetLocalPoint(anchor);
+	localAnchor2 = body2->GetLocalPoint(anchor);
+	referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	m_referenceAngle = def->referenceAngle;
+
+	m_pivotForce.Set(0.0f, 0.0f);
+	m_motorForce = 0.0f;
+	m_limitForce = 0.0f;
+	m_limitPositionImpulse = 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;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	// 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());
+
+	// 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 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	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;
+	m_pivotMass = K.Invert();
+
+	m_motorMass = 1.0f / (invI1 + invI2);
+
+	if (m_enableMotor == false)
+	{
+		m_motorForce = 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_limitForce = 0.0f;
+			}
+			m_limitState = e_atLowerLimit;
+		}
+		else if (jointAngle >= m_upperAngle)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_limitForce = 0.0f;
+			}
+			m_limitState = e_atUpperLimit;
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_limitForce = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitForce = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		b1->m_linearVelocity -= B2FORCE_SCALE(step.dt) * invMass1 * m_pivotForce;
+		b1->m_angularVelocity -= B2FORCE_SCALE(step.dt) * invI1 * (b2Cross(r1, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+
+		b2->m_linearVelocity += B2FORCE_SCALE(step.dt) * invMass2 * m_pivotForce;
+		b2->m_angularVelocity += B2FORCE_SCALE(step.dt) * invI2 * (b2Cross(r2, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+	}
+	else
+	{
+		m_pivotForce.SetZero();
+		m_motorForce = 0.0f;
+		m_limitForce = 0.0f;
+	}
+
+	m_limitPositionImpulse = 0.0f;
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// Solve point-to-point constraint
+	b2Vec2 pivotCdot = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
+	b2Vec2 pivotForce = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_pivotMass, pivotCdot);
+	m_pivotForce += pivotForce;
+
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * pivotForce;
+	b1->m_linearVelocity -= b1->m_invMass * P;
+	b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+
+	b2->m_linearVelocity += b2->m_invMass * P;
+	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 motorCdot = b2->m_angularVelocity - b1->m_angularVelocity - m_motorSpeed;
+		float32 motorForce = -step.inv_dt * m_motorMass * motorCdot;
+		float32 oldMotorForce = m_motorForce;
+		m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
+		motorForce = m_motorForce - oldMotorForce;
+
+		float32 P = step.dt * motorForce;
+		b1->m_angularVelocity -= b1->m_invI * P;
+		b2->m_angularVelocity += b2->m_invI * P;
+	}
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 limitCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+		float32 limitForce = -step.inv_dt * m_motorMass * limitCdot;
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_limitForce += limitForce;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+
+		float32 P = step.dt * limitForce;
+		b1->m_angularVelocity -= b1->m_invI * P;
+		b2->m_angularVelocity += b2->m_invI * P;
+	}
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 positionError = 0.0f;
+
+	// Solve point-to-point position error.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 ptpC = p2 - p1;
+
+	positionError = ptpC.Length();
+
+	// Prevent overly large corrections.
+	//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
+	//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	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(-ptpC);
+
+	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();
+
+	// Handle limits.
+	float32 angularError = 0.0f;
+
+	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 limitC = b2Clamp(angle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			angularError = b2Abs(limitC);
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 limitC = angle - m_lowerAngle;
+			angularError = b2Max(0.0f, -limitC);
+
+			// Prevent large angular corrections and allow some slop.
+			limitC = b2Clamp(limitC + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 limitC = angle - m_upperAngle;
+			angularError = b2Max(0.0f, limitC);
+
+			// Prevent large angular corrections and allow some slop.
+			limitC = b2Clamp(limitC - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+
+		b1->m_sweep.a -= b1->m_invI * limitImpulse;
+		b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce() const
+{
+	return B2FORCE_SCALE(float32(1.0))*m_pivotForce;
+}
+
+float32 b2RevoluteJoint::GetReactionTorque() const
+{
+	return m_limitForce;
+}
+
+float32 b2RevoluteJoint::GetJointAngle() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+	return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+}
+
+float32 b2RevoluteJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+	return b2->m_angularVelocity - b1->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+	m_enableMotor = flag;
+}
+
+float32 b2RevoluteJoint::GetMotorTorque() const
+{
+	return m_motorForce;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+	m_motorSpeed = speed;
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+	m_maxMotorTorque = torque;
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+	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_lowerAngle = lower;
+	m_upperAngle = upper;
+}

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

@@ -0,0 +1,172 @@
+/*
+* 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_REVOLUTE_JOINT_H
+#define B2_REVOLUTE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Revolute joint definition. This requires defining an
+/// anchor point where the bodies are joined. The definition
+/// uses local anchor points so that the initial configuration
+/// can violate the constraint slightly. You also need to
+/// specify the initial relative angle for joint limits. This
+/// helps when saving and loading a game.
+/// The local anchor points are measured from the body's origin
+/// rather than the center of mass because:
+/// 1. you might not know where the center of mass will be.
+/// 2. if you add/remove shapes from a body and recompute the mass,
+///    the joints will be broken.
+struct b2RevoluteJointDef : public b2JointDef
+{
+	b2RevoluteJointDef()
+	{
+		type = e_revoluteJoint;
+		localAnchor1.Set(0.0f, 0.0f);
+		localAnchor2.Set(0.0f, 0.0f);
+		referenceAngle = 0.0f;
+		lowerAngle = 0.0f;
+		upperAngle = 0.0f;
+		maxMotorTorque = 0.0f;
+		motorSpeed = 0.0f;
+		enableLimit = false;
+		enableMotor = false;
+	}
+
+	/// Initialize the bodies, anchors, and reference angle using the world
+	/// anchor.
+	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The body2 angle minus body1 angle in the reference state (radians).
+	float32 referenceAngle;
+
+	/// A flag to enable joint limits.
+	bool enableLimit;
+
+	/// The lower angle for the joint limit (radians).
+	float32 lowerAngle;
+
+	/// The upper angle for the joint limit (radians).
+	float32 upperAngle;
+
+	/// A flag to enable the joint motor.
+	bool enableMotor;
+
+	/// The desired motor speed. Usually in radians per second.
+	float32 motorSpeed;
+
+	/// The maximum motor torque used to achieve the desired motor speed.
+	/// Usually in N-m.
+	float32 maxMotorTorque;
+};
+
+/// A revolute joint constrains to 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
+/// to drive the relative rotation about the shared point. A maximum motor torque
+/// is provided so that infinite forces are not generated.
+class b2RevoluteJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the current joint angle in radians.
+	float32 GetJointAngle() const;
+
+	/// Get the current joint angle speed in radians 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 in radians.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit in radians.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits in radians.
+	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 in radians per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed in radians per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set the maximum motor torque, usually in N-m.
+	void SetMaxMotorTorque(float32 torque);
+
+	/// Get the current motor torque, usually in N-m.
+	float32 GetMotorTorque() const;
+
+	//--------------- Internals Below -------------------
+	b2RevoluteJoint(const b2RevoluteJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;	// relative
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_pivotForce;
+	float32 m_motorForce;
+	float32 m_limitForce;
+	float32 m_limitPositionImpulse;
+
+	b2Mat22 m_pivotMass;		// effective mass for point-to-point constraint.
+	float32 m_motorMass;	// effective mass for motor/limit angular constraint.
+	
+	bool m_enableMotor;
+	float32 m_maxMotorTorque;
+	float32 m_motorSpeed;
+
+	bool m_enableLimit;
+	float32 m_referenceAngle;
+	float32 m_lowerAngle;
+	float32 m_upperAngle;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2RevoluteJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

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

@@ -0,0 +1,394 @@
+/*
+* 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 "b2Body.h"
+#include "b2World.h"
+#include "Joints/b2Joint.h"
+#include "../Collision/Shapes/b2Shape.h"
+
+b2Body::b2Body(const b2BodyDef* bd, b2World* world)
+{
+	b2Assert(world->m_lock == false);
+
+	m_flags = 0;
+
+	if (bd->isBullet)
+	{
+		m_flags |= e_bulletFlag;
+	}
+	if (bd->fixedRotation)
+	{
+		m_flags |= e_fixedRotationFlag;
+	}
+	if (bd->allowSleep)
+	{
+		m_flags |= e_allowSleepFlag;
+	}
+	if (bd->isSleeping)
+	{
+		m_flags |= e_sleepFlag;
+	}
+
+	m_world = world;
+
+	m_xf.position = bd->position;
+	m_xf.R.Set(bd->angle);
+
+	m_sweep.localCenter = bd->massData.center;
+	m_sweep.t0 = 1.0f;
+	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_linearDamping = bd->linearDamping;
+	m_angularDamping = bd->angularDamping;
+
+	m_force.Set(0.0f, 0.0f);
+	m_torque = 0.0f;
+
+	m_linearVelocity.SetZero();
+	m_angularVelocity = 0.0f;
+
+	m_sleepTime = 0.0f;
+
+	m_invMass = 0.0f;
+	m_I = 0.0f;
+	m_invI = 0.0f;
+
+	m_mass = bd->massData.mass;
+
+	if (m_mass > 0.0f)
+	{
+		m_invMass = 1.0f / m_mass;
+	}
+
+	if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
+	{
+		m_I = bd->massData.I;
+	}
+	
+	if (m_I > 0.0f)
+	{
+		m_invI = 1.0f / m_I;
+	}
+
+	if (m_invMass == 0.0f && m_invI == 0.0f)
+	{
+		m_type = e_staticType;
+	}
+	else
+	{
+		m_type = e_dynamicType;
+	}
+
+	m_userData = bd->userData;
+
+	m_shapeList = NULL;
+	m_shapeCount = 0;
+}
+
+b2Body::~b2Body()
+{
+	b2Assert(m_world->m_lock == false);
+	// shapes and joints are destroyed in b2World::Destroy
+}
+
+b2Shape* b2Body::CreateShape(b2ShapeDef* def)
+{
+	b2Assert(m_world->m_lock == false);
+	if (m_world->m_lock == true)
+	{
+		return NULL;
+	}
+
+	b2Shape* s = b2Shape::Create(def, &m_world->m_blockAllocator);
+
+	s->m_next = m_shapeList;
+	m_shapeList = s;
+	++m_shapeCount;
+
+	s->m_body = this;
+
+	// Add the shape to the world's broad-phase.
+	s->CreateProxy(m_world->m_broadPhase, m_xf);
+
+	// Compute the sweep radius for CCD.
+	s->UpdateSweepRadius(m_sweep.localCenter);
+
+	return s;
+}
+
+void b2Body::DestroyShape(b2Shape* s)
+{
+	b2Assert(m_world->m_lock == false);
+	if (m_world->m_lock == true)
+	{
+		return;
+	}
+
+	b2Assert(s->GetBody() == this);
+	s->DestroyProxy(m_world->m_broadPhase);
+
+	b2Assert(m_shapeCount > 0);
+	b2Shape** node = &m_shapeList;
+	bool found = false;
+	while (*node != NULL)
+	{
+		if (*node == s)
+		{
+			*node = s->m_next;
+			found = true;
+			break;
+		}
+
+		node = &(*node)->m_next;
+	}
+
+	// You tried to remove a shape that is not attached to this body.
+	b2Assert(found);
+
+	s->m_body = NULL;
+	s->m_next = NULL;
+
+	--m_shapeCount;
+
+	b2Shape::Destroy(s, &m_world->m_blockAllocator);
+}
+
+// TODO_ERIN adjust linear velocity and torque to account for movement of center.
+void b2Body::SetMass(const b2MassData* massData)
+{
+	b2Assert(m_world->m_lock == false);
+	if (m_world->m_lock == true)
+	{
+		return;
+	}
+
+	m_invMass = 0.0f;
+	m_I = 0.0f;
+	m_invI = 0.0f;
+
+	m_mass = massData->mass;
+
+	if (m_mass > 0.0f)
+	{
+		m_invMass = 1.0f / m_mass;
+	}
+
+	if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
+	{
+		m_I = massData->I;
+	}
+
+	if (m_I > 0.0f)
+	{
+		m_invI = 1.0f / m_I;
+	}
+
+	// Move center of mass.
+	m_sweep.localCenter = massData->center;
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+	// Update the sweep radii of all child shapes.
+	for (b2Shape* s = m_shapeList; s; s = s->m_next)
+	{
+		s->UpdateSweepRadius(m_sweep.localCenter);
+	}
+
+	int16 oldType = m_type;
+	if (m_invMass == 0.0f && m_invI == 0.0f)
+	{
+		m_type = e_staticType;
+	}
+	else
+	{
+		m_type = e_dynamicType;
+	}
+
+	// If the body type changed, we need to refilter the broad-phase proxies.
+	if (oldType != m_type)
+	{
+		for (b2Shape* s = m_shapeList; s; s = s->m_next)
+		{
+			s->RefilterProxy(m_world->m_broadPhase, m_xf);
+		}
+	}
+}
+
+// TODO_ERIN adjust linear velocity and torque to account for movement of center.
+void b2Body::SetMassFromShapes()
+{
+	b2Assert(m_world->m_lock == false);
+	if (m_world->m_lock == true)
+	{
+		return;
+	}
+
+	// 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;
+
+	b2Vec2 center = b2Vec2_zero;
+	for (b2Shape* s = m_shapeList; s; s = s->m_next)
+	{
+		b2MassData massData;
+		s->ComputeMass(&massData);
+		m_mass += massData.mass;
+		center += massData.mass * massData.center;
+		m_I += massData.I;
+	}
+
+	// Compute center of mass, and shift the origin to the COM.
+	if (m_mass > 0.0f)
+	{
+		m_invMass = 1.0f / m_mass;
+		center *= m_invMass;
+	}
+
+	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.
+	m_sweep.localCenter = center;
+	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+
+	// Update the sweep radii of all child shapes.
+	for (b2Shape* s = m_shapeList; s; s = s->m_next)
+	{
+		s->UpdateSweepRadius(m_sweep.localCenter);
+	}
+
+	int16 oldType = m_type;
+	if (m_invMass == 0.0f && m_invI == 0.0f)
+	{
+		m_type = e_staticType;
+	}
+	else
+	{
+		m_type = e_dynamicType;
+	}
+
+	// If the body type changed, we need to refilter the broad-phase proxies.
+	if (oldType != m_type)
+	{
+		for (b2Shape* s = m_shapeList; s; s = s->m_next)
+		{
+			s->RefilterProxy(m_world->m_broadPhase, m_xf);
+		}
+	}
+}
+
+bool b2Body::SetXForm(const b2Vec2& position, float32 angle)
+{
+	b2Assert(m_world->m_lock == false);
+	if (m_world->m_lock == true)
+	{
+		return true;
+	}
+
+	if (IsFrozen())
+	{
+		return false;
+	}
+
+	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;
+
+	bool freeze = false;
+	for (b2Shape* s = m_shapeList; s; s = s->m_next)
+	{
+		bool inRange = s->Synchronize(m_world->m_broadPhase, m_xf, m_xf);
+
+		if (inRange == false)
+		{
+			freeze = true;
+			break;
+		}
+	}
+
+	if (freeze == true)
+	{
+		m_flags |= e_frozenFlag;
+		m_linearVelocity.SetZero();
+		m_angularVelocity = 0.0f;
+		for (b2Shape* s = m_shapeList; s; s = s->m_next)
+		{
+			s->DestroyProxy(m_world->m_broadPhase);
+		}
+
+		// Failure
+		return false;
+	}
+
+	// Success
+	m_world->m_broadPhase->Commit();
+	return true;
+}
+
+bool b2Body::SynchronizeShapes()
+{
+	b2XForm xf1;
+	xf1.R.Set(m_sweep.a0);
+	xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
+
+	bool inRange = true;
+	for (b2Shape* s = m_shapeList; s; s = s->m_next)
+	{
+		inRange = s->Synchronize(m_world->m_broadPhase, xf1, m_xf);
+		if (inRange == false)
+		{
+			break;
+		}
+	}
+
+	if (inRange == false)
+	{
+		m_flags |= e_frozenFlag;
+		m_linearVelocity.SetZero();
+		m_angularVelocity = 0.0f;
+		for (b2Shape* s = m_shapeList; s; s = s->m_next)
+		{
+			s->DestroyProxy(m_world->m_broadPhase);
+		}
+
+		// Failure
+		return false;
+	}
+
+	// Success
+	return true;
+}

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

@@ -0,0 +1,587 @@
+/*
+* 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_BODY_H
+#define B2_BODY_H
+
+#include "../Common/b2Math.h"
+#include "../Collision/Shapes/b2Shape.h"
+#include "Joints/b2Joint.h"
+
+#include <memory>
+
+class b2Joint;
+class b2Contact;
+class b2World;
+struct b2JointEdge;
+struct b2ContactEdge;
+
+/// A body definition holds all the data needed to construct a rigid body.
+/// You can safely re-use body definitions.
+struct b2BodyDef
+{
+	/// This constructor sets the body definition default values.
+	b2BodyDef()
+	{
+		massData.center.SetZero();
+		massData.mass = 0.0f;
+		massData.I = 0.0f;
+		userData = NULL;
+		position.Set(0.0f, 0.0f);
+		angle = 0.0f;
+		linearDamping = 0.0f;
+		angularDamping = 0.0f;
+		allowSleep = true;
+		isSleeping = false;
+		fixedRotation = false;
+		isBullet = false;
+	}
+
+	/// You can use this to initialized the mass properties of the body.
+	/// If you prefer, you can set the mass properties after the shapes
+	/// have been added using b2Body::SetMassFromShapes.
+	b2MassData massData;
+
+	/// Use this to store application specific body data.
+	void* userData;
+
+	/// 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;
+
+	/// 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 sleeping?
+	bool isSleeping;
+
+	/// 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
+	/// static bodies.
+	/// @warning You should use this flag sparingly since it increases processing time.
+	bool isBullet;
+};
+
+/// A rigid body.
+class b2Body
+{
+public:
+	/// Creates a shape and attach it to this body.
+	/// @param shapeDef the shape definition.
+	/// @warning This function is locked during callbacks.
+	b2Shape* CreateShape(b2ShapeDef* shapeDef);
+
+	/// Destroy a shape. This removes the shape from the broad-phase and
+	/// therefore destroys any contacts associated with this shape. All shapes
+	/// attached to a body are implicitly destroyed when the body is destroyed.
+	/// @param shape the shape to be removed.
+	/// @warning This function is locked during callbacks.
+	void DestroyShape(b2Shape* shape);
+
+	/// Set the mass properties. Note that this changes the center of mass position.
+	/// If you are not sure how to compute mass properties, use SetMassFromShapes.
+	/// The inertia tensor is assumed to be relative to the center of mass.
+	/// @param massData the mass properties.
+	void SetMass(const b2MassData* massData);
+
+	/// Compute the mass properties from the attached shapes. You typically call this
+	/// after adding all the shapes. If you add or remove shapes later, you may want
+	/// to call this again. Note that this changes the center of mass position.
+	void SetMassFromShapes();
+
+	/// Set the position of the body's origin and rotation (radians).
+	/// This breaks any contacts and wakes the other bodies.
+	/// @param position the new world position of the body's origin (not necessarily
+	/// the center of mass).
+	/// @param angle the new world rotation angle of the body in radians.
+	/// @return false if the movement put a shape outside the world. In this case the
+	/// body is automatically frozen.
+	bool SetXForm(const b2Vec2& position, float32 angle);
+
+	/// Get the body transform for the body's origin.
+	/// @return the world transform of the body's origin.
+	const b2XForm& GetXForm() 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 ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point);
+
+	/// Get the total mass of the body.
+	/// @return the mass, usually in kilograms (kg).
+	float32 GetMass() const;
+
+	/// Get the central rotational inertia of the body.
+	/// @return the rotational inertia, usually in kg-m^2.
+	float32 GetInertia() const;
+
+	/// 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;
+
+	/// Is this body treated like a bullet for continuous collision detection?
+	bool IsBullet() const;
+
+	/// Should this body be treated like a bullet for continuous collision detection?
+	void SetBullet(bool flag);
+
+	/// Is this body static (immovable)?
+	bool IsStatic() const;
+
+	/// Is this body dynamic (movable)?
+	bool IsDynamic() const;
+
+	/// Is this body frozen?
+	bool IsFrozen() const;
+
+	/// Is this body sleeping (not simulating).
+	bool IsSleeping() const;
+
+	/// You can disable sleeping on this body.
+	void AllowSleeping(bool flag);
+
+	/// Wake up this body so it will begin simulating.
+	void WakeUp();
+
+	/// Put this body to sleep so it will stop simulating.
+	/// This also sets the velocity to zero.
+	void PutToSleep();
+
+	/// Get the list of all shapes attached to this body.
+	b2Shape* GetShapeList();
+
+	/// Get the list of all joints attached to this body.
+	b2JointEdge* GetJointList();
+
+	/// Get the next body in the world's body list.
+	b2Body* GetNext();
+
+	/// Get the user data pointer that was provided in the body definition.
+	void* GetUserData();
+
+	/// 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();
+
+public:
+
+	friend class b2World;
+	friend class b2Island;
+	friend class b2ContactManager;
+	friend class b2ContactSolver;
+	
+	friend class b2DistanceJoint;
+	friend class b2GearJoint;
+	friend class b2MouseJoint;
+	friend class b2PrismaticJoint;
+	friend class b2PulleyJoint;
+	friend class b2RevoluteJoint;
+
+	// m_flags
+	enum
+	{
+		e_frozenFlag		= 0x0002,
+		e_islandFlag		= 0x0004,
+		e_sleepFlag			= 0x0008,
+		e_allowSleepFlag	= 0x0010,
+		e_bulletFlag		= 0x0020,
+		e_fixedRotationFlag	= 0x0040,
+	};
+
+	// m_type
+	enum
+	{
+		e_staticType,
+		e_dynamicType,
+		e_maxTypes,
+	};
+
+	b2Body(const b2BodyDef* bd, b2World* world);
+	~b2Body();
+
+	bool SynchronizeShapes();
+
+	void SynchronizeTransform();
+
+	// This is used to prevent connected bodies from colliding.
+	// It may lie, depending on the collideConnected flag.
+	bool IsConnected(const b2Body* other) const;
+
+	void Advance(float32 t);
+
+	uint16 m_flags;
+	int16 m_type;
+
+	b2XForm 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;
+
+	b2Shape* m_shapeList;
+	int32 m_shapeCount;
+
+	b2JointEdge* m_jointList;
+	b2ContactEdge* m_contactList;
+
+	float32 m_mass, m_invMass;
+	float32 m_I, m_invI;
+
+	float32 m_linearDamping;
+	float32 m_angularDamping;
+
+	float32 m_sleepTime;
+
+	void* m_userData;
+};
+
+inline const b2XForm& b2Body::GetXForm() 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)
+{
+	m_linearVelocity = v;
+}
+
+inline b2Vec2 b2Body::GetLinearVelocity() const
+{
+	return m_linearVelocity;
+}
+
+inline void b2Body::SetAngularVelocity(float32 w)
+{
+	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;
+}
+
+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 bool b2Body::IsBullet() const
+{
+	return (m_flags & e_bulletFlag) == e_bulletFlag;
+}
+
+inline void b2Body::SetBullet(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_bulletFlag;
+	}
+	else
+	{
+		m_flags &= ~e_bulletFlag;
+	}
+}
+
+inline bool b2Body::IsStatic() const
+{
+	return m_type == e_staticType;
+}
+
+inline bool b2Body::IsDynamic() const
+{
+	return m_type == e_dynamicType;
+}
+
+inline bool b2Body::IsFrozen() const
+{
+	return (m_flags & e_frozenFlag) == e_frozenFlag;
+}
+
+inline bool b2Body::IsSleeping() const
+{
+	return (m_flags & e_sleepFlag) == e_sleepFlag;
+}
+
+inline void b2Body::AllowSleeping(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_allowSleepFlag;
+	}
+	else
+	{
+		m_flags &= ~e_allowSleepFlag;
+		WakeUp();
+	}
+}
+
+inline void b2Body::WakeUp()
+{
+	m_flags &= ~e_sleepFlag;
+	m_sleepTime = 0.0f;
+}
+
+inline void b2Body::PutToSleep()
+{
+	m_flags |= e_sleepFlag;
+	m_sleepTime = 0.0f;
+	m_linearVelocity.SetZero();
+	m_angularVelocity = 0.0f;
+	m_force.SetZero();
+	m_torque = 0.0f;
+}
+
+inline b2Shape* b2Body::GetShapeList()
+{
+	return m_shapeList;
+}
+
+inline b2JointEdge* b2Body::GetJointList()
+{
+	return m_jointList;
+}
+
+inline b2Body* b2Body::GetNext()
+{
+	return m_next;
+}
+
+inline void* b2Body::GetUserData()
+{
+	return m_userData;
+}
+
+inline void b2Body::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+inline bool b2Body::IsConnected(const b2Body* other) const
+{
+	for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
+	{
+		if (jn->other == other)
+			return jn->joint->m_collideConnected == false;
+	}
+
+	return false;
+}
+
+inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
+{
+	if (IsSleeping())
+	{
+		WakeUp();
+	}
+	m_force += force;
+	m_torque += b2Cross(point - m_sweep.c, force);
+}
+
+inline void b2Body::ApplyTorque(float32 torque)
+{
+	if (IsSleeping())
+	{
+		WakeUp();
+	}
+	m_torque += torque;
+}
+
+inline void b2Body::ApplyImpulse(const b2Vec2& impulse, const b2Vec2& point)
+{
+	if (IsSleeping())
+	{
+		WakeUp();
+	}
+	m_linearVelocity += m_invMass * impulse;
+	m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, 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;
+}
+
+#endif

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

@@ -0,0 +1,240 @@
+/*
+* 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 "b2ContactManager.h"
+#include "b2World.h"
+#include "b2Body.h"
+
+// This is a callback from the broadphase when two AABB proxies begin
+// to overlap. We create a b2Contact to manage the narrow phase.
+void* b2ContactManager::PairAdded(void* proxyUserData1, void* proxyUserData2)
+{
+	b2Shape* shape1 = (b2Shape*)proxyUserData1;
+	b2Shape* shape2 = (b2Shape*)proxyUserData2;
+
+	b2Body* body1 = shape1->GetBody();
+	b2Body* body2 = shape2->GetBody();
+
+	if (body1->IsStatic() && body2->IsStatic())
+	{
+		return &m_nullContact;
+	}
+
+	if (shape1->GetBody() == shape2->GetBody())
+	{
+		return &m_nullContact;
+	}
+
+	if (body2->IsConnected(body1))
+	{
+		return &m_nullContact;
+	}
+
+	if (m_world->m_contactFilter != NULL && m_world->m_contactFilter->ShouldCollide(shape1, shape2) == false)
+	{
+		return &m_nullContact;
+	}
+
+	// Call the factory.
+	b2Contact* c = b2Contact::Create(shape1, shape2, &m_world->m_blockAllocator);
+
+	if (c == NULL)
+	{
+		return &m_nullContact;
+	}
+
+	// Contact creation may swap shapes.
+	shape1 = c->GetShape1();
+	shape2 = c->GetShape2();
+	body1 = shape1->GetBody();
+	body2 = shape2->GetBody();
+
+	// Insert into the world.
+	c->m_prev = NULL;
+	c->m_next = m_world->m_contactList;
+	if (m_world->m_contactList != NULL)
+	{
+		m_world->m_contactList->m_prev = c;
+	}
+	m_world->m_contactList = c;
+
+	// Connect to island graph.
+
+	// Connect to body 1
+	c->m_node1.contact = c;
+	c->m_node1.other = body2;
+
+	c->m_node1.prev = NULL;
+	c->m_node1.next = body1->m_contactList;
+	if (body1->m_contactList != NULL)
+	{
+		body1->m_contactList->prev = &c->m_node1;
+	}
+	body1->m_contactList = &c->m_node1;
+
+	// Connect to body 2
+	c->m_node2.contact = c;
+	c->m_node2.other = body1;
+
+	c->m_node2.prev = NULL;
+	c->m_node2.next = body2->m_contactList;
+	if (body2->m_contactList != NULL)
+	{
+		body2->m_contactList->prev = &c->m_node2;
+	}
+	body2->m_contactList = &c->m_node2;
+
+	++m_world->m_contactCount;
+	return c;
+}
+
+// This is a callback from the broadphase when two AABB proxies cease
+// to overlap. We retire the b2Contact.
+void b2ContactManager::PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData)
+{
+	B2_NOT_USED(proxyUserData1);
+	B2_NOT_USED(proxyUserData2);
+
+	if (pairUserData == NULL)
+	{
+		return;
+	}
+
+	b2Contact* c = (b2Contact*)pairUserData;
+	if (c == &m_nullContact)
+	{
+		return;
+	}
+
+	// An attached body is being destroyed, we must destroy this contact
+	// immediately to avoid orphaned shape pointers.
+	Destroy(c);
+}
+
+void b2ContactManager::Destroy(b2Contact* c)
+{
+	b2Shape* shape1 = c->GetShape1();
+	b2Shape* shape2 = c->GetShape2();
+
+	// Inform the user that this contact is ending.
+	int32 manifoldCount = c->GetManifoldCount();
+	if (manifoldCount > 0 && m_world->m_contactListener)
+	{
+		b2Body* b1 = shape1->GetBody();
+		b2Body* b2 = shape2->GetBody();
+
+		b2Manifold* manifolds = c->GetManifolds();
+		b2ContactPoint cp;
+		cp.shape1 = c->GetShape1();
+		cp.shape2 = c->GetShape2();
+		cp.friction = c->m_friction;
+		cp.restitution = c->m_restitution;
+
+		for (int32 i = 0; i < manifoldCount; ++i)
+		{
+			b2Manifold* manifold = manifolds + i;
+			cp.normal = manifold->normal;
+
+			for (int32 j = 0; j < manifold->pointCount; ++j)
+			{
+				b2ManifoldPoint* mp = manifold->points + j;
+				cp.position = b1->GetWorldPoint(mp->localPoint1);
+				b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
+				b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
+				cp.velocity = v2 - v1;
+				cp.separation = mp->separation;
+				cp.id = mp->id;
+				m_world->m_contactListener->Remove(&cp);
+			}
+		}
+	}
+
+	// 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_world->m_contactList)
+	{
+		m_world->m_contactList = c->m_next;
+	}
+
+	b2Body* body1 = shape1->GetBody();
+	b2Body* body2 = shape2->GetBody();
+
+	// Remove from body 1
+	if (c->m_node1.prev)
+	{
+		c->m_node1.prev->next = c->m_node1.next;
+	}
+
+	if (c->m_node1.next)
+	{
+		c->m_node1.next->prev = c->m_node1.prev;
+	}
+
+	if (&c->m_node1 == body1->m_contactList)
+	{
+		body1->m_contactList = c->m_node1.next;
+	}
+
+	// Remove from body 2
+	if (c->m_node2.prev)
+	{
+		c->m_node2.prev->next = c->m_node2.next;
+	}
+
+	if (c->m_node2.next)
+	{
+		c->m_node2.next->prev = c->m_node2.prev;
+	}
+
+	if (&c->m_node2 == body2->m_contactList)
+	{
+		body2->m_contactList = c->m_node2.next;
+	}
+
+	// Call the factory.
+	b2Contact::Destroy(c, &m_world->m_blockAllocator);
+	--m_world->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.
+	for (b2Contact* c = m_world->m_contactList; c; c = c->GetNext())
+	{
+		b2Body* body1 = c->GetShape1()->GetBody();
+		b2Body* body2 = c->GetShape2()->GetBody();
+		if (body1->IsSleeping() && body2->IsSleeping())
+		{
+			continue;
+		}
+
+		c->Update(m_world->m_contactListener);
+	}
+}

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

@@ -0,0 +1,54 @@
+/*
+* 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_CONTACT_MANAGER_H
+#define B2_CONTACT_MANAGER_H
+
+#include "../Collision/b2BroadPhase.h"
+#include "../Dynamics/Contacts/b2NullContact.h"
+
+class b2World;
+class b2Contact;
+struct b2TimeStep;
+
+// Delegate of b2World.
+class b2ContactManager : public b2PairCallback
+{
+public:
+	b2ContactManager() : m_world(NULL), m_destroyImmediate(false) {}
+
+	// Implements PairCallback
+	void* PairAdded(void* proxyUserData1, void* proxyUserData2);
+
+	// Implements PairCallback
+	void PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData);
+
+	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;
+};
+
+#endif

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

@@ -0,0 +1,420 @@
+/*
+* 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 "b2Island.h"
+#include "b2Body.h"
+#include "b2World.h"
+#include "Contacts/b2Contact.h"
+#include "Contacts/b2ContactSolver.h"
+#include "Joints/b2Joint.h"
+#include "../Common/b2StackAllocator.h"
+
+/*
+Position Correction Notes
+=========================
+I tried the several algorithms for position correction of the 2D revolute joint.
+I looked at these systems:
+- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
+- suspension bridge with 30 1m long planks of length 1m.
+- multi-link chain with 30 1m long links.
+
+Here are the algorithms:
+
+Baumgarte - A fraction of the position error is added to the velocity error. There is no
+separate position solver.
+
+Pseudo Velocities - After the velocity solver and position integration,
+the position error, Jacobian, and effective mass are recomputed. Then
+the velocity constraints are solved with pseudo velocities and a fraction
+of the position error is added to the pseudo velocity error. The pseudo
+velocities are initialized to zero and there is no warm-starting. After
+the position solver, the pseudo velocities are added to the positions.
+This is also called the First Order World method or the Position LCP method.
+
+Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
+position error is re-computed for each constraint and the positions are updated
+after the constraint is solved. The radius vectors (aka Jacobians) are
+re-computed too (otherwise the algorithm has horrible instability). The pseudo
+velocity states are not needed because they are effectively zero at the beginning
+of each iteration. Since we have the current position error, we allow the
+iterations to terminate early if the error becomes smaller than b2_linearSlop.
+
+Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
+each time a constraint is solved.
+
+Here are the results:
+Baumgarte - this is the cheapest algorithm but it has some stability problems,
+especially with the bridge. The chain links separate easily close to the root
+and they jitter as they struggle to pull together. This is one of the most common
+methods in the field. The big drawback is that the position correction artificially
+affects the momentum, thus leading to instabilities and false bounce. I used a
+bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
+factor makes joints and contacts more spongy.
+
+Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
+stable. However, joints still separate with large angular velocities. Drag the
+simple pendulum in a circle quickly and the joint will separate. The chain separates
+easily and does not recover. I used a bias factor of 0.2. A larger value lead to
+the bridge collapsing when a heavy cube drops on it.
+
+Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
+Velocities, but in other ways it is worse. The bridge and chain are much more
+stable, but the simple pendulum goes unstable at high angular velocities.
+
+Full NGS - stable in all tests. The joints display good stiffness. The bridge
+still sags, but this is better than infinite forces.
+
+Recommendations
+Pseudo Velocities are not really worthwhile because the bridge and chain cannot
+recover from joint separation. In other cases the benefit over Baumgarte is small.
+
+Modified NGS is not a robust method for the revolute joint due to the violent
+instability seen in the simple pendulum. Perhaps it is viable with other constraint
+types, especially scalar constraints where the effective mass is a scalar.
+
+This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
+and is very fast. I don't think we can escape Baumgarte, especially in highly
+demanding cases where high constraint fidelity is not needed.
+
+Full NGS is robust and easy on the eyes. I recommend this as an option for
+higher fidelity simulation and certainly for suspension bridges and long chains.
+Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
+joint separation can be problematic. The number of NGS iterations can be reduced
+for better performance without harming robustness much.
+
+Each joint in a can be handled differently in the position solver. So I recommend
+a system where the user can select the algorithm on a per joint basis. I would
+probably default to the slower Full NGS and let the user select the faster
+Baumgarte method in performance critical scenarios.
+*/
+
+b2Island::b2Island(
+	int32 bodyCapacity,
+	int32 contactCapacity,
+	int32 jointCapacity,
+	b2StackAllocator* allocator,
+	b2ContactListener* listener)
+{
+	m_bodyCapacity = bodyCapacity;
+	m_contactCapacity = contactCapacity;
+	m_jointCapacity	 = jointCapacity;
+	m_bodyCount = 0;
+	m_contactCount = 0;
+	m_jointCount = 0;
+
+	m_allocator = allocator;
+	m_listener = listener;
+
+	m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
+	m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity	 * sizeof(b2Contact*));
+	m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
+
+	m_positionIterationCount = 0;
+}
+
+b2Island::~b2Island()
+{
+	// Warning: the order should reverse the constructor order.
+	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)
+{
+	// Integrate velocities and apply damping.
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Body* b = m_bodies[i];
+
+		if (b->IsStatic())
+			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)
+		// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+		// v2 = exp(-c * dt) * v1
+		// Taylor expansion:
+		// 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);
+
+	// Initialize velocity constraints.
+	contactSolver.InitVelocityConstraints(step);
+
+	for (int32 i = 0; i < m_jointCount; ++i)
+	{
+		m_joints[i]->InitVelocityConstraints(step);
+	}
+
+	// Solve velocity constraints.
+	for (int32 i = 0; i < step.maxIterations; ++i)
+	{
+		contactSolver.SolveVelocityConstraints();
+
+		for (int32 j = 0; j < m_jointCount; ++j)
+		{
+			m_joints[j]->SolveVelocityConstraints(step);
+		}
+	}
+
+	// Post-solve (store impulses for warm starting).
+	contactSolver.FinalizeVelocityConstraints();
+
+	// 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 += step.dt * b->m_linearVelocity;
+		b->m_sweep.a += step.dt * b->m_angularVelocity;
+
+		// Compute new transform
+		b->SynchronizeTransform();
+
+		// Note: shapes are synchronized later.
+	}
+
+	if (correctPositions)
+	{
+		// Initialize position constraints.
+		// Contacts don't need initialization.
+		for (int32 i = 0; i < m_jointCount; ++i)
+		{
+			m_joints[i]->InitPositionConstraints();
+		}
+
+		// Iterate over constraints.
+		for (m_positionIterationCount = 0; m_positionIterationCount < step.maxIterations; ++m_positionIterationCount)
+		{
+			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;
+			}
+		}
+	}
+
+	Report(contactSolver.m_constraints);
+
+	if (allowSleep)
+	{
+		float32 minSleepTime = B2_FLT_MAX;
+
+#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)
+			{
+				continue;
+			}
+
+			if ((b->m_flags & b2Body::e_allowSleepFlag) == 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
+				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
+				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
+#endif
+			{
+				b->m_sleepTime = 0.0f;
+				minSleepTime = 0.0f;
+			}
+			else
+			{
+				b->m_sleepTime += step.dt;
+				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
+			}
+		}
+
+		if (minSleepTime >= b2_timeToSleep)
+		{
+			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;
+			}
+		}
+	}
+}
+
+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)
+{
+	if (m_listener == NULL)
+	{
+		return;
+	}
+
+	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)
+		{
+			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);
+			}
+		}
+	}
+}

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

@@ -0,0 +1,88 @@
+/*
+* 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_ISLAND_H
+#define B2_ISLAND_H
+
+#include "../Common/b2Math.h"
+
+class b2Contact;
+class b2Body;
+class b2Joint;
+class b2StackAllocator;
+class b2ContactListener;
+struct b2ContactConstraint;
+struct b2TimeStep;
+
+class b2Island
+{
+public:
+	b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity,
+			b2StackAllocator* allocator, b2ContactListener* listener);
+	~b2Island();
+
+	void Clear()
+	{
+		m_bodyCount = 0;
+		m_contactCount = 0;
+		m_jointCount = 0;
+	}
+
+	void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool correctPositions, bool allowSleep);
+
+	void SolveTOI(const b2TimeStep& subStep);
+
+	void Add(b2Body* body)
+	{
+		b2Assert(m_bodyCount < m_bodyCapacity);
+		m_bodies[m_bodyCount++] = body;
+	}
+
+	void Add(b2Contact* contact)
+	{
+		b2Assert(m_contactCount < m_contactCapacity);
+		m_contacts[m_contactCount++] = contact;
+	}
+
+	void Add(b2Joint* joint)
+	{
+		b2Assert(m_jointCount < m_jointCapacity);
+		m_joints[m_jointCount++] = joint;
+	}
+
+	void Report(b2ContactConstraint* constraints);
+
+	b2StackAllocator* m_allocator;
+	b2ContactListener* m_listener;
+
+	b2Body** m_bodies;
+	b2Contact** m_contacts;
+	b2Joint** m_joints;
+
+	int32 m_bodyCount;
+	int32 m_jointCount;
+	int32 m_contactCount;
+
+	int32 m_bodyCapacity;
+	int32 m_contactCapacity;
+	int32 m_jointCapacity;
+
+	int32 m_positionIterationCount;
+};
+
+#endif

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

@@ -0,0 +1,253 @@
+/*
+* 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

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

@@ -0,0 +1,63 @@
+/*
+* 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 "b2WorldCallbacks.h"
+#include "../Collision/Shapes/b2Shape.h"
+
+b2ContactFilter b2_defaultFilter;
+
+// 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)
+{
+	const b2FilterData& filter1 = shape1->GetFilterData();
+	const b2FilterData& filter2 = shape2->GetFilterData();
+
+	if (filter1.groupIndex == filter2.groupIndex && filter1.groupIndex != 0)
+	{
+		return filter1.groupIndex > 0;
+	}
+
+	bool collide = (filter1.maskBits & filter2.categoryBits) != 0 && (filter1.categoryBits & filter2.maskBits) != 0;
+	return collide;
+}
+
+b2DebugDraw::b2DebugDraw()
+{
+	m_drawFlags = 0;
+}
+
+void b2DebugDraw::SetFlags(uint32 flags)
+{
+	m_drawFlags = flags;
+}
+
+uint32 b2DebugDraw::GetFlags() const
+{
+	return m_drawFlags;
+}
+
+void b2DebugDraw::AppendFlags(uint32 flags)
+{
+	m_drawFlags |= flags;
+}
+
+void b2DebugDraw::ClearFlags(uint32 flags)
+{
+	m_drawFlags &= ~flags;
+}

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

@@ -0,0 +1,172 @@
+/*
+* 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_CALLBACKS_H
+#define B2_WORLD_CALLBACKS_H
+
+#include "../Common/b2Settings.h"
+
+struct b2Vec2;
+struct b2XForm;
+class b2Shape;
+class b2Body;
+class b2Joint;
+class b2Contact;
+struct b2ContactPoint;
+struct b2ContactResult;
+
+/// Joints and shapes are destroyed when their associated
+/// body is destroyed. Implement this listener so that you
+/// may nullify references to these joints and shapes.
+class b2DestructionListener
+{
+public:
+	virtual ~b2DestructionListener() {}
+
+	/// Called when any joint is about to be destroyed due
+	/// 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
+	/// to the destruction of its parent body.
+	virtual void SayGoodbye(b2Shape* shape) = 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
+{
+public:
+	virtual ~b2ContactFilter() {}
+
+	/// 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);
+};
+
+/// The default contact filter.
+extern b2ContactFilter b2_defaultFilter;
+
+/// Implement this class to get collision results. 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.
+/// Additionally you may receive multiple callbacks for the same contact in a
+/// 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 a contact point persists. This includes the geometry
+	/// and the forces.
+	virtual void Persist(const b2ContactPoint* point) { }
+
+	/// Called when a contact point is removed. This includes the last
+	/// computed geometry and forces.
+	virtual void Remove(const b2ContactPoint* point) { }
+
+	/// Called after a contact point is solved.
+	virtual void Result(const b2ContactResult* point) { }
+};
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+	b2Color() {}
+	b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
+	float32 r, g, b;
+};
+
+/// Implement and register this class with a b2World to provide debug drawing of physics
+/// entities in your game.
+class b2DebugDraw
+{
+public:
+	b2DebugDraw();
+
+	virtual ~b2DebugDraw() {}
+
+	enum
+	{
+		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
+	};
+
+	/// Set the drawing flags.
+	void SetFlags(uint32 flags);
+
+	/// Get the drawing flags.
+	uint32 GetFlags() const;
+	
+	/// Append flags to the current flags.
+	void AppendFlags(uint32 flags);
+
+	/// Clear flags from the current flags.
+	void ClearFlags(uint32 flags);
+
+	/// Draw a closed polygon provided in CCW order.
+	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a solid closed polygon provided in CCW order.
+	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a circle.
+	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
+	
+	/// Draw a solid circle.
+	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
+	
+	/// Draw a line segment.
+	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
+
+	/// Draw a transform. Choose your own length scale.
+	/// @param xf a transform.
+	virtual void DrawXForm(const b2XForm& xf) = 0;
+
+protected:
+	uint32 m_drawFlags;
+};
+
+#endif

src/modules/physics/box2d/World.cpp

@@ -0,0 +1,234 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "World.h"
+
+#include "Shape.h"
+#include "Contact.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+
+	World::World(b2AABB aabb)
+		: add_ref(0), meter(DEFAULT_METER)
+	{
+		world = new b2World(scaleDown(aabb), b2Vec2(0,0), true);
+		world->SetContactListener(this);
+		add_contacts.reserve(10);
+	}
+
+	World::World(b2AABB aabb, b2Vec2 gravity, bool sleep)
+		: add_ref(0), meter(DEFAULT_METER)
+	{
+		world = new b2World(scaleDown(aabb), scaleDown(gravity), sleep);
+		world->SetContactListener(this);
+		add_contacts.reserve(10);
+	}
+
+	World::~World()
+	{
+		if(add_ref != 0)
+			delete add_ref;
+
+		delete world;
+	}
+
+	void World::update(float dt)
+	{
+		world->Step(dt, 10);
+
+		// Process contacts.
+		if(add_ref != 0)
+		{
+			lua_State * L = add_ref->getL();
+			for(int i = 0;i<(int)add_contacts.size();i++)
+			{
+				// Push the function.
+				add_ref->push();
+				
+				// Push first userdata.
+				{
+					shapeudata * d = (shapeudata *)(add_contacts[i]->point.shape1->GetUserData());
+					if(d->ref != 0) 
+						d->ref->push(); 
+					else 
+						lua_pushnil(L);
+				}
+
+				// Push first userdata.
+				{
+					shapeudata * d = (shapeudata *)(add_contacts[i]->point.shape2->GetUserData());
+					if(d->ref != 0) 
+						d->ref->push(); 
+					else 
+						lua_pushnil(L);
+				}
+
+				luax_newtype(L, "Contact", (LOVE_PHYSICS_CONTACT_BITS), (void*)add_contacts[i], false);
+				lua_call(L, 3, 0);
+			}
+
+			// Clear contacts.
+			for(int i = 0;i<(int)add_contacts.size();i++)
+				delete add_contacts[i];
+			add_contacts.clear();
+		}
+
+	}
+
+	void World::Add(const b2ContactPoint* point)
+	{
+		/**
+		* We must copy contacts, since we're not allowed to process
+		* them inside this function. Removing bodies in this function
+		* pretty much guarantees segfault. ^^
+		**/
+
+		if(add_ref != 0)
+			add_contacts.push_back(new Contact(this, point));
+	}
+
+	int World::setCallback(lua_State * L)
+	{
+		luax_assert_argc(L, 1, 1);
+		luax_assert_function(L, -1);
+
+		if(add_ref != 0)
+		{
+			delete add_ref;
+			add_ref = 0;
+		}
+
+		add_ref = new Reference(L);
+		return 0;
+	}
+
+	int World::getCallback(lua_State * L)
+	{
+		if(add_ref != 0)
+			add_ref->push();
+		else
+			lua_pushnil(L);
+
+		return 1;
+	}
+
+	void World::setGravity(float x, float y)
+	{
+		world->SetGravity(scaleDown(b2Vec2(x, y)));
+	}
+
+	int World::getGravity(lua_State * L)
+	{
+		b2Vec2 v = scaleUp(world->m_gravity);
+		lua_pushnumber(L, v.x);
+		lua_pushnumber(L, v.y);
+		return 2;
+	}
+
+	void World::setAllowSleep(bool allow)
+	{
+		world->m_allowSleep = allow;
+	}
+
+	bool World::isAllowSleep() const
+	{
+		return world->m_allowSleep;
+	}
+
+	int World::getBodyCount()
+	{
+		return world->GetBodyCount();
+	}
+
+	int World::getJointCount()
+	{
+		return world->GetJointCount();
+	}
+
+	void World::setMeter(int meter)
+	{
+		this->meter = meter;
+	}
+
+	int World::getMeter() const
+	{
+		return this->meter;
+	}
+
+	void World::scaleDown(float & x, float & y)
+	{
+		x /= (float)meter;
+		y /= (float)meter;
+	}
+
+	void World::scaleUp(float & x, float & y)
+	{
+		x *= (float)meter;
+		y *= (float)meter;
+	}
+
+	float World::scaleDown(float f)
+	{
+		return f/(float)meter;
+	}
+
+	float World::scaleUp(float f)
+	{
+		return f*(float)meter;
+	}
+
+	b2Vec2 World::scaleDown(const b2Vec2 & v)
+	{
+		b2Vec2 t = v;
+		scaleDown(t.x, t.y);
+		return t;
+	}
+
+	b2Vec2 World::scaleUp(const b2Vec2 & v)
+	{
+		b2Vec2 t = v;
+		scaleUp(t.x, t.y);
+		return t;
+	}
+
+	b2AABB World::scaleDown(const b2AABB & aabb)
+	{
+		b2AABB t;
+		t.lowerBound = scaleDown(aabb.lowerBound);
+		t.upperBound = scaleDown(aabb.upperBound);
+		return t;
+	}
+
+	b2AABB World::scaleUp(const b2AABB & aabb)
+	{
+		b2AABB t;
+		t.lowerBound = scaleUp(aabb.lowerBound);
+		t.upperBound = scaleUp(aabb.upperBound);
+		return t;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/World.h

@@ -0,0 +1,237 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_WORLD_H
+#define LOVE_PHYSICS_BOX2D_WORLD_H
+
+// LOVE
+#include <common/Object.h>
+#include <common/runtime.h>
+#include <common/Reference.h>
+
+// STD
+#include <vector>
+
+// Box2D
+#include "Include/Box2D.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	
+	class Contact;
+
+	/**
+	* The World is the "God" container class, 
+	* which contains all Bodies and Joints. Shapes
+	* are contained in their associated Body. 
+	* 
+	* Bodies in different worlds can obviously not
+	* collide.
+	* 
+	* The world also controls global parameters, like 
+	* gravity.
+	**/
+	class World : public Object, public b2ContactListener
+	{
+		// Friends.
+		friend class Joint;
+		friend class DistanceJoint;
+		friend class MouseJoint;
+		friend class Body;
+
+	private:
+
+		// Pointer to the Box2D world.
+		b2World * world;
+
+		// Contact callbacks.
+		Reference * add_ref;
+
+		// Contacts buffers.
+		std::vector<Contact *> add_contacts;
+
+		// The length of one meter in pixels.
+		int meter;
+
+	public:
+
+		/**
+		* 30 pixels in one meter by default.
+		**/
+		static const int DEFAULT_METER = 30;
+
+		/**
+		* Creates a new world with the given bounding box.
+		* @param aabb The bounding box.
+		**/
+		World(b2AABB aabb);
+
+		/**
+		* Creates a new world with the given bounding box, gravity
+		* and whether or not the bodies should sleep when appropriate.
+		* @param aabb The bounding box.
+		* @param gravity The gravity of the World.
+		* @param sleep True if the bodies should be able to sleep,
+		* false otherwise.
+		**/
+		World(b2AABB aabb, b2Vec2 gravity, bool sleep);
+
+		~World();
+
+		/**
+		* Updates everything in the world one timestep.
+		* This is called update() and not step() to conform
+		* with all other objects in LOVE.
+		* @param dt The timestep.
+		**/
+		void update(float dt);
+
+		// From b2ContactListener
+		void Add(const b2ContactPoint* point);
+
+		/**
+		* Recieves a Lua function as argument, and
+		* stores it for use when a collision occurs.
+		**/
+		int setCallback(lua_State * L);
+
+		/**
+		* Returns the stored Lua function for collision
+		* handling, or nil if there is none.
+		**/
+		int getCallback(lua_State * L);
+		
+		/**
+		* Sets the current gravity of the World.
+		* @param x Gravity in the x-direction.
+		* @param y Gravity in the y-direction.
+		**/
+		void setGravity(float x, float y);
+
+		/**
+		* Gets the current gravity.
+		* @returns Gravity in the x-direction.
+		* @returns Gravity in the y-direction.
+		**/
+		int getGravity(lua_State * L);
+
+		/**
+		* Sets whether this World allows sleep.
+		* @param allow True to allow, false to disallow.
+		**/
+		void setAllowSleep(bool allow);
+
+		/**
+		* Returns whether this World allows sleep.
+		* @return True if allowed, false if disallowed.
+		**/
+		bool isAllowSleep() const;
+
+		/**
+		* Get the current body count.
+		* @return The number of bodies.
+		**/
+		int getBodyCount();
+
+		/**
+		* Get the current joint count.
+		* @return The number of joints.
+		**/
+		int getJointCount();
+
+		/**
+		* Sets the number of pixels in one meter. 
+		* @param pixels The number of pixels in one meter. (1m ~= 3.3ft).
+		**/
+		void setMeter(int meter);
+
+		/**
+		* Gets the number of pixels in one meter.
+		* @param pixels The number of pixels in one meter. (1m ~= 3.3ft).
+		**/
+		int getMeter() const;
+
+		/**
+		* Scales a value down according to the current meter in pixels.
+		* @param f The unscaled input value.
+		**/
+		float scaleDown(float f);
+
+		/**
+		* Scales a value up according to the current meter in pixels.
+		* @param f The unscaled input value.
+		**/
+		float scaleUp(float f);
+
+		/**
+		* Scales a point down according to the current meter
+		* in pixels, for instance x = x0/meter, y = x0/meter.
+		* @param x The x-coordinate of the point to scale.
+		* @param y The y-coordinate of the point to scale.
+		**/
+		void scaleDown(float & x, float & y);
+
+		/**
+		* Scales a point up according to the current meter
+		* in pixels, for instance x = x0/meter, y = x0/meter.
+		* @param x The x-coordinate of the point to scale.
+		* @param y The y-coordinate of the point to scale.
+		**/
+		void scaleUp(float & x, float & y);
+
+		/**
+		* Scales a b2Vec2 down according to the current meter in pixels.
+		* @param v The unscaled input vector.
+		* @return The scaled vector.
+		**/
+		b2Vec2 scaleDown(const b2Vec2 & v);
+
+		/**
+		* Scales a b2Vec up according to the current meter in pixels.
+		* @param v The unscaled input vector.
+		* @return The scaled vector.
+		**/
+		b2Vec2 scaleUp(const b2Vec2 & v);
+
+		/**
+		* Scales a b2AABB down according to the current meter in pixels.
+		* @param v The unscaled input AABB.
+		* @return The scaled AABB.
+		**/
+		b2AABB scaleDown(const b2AABB & aabb);
+
+		/**
+		* Scales a b2AABB up according to the current meter in pixels.
+		* @param v The unscaled input AABB.
+		* @return The scaled AABB.
+		**/
+		b2AABB scaleUp(const b2AABB & aabb);
+
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_WORLD_H

src/modules/physics/box2d/graham/GrahamScanConvexHull.cpp

@@ -0,0 +1,122 @@
+#include "GrahamScanConvexHull.h"
+
+#include <cmath>
+
+bool GrahamScanConvexHull::operator()(const std::vector < point2d >& pnt, std::vector< point2d >& final_hull)
+{
+   final_hull.clear();
+
+   if (pnt.size() <= 3)
+   {
+      std::copy(pnt.begin(), pnt.end(), std::back_inserter(final_hull));
+      return true;
+   }
+
+   unsigned int j = 0;
+   gs_point2d   tmp_pnt;
+
+   for(unsigned int i = 0; i < pnt.size(); i++)
+   {
+      point.push_back(gs_point2d(pnt[i].x,pnt[i].y,0.0));
+
+      if (point[i].y < point[j].y)
+        j = i;
+      else if (point[i].y == point[j].y)
+        if (point[i].x < point[j].x)
+          j = i;
+   }
+
+   tmp_pnt  = point[0];
+   point[0] = point[j];
+   point[j] = tmp_pnt;
+
+   anchor = point[0];
+
+   for (unsigned int i = 1; i < point.size(); i++)
+   {
+      point[i].angle = cartesian_angle(point[i].x - anchor.x, point[i].y - anchor.y);
+   }
+
+   sort(++point.begin(),point.end(),GSPoint2DCompare(&anchor));
+
+   graham_scan(final_hull);
+
+   return true;
+}
+
+
+void GrahamScanConvexHull::graham_scan(std::vector< point2d >& final_hull)
+{
+   const std::size_t HEAD     = 0;
+   const std::size_t PRE_HEAD = 1;
+
+   std::deque<gs_point2d> pnt_queue;
+
+   pnt_queue.push_front(point[0]);
+   pnt_queue.push_front(point[1]);
+
+   unsigned int i = 2;
+
+   while(i < point.size())
+   {
+      if (pnt_queue.size() > 1)
+      {
+         if (orientation(pnt_queue[PRE_HEAD],pnt_queue[HEAD],point[i]) == counter_clock_wise)
+            pnt_queue.push_front(point[i++]);
+         else
+          pnt_queue.pop_front();
+      }
+      else
+         pnt_queue.push_front(point[i++]);
+   }
+
+   for(std::deque<gs_point2d>::iterator it = pnt_queue.begin(); it != pnt_queue.end(); it++)
+   {
+      final_hull.push_back(point2d((*it).x, (*it).y));
+   }
+}
+
+
+inline double GrahamScanConvexHull::cartesian_angle(double x, double y)
+{
+  if      ((x >  0.0) && (y >  0.0)) return (atan( y / x) * _180DivPI);
+  else if ((x <  0.0) && (y >  0.0)) return (atan(-x / y) * _180DivPI) +  90.0;
+  else if ((x <  0.0) && (y <  0.0)) return (atan( y / x) * _180DivPI) + 180.0;
+  else if ((x >  0.0) && (y <  0.0)) return (atan(-x / y) * _180DivPI) + 270.0;
+  else if ((x == 0.0) && (y >  0.0)) return  90.0;
+  else if ((x <  0.0) && (y == 0.0)) return 180.0;
+  else if ((x == 0.0) && (y <  0.0)) return 270.0;
+  else
+    return 0.0;
+}
+
+
+inline int GrahamScanConvexHull::orientation(const gs_point2d& p1, const gs_point2d& p2, const gs_point2d& p3)
+{
+   return orientation(p1.x,p1.y,p2.x,p2.y,p3.x,p3.y);
+}
+
+
+inline int GrahamScanConvexHull::orientation(const double x1, const double y1,
+                                             const double x2, const double y2,
+                                             const double px, const double py)
+{
+   double orin = (x2 - x1) * (py - y1) - (px - x1) * (y2 - y1);
+
+   if (is_equal(orin,0.0))
+     return 0;              /* Orientaion is neutral aka collinear  */
+   else if (orin < 0.0)
+     return -1;             /* Orientaion is to the right-hand side */
+   else
+     return +1;             /* Orientaion is to the left-hand side  */
+
+}
+
+
+inline bool GrahamScanConvexHull::is_equal(const double v1, const double& v2, const double epsilon)
+{
+   double diff = v1 - v2;
+   return  (-epsilon <= diff) && (diff <= epsilon);
+}
+
+

src/modules/physics/box2d/graham/GrahamScanConvexHull.h

@@ -0,0 +1,137 @@
+/*
+ **************************************************************************
+ * Class: Graham Scan Convex Hull                                         *
+ * By Arash Partow - 2001                                                 *
+ * URL: http://www.partow.net                                             *
+ *                                                                        *
+ * Copyright Notice:                                                      *
+ * Free use of this library is permitted under the guidelines and         *
+ * in accordance with the most current version of the Common Public       *
+ * License.                                                               *
+ * http://www.opensource.org/licenses/cpl.php                             *
+ *                                                                        *
+ **************************************************************************
+*/
+
+
+#ifndef INCLUDE_GRAHAMSCANCONVEXHULL_H
+#define INCLUDE_GRAHAMSCANCONVEXHULL_H
+
+#include <iostream>
+#include <deque>
+#include <vector>
+#include <algorithm>
+#include <math.h>
+
+#include <vector>
+
+
+struct point2d
+{
+   point2d(double _x = 0.0 , double _y = 0.0) : x(_x), y(_y){}
+   double x;
+   double y;
+};
+
+class ConvexHull
+{
+   public:
+
+     virtual ~ConvexHull(){};
+     virtual bool operator()(const std::vector<point2d>& pnt, std::vector<point2d>& final_hull) = 0;
+
+};
+
+
+
+
+
+struct gs_point2d
+{
+public:
+   gs_point2d(double _x = 0.0, double _y = 0.0, double _angle = 0.0) : x(_x), y(_y), angle(_angle){}
+   double x;
+   double y;
+   double angle;
+};
+
+const double _180DivPI  = 57.295779513082320876798154814105000;
+const int    counter_clock_wise = +1;
+const int    clock_wise         = -1;
+
+
+class GSPoint2DCompare
+{
+public:
+
+   GSPoint2DCompare(gs_point2d* _anchor):anchor(_anchor){};
+
+   bool operator()(const gs_point2d& p1, const gs_point2d& p2)
+   {
+      if (p1.angle < p2.angle)      return true;
+      else if (p1.angle > p2.angle) return false;
+      else if (is_equal(p1,p2))     return false;
+      else if (lay_distance(anchor->x, anchor->y, p1.x, p1.y) < lay_distance(anchor->x, anchor->y, p2.x, p2.y))
+         return true;
+      else
+         return false;
+   }
+
+private:
+
+   inline bool is_equal(const gs_point2d p1, gs_point2d p2)
+   {
+      return  is_equal(p1.x,p2.x) && is_equal(p1.y,p2.y);
+   }
+
+   inline bool is_equal(const double v1, const double& v2, const double epsilon = 1.0e-12)
+   {
+      double diff = v1 - v2;
+      return  (-epsilon <= diff) && (diff <= epsilon);
+   }
+
+   inline double lay_distance(const double& x1, const double& y1, const double& x2, const double& y2)
+   {
+      double dx = (x1 - x2);
+      double dy = (y1 - y2);
+      return (dx * dx + dy * dy);
+   }
+
+   gs_point2d* anchor;
+
+};
+
+
+class GrahamScanConvexHull : public ConvexHull
+{
+public:
+
+   GrahamScanConvexHull(){};
+  ~GrahamScanConvexHull(){};
+
+   virtual bool operator()(const std::vector < point2d >& pnt, std::vector< point2d >& final_hull);
+
+private:
+
+   void graham_scan(std::vector< point2d >& final_hull);
+
+   inline double cartesian_angle(double x, double y);
+
+   inline int orientation(const gs_point2d& p1,
+                          const gs_point2d& p2,
+                          const gs_point2d& p3);
+
+   inline int orientation(const double x1, const double y1,
+                          const double x2, const double y2,
+                          const double px, const double py);
+
+   inline bool is_equal(const double v1, const double& v2, const double epsilon = 1.0e-12);
+
+   std::vector<gs_point2d> point;
+   gs_point2d              anchor;
+
+};
+
+
+
+#endif

src/modules/physics/box2d/wrap_Body.cpp

@@ -0,0 +1,374 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "wrap_Body.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Body * luax_checkbody(lua_State * L, int idx)
+	{
+		return luax_checktype<Body>(L, idx, "Body", LOVE_PHYSICS_BODY_BITS);
+	}
+
+	int _wrap_Body_getX(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getX());
+		return 1;
+	}
+
+	int _wrap_Body_getY(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getY());
+		return 1;
+	}
+
+	int _wrap_Body_getAngle(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getAngle());
+		return 1;
+	}
+
+	int _wrap_Body_getPosition(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getPosition(L);
+	}
+
+	int _wrap_Body_getVelocity(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getVelocity(L);
+	}
+
+	int _wrap_Body_getWorldCenter(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getWorldCenter(L);
+	}
+
+	int _wrap_Body_getLocalCenter(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getLocalCenter(L);
+	}
+
+	int _wrap_Body_getSpin(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getSpin());
+		return 1;
+	}
+
+	int _wrap_Body_getMass(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getMass());
+		return 1;
+	}
+
+	int _wrap_Body_getInertia(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getInertia());
+		return 1;
+	}
+
+	int _wrap_Body_getAngularDamping(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getAngularDamping());
+		return 1;
+	}
+
+	int _wrap_Body_getDamping(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		lua_pushnumber(L, t->getDamping());
+		return 1;
+	}
+
+	int _wrap_Body_applyImpulse(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float jx = (float)luaL_checknumber(L, 2);
+		float jy = (float)luaL_checknumber(L, 3);
+		float rx = (float)luaL_optnumber(L, 4, 0);
+		float ry = (float)luaL_optnumber(L, 5, 0);
+		t->applyImpulse(jx, jy, rx, ry);
+		return 0;
+	}
+
+	int _wrap_Body_applyTorque(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg = (float)luaL_checknumber(L, 2);
+		t->applyTorque(arg);
+		return 0;
+	}
+
+	int _wrap_Body_applyForce(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float fx = (float)luaL_checknumber(L, 2);
+		float fy = (float)luaL_checknumber(L, 3);
+		float rx = (float)luaL_optnumber(L, 4, 0);
+		float ry = (float)luaL_optnumber(L, 5, 0);
+		t->applyForce(fx, fy, rx, ry);
+		return 0;
+	}
+
+	int _wrap_Body_setX(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setX(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_setY(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setY(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_setVelocity(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		float arg2 = (float)luaL_checknumber(L, 3);
+		t->setVelocity(arg1, arg2);
+		return 0;
+	}
+
+	int _wrap_Body_setAngle(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setAngle(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_setSpin(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setSpin(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_setPosition(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		float arg2 = (float)luaL_checknumber(L, 3);
+		t->setPosition(arg1, arg2);
+		return 0;
+	}
+
+	int _wrap_Body_setMassFromShapes(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		t->setMassFromShapes();
+		return 0;
+	}
+
+	int _wrap_Body_setMass(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float x = (float)luaL_checknumber(L, 2);
+		float y = (float)luaL_checknumber(L, 3);
+		float m = (float)luaL_checknumber(L, 4);
+		float i = (float)luaL_checknumber(L, 5);
+		t->setMass(x, y, m, i);
+		return 0;
+	}
+
+	int _wrap_Body_setAngularDamping(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setAngularDamping(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_setDamping(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setDamping(arg1);
+		return 0;
+	}
+
+	int _wrap_Body_getWorldPoint(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getWorldPoint(L);
+	}
+
+	int _wrap_Body_getWorldVector(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getWorldVector(L);
+	}
+
+	int _wrap_Body_getLocalPoint(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getLocalPoint(L);
+	}
+
+	int _wrap_Body_getLocalVector(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getLocalVector(L);
+	}
+
+	int _wrap_Body_getVelocityWorldPoint(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getVelocityWorldPoint(L);
+	}
+
+	int _wrap_Body_getVelocityLocalPoint(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		return t->getVelocityLocalPoint(L);
+	}
+
+	int _wrap_Body_isBullet(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		luax_pushboolean(L, t->isBullet());
+		return 1;
+	}
+
+	int _wrap_Body_setBullet(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		bool b = luax_toboolean(L, 2);
+		t->setBullet(b);
+		return 0;
+	}
+
+	int _wrap_Body_isStatic(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		luax_pushboolean(L, t->isStatic());
+		return 1;
+	}
+
+	int _wrap_Body_isDynamic(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		luax_pushboolean(L, t->isDynamic());
+		return 1;
+	}
+
+	int _wrap_Body_isFrozen(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		luax_pushboolean(L, t->isFrozen());
+		return 1;
+	}
+
+	int _wrap_Body_isSleeping(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		luax_pushboolean(L, t->isSleeping());
+		return 1;
+	}
+
+	int _wrap_Body_setAllowSleep(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		bool b = luax_toboolean(L, 2);
+		t->setAllowSleep(b);
+		return 0;
+	}
+
+	int _wrap_Body_setSleep(lua_State * L)
+	{
+		Body * t = luax_checkbody(L, 1);
+		bool b = luax_toboolean(L, 2);
+		t->setSleep(b);
+		return 0;
+	}
+
+	static const luaL_Reg wrap_Body_functions[] = {
+		{ "getX", _wrap_Body_getX },
+		{ "getY", _wrap_Body_getY },
+		{ "getAngle", _wrap_Body_getAngle },
+		{ "getPosition", _wrap_Body_getPosition },
+		{ "getVelocity", _wrap_Body_getVelocity },
+		{ "getWorldCenter", _wrap_Body_getWorldCenter },
+		{ "getLocalCenter", _wrap_Body_getLocalCenter },
+		{ "getSpin", _wrap_Body_getSpin },
+		{ "getMass", _wrap_Body_getMass },
+		{ "getInertia", _wrap_Body_getInertia },
+		{ "getAngularDamping", _wrap_Body_getAngularDamping },
+		{ "getDamping", _wrap_Body_getDamping },
+		{ "applyImpulse", _wrap_Body_applyImpulse },
+		{ "applyTorque", _wrap_Body_applyTorque },
+		{ "applyForce", _wrap_Body_applyForce },
+		{ "setX", _wrap_Body_setX },
+		{ "setY", _wrap_Body_setY },
+		{ "setVelocity", _wrap_Body_setVelocity },
+		{ "setAngle", _wrap_Body_setAngle },
+		{ "setSpin", _wrap_Body_setSpin },
+		{ "setPosition", _wrap_Body_setPosition },
+		{ "setMassFromShapes", _wrap_Body_setMassFromShapes },
+		{ "setMass", _wrap_Body_setMass },
+		{ "setAngularDamping", _wrap_Body_setAngularDamping },
+		{ "setDamping", _wrap_Body_setDamping },
+		{ "getWorldPoint", _wrap_Body_getWorldPoint },
+		{ "getWorldVector", _wrap_Body_getWorldVector },
+		{ "getLocalPoint", _wrap_Body_getLocalPoint },
+		{ "getLocalVector", _wrap_Body_getLocalVector },
+		{ "getVelocityWorldPoint", _wrap_Body_getVelocityWorldPoint },
+		{ "getVelocityLocalPoint", _wrap_Body_getVelocityLocalPoint },
+		{ "isBullet", _wrap_Body_isBullet },
+		{ "setBullet", _wrap_Body_setBullet },
+		{ "isStatic", _wrap_Body_isStatic },
+		{ "isDynamic", _wrap_Body_isDynamic },
+		{ "isFrozen", _wrap_Body_isFrozen },
+		{ "isSleeping", _wrap_Body_isSleeping },
+		{ "setAllowSleep", _wrap_Body_setAllowSleep },
+		{ "setSleep", _wrap_Body_setSleep },
+		{ 0, 0 }
+	};
+
+	int wrap_Body_open(lua_State * L)
+	{
+		luax_register_type(L, "Body", wrap_Body_functions);
+		return 0;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/wrap_Body.h

@@ -0,0 +1,81 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_WRAP_BODY_H
+#define LOVE_PHYSICS_BOX2D_WRAP_BODY_H
+
+// LOVE
+#include <common/runtime.h>
+#include "Body.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+
+	Body * luax_checkbody(lua_State * L, int idx);
+	int _wrap_Body_getX(lua_State * L);
+	int _wrap_Body_getY(lua_State * L);
+	int _wrap_Body_getAngle(lua_State * L);
+	int _wrap_Body_getPosition(lua_State * L);
+	int _wrap_Body_getVelocity(lua_State * L);
+	int _wrap_Body_getWorldCenter(lua_State * L);
+	int _wrap_Body_getLocalCenter(lua_State * L);
+	int _wrap_Body_getSpin(lua_State * L);
+	int _wrap_Body_getMass(lua_State * L);
+	int _wrap_Body_getInertia(lua_State * L);
+	int _wrap_Body_getAngularDamping(lua_State * L);
+	int _wrap_Body_getDamping(lua_State * L);
+	int _wrap_Body_applyImpulse(lua_State * L);
+	int _wrap_Body_applyTorque(lua_State * L);
+	int _wrap_Body_applyForce(lua_State * L);
+	int _wrap_Body_setX(lua_State * L);
+	int _wrap_Body_setY(lua_State * L);
+	int _wrap_Body_setVelocity(lua_State * L);
+	int _wrap_Body_setAngle(lua_State * L);
+	int _wrap_Body_setSpin(lua_State * L);
+	int _wrap_Body_setPosition(lua_State * L);
+	int _wrap_Body_setMassFromShapes(lua_State * L);
+	int _wrap_Body_setMass(lua_State * L);
+	int _wrap_Body_setAngularDamping(lua_State * L);
+	int _wrap_Body_setDamping(lua_State * L);
+	int _wrap_Body_getWorldPoint(lua_State * L);
+	int _wrap_Body_getWorldVector(lua_State * L);
+	int _wrap_Body_getLocalPoint(lua_State * L);
+	int _wrap_Body_getLocalVector(lua_State * L);
+	int _wrap_Body_getVelocityWorldPoint(lua_State * L);
+	int _wrap_Body_getVelocityLocalPoint(lua_State * L);
+	int _wrap_Body_isBullet(lua_State * L);
+	int _wrap_Body_setBullet(lua_State * L);
+	int _wrap_Body_isStatic(lua_State * L);
+	int _wrap_Body_isDynamic(lua_State * L);
+	int _wrap_Body_isFrozen(lua_State * L);
+	int _wrap_Body_isSleeping(lua_State * L);
+	int _wrap_Body_setAllowSleep(lua_State * L);
+	int _wrap_Body_setSleep(lua_State * L);
+	int wrap_Body_open(lua_State * L);
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_WRAP_BODY_H

src/modules/physics/box2d/wrap_CircleShape.cpp

@@ -0,0 +1,73 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "wrap_CircleShape.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	CircleShape * luax_checkcircleshape(lua_State * L, int idx)
+	{
+		return luax_checktype<CircleShape>(L, idx, "CircleShape", LOVE_PHYSICS_CIRCLE_SHAPE_BITS);
+	}
+
+	int _wrap_CircleShape_getRadius(lua_State * L)
+	{
+		CircleShape * c = luax_checkcircleshape(L, 1);
+		lua_pushnumber(L, c->getRadius());
+		return 1;
+	}
+
+	static const luaL_Reg wrap_CircleShape_functions[] = {
+		{ "getRadius", _wrap_CircleShape_getRadius },
+		// From Shape.
+		{ "getType", _wrap_Shape_getType },
+		{ "setFriction", _wrap_Shape_setFriction },
+		{ "setRestitution", _wrap_Shape_setRestitution },
+		{ "setDensity", _wrap_Shape_setDensity },
+		{ "setSensor", _wrap_Shape_setSensor },
+		{ "getFriction", _wrap_Shape_getFriction },
+		{ "getRestituion", _wrap_Shape_getRestituion },
+		{ "getDensity", _wrap_Shape_getDensity },
+		{ "isSensor", _wrap_Shape_isSensor },
+		{ "testPoint", _wrap_Shape_testPoint },
+		{ "testSegment", _wrap_Shape_testSegment },
+		{ "setCategory", _wrap_Shape_setCategory },
+		{ "getCategory", _wrap_Shape_getCategory },
+		{ "setMask", _wrap_Shape_setMask },
+		{ "getMask", _wrap_Shape_getMask },
+		{ "setData", _wrap_Shape_setData },
+		{ "getData", _wrap_Shape_getData },
+		{ "getBoundingBox", _wrap_Shape_getBoundingBox },
+		{ 0, 0 }
+	};
+
+	int wrap_CircleShape_open(lua_State * L)
+	{
+		luax_register_type(L, "CircleShape", wrap_CircleShape_functions);
+		return 0;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/wrap_CircleShape.h

@@ -0,0 +1,43 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_WRAP_CIRCLE_SHAPE_H
+#define LOVE_PHYSICS_BOX2D_WRAP_CIRCLE_SHAPE_H
+
+// LOVE
+#include <common/runtime.h>
+#include "wrap_Shape.h"
+#include "CircleShape.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	CircleShape * luax_checkcircleshape(lua_State * L, int idx);
+	int _wrap_CircleShape_getRadius(lua_State * L);
+	int wrap_CircleShape_open(lua_State * L);
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_WRAP_CIRCLE_SHAPE_H

src/modules/physics/box2d/wrap_Contact.cpp

@@ -0,0 +1,92 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "wrap_Contact.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+
+	Contact * luax_checkcontact(lua_State * L, int idx)
+	{
+		return luax_checktype<Contact>(L, idx, "Contact", LOVE_PHYSICS_CONTACT_BITS);
+	}
+
+	int _wrap_Contact_getPosition(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		return t->getPosition(L);
+	}
+
+	int _wrap_Contact_getVelocity(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		return t->getVelocity(L);
+	}
+
+	int _wrap_Contact_getNormal(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		return t->getNormal(L);
+	}
+
+	int _wrap_Contact_getSeparation(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		lua_pushnumber(L, t->getSeparation());
+		return 1;
+	}
+
+	int _wrap_Contact_getFriction(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		lua_pushnumber(L, t->getFriction());
+		return 1;
+	}
+
+	int _wrap_Contact_getRestitution(lua_State * L)
+	{
+		Contact * t = luax_checkcontact(L, 1);
+		lua_pushnumber(L, t->getRestitution());
+		return 1;
+	}
+
+	static const luaL_Reg wrap_Contact_functions[] = {
+		{ "getPosition", _wrap_Contact_getPosition },
+		{ "getVelocity", _wrap_Contact_getVelocity },
+		{ "getNormal", _wrap_Contact_getNormal },
+		{ "getSeparation", _wrap_Contact_getSeparation },
+		{ "getFriction", _wrap_Contact_getFriction },
+		{ "getRestitution", _wrap_Contact_getRestitution },
+		{ 0, 0 }
+	};
+
+	int wrap_Contact_open(lua_State * L)
+	{
+		luax_register_type(L, "Contact", wrap_Contact_functions);
+		return 0;
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/wrap_Contact.h

@@ -0,0 +1,47 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_WRAP_CONTACT_H
+#define LOVE_PHYSICS_BOX2D_WRAP_CONTACT_H
+
+// LOVE
+#include <common/runtime.h>
+#include "Contact.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	Contact * luax_checkcontact(lua_State * L, int idx);
+	int _wrap_Contact_getPosition(lua_State * L);
+	int _wrap_Contact_getVelocity(lua_State * L);
+	int _wrap_Contact_getNormal(lua_State * L);
+	int _wrap_Contact_getSeparation(lua_State * L);
+	int _wrap_Contact_getFriction(lua_State * L);
+	int _wrap_Contact_getRestitution(lua_State * L);
+	int wrap_Contact_open(lua_State * L);
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_WRAP_CONTACT_H

src/modules/physics/box2d/wrap_DistanceJoint.cpp

@@ -0,0 +1,104 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 "wrap_DistanceJoint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	DistanceJoint * luax_checkdistancejoint(lua_State * L, int idx)
+	{
+		return luax_checktype<DistanceJoint>(L, idx, "DistanceJoint", LOVE_PHYSICS_DISTANCE_JOINT_BITS);
+	}
+
+	int _wrap_DistanceJoint_setLength(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setLength(arg1);
+		return 0;
+	}
+
+	int _wrap_DistanceJoint_getLength(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		lua_pushnumber(L, t->getLength());
+		return 1;
+	}
+
+	int _wrap_DistanceJoint_setFrequency(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setFrequency(arg1);
+		return 0;
+	}
+
+	int _wrap_DistanceJoint_getFrequency(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		lua_pushnumber(L, t->getFrequency());
+		return 1;
+	}
+
+	int _wrap_DistanceJoint_setDamping(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		float arg1 = (float)luaL_checknumber(L, 2);
+		t->setDamping(arg1);
+		return 0;
+	}
+
+	int _wrap_DistanceJoint_getDamping(lua_State * L)
+	{
+		DistanceJoint * t = luax_checkdistancejoint(L, 1);
+		lua_pushnumber(L, t->getDamping());
+		return 1;
+	}
+
+	static const luaL_Reg wrap_DistanceJoint_functions[] = {
+		{ "setLength", _wrap_DistanceJoint_setLength },
+		{ "getLength", _wrap_DistanceJoint_getLength },
+		{ "setFrequency", _wrap_DistanceJoint_setFrequency },
+		{ "getFrequency", _wrap_DistanceJoint_getFrequency },
+		{ "setDamping", _wrap_DistanceJoint_setDamping },
+		{ "getDamping", _wrap_DistanceJoint_getDamping },
+		// From Joint.
+		{ "getType", _wrap_Joint_getType },
+		{ "getAnchors", _wrap_Joint_getAnchors },
+		{ "getReactionForce", _wrap_Joint_getReactionForce },
+		{ "getReactionTorque", _wrap_Joint_getReactionTorque },
+		{ "setCollideConnected", _wrap_Joint_setCollideConnected },
+		{ "getCollideConnected", _wrap_Joint_getCollideConnected },
+		{ 0, 0 }
+	};
+
+	int wrap_DistanceJoint_open(lua_State * L)
+	{
+		luax_register_type(L, "DistanceJoint", wrap_DistanceJoint_functions);
+		return 0; 
+	}
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/wrap_DistanceJoint.h

@@ -0,0 +1,48 @@
+/**
+* Copyright (c) 2006-2009 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_WRAP_DISTANCE_JOINT_H
+#define LOVE_PHYSICS_BOX2D_WRAP_DISTANCE_JOINT_H
+
+// LOVE
+#include <common/runtime.h>
+#include "wrap_Joint.h"
+#include "DistanceJoint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	DistanceJoint * luax_checkdistancejoint(lua_State * L, int idx);
+	int _wrap_DistanceJoint_setLength(lua_State * L);
+	int _wrap_DistanceJoint_getLength(lua_State * L);
+	int _wrap_DistanceJoint_setFrequency(lua_State * L);
+	int _wrap_DistanceJoint_getFrequency(lua_State * L);
+	int _wrap_DistanceJoint_setDamping(lua_State * L);
+	int _wrap_DistanceJoint_getDamping(lua_State * L);
+	int wrap_DistanceJoint_open(lua_State * L);
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_WRAP_DISTANCE_JOINT_H