initial commit. includes PhsyicsBox2dExtension

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