initial commit. includes PhsyicsBox2dExtension

AndEngine/jni/Box2D/Dynamics/Contacts/b2CircleContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Contacts/b2CircleContact.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+
+#include <new>
+
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2CircleContact));
+	return new (mem) b2CircleContact(fixtureA, fixtureB);
+}
+
+void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2CircleContact*)contact)->~b2CircleContact();
+	allocator->Free(contact, sizeof(b2CircleContact));
+}
+
+b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideCircles(manifold,
+					(b2CircleShape*)m_fixtureA->GetShape(), xfA,
+					(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

AndEngine/jni/Box2D/Dynamics/Contacts/b2CircleContact.h

@@ -0,0 +1,38 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_CIRCLE_CONTACT_H
+#define B2_CIRCLE_CONTACT_H
+
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2CircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2CircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Contacts/b2Contact.cpp

@@ -0,0 +1,226 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+#include "Box2D/Dynamics/Contacts/b2CircleContact.h"
+#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h"
+#include "Box2D/Dynamics/Contacts/b2PolygonContact.h"
+#include "Box2D/Dynamics/Contacts/b2ContactSolver.h"
+
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+#include "Box2D/Collision/Shapes/b2Shape.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2World.h"
+
+b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
+bool b2Contact::s_initialized = false;
+
+void b2Contact::InitializeRegisters()
+{
+	AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
+	AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
+	AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
+}
+
+void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
+						b2Shape::Type type1, b2Shape::Type type2)
+{
+	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	
+	s_registers[type1][type2].createFcn = createFcn;
+	s_registers[type1][type2].destroyFcn = destoryFcn;
+	s_registers[type1][type2].primary = true;
+
+	if (type1 != type2)
+	{
+		s_registers[type2][type1].createFcn = createFcn;
+		s_registers[type2][type1].destroyFcn = destoryFcn;
+		s_registers[type2][type1].primary = false;
+	}
+}
+
+b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	if (s_initialized == false)
+	{
+		InitializeRegisters();
+		s_initialized = true;
+	}
+
+	b2Shape::Type type1 = fixtureA->GetType();
+	b2Shape::Type type2 = fixtureB->GetType();
+
+	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	
+	b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
+	if (createFcn)
+	{
+		if (s_registers[type1][type2].primary)
+		{
+			return createFcn(fixtureA, fixtureB, allocator);
+		}
+		else
+		{
+			return createFcn(fixtureB, fixtureA, allocator);
+		}
+	}
+	else
+	{
+		return NULL;
+	}
+}
+
+void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	b2Assert(s_initialized == true);
+
+	if (contact->m_manifold.pointCount > 0)
+	{
+		contact->GetFixtureA()->GetBody()->SetAwake(true);
+		contact->GetFixtureB()->GetBody()->SetAwake(true);
+	}
+
+	b2Shape::Type typeA = contact->GetFixtureA()->GetType();
+	b2Shape::Type typeB = contact->GetFixtureB()->GetType();
+
+	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+
+	b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
+	destroyFcn(contact, allocator);
+}
+
+b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB)
+{
+	m_flags = e_enabledFlag;
+
+	m_fixtureA = fA;
+	m_fixtureB = fB;
+
+	m_manifold.pointCount = 0;
+
+	m_prev = NULL;
+	m_next = NULL;
+
+	m_nodeA.contact = NULL;
+	m_nodeA.prev = NULL;
+	m_nodeA.next = NULL;
+	m_nodeA.other = NULL;
+
+	m_nodeB.contact = NULL;
+	m_nodeB.prev = NULL;
+	m_nodeB.next = NULL;
+	m_nodeB.other = NULL;
+
+	m_toiCount = 0;
+}
+
+// Update the contact manifold and touching status.
+// Note: do not assume the fixture AABBs are overlapping or are valid.
+void b2Contact::Update(b2ContactListener* listener)
+{
+	b2Manifold oldManifold = m_manifold;
+
+	// Re-enable this contact.
+	m_flags |= e_enabledFlag;
+
+	bool touching = false;
+	bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
+
+	bool sensorA = m_fixtureA->IsSensor();
+	bool sensorB = m_fixtureB->IsSensor();
+	bool sensor = sensorA || sensorB;
+
+	b2Body* bodyA = m_fixtureA->GetBody();
+	b2Body* bodyB = m_fixtureB->GetBody();
+	const b2Transform& xfA = bodyA->GetTransform();
+	const b2Transform& xfB = bodyB->GetTransform();
+
+	// Is this contact a sensor?
+	if (sensor)
+	{
+		const b2Shape* shapeA = m_fixtureA->GetShape();
+		const b2Shape* shapeB = m_fixtureB->GetShape();
+		touching = b2TestOverlap(shapeA, shapeB, xfA, xfB);
+
+		// Sensors don't generate manifolds.
+		m_manifold.pointCount = 0;
+	}
+	else
+	{
+		Evaluate(&m_manifold, xfA, xfB);
+		touching = m_manifold.pointCount > 0;
+
+		// Match old contact ids to new contact ids and copy the
+		// stored impulses to warm start the solver.
+		for (int32 i = 0; i < m_manifold.pointCount; ++i)
+		{
+			b2ManifoldPoint* mp2 = m_manifold.points + i;
+			mp2->normalImpulse = 0.0f;
+			mp2->tangentImpulse = 0.0f;
+			b2ContactID id2 = mp2->id;
+
+			for (int32 j = 0; j < oldManifold.pointCount; ++j)
+			{
+				b2ManifoldPoint* mp1 = oldManifold.points + j;
+
+				if (mp1->id.key == id2.key)
+				{
+					mp2->normalImpulse = mp1->normalImpulse;
+					mp2->tangentImpulse = mp1->tangentImpulse;
+					break;
+				}
+			}
+		}
+
+		if (touching != wasTouching)
+		{
+			bodyA->SetAwake(true);
+			bodyB->SetAwake(true);
+		}
+	}
+
+	if (touching)
+	{
+		m_flags |= e_touchingFlag;
+	}
+	else
+	{
+		m_flags &= ~e_touchingFlag;
+	}
+
+	if (wasTouching == false && touching == true && listener)
+	{
+		listener->BeginContact(this);
+	}
+
+	if (wasTouching == true && touching == false && listener)
+	{
+		listener->EndContact(this);
+	}
+
+	if (sensor == false && touching && listener)
+	{
+		listener->PreSolve(this, &oldManifold);
+	}
+}

AndEngine/jni/Box2D/Dynamics/Contacts/b2Contact.h

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

AndEngine/jni/Box2D/Dynamics/Contacts/b2ContactSolver.cpp

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

AndEngine/jni/Box2D/Dynamics/Contacts/b2ContactSolver.h

@@ -0,0 +1,78 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_CONTACT_SOLVER_H
+#define B2_CONTACT_SOLVER_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Dynamics/b2Island.h"
+
+class b2Contact;
+class b2Body;
+class b2StackAllocator;
+
+struct b2ContactConstraintPoint
+{
+	b2Vec2 localPoint;
+	b2Vec2 rA;
+	b2Vec2 rB;
+	float32 normalImpulse;
+	float32 tangentImpulse;
+	float32 normalMass;
+	float32 tangentMass;
+	float32 velocityBias;
+};
+
+struct b2ContactConstraint
+{
+	b2ContactConstraintPoint points[b2_maxManifoldPoints];
+	b2Vec2 localNormal;
+	b2Vec2 localPoint;
+	b2Vec2 normal;
+	b2Mat22 normalMass;
+	b2Mat22 K;
+	b2Body* bodyA;
+	b2Body* bodyB;
+	b2Manifold::Type type;
+	float32 radius;
+	float32 friction;
+	int32 pointCount;
+	b2Manifold* manifold;
+};
+
+class b2ContactSolver
+{
+public:
+	b2ContactSolver(b2Contact** contacts, int32 contactCount,
+					b2StackAllocator* allocator, float32 impulseRatio);
+
+	~b2ContactSolver();
+
+	void WarmStart();
+	void SolveVelocityConstraints();
+	void StoreImpulses();
+
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2StackAllocator* m_allocator;
+	b2ContactConstraint* m_constraints;
+	int m_constraintCount;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+
+#include <new>
+
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
+	return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
+	allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
+}
+
+b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygonAndCircle(	manifold,
+								(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

AndEngine/jni/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h

@@ -0,0 +1,38 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H
+#define B2_POLYGON_AND_CIRCLE_CONTACT_H
+
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2PolygonAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Contacts/b2PolygonContact.cpp

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Contacts/b2PolygonContact.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Collision/b2TimeOfImpact.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+
+#include <new>
+
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
+	return new (mem) b2PolygonContact(fixtureA, fixtureB);
+}
+
+void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2PolygonContact*)contact)->~b2PolygonContact();
+	allocator->Free(contact, sizeof(b2PolygonContact));
+}
+
+b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+	: b2Contact(fixtureA, fixtureB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollidePolygons(	manifold,
+						(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
+						(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

AndEngine/jni/Box2D/Dynamics/Contacts/b2PolygonContact.h

@@ -0,0 +1,38 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_POLYGON_CONTACT_H
+#define B2_POLYGON_CONTACT_H
+
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+
+class b2BlockAllocator;
+
+class b2PolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2PolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Contacts/b2TOISolver.cpp

@@ -0,0 +1,231 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Contacts/b2TOISolver.h"
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Common/b2StackAllocator.h"
+
+struct b2TOIConstraint
+{
+	b2Vec2 localPoints[b2_maxManifoldPoints];
+	b2Vec2 localNormal;
+	b2Vec2 localPoint;
+	b2Manifold::Type type;
+	float32 radius;
+	int32 pointCount;
+	b2Body* bodyA;
+	b2Body* bodyB;
+};
+
+b2TOISolver::b2TOISolver(b2StackAllocator* allocator)
+{
+	m_allocator = allocator;
+	m_constraints = NULL;
+	m_count = NULL;
+	m_toiBody = NULL;
+}
+
+b2TOISolver::~b2TOISolver()
+{
+	Clear();
+}
+
+void b2TOISolver::Clear()
+{
+	if (m_allocator && m_constraints)
+	{
+		m_allocator->Free(m_constraints);
+		m_constraints = NULL;
+	}
+}
+
+void b2TOISolver::Initialize(b2Contact** contacts, int32 count, b2Body* toiBody)
+{
+	Clear();
+
+	m_count = count;
+	m_toiBody = toiBody;
+
+	m_constraints = (b2TOIConstraint*) m_allocator->Allocate(m_count * sizeof(b2TOIConstraint));
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2Contact* contact = contacts[i];
+
+		b2Fixture* fixtureA = contact->GetFixtureA();
+		b2Fixture* fixtureB = contact->GetFixtureB();
+		b2Shape* shapeA = fixtureA->GetShape();
+		b2Shape* shapeB = fixtureB->GetShape();
+		float32 radiusA = shapeA->m_radius;
+		float32 radiusB = shapeB->m_radius;
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+		b2Manifold* manifold = contact->GetManifold();
+
+		b2Assert(manifold->pointCount > 0);
+
+		b2TOIConstraint* constraint = m_constraints + i;
+		constraint->bodyA = bodyA;
+		constraint->bodyB = bodyB;
+		constraint->localNormal = manifold->localNormal;
+		constraint->localPoint = manifold->localPoint;
+		constraint->type = manifold->type;
+		constraint->pointCount = manifold->pointCount;
+		constraint->radius = radiusA + radiusB;
+
+		for (int32 j = 0; j < constraint->pointCount; ++j)
+		{
+			b2ManifoldPoint* cp = manifold->points + j;
+			constraint->localPoints[j] = cp->localPoint;
+		}
+	}
+}
+
+struct b2TOISolverManifold
+{
+	void Initialize(b2TOIConstraint* cc, int32 index)
+	{
+		b2Assert(cc->pointCount > 0);
+
+		switch (cc->type)
+		{
+		case b2Manifold::e_circles:
+			{
+				b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
+				b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
+				if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+				{
+					normal = pointB - pointA;
+					normal.Normalize();
+				}
+				else
+				{
+					normal.Set(1.0f, 0.0f);
+				}
+
+				point = 0.5f * (pointA + pointB);
+				separation = b2Dot(pointB - pointA, normal) - cc->radius;
+			}
+			break;
+
+		case b2Manifold::e_faceA:
+			{
+				normal = cc->bodyA->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+			}
+			break;
+
+		case b2Manifold::e_faceB:
+			{
+				normal = cc->bodyB->GetWorldVector(cc->localNormal);
+				b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
+
+				b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				point = clipPoint;
+
+				// Ensure normal points from A to B
+				normal = -normal;
+			}
+			break;
+		}
+	}
+
+	b2Vec2 normal;
+	b2Vec2 point;
+	float32 separation;
+};
+
+// Push out the toi body to provide clearance for further simulation.
+bool b2TOISolver::Solve(float32 baumgarte)
+{
+	float32 minSeparation = 0.0f;
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2TOIConstraint* c = m_constraints + i;
+		b2Body* bodyA = c->bodyA;
+		b2Body* bodyB = c->bodyB;
+
+		float32 massA = bodyA->m_mass;
+		float32 massB = bodyB->m_mass;
+
+		// Only the TOI body should move.
+		if (bodyA == m_toiBody)
+		{
+			massB = 0.0f;
+		}
+		else
+		{
+			massA = 0.0f;
+		}
+
+		float32 invMassA = massA * bodyA->m_invMass;
+		float32 invIA = massA * bodyA->m_invI;
+		float32 invMassB = massB * bodyB->m_invMass;
+		float32 invIB = massB * bodyB->m_invI;
+
+		// Solve normal constraints
+		for (int32 j = 0; j < c->pointCount; ++j)
+		{
+			b2TOISolverManifold psm;
+			psm.Initialize(c, j);
+			b2Vec2 normal = psm.normal;
+
+			b2Vec2 point = psm.point;
+			float32 separation = psm.separation;
+
+			b2Vec2 rA = point - bodyA->m_sweep.c;
+			b2Vec2 rB = point - bodyB->m_sweep.c;
+
+			// Track max constraint error.
+			minSeparation = b2Min(minSeparation, separation);
+
+			// Prevent large corrections and allow slop.
+			float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+			// Compute the effective mass.
+			float32 rnA = b2Cross(rA, normal);
+			float32 rnB = b2Cross(rB, normal);
+			float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
+
+			// Compute normal impulse
+			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+			b2Vec2 P = impulse * normal;
+
+			bodyA->m_sweep.c -= invMassA * P;
+			bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
+			bodyA->SynchronizeTransform();
+
+			bodyB->m_sweep.c += invMassB * P;
+			bodyB->m_sweep.a += invIB * b2Cross(rB, P);
+			bodyB->SynchronizeTransform();
+		}
+	}
+
+	// We can't expect minSpeparation >= -b2_linearSlop because we don't
+	// push the separation above -b2_linearSlop.
+	return minSeparation >= -1.5f * b2_linearSlop;
+}

AndEngine/jni/Box2D/Dynamics/Contacts/b2TOISolver.h

@@ -0,0 +1,51 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_TOI_SOLVER_H
+#define B2_TOI_SOLVER_H
+
+#include "Box2D/Common/b2Math.h"
+
+class b2Contact;
+class b2Body;
+struct b2TOIConstraint;
+class b2StackAllocator;
+
+/// This is a pure position solver for a single movable body in contact with
+/// multiple non-moving bodies.
+class b2TOISolver
+{
+public:
+	b2TOISolver(b2StackAllocator* allocator);
+	~b2TOISolver();
+
+	void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);
+	void Clear();
+
+	// Perform one solver iteration. Returns true if converged.
+	bool Solve(float32 baumgarte);
+
+private:
+
+	b2TOIConstraint* m_constraints;
+	int32 m_count;
+	b2Body* m_toiBody;
+	b2StackAllocator* m_allocator;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2DistanceJoint.cpp

@@ -0,0 +1,211 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2DistanceJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.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)
+{
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor1);
+	localAnchorB = bodyB->GetLocalPoint(anchor2);
+	b2Vec2 d = anchor2 - anchor1;
+	length = d.Length();
+}
+
+
+b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+	m_length = def->length;
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+	m_impulse = 0.0f;
+	m_gamma = 0.0f;
+	m_bias = 0.0f;
+}
+
+void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	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;
+
+	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+	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 = step.dt * (d + step.dt * k);
+		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
+		m_bias = C * step.dt * k * m_gamma;
+
+		m_mass = invMass + m_gamma;
+		m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		// Scale the impulse to support a variable time step.
+		m_impulse *= step.dtRatio;
+
+		b2Vec2 P = m_impulse * m_u;
+		b1->m_linearVelocity -= b1->m_invMass * P;
+		b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+		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_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// 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(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	if (m_frequencyHz > 0.0f)
+	{
+		// There is no position correction for soft distance constraints.
+		return true;
+	}
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 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::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2DistanceJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 F = (inv_dt * m_impulse) * m_u;
+	return F;
+}
+
+float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
+{
+	B2_NOT_USED(inv_dt);
+	return 0.0f;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2DistanceJoint.h

@@ -0,0 +1,140 @@
+/*
+* 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 "Box2D/Dynamics/Joints/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;
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.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* bodyA, b2Body* bodyB,
+					const b2Vec2& anchorA, const b2Vec2& anchorB);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The natural length between the anchor points.
+	float32 length;
+
+	/// The mass-spring-damper frequency in Hertz.
+	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 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Set/get the natural length.
+	/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
+	void SetLength(float32 length);
+	float32 GetLength() const;
+
+	// Set/get frequency in Hz.
+	void SetFrequency(float32 hz);
+	float32 GetFrequency() const;
+
+	// Set/get damping ratio.
+	void SetDampingRatio(float32 ratio);
+	float32 GetDampingRatio() const;
+
+protected:
+
+	friend class b2Joint;
+	b2DistanceJoint(const b2DistanceJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	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;
+	float32 m_length;
+};
+
+inline void b2DistanceJoint::SetLength(float32 length)
+{
+	m_length = length;
+}
+
+inline float32 b2DistanceJoint::GetLength() const
+{
+	return m_length;
+}
+
+inline void b2DistanceJoint::SetFrequency(float32 hz)
+{
+	m_frequencyHz = hz;
+}
+
+inline float32 b2DistanceJoint::GetFrequency() const
+{
+	return m_frequencyHz;
+}
+
+inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
+{
+	m_dampingRatio = ratio;
+}
+
+inline float32 b2DistanceJoint::GetDampingRatio() const
+{
+	return m_dampingRatio;
+}
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2FrictionJoint.cpp

@@ -0,0 +1,229 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2FrictionJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+
+// Point-to-point constraint
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+}
+
+b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+
+	m_linearImpulse.SetZero();
+	m_angularImpulse = 0.0f;
+
+	m_maxForce = def->maxForce;
+	m_maxTorque = def->maxTorque;
+}
+
+void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	// Compute the effective mass matrix.
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = mA + mB;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;		K1.col2.y = mA + mB;
+
+	b2Mat22 K2;
+	K2.col1.x =  iA * rA.y * rA.y;	K2.col2.x = -iA * rA.x * rA.y;
+	K2.col1.y = -iA * rA.x * rA.y;	K2.col2.y =  iA * rA.x * rA.x;
+
+	b2Mat22 K3;
+	K3.col1.x =  iB * rB.y * rB.y;	K3.col2.x = -iB * rB.x * rB.y;
+	K3.col1.y = -iB * rB.x * rB.y;	K3.col2.y =  iB * rB.x * rB.x;
+
+	b2Mat22 K = K1 + K2 + K3;
+	m_linearMass = K.GetInverse();
+
+	m_angularMass = iA + iB;
+	if (m_angularMass > 0.0f)
+	{
+		m_angularMass = 1.0f / m_angularMass;
+	}
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_linearImpulse *= step.dtRatio;
+		m_angularImpulse *= step.dtRatio;
+
+		b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
+
+		bA->m_linearVelocity -= mA * P;
+		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
+
+		bB->m_linearVelocity += mB * P;
+		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
+	}
+	else
+	{
+		m_linearImpulse.SetZero();
+		m_angularImpulse = 0.0f;
+	}
+}
+
+void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	float32 wA = bA->m_angularVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float32 wB = bB->m_angularVelocity;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// Solve angular friction
+	{
+		float32 Cdot = wB - wA;
+		float32 impulse = -m_angularMass * Cdot;
+
+		float32 oldImpulse = m_angularImpulse;
+		float32 maxImpulse = step.dt * m_maxTorque;
+		m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_angularImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// Solve linear friction
+	{
+		b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+
+		b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
+		b2Vec2 oldImpulse = m_linearImpulse;
+		m_linearImpulse += impulse;
+
+		float32 maxImpulse = step.dt * m_maxForce;
+
+		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
+		{
+			m_linearImpulse.Normalize();
+			m_linearImpulse *= maxImpulse;
+		}
+
+		impulse = m_linearImpulse - oldImpulse;
+
+		vA -= mA * impulse;
+		wA -= iA * b2Cross(rA, impulse);
+
+		vB += mB * impulse;
+		wB += iB * b2Cross(rB, impulse);
+	}
+
+	bA->m_linearVelocity = vA;
+	bA->m_angularVelocity = wA;
+	bB->m_linearVelocity = vB;
+	bB->m_angularVelocity = wB;
+}
+
+bool b2FrictionJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	return true;
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2FrictionJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * m_linearImpulse;
+}
+
+float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_angularImpulse;
+}
+
+void b2FrictionJoint::SetMaxForce(float32 force)
+{
+	b2Assert(b2IsValid(force) && force >= 0.0f);
+	m_maxForce = force;
+}
+
+float32 b2FrictionJoint::GetMaxForce() const
+{
+	return m_maxForce;
+}
+
+void b2FrictionJoint::SetMaxTorque(float32 torque)
+{
+	b2Assert(b2IsValid(torque) && torque >= 0.0f);
+	m_maxTorque = torque;
+}
+
+float32 b2FrictionJoint::GetMaxTorque() const
+{
+	return m_maxTorque;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2FrictionJoint.h

@@ -0,0 +1,99 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_FRICTION_JOINT_H
+#define B2_FRICTION_JOINT_H
+
+#include "Box2D/Dynamics/Joints/b2Joint.h"
+
+/// Friction joint definition.
+struct b2FrictionJointDef : public b2JointDef
+{
+	b2FrictionJointDef()
+	{
+		type = e_frictionJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		maxForce = 0.0f;
+		maxTorque = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
+
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
+
+	/// The maximum friction force in N.
+	float32 maxForce;
+
+	/// The maximum friction torque in N-m.
+	float32 maxTorque;
+};
+
+/// Friction joint. This is used for top-down friction.
+/// It provides 2D translational friction and angular friction.
+class b2FrictionJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Set the maximum friction force in N.
+	void SetMaxForce(float32 force);
+
+	/// Get the maximum friction force in N.
+	float32 GetMaxForce() const;
+
+	/// Set the maximum friction torque in N*m.
+	void SetMaxTorque(float32 torque);
+
+	/// Get the maximum friction torque in N*m.
+	float32 GetMaxTorque() const;
+
+protected:
+
+	friend class b2Joint;
+
+	b2FrictionJoint(const b2FrictionJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+
+	b2Mat22 m_linearMass;
+	float32 m_angularMass;
+
+	b2Vec2 m_linearImpulse;
+	float32 m_angularImpulse;
+
+	float32 m_maxForce;
+	float32 m_maxTorque;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2GearJoint.cpp

@@ -0,0 +1,259 @@
+/*
+* 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 "Box2D/Dynamics/Joints/b2GearJoint.h"
+#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h"
+#include "Box2D/Dynamics/Joints/b2PrismaticJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// 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->GetBodyA()->GetType() == b2_staticBody);
+	b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
+
+	m_revolute1 = NULL;
+	m_prismatic1 = NULL;
+	m_revolute2 = NULL;
+	m_prismatic2 = NULL;
+
+	float32 coordinate1, coordinate2;
+
+	m_ground1 = def->joint1->GetBodyA();
+	m_bodyA = def->joint1->GetBodyB();
+	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->GetBodyA();
+	m_bodyB = def->joint2->GetBodyB();
+	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_impulse = 0.0f;
+}
+
+void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* g1 = m_ground1;
+	b2Body* g2 = m_ground2;
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	float32 K = 0.0f;
+	m_J.SetZero();
+
+	if (m_revolute1)
+	{
+		m_J.angularA = -1.0f;
+		K += b1->m_invI;
+	}
+	else
+	{
+		b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
+		b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		float32 crug = b2Cross(r, ug);
+		m_J.linearA = -ug;
+		m_J.angularA = -crug;
+		K += b1->m_invMass + b1->m_invI * crug * crug;
+	}
+
+	if (m_revolute2)
+	{
+		m_J.angularB = -m_ratio;
+		K += m_ratio * m_ratio * b2->m_invI;
+	}
+	else
+	{
+		b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
+		b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+		float32 crug = b2Cross(r, ug);
+		m_J.linearB = -m_ratio * ug;
+		m_J.angularB = -m_ratio * crug;
+		K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
+	}
+
+	// Compute effective mass.
+	m_mass = K > 0.0f ? 1.0f / K : 0.0f;
+
+	if (step.warmStarting)
+	{
+		// Warm starting.
+		b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
+		b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
+		b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
+		b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
+	}
+	else
+	{
+		m_impulse = 0.0f;
+	}
+}
+
+void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	float32 Cdot = m_J.Compute(	b1->m_linearVelocity, b1->m_angularVelocity,
+								b2->m_linearVelocity, b2->m_angularVelocity);
+
+	float32 impulse = m_mass * (-Cdot);
+	m_impulse += impulse;
+
+	b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
+	b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
+	b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
+	b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
+}
+
+bool b2GearJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+	
+	float32 linearError = 0.0f;
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	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.linearA;
+	b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
+	b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
+	b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	// TODO_ERIN not implemented
+	return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2GearJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2GearJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
+{
+	// TODO_ERIN not tested
+	b2Vec2 P = m_impulse * m_J.linearB;
+	return inv_dt * P;
+}
+
+float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
+{
+	// TODO_ERIN not tested
+	b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
+	b2Vec2 P = m_impulse * m_J.linearB;
+	float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
+	return inv_dt * L;
+}
+
+void b2GearJoint::SetRatio(float32 ratio)
+{
+	b2Assert(b2IsValid(ratio));
+	m_ratio = ratio;
+}
+
+float32 b2GearJoint::GetRatio() const
+{
+	return m_ratio;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2GearJoint.h

@@ -0,0 +1,111 @@
+/*
+* 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 "Box2D/Dynamics/Joints/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 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Set/Get the gear ratio.
+	void SetRatio(float32 ratio);
+	float32 GetRatio() const;
+
+protected:
+
+	friend class b2Joint;
+	b2GearJoint(const b2GearJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	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_impulse;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2Joint.cpp

@@ -0,0 +1,186 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2Joint.h"
+#include "Box2D/Dynamics/Joints/b2DistanceJoint.h"
+#include "Box2D/Dynamics/Joints/b2LineJoint.h"
+#include "Box2D/Dynamics/Joints/b2MouseJoint.h"
+#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h"
+#include "Box2D/Dynamics/Joints/b2PrismaticJoint.h"
+#include "Box2D/Dynamics/Joints/b2PulleyJoint.h"
+#include "Box2D/Dynamics/Joints/b2GearJoint.h"
+#include "Box2D/Dynamics/Joints/b2WeldJoint.h"
+#include "Box2D/Dynamics/Joints/b2FrictionJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2World.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+
+#include <new>
+
+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;
+
+	case e_lineJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2LineJoint));
+			joint = new (mem) b2LineJoint((b2LineJointDef*)def);
+		}
+		break;
+
+	case e_weldJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2WeldJoint));
+			joint = new (mem) b2WeldJoint((b2WeldJointDef*)def);
+		}
+		break;
+        
+	case e_frictionJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
+			joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def);
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	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;
+
+	case e_lineJoint:
+		allocator->Free(joint, sizeof(b2LineJoint));
+		break;
+    
+	case e_weldJoint:
+		allocator->Free(joint, sizeof(b2WeldJoint));
+		break;
+
+	case e_frictionJoint:
+		allocator->Free(joint, sizeof(b2FrictionJoint));
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+	b2Assert(def->bodyA != def->bodyB);
+
+	m_type = def->type;
+	m_prev = NULL;
+	m_next = NULL;
+	m_bodyA = def->bodyA;
+	m_bodyB = def->bodyB;
+	m_collideConnected = def->collideConnected;
+	m_islandFlag = false;
+	m_userData = def->userData;
+
+	m_edgeA.joint = NULL;
+	m_edgeA.other = NULL;
+	m_edgeA.prev = NULL;
+	m_edgeA.next = NULL;
+
+	m_edgeB.joint = NULL;
+	m_edgeB.other = NULL;
+	m_edgeB.prev = NULL;
+	m_edgeB.next = NULL;
+}
+
+bool b2Joint::IsActive() const
+{
+	return m_bodyA->IsActive() && m_bodyB->IsActive();
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2Joint.h

@@ -0,0 +1,226 @@
+/*
+* 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_JOINT_H
+#define B2_JOINT_H
+
+#include "Box2D/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,
+	e_lineJoint,
+    e_weldJoint,
+	e_frictionJoint,
+};
+
+enum b2LimitState
+{
+	e_inactiveLimit,
+	e_atLowerLimit,
+	e_atUpperLimit,
+	e_equalLimits
+};
+
+struct b2Jacobian
+{
+	b2Vec2 linearA;
+	float32 angularA;
+	b2Vec2 linearB;
+	float32 angularB;
+
+	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;
+		bodyA = NULL;
+		bodyB = 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* bodyA;
+
+	/// The second attached body.
+	b2Body* bodyB;
+
+	/// 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* GetBodyA();
+
+	/// Get the second body attached to this joint.
+	b2Body* GetBodyB();
+
+	/// Get the anchor point on bodyA in world coordinates.
+	virtual b2Vec2 GetAnchorA() const = 0;
+
+	/// Get the anchor point on bodyB in world coordinates.
+	virtual b2Vec2 GetAnchorB() const = 0;
+
+	/// Get the reaction force on body2 at the joint anchor in Newtons.
+	virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
+
+	/// Get the reaction torque on body2 in N*m.
+	virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
+
+	/// Get the next joint the world joint list.
+	b2Joint* GetNext();
+
+	/// Get the user data pointer.
+	void* GetUserData() const;
+
+	/// Set the user data pointer.
+	void SetUserData(void* data);
+
+	/// Short-cut function to determine if either body is inactive.
+	bool IsActive() const;
+
+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 bool SolvePositionConstraints(float32 baumgarte) = 0;
+
+	b2JointType m_type;
+	b2Joint* m_prev;
+	b2Joint* m_next;
+	b2JointEdge m_edgeA;
+	b2JointEdge m_edgeB;
+	b2Body* m_bodyA;
+	b2Body* m_bodyB;
+
+	bool m_islandFlag;
+	bool m_collideConnected;
+
+	void* m_userData;
+
+	// Cache here per time step to reduce cache misses.
+	b2Vec2 m_localCenterA, m_localCenterB;
+	float32 m_invMassA, m_invIA;
+	float32 m_invMassB, m_invIB;
+};
+
+inline void b2Jacobian::SetZero()
+{
+	linearA.SetZero(); angularA = 0.0f;
+	linearB.SetZero(); angularB = 0.0f;
+}
+
+inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+	linearA = x1; angularA = a1;
+	linearB = x2; angularB = a2;
+}
+
+inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+	return b2Dot(linearA, x1) + angularA * a1 + b2Dot(linearB, x2) + angularB * a2;
+}
+
+inline b2JointType b2Joint::GetType() const
+{
+	return m_type;
+}
+
+inline b2Body* b2Joint::GetBodyA()
+{
+	return m_bodyA;
+}
+
+inline b2Body* b2Joint::GetBodyB()
+{
+	return m_bodyB;
+}
+
+inline b2Joint* b2Joint::GetNext()
+{
+	return m_next;
+}
+
+inline void* b2Joint::GetUserData() const
+{
+	return m_userData;
+}
+
+inline void b2Joint::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2LineJoint.cpp

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

AndEngine/jni/Box2D/Dynamics/Joints/b2LineJoint.h

@@ -0,0 +1,170 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_LINE_JOINT_H
+#define B2_LINE_JOINT_H
+
+#include "Box2D/Dynamics/Joints/b2Joint.h"
+
+/// Line joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space. Using local
+/// anchors and a local axis helps when saving and loading a game.
+struct b2LineJointDef : public b2JointDef
+{
+	b2LineJointDef()
+	{
+		type = e_lineJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		localAxisA.Set(1.0f, 0.0f);
+		enableLimit = false;
+		lowerTranslation = 0.0f;
+		upperTranslation = 0.0f;
+		enableMotor = false;
+		maxMotorForce = 0.0f;
+		motorSpeed = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The local translation axis in body1.
+	b2Vec2 localAxisA;
+
+	/// Enable/disable the joint limit.
+	bool enableLimit;
+
+	/// The lower translation limit, usually in meters.
+	float32 lowerTranslation;
+
+	/// The upper translation limit, usually in meters.
+	float32 upperTranslation;
+
+	/// Enable/disable the joint motor.
+	bool enableMotor;
+
+	/// The maximum motor torque, usually in N-m.
+	float32 maxMotorForce;
+
+	/// The desired motor speed in radians per second.
+	float32 motorSpeed;
+};
+
+/// A line joint. This joint provides two degrees of freedom: translation
+/// along an axis fixed in body1 and rotation in the plane. You can use a
+/// joint limit to restrict the range of motion and a joint motor to drive
+/// the motion or to model joint friction.
+class b2LineJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the current joint translation, usually in meters.
+	float32 GetJointTranslation() const;
+
+	/// Get the current joint translation speed, usually in meters per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	/// Enable/disable the joint limit.
+	void EnableLimit(bool flag);
+
+	/// Get the lower joint limit, usually in meters.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit, usually in meters.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits, usually in meters.
+	void SetLimits(float32 lower, float32 upper);
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed, usually in meters per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed, usually in meters per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set/Get the maximum motor force, usually in N.
+	void SetMaxMotorForce(float32 force);
+	float32 GetMaxMotorForce() const;
+
+	/// Get the current motor force, usually in N.
+	float32 GetMotorForce() const;
+
+protected:
+
+	friend class b2Joint;
+	b2LineJoint(const b2LineJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_localXAxis1;
+	b2Vec2 m_localYAxis1;
+
+	b2Vec2 m_axis, m_perp;
+	float32 m_s1, m_s2;
+	float32 m_a1, m_a2;
+
+	b2Mat22 m_K;
+	b2Vec2 m_impulse;
+
+	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
+	float32 m_motorImpulse;
+
+	float32 m_lowerTranslation;
+	float32 m_upperTranslation;
+	float32 m_maxMotorForce;
+	float32 m_motorSpeed;
+
+	bool m_enableLimit;
+	bool m_enableMotor;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2LineJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2MouseJoint.cpp

@@ -0,0 +1,198 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2MouseJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+#include <stdio.h>
+
+// p = attached point, m = mouse point
+// C = p - m
+// Cdot = v
+//      = v + cross(w, r)
+// J = [I r_skew]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
+: b2Joint(def)
+{
+	b2Assert(def->target.IsValid());
+	b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
+	b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
+	b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
+
+	m_target = def->target;
+	m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target);
+
+	m_maxForce = def->maxForce;
+	m_impulse.SetZero();
+
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+
+	m_beta = 0.0f;
+	m_gamma = 0.0f;
+}
+
+void b2MouseJoint::SetTarget(const b2Vec2& target)
+{
+	if (m_bodyB->IsAwake() == false)
+	{
+		m_bodyB->SetAwake(true);
+	}
+	m_target = target;
+}
+
+const b2Vec2& b2MouseJoint::GetTarget() const
+{
+	return m_target;
+}
+
+void b2MouseJoint::SetMaxForce(float32 force)
+{
+	m_maxForce = force;
+}
+
+float32 b2MouseJoint::GetMaxForce() const
+{
+	return m_maxForce;
+}
+
+void b2MouseJoint::SetFrequency(float32 hz)
+{
+	m_frequencyHz = hz;
+}
+
+float32 b2MouseJoint::GetFrequency() const
+{
+	return m_frequencyHz;
+}
+
+void b2MouseJoint::SetDampingRatio(float32 ratio)
+{
+	m_dampingRatio = ratio;
+}
+
+float32 b2MouseJoint::GetDampingRatio() const
+{
+	return m_dampingRatio;
+}
+
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_bodyB;
+
+	float32 mass = b->GetMass();
+
+	// Frequency
+	float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+	// Damping coefficient
+	float32 d = 2.0f * mass * m_dampingRatio * omega;
+
+	// Spring stiffness
+	float32 k = mass * (omega * omega);
+
+	// magic formulas
+	// gamma has units of inverse mass.
+	// beta has units of inverse time.
+	b2Assert(d + step.dt * k > b2_epsilon);
+	m_gamma = step.dt * (d + step.dt * k);
+	if (m_gamma != 0.0f)
+	{
+		m_gamma = 1.0f / m_gamma;
+	}
+	m_beta = step.dt * k * m_gamma;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+
+	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	float32 invMass = b->m_invMass;
+	float32 invI = b->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = invMass;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;		K1.col2.y = invMass;
+
+	b2Mat22 K2;
+	K2.col1.x =  invI * r.y * r.y;	K2.col2.x = -invI * r.x * r.y;
+	K2.col1.y = -invI * r.x * r.y;	K2.col2.y =  invI * r.x * r.x;
+
+	b2Mat22 K = K1 + K2;
+	K.col1.x += m_gamma;
+	K.col2.y += m_gamma;
+
+	m_mass = K.GetInverse();
+
+	m_C = b->m_sweep.c + r - m_target;
+
+	// Cheat with some damping
+	b->m_angularVelocity *= 0.98f;
+
+	// Warm starting.
+	m_impulse *= step.dtRatio;
+	b->m_linearVelocity += invMass * m_impulse;
+	b->m_angularVelocity += invI * b2Cross(r, m_impulse);
+}
+
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_bodyB;
+
+	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+
+	// Cdot = v + cross(w, r)
+	b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+	b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
+
+	b2Vec2 oldImpulse = m_impulse;
+	m_impulse += impulse;
+	float32 maxImpulse = step.dt * m_maxForce;
+	if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
+	{
+		m_impulse *= maxImpulse / m_impulse.Length();
+	}
+	impulse = m_impulse - oldImpulse;
+
+	b->m_linearVelocity += b->m_invMass * impulse;
+	b->m_angularVelocity += b->m_invI * b2Cross(r, impulse);
+}
+
+b2Vec2 b2MouseJoint::GetAnchorA() const
+{
+	return m_target;
+}
+
+b2Vec2 b2MouseJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor);
+}
+
+b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * m_impulse;
+}
+
+float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * 0.0f;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2MouseJoint.h

@@ -0,0 +1,114 @@
+/*
+* 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 "Box2D/Dynamics/Joints/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;
+	}
+
+	/// 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;
+};
+
+/// A mouse joint is used to make a point on a body track a
+/// specified world point. This a soft constraint with a maximum
+/// force. This allows the constraint to stretch and without
+/// applying huge forces.
+/// NOTE: this joint is not documented in the manual because it was
+/// developed to be used in the testbed. If you want to learn how to
+/// use the mouse joint, look at the testbed.
+class b2MouseJoint : public b2Joint
+{
+public:
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchorA() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchorB() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+
+	/// Implements b2Joint.
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Use this to update the target point.
+	void SetTarget(const b2Vec2& target);
+	const b2Vec2& GetTarget() const;
+
+	/// Set/get the maximum force in Newtons.
+	void SetMaxForce(float32 force);
+	float32 GetMaxForce() const;
+
+	/// Set/get the frequency in Hertz.
+	void SetFrequency(float32 hz);
+	float32 GetFrequency() const;
+
+	/// Set/get the damping ratio (dimensionless).
+	void SetDampingRatio(float32 ratio);
+	float32 GetDampingRatio() const;
+
+protected:
+	friend class b2Joint;
+
+	b2MouseJoint(const b2MouseJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); 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_frequencyHz;
+	float32 m_dampingRatio;
+	float32 m_beta;
+	float32 m_gamma;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp

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

AndEngine/jni/Box2D/Dynamics/Joints/b2PrismaticJoint.h

@@ -0,0 +1,175 @@
+/*
+* 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 "Box2D/Dynamics/Joints/b2Joint.h"
+
+/// Prismatic joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// 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.
+/// @warning at least one body should by dynamic with a non-fixed rotation.
+struct b2PrismaticJointDef : public b2JointDef
+{
+	b2PrismaticJointDef()
+	{
+		type = e_prismaticJoint;
+		localAnchorA.SetZero();
+		localAnchorB.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* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The local translation axis in body1.
+	b2Vec2 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 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the current joint translation, usually in meters.
+	float32 GetJointTranslation() const;
+
+	/// Get the current joint translation speed, usually in meters per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	/// Enable/disable the joint limit.
+	void EnableLimit(bool flag);
+
+	/// Get the lower joint limit, usually in meters.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit, usually in meters.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits, usually in meters.
+	void SetLimits(float32 lower, float32 upper);
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed, usually in meters per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed, usually in meters per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set the maximum motor force, usually in N.
+	void SetMaxMotorForce(float32 force);
+
+	/// Get the current motor force, usually in N.
+	float32 GetMotorForce() const;
+
+protected:
+	friend class b2Joint;
+	friend class b2GearJoint;
+	b2PrismaticJoint(const b2PrismaticJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_localXAxis1;
+	b2Vec2 m_localYAxis1;
+	float32 m_refAngle;
+
+	b2Vec2 m_axis, m_perp;
+	float32 m_s1, m_s2;
+	float32 m_a1, m_a2;
+
+	b2Mat33 m_K;
+	b2Vec3 m_impulse;
+
+	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
+	float32 m_motorImpulse;
+
+	float32 m_lowerTranslation;
+	float32 m_upperTranslation;
+	float32 m_maxMotorForce;
+	float32 m_motorSpeed;
+	
+	bool m_enableLimit;
+	bool m_enableMotor;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2PrismaticJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2PulleyJoint.cpp

@@ -0,0 +1,427 @@
+/*
+* 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 "Box2D/Dynamics/Joints/b2PulleyJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.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)
+{
+	bodyA = b1;
+	bodyB = b2;
+	groundAnchorA = ga1;
+	groundAnchorB = ga2;
+	localAnchorA = bodyA->GetLocalPoint(anchor1);
+	localAnchorB = bodyB->GetLocalPoint(anchor2);
+	b2Vec2 d1 = anchor1 - ga1;
+	lengthA = d1.Length();
+	b2Vec2 d2 = anchor2 - ga2;
+	lengthB = d2.Length();
+	ratio = r;
+	b2Assert(ratio > b2_epsilon);
+	float32 C = lengthA + ratio * lengthB;
+	maxLengthA = C - ratio * b2_minPulleyLength;
+	maxLengthB = (C - b2_minPulleyLength) / ratio;
+}
+
+b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
+: b2Joint(def)
+{
+	m_groundAnchor1 = def->groundAnchorA;
+	m_groundAnchor2 = def->groundAnchorB;
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+
+	b2Assert(def->ratio != 0.0f);
+	m_ratio = def->ratio;
+
+	m_constant = def->lengthA + m_ratio * def->lengthB;
+
+	m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength);
+	m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio);
+
+	m_impulse = 0.0f;
+	m_limitImpulse1 = 0.0f;
+	m_limitImpulse2 = 0.0f;
+}
+
+void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+
+	b2Vec2 s1 = m_groundAnchor1;
+	b2Vec2 s2 = 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_impulse = 0.0f;
+	}
+	else
+	{
+		m_state = e_atUpperLimit;
+	}
+
+	if (length1 < m_maxLength1)
+	{
+		m_limitState1 = e_inactiveLimit;
+		m_limitImpulse1 = 0.0f;
+	}
+	else
+	{
+		m_limitState1 = e_atUpperLimit;
+	}
+
+	if (length2 < m_maxLength2)
+	{
+		m_limitState2 = e_inactiveLimit;
+		m_limitImpulse2 = 0.0f;
+	}
+	else
+	{
+		m_limitState2 = e_atUpperLimit;
+	}
+
+	// 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_epsilon);
+	b2Assert(m_limitMass2 > b2_epsilon);
+	b2Assert(m_pulleyMass > b2_epsilon);
+	m_limitMass1 = 1.0f / m_limitMass1;
+	m_limitMass2 = 1.0f / m_limitMass2;
+	m_pulleyMass = 1.0f / m_pulleyMass;
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support variable time steps.
+		m_impulse *= step.dtRatio;
+		m_limitImpulse1 *= step.dtRatio;
+		m_limitImpulse2 *= step.dtRatio;
+
+		// Warm starting.
+		b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;
+		b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+	else
+	{
+		m_impulse = 0.0f;
+		m_limitImpulse1 = 0.0f;
+		m_limitImpulse2 = 0.0f;
+	}
+}
+
+void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	if (m_state == e_atUpperLimit)
+	{
+		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 impulse = m_pulleyMass * (-Cdot);
+		float32 oldImpulse = m_impulse;
+		m_impulse = b2Max(0.0f, m_impulse + impulse);
+		impulse = m_impulse - oldImpulse;
+
+		b2Vec2 P1 = -impulse * m_u1;
+		b2Vec2 P2 = -m_ratio * impulse * m_u2;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		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 impulse = -m_limitMass1 * Cdot;
+		float32 oldImpulse = m_limitImpulse1;
+		m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse);
+		impulse = m_limitImpulse1 - oldImpulse;
+
+		b2Vec2 P1 = -impulse * 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 impulse = -m_limitMass2 * Cdot;
+		float32 oldImpulse = m_limitImpulse2;
+		m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse);
+		impulse = m_limitImpulse2 - oldImpulse;
+
+		b2Vec2 P2 = -impulse * m_u2;
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+}
+
+bool b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 s1 = m_groundAnchor1;
+	b2Vec2 s2 = m_groundAnchor2;
+
+	float32 linearError = 0.0f;
+
+	if (m_state == e_atUpperLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+
+		// 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;
+
+		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->GetTransform().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;
+
+		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->GetTransform().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;
+
+		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::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PulleyJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 P = m_impulse * m_u2;
+	return inv_dt * P;
+}
+
+float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
+{
+	B2_NOT_USED(inv_dt);
+	return 0.0f;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
+{
+	return m_groundAnchor1;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
+{
+	return m_groundAnchor2;
+}
+
+float32 b2PulleyJoint::GetLength1() const
+{
+	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchor1);
+	b2Vec2 s = m_groundAnchor1;
+	b2Vec2 d = p - s;
+	return d.Length();
+}
+
+float32 b2PulleyJoint::GetLength2() const
+{
+	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchor2);
+	b2Vec2 s = m_groundAnchor2;
+	b2Vec2 d = p - s;
+	return d.Length();
+}
+
+float32 b2PulleyJoint::GetRatio() const
+{
+	return m_ratio;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2PulleyJoint.h

@@ -0,0 +1,148 @@
+/*
+* 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 "Box2D/Dynamics/Joints/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;
+		groundAnchorA.Set(-1.0f, 1.0f);
+		groundAnchorB.Set(1.0f, 1.0f);
+		localAnchorA.Set(-1.0f, 0.0f);
+		localAnchorB.Set(1.0f, 0.0f);
+		lengthA = 0.0f;
+		maxLengthA = 0.0f;
+		lengthB = 0.0f;
+		maxLengthB = 0.0f;
+		ratio = 1.0f;
+		collideConnected = true;
+	}
+
+	/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
+	void Initialize(b2Body* bodyA, b2Body* bodyB,
+					const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,
+					const b2Vec2& anchorA, const b2Vec2& anchorB,
+					float32 ratio);
+
+	/// The first ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchorA;
+
+	/// The second ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchorB;
+
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
+
+	/// The a reference length for the segment attached to bodyA.
+	float32 lengthA;
+
+	/// The maximum length of the segment attached to bodyA.
+	float32 maxLengthA;
+
+	/// The a reference length for the segment attached to bodyB.
+	float32 lengthB;
+
+	/// The maximum length of the segment attached to bodyB.
+	float32 maxLengthB;
+
+	/// 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 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the first ground anchor.
+	b2Vec2 GetGroundAnchorA() const;
+
+	/// Get the second ground anchor.
+	b2Vec2 GetGroundAnchorB() 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;
+
+protected:
+
+	friend class b2Joint;
+	b2PulleyJoint(const b2PulleyJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	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_impulse;
+	float32 m_limitImpulse1;
+	float32 m_limitImpulse2;
+
+	b2LimitState m_state;
+	b2LimitState m_limitState1;
+	b2LimitState m_limitState2;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp

@@ -0,0 +1,478 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2RevoluteJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
+{
+	bodyA = b1;
+	bodyB = b2;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchorA;
+	m_localAnchor2 = def->localAnchorB;
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+	m_motorImpulse = 0.0f;
+
+	m_lowerAngle = def->lowerAngle;
+	m_upperAngle = def->upperAngle;
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+	m_limitState = e_inactiveLimit;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	if (m_enableMotor || m_enableLimit)
+	{
+		// You cannot create a rotation limit between bodies that
+		// both have fixed rotation.
+		b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f);
+	}
+
+	// Compute the effective mass matrix.
+	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
+	//     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
+	//     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+
+	float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
+	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+
+	m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
+	m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
+	m_mass.col3.x = -r1.y * i1 - r2.y * i2;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
+	m_mass.col3.y = r1.x * i1 + r2.x * i2;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = i1 + i2;
+
+	m_motorMass = i1 + i2;
+	if (m_motorMass > 0.0f)
+	{
+		m_motorMass = 1.0f / m_motorMass;
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorImpulse = 0.0f;
+	}
+
+	if (m_enableLimit)
+	{
+		float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
+		{
+			m_limitState = e_equalLimits;
+		}
+		else if (jointAngle <= m_lowerAngle)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_impulse.z = 0.0f;
+			}
+			m_limitState = e_atLowerLimit;
+		}
+		else if (jointAngle >= m_upperAngle)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_impulse.z = 0.0f;
+			}
+			m_limitState = e_atUpperLimit;
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_impulse.z = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitState = e_inactiveLimit;
+	}
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_impulse *= step.dtRatio;
+		m_motorImpulse *= step.dtRatio;
+
+		b2Vec2 P(m_impulse.x, m_impulse.y);
+
+		b1->m_linearVelocity -= m1 * P;
+		b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z);
+
+		b2->m_linearVelocity += m2 * P;
+		b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z);
+	}
+	else
+	{
+		m_impulse.SetZero();
+		m_motorImpulse = 0.0f;
+	}
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
+	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+
+	// Solve motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 Cdot = w2 - w1 - m_motorSpeed;
+		float32 impulse = m_motorMass * (-Cdot);
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = step.dt * m_maxMotorTorque;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		w1 -= i1 * impulse;
+		w2 += i2 * impulse;
+	}
+
+	// Solve limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		// Solve point-to-point constraint
+		b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+		float32 Cdot2 = w2 - w1;
+		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+
+		b2Vec3 impulse = m_mass.Solve33(-Cdot);
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_impulse += impulse;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 newImpulse = m_impulse.z + impulse.z;
+			if (newImpulse < 0.0f)
+			{
+				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				impulse.x = reduced.x;
+				impulse.y = reduced.y;
+				impulse.z = -m_impulse.z;
+				m_impulse.x += reduced.x;
+				m_impulse.y += reduced.y;
+				m_impulse.z = 0.0f;
+			}
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 newImpulse = m_impulse.z + impulse.z;
+			if (newImpulse > 0.0f)
+			{
+				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				impulse.x = reduced.x;
+				impulse.y = reduced.y;
+				impulse.z = -m_impulse.z;
+				m_impulse.x += reduced.x;
+				m_impulse.y += reduced.y;
+				m_impulse.z = 0.0f;
+			}
+		}
+
+		b2Vec2 P(impulse.x, impulse.y);
+
+		v1 -= m1 * P;
+		w1 -= i1 * (b2Cross(r1, P) + impulse.z);
+
+		v2 += m2 * P;
+		w2 += i2 * (b2Cross(r2, P) + impulse.z);
+	}
+	else
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		// Solve point-to-point constraint
+		b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+		b2Vec2 impulse = m_mass.Solve22(-Cdot);
+
+		m_impulse.x += impulse.x;
+		m_impulse.y += impulse.y;
+
+		v1 -= m1 * impulse;
+		w1 -= i1 * b2Cross(r1, impulse);
+
+		v2 += m2 * impulse;
+		w2 += i2 * b2Cross(r2, impulse);
+	}
+
+	b1->m_linearVelocity = v1;
+	b1->m_angularVelocity = w1;
+	b2->m_linearVelocity = v2;
+	b2->m_angularVelocity = w2;
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	// TODO_ERIN block solve with limit.
+
+	B2_NOT_USED(baumgarte);
+
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+
+	float32 angularError = 0.0f;
+	float32 positionError = 0.0f;
+
+	// Solve angular limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		float32 limitImpulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			// Prevent large angular corrections
+			float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * C;
+			angularError = b2Abs(C);
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 C = angle - m_lowerAngle;
+			angularError = -C;
+
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+			limitImpulse = -m_motorMass * C;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 C = angle - m_upperAngle;
+			angularError = C;
+
+			// Prevent large angular corrections and allow some slop.
+			C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * C;
+		}
+
+		b1->m_sweep.a -= b1->m_invI * limitImpulse;
+		b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	// Solve point-to-point constraint.
+	{
+		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+		positionError = C.Length();
+
+		float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+		float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+		// Handle large detachment.
+		const float32 k_allowedStretch = 10.0f * b2_linearSlop;
+		if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
+		{
+			// Use a particle solution (no rotation).
+			b2Vec2 u = C; u.Normalize();
+			float32 m = invMass1 + invMass2;
+			if (m > 0.0f)
+			{
+				m = 1.0f / m;
+			}
+			b2Vec2 impulse = m * (-C);
+			const float32 k_beta = 0.5f;
+			b1->m_sweep.c -= k_beta * invMass1 * impulse;
+			b2->m_sweep.c += k_beta * invMass2 * impulse;
+
+			C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+		}
+
+		b2Mat22 K1;
+		K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
+		K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;
+
+		b2Mat22 K2;
+		K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
+		K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;
+
+		b2Mat22 K3;
+		K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
+		K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;
+
+		b2Mat22 K = K1 + K2 + K3;
+		b2Vec2 impulse = K.Solve(-C);
+
+		b1->m_sweep.c -= b1->m_invMass * impulse;
+		b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
+
+		b2->m_sweep.c += b2->m_invMass * impulse;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+	
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 P(m_impulse.x, m_impulse.y);
+	return inv_dt * P;
+}
+
+float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_impulse.z;
+}
+
+float32 b2RevoluteJoint::GetJointAngle() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+	return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+}
+
+float32 b2RevoluteJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_bodyA;
+	b2Body* b2 = m_bodyB;
+	return b2->m_angularVelocity - b1->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableMotor = flag;
+}
+
+float32 b2RevoluteJoint::GetMotorTorque() const
+{
+	return m_motorImpulse;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_maxMotorTorque = torque;
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableLimit = flag;
+}
+
+float32 b2RevoluteJoint::GetLowerLimit() const
+{
+	return m_lowerAngle;
+}
+
+float32 b2RevoluteJoint::GetUpperLimit() const
+{
+	return m_upperAngle;
+}
+
+void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_lowerAngle = lower;
+	m_upperAngle = upper;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2RevoluteJoint.h

@@ -0,0 +1,174 @@
+/*
+* 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 "Box2D/Dynamics/Joints/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;
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.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 a world
+	/// anchor point.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The body2 angle minus body1 angle in the reference state (radians).
+	float32 referenceAngle;
+
+	/// A 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 two bodies to share a common point while they
+/// are free to rotate about the point. The relative rotation about the shared
+/// point is the joint angle. You can limit the relative rotation with
+/// a joint limit that specifies a lower and upper angle. You can use a motor
+/// 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 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) 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;
+
+protected:
+	
+	friend class b2Joint;
+	friend class b2GearJoint;
+
+	b2RevoluteJoint(const b2RevoluteJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchor1;	// relative
+	b2Vec2 m_localAnchor2;
+	b2Vec3 m_impulse;
+	float32 m_motorImpulse;
+
+	b2Mat33 m_mass;			// 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

AndEngine/jni/Box2D/Dynamics/Joints/b2WeldJoint.cpp

@@ -0,0 +1,219 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/Joints/b2WeldJoint.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Angle constraint
+// C = angle2 - angle1 - referenceAngle
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
+}
+
+b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_referenceAngle = def->referenceAngle;
+
+	m_impulse.SetZero();
+}
+
+void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	// Compute the effective mass matrix.
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// J = [-I -r1_skew I r2_skew]
+	//     [ 0       -1 0       1]
+	// r_skew = [-ry; rx]
+
+	// Matlab
+	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
+	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
+	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	m_mass.col3.x = -rA.y * iA - rB.y * iB;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	m_mass.col3.y = rA.x * iA + rB.x * iB;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = iA + iB;
+
+	if (step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_impulse *= step.dtRatio;
+
+		b2Vec2 P(m_impulse.x, m_impulse.y);
+
+		bA->m_linearVelocity -= mA * P;
+		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z);
+
+		bB->m_linearVelocity += mB * P;
+		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_impulse.z);
+	}
+	else
+	{
+		m_impulse.SetZero();
+	}
+}
+
+void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 vA = bA->m_linearVelocity;
+	float32 wA = bA->m_angularVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float32 wB = bB->m_angularVelocity;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	// Solve point-to-point constraint
+	b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+	float32 Cdot2 = wB - wA;
+	b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+
+	b2Vec3 impulse = m_mass.Solve33(-Cdot);
+	m_impulse += impulse;
+
+	b2Vec2 P(impulse.x, impulse.y);
+
+	vA -= mA * P;
+	wA -= iA * (b2Cross(rA, P) + impulse.z);
+
+	vB += mB * P;
+	wB += iB * (b2Cross(rB, P) + impulse.z);
+
+	bA->m_linearVelocity = vA;
+	bA->m_angularVelocity = wA;
+	bB->m_linearVelocity = vB;
+	bB->m_angularVelocity = wB;
+}
+
+bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
+{
+	B2_NOT_USED(baumgarte);
+
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	float32 mA = bA->m_invMass, mB = bB->m_invMass;
+	float32 iA = bA->m_invI, iB = bB->m_invI;
+
+	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+	b2Vec2 C1 =  bB->m_sweep.c + rB - bA->m_sweep.c - rA;
+	float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
+
+	// Handle large detachment.
+	const float32 k_allowedStretch = 10.0f * b2_linearSlop;
+	float32 positionError = C1.Length();
+	float32 angularError = b2Abs(C2);
+	if (positionError > k_allowedStretch)
+	{
+		iA *= 1.0f;
+		iB *= 1.0f;
+	}
+
+	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	m_mass.col3.x = -rA.y * iA - rB.y * iB;
+	m_mass.col1.y = m_mass.col2.x;
+	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	m_mass.col3.y = rA.x * iA + rB.x * iB;
+	m_mass.col1.z = m_mass.col3.x;
+	m_mass.col2.z = m_mass.col3.y;
+	m_mass.col3.z = iA + iB;
+
+	b2Vec3 C(C1.x, C1.y, C2);
+
+	b2Vec3 impulse = m_mass.Solve33(-C);
+
+	b2Vec2 P(impulse.x, impulse.y);
+
+	bA->m_sweep.c -= mA * P;
+	bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);
+
+	bB->m_sweep.c += mB * P;
+	bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
+
+	bA->SynchronizeTransform();
+	bB->SynchronizeTransform();
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2WeldJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2WeldJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 P(m_impulse.x, m_impulse.y);
+	return inv_dt * P;
+}
+
+float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_impulse.z;
+}

AndEngine/jni/Box2D/Dynamics/Joints/b2WeldJoint.h

@@ -0,0 +1,82 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_WELD_JOINT_H
+#define B2_WELD_JOINT_H
+
+#include "Box2D/Dynamics/Joints/b2Joint.h"
+
+/// Weld joint definition. You need to specify local anchor points
+/// where they are attached and the relative body angle. The position
+/// of the anchor points is important for computing the reaction torque.
+struct b2WeldJointDef : public b2JointDef
+{
+	b2WeldJointDef()
+	{
+		type = e_weldJoint;
+		localAnchorA.Set(0.0f, 0.0f);
+		localAnchorB.Set(0.0f, 0.0f);
+		referenceAngle = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, and reference angle using a world
+	/// anchor point.
+	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The body2 angle minus body1 angle in the reference state (radians).
+	float32 referenceAngle;
+};
+
+/// A weld joint essentially glues two bodies together. A weld joint may
+/// distort somewhat because the island constraint solver is approximate.
+class b2WeldJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+protected:
+
+	friend class b2Joint;
+
+	b2WeldJoint(const b2WeldJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+
+	bool SolvePositionConstraints(float32 baumgarte);
+
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	float32 m_referenceAngle;
+
+	b2Vec3 m_impulse;
+
+	b2Mat33 m_mass;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/b2Body.cpp

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

AndEngine/jni/Box2D/Dynamics/b2Body.h

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

AndEngine/jni/Box2D/Dynamics/b2ContactManager.cpp

@@ -0,0 +1,266 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/b2ContactManager.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+
+b2ContactFilter b2_defaultFilter;
+b2ContactListener b2_defaultListener;
+
+b2ContactManager::b2ContactManager()
+{
+	m_contactList = NULL;
+	m_contactCount = 0;
+	m_contactFilter = &b2_defaultFilter;
+	m_contactListener = &b2_defaultListener;
+	m_allocator = NULL;
+}
+
+void b2ContactManager::Destroy(b2Contact* c)
+{
+	b2Fixture* fixtureA = c->GetFixtureA();
+	b2Fixture* fixtureB = c->GetFixtureB();
+	b2Body* bodyA = fixtureA->GetBody();
+	b2Body* bodyB = fixtureB->GetBody();
+
+	if (m_contactListener && c->IsTouching())
+	{
+		m_contactListener->EndContact(c);
+	}
+
+	// Remove from the world.
+	if (c->m_prev)
+	{
+		c->m_prev->m_next = c->m_next;
+	}
+
+	if (c->m_next)
+	{
+		c->m_next->m_prev = c->m_prev;
+	}
+
+	if (c == m_contactList)
+	{
+		m_contactList = c->m_next;
+	}
+
+	// Remove from body 1
+	if (c->m_nodeA.prev)
+	{
+		c->m_nodeA.prev->next = c->m_nodeA.next;
+	}
+
+	if (c->m_nodeA.next)
+	{
+		c->m_nodeA.next->prev = c->m_nodeA.prev;
+	}
+
+	if (&c->m_nodeA == bodyA->m_contactList)
+	{
+		bodyA->m_contactList = c->m_nodeA.next;
+	}
+
+	// Remove from body 2
+	if (c->m_nodeB.prev)
+	{
+		c->m_nodeB.prev->next = c->m_nodeB.next;
+	}
+
+	if (c->m_nodeB.next)
+	{
+		c->m_nodeB.next->prev = c->m_nodeB.prev;
+	}
+
+	if (&c->m_nodeB == bodyB->m_contactList)
+	{
+		bodyB->m_contactList = c->m_nodeB.next;
+	}
+
+	// Call the factory.
+	b2Contact::Destroy(c, m_allocator);
+	--m_contactCount;
+}
+
+// This is the top level collision call for the time step. Here
+// all the narrow phase collision is processed for the world
+// contact list.
+void b2ContactManager::Collide()
+{
+	// Update awake contacts.
+	b2Contact* c = m_contactList;
+	while (c)
+	{
+		b2Fixture* fixtureA = c->GetFixtureA();
+		b2Fixture* fixtureB = c->GetFixtureB();
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+
+		if (bodyA->IsAwake() == false && bodyB->IsAwake() == false)
+		{
+			c = c->GetNext();
+			continue;
+		}
+
+		// Is this contact flagged for filtering?
+		if (c->m_flags & b2Contact::e_filterFlag)
+		{
+			// Should these bodies collide?
+			if (bodyB->ShouldCollide(bodyA) == false)
+			{
+				b2Contact* cNuke = c;
+				c = cNuke->GetNext();
+				Destroy(cNuke);
+				continue;
+			}
+
+			// Check user filtering.
+			if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
+			{
+				b2Contact* cNuke = c;
+				c = cNuke->GetNext();
+				Destroy(cNuke);
+				continue;
+			}
+
+			// Clear the filtering flag.
+			c->m_flags &= ~b2Contact::e_filterFlag;
+		}
+
+		int32 proxyIdA = fixtureA->m_proxyId;
+		int32 proxyIdB = fixtureB->m_proxyId;
+		bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
+
+		// Here we destroy contacts that cease to overlap in the broad-phase.
+		if (overlap == false)
+		{
+			b2Contact* cNuke = c;
+			c = cNuke->GetNext();
+			Destroy(cNuke);
+			continue;
+		}
+
+		// The contact persists.
+		c->Update(m_contactListener);
+		c = c->GetNext();
+	}
+}
+
+void b2ContactManager::FindNewContacts()
+{
+	m_broadPhase.UpdatePairs(this);
+}
+
+void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
+{
+	b2Fixture* fixtureA = (b2Fixture*)proxyUserDataA;
+	b2Fixture* fixtureB = (b2Fixture*)proxyUserDataB;
+
+	b2Body* bodyA = fixtureA->GetBody();
+	b2Body* bodyB = fixtureB->GetBody();
+
+	// Are the fixtures on the same body?
+	if (bodyA == bodyB)
+	{
+		return;
+	}
+
+	// Does a contact already exist?
+	b2ContactEdge* edge = bodyB->GetContactList();
+	while (edge)
+	{
+		if (edge->other == bodyA)
+		{
+			b2Fixture* fA = edge->contact->GetFixtureA();
+			b2Fixture* fB = edge->contact->GetFixtureB();
+			if (fA == fixtureA && fB == fixtureB)
+			{
+				// A contact already exists.
+				return;
+			}
+
+			if (fA == fixtureB && fB == fixtureA)
+			{
+				// A contact already exists.
+				return;
+			}
+		}
+
+		edge = edge->next;
+	}
+
+	// Does a joint override collision? Is at least one body dynamic?
+	if (bodyB->ShouldCollide(bodyA) == false)
+	{
+		return;
+	}
+
+	// Check user filtering.
+	if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
+	{
+		return;
+	}
+
+	// Call the factory.
+	b2Contact* c = b2Contact::Create(fixtureA, fixtureB, m_allocator);
+
+	// Contact creation may swap fixtures.
+	fixtureA = c->GetFixtureA();
+	fixtureB = c->GetFixtureB();
+	bodyA = fixtureA->GetBody();
+	bodyB = fixtureB->GetBody();
+
+	// Insert into the world.
+	c->m_prev = NULL;
+	c->m_next = m_contactList;
+	if (m_contactList != NULL)
+	{
+		m_contactList->m_prev = c;
+	}
+	m_contactList = c;
+
+	// Connect to island graph.
+
+	// Connect to body A
+	c->m_nodeA.contact = c;
+	c->m_nodeA.other = bodyB;
+
+	c->m_nodeA.prev = NULL;
+	c->m_nodeA.next = bodyA->m_contactList;
+	if (bodyA->m_contactList != NULL)
+	{
+		bodyA->m_contactList->prev = &c->m_nodeA;
+	}
+	bodyA->m_contactList = &c->m_nodeA;
+
+	// Connect to body B
+	c->m_nodeB.contact = c;
+	c->m_nodeB.other = bodyA;
+
+	c->m_nodeB.prev = NULL;
+	c->m_nodeB.next = bodyB->m_contactList;
+	if (bodyB->m_contactList != NULL)
+	{
+		bodyB->m_contactList->prev = &c->m_nodeB;
+	}
+	bodyB->m_contactList = &c->m_nodeB;
+
+	++m_contactCount;
+}

AndEngine/jni/Box2D/Dynamics/b2ContactManager.h

@@ -0,0 +1,52 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_CONTACT_MANAGER_H
+#define B2_CONTACT_MANAGER_H
+
+#include "Box2D/Collision/b2BroadPhase.h"
+
+class b2Contact;
+class b2ContactFilter;
+class b2ContactListener;
+class b2BlockAllocator;
+
+// Delegate of b2World.
+class b2ContactManager
+{
+public:
+	b2ContactManager();
+
+	// Broad-phase callback.
+	void AddPair(void* proxyUserDataA, void* proxyUserDataB);
+
+	void FindNewContacts();
+
+	void Destroy(b2Contact* c);
+
+	void Collide();
+            
+	b2BroadPhase m_broadPhase;
+	b2Contact* m_contactList;
+	int32 m_contactCount;
+	b2ContactFilter* m_contactFilter;
+	b2ContactListener* m_contactListener;
+	b2BlockAllocator* m_allocator;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/b2Fixture.cpp

@@ -0,0 +1,163 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+#include "Box2D/Collision/Shapes/b2CircleShape.h"
+#include "Box2D/Collision/Shapes/b2PolygonShape.h"
+#include "Box2D/Collision/b2BroadPhase.h"
+#include "Box2D/Collision/b2Collision.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+
+
+b2Fixture::b2Fixture()
+{
+	m_userData = NULL;
+	m_body = NULL;
+	m_next = NULL;
+	m_proxyId = b2BroadPhase::e_nullProxy;
+	m_shape = NULL;
+	m_density = 0.0f;
+}
+
+b2Fixture::~b2Fixture()
+{
+	b2Assert(m_shape == NULL);
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+}
+
+void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def)
+{
+	m_userData = def->userData;
+	m_friction = def->friction;
+	m_restitution = def->restitution;
+
+	m_body = body;
+	m_next = NULL;
+
+	m_filter = def->filter;
+
+	m_isSensor = def->isSensor;
+
+	m_shape = def->shape->Clone(allocator);
+
+	m_density = def->density;
+}
+
+void b2Fixture::Destroy(b2BlockAllocator* allocator)
+{
+	// The proxy must be destroyed before calling this.
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+
+	// Free the child shape.
+	switch (m_shape->m_type)
+	{
+	case b2Shape::e_circle:
+		{
+			b2CircleShape* s = (b2CircleShape*)m_shape;
+			s->~b2CircleShape();
+			allocator->Free(s, sizeof(b2CircleShape));
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			b2PolygonShape* s = (b2PolygonShape*)m_shape;
+			s->~b2PolygonShape();
+			allocator->Free(s, sizeof(b2PolygonShape));
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	m_shape = NULL;
+}
+
+void b2Fixture::CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf)
+{
+	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+
+	// Create proxy in the broad-phase.
+	m_shape->ComputeAABB(&m_aabb, xf);
+	m_proxyId = broadPhase->CreateProxy(m_aabb, this);
+}
+
+void b2Fixture::DestroyProxy(b2BroadPhase* broadPhase)
+{
+	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	{
+		return;
+	}
+
+	// Destroy proxy in the broad-phase.
+	broadPhase->DestroyProxy(m_proxyId);
+	m_proxyId = b2BroadPhase::e_nullProxy;
+}
+
+void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2)
+{
+	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	{	
+		return;
+	}
+
+	// Compute an AABB that covers the swept shape (may miss some rotation effect).
+	b2AABB aabb1, aabb2;
+	m_shape->ComputeAABB(&aabb1, transform1);
+	m_shape->ComputeAABB(&aabb2, transform2);
+	
+	m_aabb.Combine(aabb1, aabb2);
+
+	b2Vec2 displacement = transform2.position - transform1.position;
+
+	broadPhase->MoveProxy(m_proxyId, m_aabb, displacement);
+}
+
+void b2Fixture::SetFilterData(const b2Filter& filter)
+{
+	m_filter = filter;
+
+	if (m_body == NULL)
+	{
+		return;
+	}
+
+	// Flag associated contacts for filtering.
+	b2ContactEdge* edge = m_body->GetContactList();
+	while (edge)
+	{
+		b2Contact* contact = edge->contact;
+		b2Fixture* fixtureA = contact->GetFixtureA();
+		b2Fixture* fixtureB = contact->GetFixtureB();
+		if (fixtureA == this || fixtureB == this)
+		{
+			contact->FlagForFiltering();
+		}
+
+		edge = edge->next;
+	}
+}
+
+void b2Fixture::SetSensor(bool sensor)
+{
+	m_isSensor = sensor;
+}
+

AndEngine/jni/Box2D/Dynamics/b2Fixture.h

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

AndEngine/jni/Box2D/Dynamics/b2Island.cpp

@@ -0,0 +1,374 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/b2Island.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+#include "Box2D/Dynamics/b2World.h"
+#include "Box2D/Dynamics/Contacts/b2Contact.h"
+#include "Box2D/Dynamics/Contacts/b2ContactSolver.h"
+#include "Box2D/Dynamics/Joints/b2Joint.h"
+#include "Box2D/Common/b2StackAllocator.h"
+
+/*
+Position Correction Notes
+=========================
+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.
+*/
+
+/*
+Cache Performance
+
+The Box2D solvers are dominated by cache misses. Data structures are designed
+to increase the number of cache hits. Much of misses are due to random access
+to body data. The constraint structures are iterated over linearly, which leads
+to few cache misses.
+
+The bodies are not accessed during iteration. Instead read only data, such as
+the mass values are stored with the constraints. The mutable data are the constraint
+impulses and the bodies velocities/positions. The impulses are held inside the
+constraint structures. The body velocities/positions are held in compact, temporary
+arrays to increase the number of cache hits. Linear and angular velocity are
+stored in a single array since multiple arrays lead to multiple misses.
+*/
+
+/*
+2D Rotation
+
+R = [cos(theta) -sin(theta)]
+    [sin(theta) cos(theta) ]
+
+thetaDot = omega
+
+Let q1 = cos(theta), q2 = sin(theta).
+R = [q1 -q2]
+    [q2  q1]
+
+q1Dot = -thetaDot * q2
+q2Dot = thetaDot * q1
+
+q1_new = q1_old - dt * w * q2
+q2_new = q2_old + dt * w * q1
+then normalize.
+
+This might be faster than computing sin+cos.
+However, we can compute sin+cos of the same angle fast.
+*/
+
+b2Island::b2Island(
+	int32 bodyCapacity,
+	int32 contactCapacity,
+	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_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
+	m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
+}
+
+b2Island::~b2Island()
+{
+	// Warning: the order should reverse the constructor order.
+	m_allocator->Free(m_positions);
+	m_allocator->Free(m_velocities);
+	m_allocator->Free(m_joints);
+	m_allocator->Free(m_contacts);
+	m_allocator->Free(m_bodies);
+}
+
+void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
+{
+	// Integrate velocities and apply damping.
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Body* b = m_bodies[i];
+
+		if (b->GetType() != b2_dynamicBody)
+		{
+			continue;
+		}
+
+		// Integrate velocities.
+		b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
+		b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
+
+		// 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);
+	}
+
+	// Partition contacts so that contacts with static bodies are solved last.
+	int32 i1 = -1;
+	for (int32 i2 = 0; i2 < m_contactCount; ++i2)
+	{
+		b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
+		b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
+		b2Body* bodyA = fixtureA->GetBody();
+		b2Body* bodyB = fixtureB->GetBody();
+		bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
+		if (nonStatic)
+		{
+			++i1;
+			b2Swap(m_contacts[i1], m_contacts[i2]);
+		}
+	}
+
+	// Initialize velocity constraints.
+	b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
+	contactSolver.WarmStart();
+	for (int32 i = 0; i < m_jointCount; ++i)
+	{
+		m_joints[i]->InitVelocityConstraints(step);
+	}
+
+	// Solve velocity constraints.
+	for (int32 i = 0; i < step.velocityIterations; ++i)
+	{
+		for (int32 j = 0; j < m_jointCount; ++j)
+		{
+			m_joints[j]->SolveVelocityConstraints(step);
+		}
+
+		contactSolver.SolveVelocityConstraints();
+	}
+
+	// Post-solve (store impulses for warm starting).
+	contactSolver.StoreImpulses();
+
+	// Integrate positions.
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Body* b = m_bodies[i];
+
+		if (b->GetType() == b2_staticBody)
+		{
+			continue;
+		}
+
+		// Check for large velocities.
+		b2Vec2 translation = step.dt * b->m_linearVelocity;
+		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
+		{
+			float32 ratio = b2_maxTranslation / translation.Length();
+			b->m_linearVelocity *= ratio;
+		}
+
+		float32 rotation = step.dt * b->m_angularVelocity;
+		if (rotation * rotation > b2_maxRotationSquared)
+		{
+			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			b->m_angularVelocity *= ratio;
+		}
+
+		// Store positions for continuous collision.
+		b->m_sweep.c0 = b->m_sweep.c;
+		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.
+	}
+
+	// Iterate over constraints.
+	for (int32 i = 0; i < step.positionIterations; ++i)
+	{
+		bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
+
+		bool jointsOkay = true;
+		for (int32 i = 0; i < m_jointCount; ++i)
+		{
+			bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
+			jointsOkay = jointsOkay && jointOkay;
+		}
+
+		if (contactsOkay && jointsOkay)
+		{
+			// Exit early if the position errors are small.
+			break;
+		}
+	}
+
+	Report(contactSolver.m_constraints);
+
+	if (allowSleep)
+	{
+		float32 minSleepTime = b2_maxFloat;
+
+		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
+		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
+
+		for (int32 i = 0; i < m_bodyCount; ++i)
+		{
+			b2Body* b = m_bodies[i];
+			if (b->GetType() == b2_staticBody)
+			{
+				continue;
+			}
+
+			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
+			{
+				b->m_sleepTime = 0.0f;
+				minSleepTime = 0.0f;
+			}
+
+			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
+				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
+				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
+			{
+				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->SetAwake(false);
+			}
+		}
+	}
+}
+
+void b2Island::Report(const b2ContactConstraint* constraints)
+{
+	if (m_listener == NULL)
+	{
+		return;
+	}
+
+	for (int32 i = 0; i < m_contactCount; ++i)
+	{
+		b2Contact* c = m_contacts[i];
+
+		const b2ContactConstraint* cc = constraints + i;
+		
+		b2ContactImpulse impulse;
+		for (int32 j = 0; j < cc->pointCount; ++j)
+		{
+			impulse.normalImpulses[j] = cc->points[j].normalImpulse;
+			impulse.tangentImpulses[j] = cc->points[j].tangentImpulse;
+		}
+
+		m_listener->PostSolve(c, &impulse);
+	}
+}

AndEngine/jni/Box2D/Dynamics/b2Island.h

@@ -0,0 +1,105 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_ISLAND_H
+#define B2_ISLAND_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Dynamics/b2Body.h"
+#include "Box2D/Dynamics/b2TimeStep.h"
+
+class b2Contact;
+class b2Joint;
+class b2StackAllocator;
+class b2ContactListener;
+struct b2ContactConstraint;
+
+/// This is an internal structure.
+struct b2Position
+{
+	b2Vec2 x;
+	float32 a;
+};
+
+/// This is an internal structure.
+struct b2Velocity
+{
+	b2Vec2 v;
+	float32 w;
+};
+
+/// This is an internal class.
+class b2Island
+{
+public:
+	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 allowSleep);
+
+	void Add(b2Body* body)
+	{
+		b2Assert(m_bodyCount < m_bodyCapacity);
+		body->m_islandIndex = m_bodyCount;
+		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(const b2ContactConstraint* constraints);
+
+	b2StackAllocator* m_allocator;
+	b2ContactListener* m_listener;
+
+	b2Body** m_bodies;
+	b2Contact** m_contacts;
+	b2Joint** m_joints;
+
+	b2Position* m_positions;
+	b2Velocity* m_velocities;
+
+	int32 m_bodyCount;
+	int32 m_jointCount;
+	int32 m_contactCount;
+
+	int32 m_bodyCapacity;
+	int32 m_contactCapacity;
+	int32 m_jointCapacity;
+
+	int32 m_positionIterationCount;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/b2TimeStep.h

@@ -0,0 +1,35 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_TIME_STEP_H
+#define B2_TIME_STEP_H
+
+#include "Box2D/Common/b2Settings.h"
+
+/// This is an internal structure.
+struct b2TimeStep
+{
+	float32 dt;			// time step
+	float32 inv_dt;		// inverse time step (0 if dt == 0).
+	float32 dtRatio;	// dt * inv_dt0
+	int32 velocityIterations;
+	int32 positionIterations;
+	bool warmStarting;
+};
+
+#endif

AndEngine/jni/Box2D/Dynamics/b2World.h

@@ -0,0 +1,285 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_WORLD_H
+#define B2_WORLD_H
+
+#include "Box2D/Common/b2Math.h"
+#include "Box2D/Common/b2BlockAllocator.h"
+#include "Box2D/Common/b2StackAllocator.h"
+#include "Box2D/Dynamics/b2ContactManager.h"
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+
+struct b2AABB;
+struct b2BodyDef;
+struct b2JointDef;
+struct b2TimeStep;
+class b2Body;
+class b2Fixture;
+class b2Joint;
+
+/// The world class manages all physics entities, dynamic simulation,
+/// and asynchronous queries. The world also contains efficient memory
+/// management facilities.
+class b2World
+{
+public:
+	/// Construct a world object.
+	/// @param gravity the world gravity vector.
+	/// @param doSleep improve performance by not simulating inactive bodies.
+	b2World(const b2Vec2& gravity, bool doSleep);
+
+	/// Destruct the world. All physics entities are destroyed and all heap memory is released.
+	~b2World();
+
+	/// Register a destruction listener. The listener is owned by you and must
+	/// remain in scope.
+	void SetDestructionListener(b2DestructionListener* listener);
+
+	/// Register a contact filter to provide specific control over collision.
+	/// Otherwise the default filter is used (b2_defaultFilter). The listener is
+	/// owned by you and must remain in scope. 
+	void SetContactFilter(b2ContactFilter* filter);
+
+	/// Register a contact event listener. The listener is owned by you and must
+	/// remain in scope.
+	void SetContactListener(b2ContactListener* listener);
+
+	/// Register a routine for debug drawing. The debug draw functions are called
+	/// inside with b2World::DrawDebugData method. The debug draw object is owned
+	/// by you and must remain in scope.
+	void SetDebugDraw(b2DebugDraw* debugDraw);
+
+	/// Create a rigid body given a definition. No reference to the definition
+	/// is retained.
+	/// @warning This function is locked during callbacks.
+	b2Body* CreateBody(const b2BodyDef* def);
+
+	/// Destroy a rigid body given a definition. No reference to the definition
+	/// is retained. This function is locked during callbacks.
+	/// @warning This automatically deletes all associated shapes and joints.
+	/// @warning This function is locked during callbacks.
+	void DestroyBody(b2Body* body);
+
+	/// Create a joint to constrain bodies together. No reference to the definition
+	/// is retained. This may cause the connected bodies to cease colliding.
+	/// @warning This function is locked during callbacks.
+	b2Joint* CreateJoint(const b2JointDef* def);
+
+	/// Destroy a joint. This may cause the connected bodies to begin colliding.
+	/// @warning This function is locked during callbacks.
+	void DestroyJoint(b2Joint* joint);
+
+	/// Take a time step. This performs collision detection, integration,
+	/// and constraint solution.
+	/// @param timeStep the amount of time to simulate, this should not vary.
+	/// @param velocityIterations for the velocity constraint solver.
+	/// @param positionIterations for the position constraint solver.
+	void Step(	float32 timeStep,
+				int32 velocityIterations,
+				int32 positionIterations);
+
+	/// Call this after you are done with time steps to clear the forces. You normally
+	/// call this after each call to Step, unless you are performing sub-steps. By default,
+	/// forces will be automatically cleared, so you don't need to call this function.
+	/// @see SetAutoClearForces
+	void ClearForces();
+
+	/// Call this to draw shapes and other debug draw data.
+	void DrawDebugData();
+
+	/// Query the world for all fixtures that potentially overlap the
+	/// provided AABB.
+	/// @param callback a user implemented callback class.
+	/// @param aabb the query box.
+	void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const;
+
+	/// Ray-cast the world for all fixtures in the path of the ray. Your callback
+	/// controls whether you get the closest point, any point, or n-points.
+	/// The ray-cast ignores shapes that contain the starting point.
+	/// @param callback a user implemented callback class.
+	/// @param point1 the ray starting point
+	/// @param point2 the ray ending point
+	void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;
+
+	/// Get the world body list. With the returned body, use b2Body::GetNext to get
+	/// the next body in the world list. A NULL body indicates the end of the list.
+	/// @return the head of the world body list.
+	b2Body* GetBodyList();
+
+	/// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
+	/// the next joint in the world list. A NULL joint indicates the end of the list.
+	/// @return the head of the world joint list.
+	b2Joint* GetJointList();
+
+	/// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
+	/// the next contact in the world list. A NULL contact indicates the end of the list.
+	/// @return the head of the world contact list.
+	/// @warning contacts are 
+	b2Contact* GetContactList();
+
+	/// Enable/disable warm starting. For testing.
+	void SetWarmStarting(bool flag) { m_warmStarting = flag; }
+
+	/// Enable/disable continuous physics. For testing.
+	void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
+
+	/// Get the number of broad-phase proxies.
+	int32 GetProxyCount() const;
+
+	/// Get the number of bodies.
+	int32 GetBodyCount() const;
+
+	/// Get the number of joints.
+	int32 GetJointCount() const;
+
+	/// Get the number of contacts (each may have 0 or more contact points).
+	int32 GetContactCount() const;
+
+	/// Change the global gravity vector.
+	void SetGravity(const b2Vec2& gravity);
+	
+	/// Get the global gravity vector.
+	b2Vec2 GetGravity() const;
+
+	/// Is the world locked (in the middle of a time step).
+	bool IsLocked() const;
+
+	/// Set flag to control automatic clearing of forces after each time step.
+	void SetAutoClearForces(bool flag);
+
+	/// Get the flag that controls automatic clearing of forces after each time step.
+	bool GetAutoClearForces() const;
+
+private:
+
+	// m_flags
+	enum
+	{
+		e_newFixture	= 0x0001,
+		e_locked		= 0x0002,
+		e_clearForces	= 0x0004,
+	};
+
+	friend class b2Body;
+	friend class b2ContactManager;
+	friend class b2Controller;
+
+	void Solve(const b2TimeStep& step);
+	void SolveTOI();
+	void SolveTOI(b2Body* body);
+
+	void DrawJoint(b2Joint* joint);
+	void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);
+
+	b2BlockAllocator m_blockAllocator;
+	b2StackAllocator m_stackAllocator;
+
+	int32 m_flags;
+
+	b2ContactManager m_contactManager;
+
+	b2Body* m_bodyList;
+	b2Joint* m_jointList;
+
+	int32 m_bodyCount;
+	int32 m_jointCount;
+
+	b2Vec2 m_gravity;
+	bool m_allowSleep;
+
+	b2Body* m_groundBody;
+
+	b2DestructionListener* m_destructionListener;
+	b2DebugDraw* m_debugDraw;
+
+	// This is used to compute the time step ratio to
+	// support a variable time step.
+	float32 m_inv_dt0;
+
+	// This is for debugging the solver.
+	bool m_warmStarting;
+
+	// This is for debugging the solver.
+	bool m_continuousPhysics;
+};
+
+inline b2Body* b2World::GetBodyList()
+{
+	return m_bodyList;
+}
+
+inline b2Joint* b2World::GetJointList()
+{
+	return m_jointList;
+}
+
+inline b2Contact* b2World::GetContactList()
+{
+	return m_contactManager.m_contactList;
+}
+
+inline int32 b2World::GetBodyCount() const
+{
+	return m_bodyCount;
+}
+
+inline int32 b2World::GetJointCount() const
+{
+	return m_jointCount;
+}
+
+inline int32 b2World::GetContactCount() const
+{
+	return m_contactManager.m_contactCount;
+}
+
+inline void b2World::SetGravity(const b2Vec2& gravity)
+{
+	m_gravity = gravity;
+}
+
+inline b2Vec2 b2World::GetGravity() const
+{
+	return m_gravity;
+}
+
+inline bool b2World::IsLocked() const
+{
+	return (m_flags & e_locked) == e_locked;
+}
+
+inline void b2World::SetAutoClearForces(bool flag)
+{
+	if (flag)
+	{
+		m_flags |= e_clearForces;
+	}
+	else
+	{
+		m_flags &= ~e_clearForces;
+	}
+}
+
+/// Get the flag that controls automatic clearing of forces after each time step.
+inline bool b2World::GetAutoClearForces() const
+{
+	return (m_flags & e_clearForces) == e_clearForces;
+}
+
+#endif

AndEngine/jni/Box2D/Dynamics/b2WorldCallbacks.cpp

@@ -0,0 +1,61 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "Box2D/Dynamics/b2WorldCallbacks.h"
+#include "Box2D/Dynamics/b2Fixture.h"
+
+// Return true if contact calculations should be performed between these two shapes.
+// If you implement your own collision filter you may want to build from this implementation.
+bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
+{
+	const b2Filter& filterA = fixtureA->GetFilterData();
+	const b2Filter& filterB = fixtureB->GetFilterData();
+
+	if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
+	{
+		return filterA.groupIndex > 0;
+	}
+
+	bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.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;
+}

AndEngine/jni/Box2D/Dynamics/b2WorldCallbacks.h

@@ -0,0 +1,217 @@
+/*
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_WORLD_CALLBACKS_H
+#define B2_WORLD_CALLBACKS_H
+
+#include "Box2D/Common/b2Settings.h"
+
+struct b2Vec2;
+struct b2Transform;
+class b2Fixture;
+class b2Body;
+class b2Joint;
+class b2Contact;
+struct b2ContactPoint;
+struct b2ContactResult;
+struct b2Manifold;
+
+/// Joints and fixtures are destroyed when their associated
+/// body is destroyed. Implement this listener so that you
+/// may nullify references to these joints and shapes.
+class b2DestructionListener
+{
+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 fixture is about to be destroyed due
+	/// to the destruction of its parent body.
+	virtual void SayGoodbye(b2Fixture* fixture) = 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(b2Fixture* fixtureA, b2Fixture* fixtureB);
+};
+
+/// Contact impulses for reporting. Impulses are used instead of forces because
+/// sub-step forces may approach infinity for rigid body collisions. These
+/// match up one-to-one with the contact points in b2Manifold.
+struct b2ContactImpulse
+{
+	float32 normalImpulses[b2_maxManifoldPoints];
+	float32 tangentImpulses[b2_maxManifoldPoints];
+};
+
+/// Implement this class to get contact information. You can use these results for
+/// things like sounds and game logic. You can also get contact results by
+/// traversing the contact lists after the time step. However, you might miss
+/// some contacts because continuous physics leads to sub-stepping.
+/// 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 You cannot create/destroy Box2D entities inside these callbacks.
+class b2ContactListener
+{
+public:
+	virtual ~b2ContactListener() {}
+
+	/// Called when two fixtures begin to touch.
+	virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); }
+
+	/// Called when two fixtures cease to touch.
+	virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); }
+
+	/// This is called after a contact is updated. This allows you to inspect a
+	/// contact before it goes to the solver. If you are careful, you can modify the
+	/// contact manifold (e.g. disable contact).
+	/// A copy of the old manifold is provided so that you can detect changes.
+	/// Note: this is called only for awake bodies.
+	/// Note: this is called even when the number of contact points is zero.
+	/// Note: this is not called for sensors.
+	/// Note: if you set the number of contact points to zero, you will not
+	/// get an EndContact callback. However, you may get a BeginContact callback
+	/// the next step.
+	virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
+	{
+		B2_NOT_USED(contact);
+		B2_NOT_USED(oldManifold);
+	}
+
+	/// This lets you inspect a contact after the solver is finished. This is useful
+	/// for inspecting impulses.
+	/// Note: the contact manifold does not include time of impact impulses, which can be
+	/// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
+	/// in a separate data structure.
+	/// Note: this is only called for contacts that are touching, solid, and awake.
+	virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse)
+	{
+		B2_NOT_USED(contact);
+		B2_NOT_USED(impulse);
+	}
+};
+
+/// Callback class for AABB queries.
+/// See b2World::Query
+class b2QueryCallback
+{
+public:
+	virtual ~b2QueryCallback() {}
+
+	/// Called for each fixture found in the query AABB.
+	/// @return false to terminate the query.
+	virtual bool ReportFixture(b2Fixture* fixture) = 0;
+};
+
+/// Callback class for ray casts.
+/// See b2World::RayCast
+class b2RayCastCallback
+{
+public:
+	virtual ~b2RayCastCallback() {}
+
+	/// Called for each fixture found in the query. You control how the ray cast
+	/// proceeds by returning a float:
+	/// return -1: ignore this fixture and continue
+	/// return 0: terminate the ray cast
+	/// return fraction: clip the ray to this point
+	/// return 1: don't clip the ray and continue
+	/// @param fixture the fixture hit by the ray
+	/// @param point the point of initial intersection
+	/// @param normal the normal vector at the point of intersection
+	/// @return -1 to filter, 0 to terminate, fraction to clip the ray for
+	/// closest hit, 1 to continue
+	virtual float32 ReportFixture(	b2Fixture* fixture, const b2Vec2& point,
+									const b2Vec2& normal, float32 fraction) = 0;
+};
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+	b2Color() {}
+	b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
+	void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }
+	float32 r, g, b;
+};
+
+/// 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_aabbBit				= 0x0004, ///< draw axis aligned bounding boxes
+		e_pairBit				= 0x0008, ///< draw broad-phase pairs
+		e_centerOfMassBit		= 0x0010, ///< draw center of mass frame
+	};
+
+	/// Set the drawing flags.
+	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 DrawTransform(const b2Transform& xf) = 0;
+
+protected:
+	uint32 m_drawFlags;
+};
+
+#endif