Initial Mercurial commit.

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

@@ -0,0 +1,206 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2DistanceJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// 1-D constrained system
+// m (v2 - v1) = lambda
+// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
+// x2 = x1 + h * v2
+
+// 1-D mass-damper-spring system
+// m (v2 - v1) + h * d * v2 + h * k * 
+
+// C = norm(p2 - p1) - L
+// u = (p2 - p1) / norm(p2 - p1)
+// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+// J = [-u -cross(r1, u) u cross(r2, u)]
+// K = J * invM * JT
+//   = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
+
+void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
+									const b2Vec2& anchor1, const b2Vec2& anchor2)
+{
+	body1 = b1;
+	body2 = b2;
+	localAnchor1 = body1->GetLocalPoint(anchor1);
+	localAnchor2 = body2->GetLocalPoint(anchor2);
+	b2Vec2 d = anchor2 - anchor1;
+	length = d.Length();
+}
+
+
+b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	m_length = def->length;
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+	m_impulse = 0.0f;
+	m_gamma = 0.0f;
+	m_bias = 0.0f;
+	m_inv_dt = 0.0f;
+}
+
+void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	m_inv_dt = step.inv_dt;
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	// Handle singularity.
+	float32 length = m_u.Length();
+	if (length > b2_linearSlop)
+	{
+		m_u *= 1.0f / length;
+	}
+	else
+	{
+		m_u.Set(0.0f, 0.0f);
+	}
+
+	float32 cr1u = b2Cross(r1, m_u);
+	float32 cr2u = b2Cross(r2, m_u);
+	float32 invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u;
+	b2Assert(invMass > B2_FLT_EPSILON);
+	m_mass = 1.0f / invMass;
+
+	if (m_frequencyHz > 0.0f)
+	{
+		float32 C = length - m_length;
+
+		// Frequency
+		float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+		// Damping coefficient
+		float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+
+		// Spring stiffness
+		float32 k = m_mass * omega * omega;
+
+		// magic formulas
+		m_gamma = 1.0f / (step.dt * (d + step.dt * k));
+		m_bias = C * step.dt * k * m_gamma;
+
+		m_mass = 1.0f / (invMass + m_gamma);
+	}
+
+	if (step.warmStarting)
+	{
+		m_impulse *= step.dtRatio;
+		b2Vec2 P = m_impulse * m_u;
+		b1->m_linearVelocity -= b1->m_invMass * P;
+		b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+		b2->m_linearVelocity += b2->m_invMass * P;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+	}
+	else
+	{
+		m_impulse = 0.0f;
+	}
+}
+
+void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	B2_NOT_USED(step);
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// Cdot = dot(u, v + cross(w, r))
+	b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+	b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+	float32 Cdot = b2Dot(m_u, v2 - v1);
+
+	float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+	m_impulse += impulse;
+
+	b2Vec2 P = impulse * m_u;
+	b1->m_linearVelocity -= b1->m_invMass * P;
+	b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+	b2->m_linearVelocity += b2->m_invMass * P;
+	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+}
+
+bool b2DistanceJoint::SolvePositionConstraints()
+{
+	if (m_frequencyHz > 0.0f)
+	{
+		return true;
+	}
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+	float32 length = d.Normalize();
+	float32 C = length - m_length;
+	C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+
+	float32 impulse = -m_mass * C;
+	m_u = d;
+	b2Vec2 P = impulse * m_u;
+
+	b1->m_sweep.c -= b1->m_invMass * P;
+	b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
+	b2->m_sweep.c += b2->m_invMass * P;
+	b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	return b2Abs(C) < b2_linearSlop;
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2DistanceJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2DistanceJoint::GetReactionForce() const
+{
+	b2Vec2 F = (m_inv_dt * m_impulse) * m_u;
+	return F;
+}
+
+float32 b2DistanceJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}

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

@@ -0,0 +1,96 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_DISTANCE_JOINT_H
+#define B2_DISTANCE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Distance joint definition. This requires defining an
+/// anchor point on both bodies and the non-zero length of the
+/// distance joint. The definition uses local anchor points
+/// so that the initial configuration can violate the constraint
+/// slightly. This helps when saving and loading a game.
+/// @warning Do not use a zero or short length.
+struct b2DistanceJointDef : public b2JointDef
+{
+	b2DistanceJointDef()
+	{
+		type = e_distanceJoint;
+		localAnchor1.Set(0.0f, 0.0f);
+		localAnchor2.Set(0.0f, 0.0f);
+		length = 1.0f;
+		frequencyHz = 0.0f;
+		dampingRatio = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, and length using the world
+	/// anchors.
+	void Initialize(b2Body* body1, b2Body* body2,
+					const b2Vec2& anchor1, const b2Vec2& anchor2);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The equilibrium length between the anchor points.
+	float32 length;
+
+	/// The response speed.
+	float32 frequencyHz;
+
+	/// The damping ratio. 0 = no damping, 1 = critical damping.
+	float32 dampingRatio;
+};
+
+/// A distance joint constrains two points on two bodies
+/// to remain at a fixed distance from each other. You can view
+/// this as a massless, rigid rod.
+class b2DistanceJoint : public b2Joint
+{
+public:
+
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	//--------------- Internals Below -------------------
+
+	b2DistanceJoint(const b2DistanceJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_u;
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+	float32 m_gamma;
+	float32 m_bias;
+	float32 m_impulse;
+	float32 m_mass;		// effective mass for the constraint.
+	float32 m_length;
+};
+
+#endif

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

@@ -0,0 +1,253 @@
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2GearJoint.h"
+#include "b2RevoluteJoint.h"
+#include "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Gear Joint:
+// C0 = (coordinate1 + ratio * coordinate2)_initial
+// C = C0 - (cordinate1 + ratio * coordinate2) = 0
+// Cdot = -(Cdot1 + ratio * Cdot2)
+// J = -[J1 ratio * J2]
+// K = J * invM * JT
+//   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
+//
+// Revolute:
+// coordinate = rotation
+// Cdot = angularVelocity
+// J = [0 0 1]
+// K = J * invM * JT = invI
+//
+// Prismatic:
+// coordinate = dot(p - pg, ug)
+// Cdot = dot(v + cross(w, r), ug)
+// J = [ug cross(r, ug)]
+// K = J * invM * JT = invMass + invI * cross(r, ug)^2
+
+b2GearJoint::b2GearJoint(const b2GearJointDef* def)
+: b2Joint(def)
+{
+	b2JointType type1 = def->joint1->GetType();
+	b2JointType type2 = def->joint2->GetType();
+
+	b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
+	b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
+	b2Assert(def->joint1->GetBody1()->IsStatic());
+	b2Assert(def->joint2->GetBody1()->IsStatic());
+
+	m_revolute1 = NULL;
+	m_prismatic1 = NULL;
+	m_revolute2 = NULL;
+	m_prismatic2 = NULL;
+
+	float32 coordinate1, coordinate2;
+
+	m_ground1 = def->joint1->GetBody1();
+	m_body1 = def->joint1->GetBody2();
+	if (type1 == e_revoluteJoint)
+	{
+		m_revolute1 = (b2RevoluteJoint*)def->joint1;
+		m_groundAnchor1 = m_revolute1->m_localAnchor1;
+		m_localAnchor1 = m_revolute1->m_localAnchor2;
+		coordinate1 = m_revolute1->GetJointAngle();
+	}
+	else
+	{
+		m_prismatic1 = (b2PrismaticJoint*)def->joint1;
+		m_groundAnchor1 = m_prismatic1->m_localAnchor1;
+		m_localAnchor1 = m_prismatic1->m_localAnchor2;
+		coordinate1 = m_prismatic1->GetJointTranslation();
+	}
+
+	m_ground2 = def->joint2->GetBody1();
+	m_body2 = def->joint2->GetBody2();
+	if (type2 == e_revoluteJoint)
+	{
+		m_revolute2 = (b2RevoluteJoint*)def->joint2;
+		m_groundAnchor2 = m_revolute2->m_localAnchor1;
+		m_localAnchor2 = m_revolute2->m_localAnchor2;
+		coordinate2 = m_revolute2->GetJointAngle();
+	}
+	else
+	{
+		m_prismatic2 = (b2PrismaticJoint*)def->joint2;
+		m_groundAnchor2 = m_prismatic2->m_localAnchor1;
+		m_localAnchor2 = m_prismatic2->m_localAnchor2;
+		coordinate2 = m_prismatic2->GetJointTranslation();
+	}
+
+	m_ratio = def->ratio;
+
+	m_constant = coordinate1 + m_ratio * coordinate2;
+
+	m_force = 0.0f;
+}
+
+void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* g1 = m_ground1;
+	b2Body* g2 = m_ground2;
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 K = 0.0f;
+	m_J.SetZero();
+
+	if (m_revolute1)
+	{
+		m_J.angular1 = -1.0f;
+		K += b1->m_invI;
+	}
+	else
+	{
+		b2Vec2 ug = b2Mul(g1->GetXForm().R, m_prismatic1->m_localXAxis1);
+		b2Vec2 r = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		float32 crug = b2Cross(r, ug);
+		m_J.linear1 = -ug;
+		m_J.angular1 = -crug;
+		K += b1->m_invMass + b1->m_invI * crug * crug;
+	}
+
+	if (m_revolute2)
+	{
+		m_J.angular2 = -m_ratio;
+		K += m_ratio * m_ratio * b2->m_invI;
+	}
+	else
+	{
+		b2Vec2 ug = b2Mul(g2->GetXForm().R, m_prismatic2->m_localXAxis1);
+		b2Vec2 r = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		float32 crug = b2Cross(r, ug);
+		m_J.linear2 = -m_ratio * ug;
+		m_J.angular2 = -m_ratio * crug;
+		K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
+	}
+
+	// Compute effective mass.
+	b2Assert(K > 0.0f);
+	m_mass = 1.0f / K;
+
+	if (step.warmStarting)
+	{
+		// Warm starting.
+		float32 P = B2FORCE_SCALE(step.dt) * m_force;
+		b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
+		b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
+		b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
+		b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+	}
+	else
+	{
+		m_force = 0.0f;
+	}
+}
+
+void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 Cdot = m_J.Compute(	b1->m_linearVelocity, b1->m_angularVelocity,
+								b2->m_linearVelocity, b2->m_angularVelocity);
+
+	float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_mass * Cdot;
+	m_force += force;
+
+	float32 P = B2FORCE_SCALE(step.dt) * force;
+	b1->m_linearVelocity += b1->m_invMass * P * m_J.linear1;
+	b1->m_angularVelocity += b1->m_invI * P * m_J.angular1;
+	b2->m_linearVelocity += b2->m_invMass * P * m_J.linear2;
+	b2->m_angularVelocity += b2->m_invI * P * m_J.angular2;
+}
+
+bool b2GearJoint::SolvePositionConstraints()
+{
+	float32 linearError = 0.0f;
+
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 coordinate1, coordinate2;
+	if (m_revolute1)
+	{
+		coordinate1 = m_revolute1->GetJointAngle();
+	}
+	else
+	{
+		coordinate1 = m_prismatic1->GetJointTranslation();
+	}
+
+	if (m_revolute2)
+	{
+		coordinate2 = m_revolute2->GetJointAngle();
+	}
+	else
+	{
+		coordinate2 = m_prismatic2->GetJointTranslation();
+	}
+
+	float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
+
+	float32 impulse = -m_mass * C;
+
+	b1->m_sweep.c += b1->m_invMass * impulse * m_J.linear1;
+	b1->m_sweep.a += b1->m_invI * impulse * m_J.angular1;
+	b2->m_sweep.c += b2->m_invMass * impulse * m_J.linear2;
+	b2->m_sweep.a += b2->m_invI * impulse * m_J.angular2;
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2GearJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2GearJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2GearJoint::GetReactionForce() const
+{
+	// TODO_ERIN not tested
+	b2Vec2 F = B2FORCE_SCALE(m_force) * m_J.linear2;
+	return F;
+}
+
+float32 b2GearJoint::GetReactionTorque() const
+{
+	// TODO_ERIN not tested
+	b2Vec2 r = b2Mul(m_body2->GetXForm().R, m_localAnchor2 - m_body2->GetLocalCenter());
+	b2Vec2 F = m_force * m_J.linear2;
+	float32 T = B2FORCE_SCALE(m_force * m_J.angular2 - b2Cross(r, F));
+	return T;
+}
+
+float32 b2GearJoint::GetRatio() const
+{
+	return m_ratio;
+}
+
+

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

@@ -0,0 +1,109 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_GEAR_JOINT_H
+#define B2_GEAR_JOINT_H
+
+#include "b2Joint.h"
+
+class b2RevoluteJoint;
+class b2PrismaticJoint;
+
+/// Gear joint definition. This definition requires two existing
+/// revolute or prismatic joints (any combination will work).
+/// The provided joints must attach a dynamic body to a static body.
+struct b2GearJointDef : public b2JointDef
+{
+	b2GearJointDef()
+	{
+		type = e_gearJoint;
+		joint1 = NULL;
+		joint2 = NULL;
+		ratio = 1.0f;
+	}
+
+	/// The first revolute/prismatic joint attached to the gear joint.
+	b2Joint* joint1;
+
+	/// The second revolute/prismatic joint attached to the gear joint.
+	b2Joint* joint2;
+
+	/// The gear ratio.
+	/// @see b2GearJoint for explanation.
+	float32 ratio;
+};
+
+/// A gear joint is used to connect two joints together. Either joint
+/// can be a revolute or prismatic joint. You specify a gear ratio
+/// to bind the motions together:
+/// coordinate1 + ratio * coordinate2 = constant
+/// The ratio can be negative or positive. If one joint is a revolute joint
+/// and the other joint is a prismatic joint, then the ratio will have units
+/// of length or units of 1/length.
+/// @warning The revolute and prismatic joints must be attached to
+/// fixed bodies (which must be body1 on those joints).
+class b2GearJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the gear ratio.
+	float32 GetRatio() const;
+
+	//--------------- Internals Below -------------------
+
+	b2GearJoint(const b2GearJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Body* m_ground1;
+	b2Body* m_ground2;
+
+	// One of these is NULL.
+	b2RevoluteJoint* m_revolute1;
+	b2PrismaticJoint* m_prismatic1;
+
+	// One of these is NULL.
+	b2RevoluteJoint* m_revolute2;
+	b2PrismaticJoint* m_prismatic2;
+
+	b2Vec2 m_groundAnchor1;
+	b2Vec2 m_groundAnchor2;
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+
+	b2Jacobian m_J;
+
+	float32 m_constant;
+	float32 m_ratio;
+
+	// Effective mass
+	float32 m_mass;
+
+	// Impulse for accumulation/warm starting.
+	float32 m_force;
+};
+
+#endif

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

@@ -0,0 +1,134 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2Joint.h"
+#include "b2DistanceJoint.h"
+#include "b2MouseJoint.h"
+#include "b2RevoluteJoint.h"
+#include "b2PrismaticJoint.h"
+#include "b2PulleyJoint.h"
+#include "b2GearJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+#include "../../Common/b2BlockAllocator.h"
+#include "../../Collision/b2BroadPhase.h"
+
+#include <new>
+
+b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
+{
+	b2Joint* joint = NULL;
+
+	switch (def->type)
+	{
+	case e_distanceJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
+			joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
+		}
+		break;
+
+	case e_mouseJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2MouseJoint));
+			joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
+		}
+		break;
+
+	case e_prismaticJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
+			joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
+		}
+		break;
+
+	case e_revoluteJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
+			joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
+		}
+		break;
+
+	case e_pulleyJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
+			joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
+		}
+		break;
+
+	case e_gearJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2GearJoint));
+			joint = new (mem) b2GearJoint((b2GearJointDef*)def);
+		}
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+
+	return joint;
+}
+
+void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
+{
+	joint->~b2Joint();
+	switch (joint->m_type)
+	{
+	case e_distanceJoint:
+		allocator->Free(joint, sizeof(b2DistanceJoint));
+		break;
+
+	case e_mouseJoint:
+		allocator->Free(joint, sizeof(b2MouseJoint));
+		break;
+
+	case e_prismaticJoint:
+		allocator->Free(joint, sizeof(b2PrismaticJoint));
+		break;
+
+	case e_revoluteJoint:
+		allocator->Free(joint, sizeof(b2RevoluteJoint));
+		break;
+
+	case e_pulleyJoint:
+		allocator->Free(joint, sizeof(b2PulleyJoint));
+		break;
+
+	case e_gearJoint:
+		allocator->Free(joint, sizeof(b2GearJoint));
+		break;
+
+	default:
+		b2Assert(false);
+		break;
+	}
+}
+
+b2Joint::b2Joint(const b2JointDef* def)
+{
+	m_type = def->type;
+	m_prev = NULL;
+	m_next = NULL;
+	m_body1 = def->body1;
+	m_body2 = def->body2;
+	m_collideConnected = def->collideConnected;
+	m_islandFlag = false;
+	m_userData = def->userData;
+}

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

@@ -0,0 +1,221 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef JOINT_H
+#define JOINT_H
+
+#include "../../Common/b2Math.h"
+
+class b2Body;
+class b2Joint;
+struct b2TimeStep;
+class b2BlockAllocator;
+
+enum b2JointType
+{
+	e_unknownJoint,
+	e_revoluteJoint,
+	e_prismaticJoint,
+	e_distanceJoint,
+	e_pulleyJoint,
+	e_mouseJoint,
+	e_gearJoint
+};
+
+enum b2LimitState
+{
+	e_inactiveLimit,
+	e_atLowerLimit,
+	e_atUpperLimit,
+	e_equalLimits
+};
+
+struct b2Jacobian
+{
+	b2Vec2 linear1;
+	float32 angular1;
+	b2Vec2 linear2;
+	float32 angular2;
+
+	void SetZero();
+	void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+	float32 Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
+};
+
+/// A joint edge is used to connect bodies and joints together
+/// in a joint graph where each body is a node and each joint
+/// is an edge. A joint edge belongs to a doubly linked list
+/// maintained in each attached body. Each joint has two joint
+/// nodes, one for each attached body.
+struct b2JointEdge
+{
+	b2Body* other;			///< provides quick access to the other body attached.
+	b2Joint* joint;			///< the joint
+	b2JointEdge* prev;		///< the previous joint edge in the body's joint list
+	b2JointEdge* next;		///< the next joint edge in the body's joint list
+};
+
+/// Joint definitions are used to construct joints.
+struct b2JointDef
+{
+	b2JointDef()
+	{
+		type = e_unknownJoint;
+		userData = NULL;
+		body1 = NULL;
+		body2 = NULL;
+		collideConnected = false;
+	}
+
+	/// The joint type is set automatically for concrete joint types.
+	b2JointType type;
+
+	/// Use this to attach application specific data to your joints.
+	void* userData;
+
+	/// The first attached body.
+	b2Body* body1;
+
+	/// The second attached body.
+	b2Body* body2;
+
+	/// Set this flag to true if the attached bodies should collide.
+	bool collideConnected;
+};
+
+/// The base joint class. Joints are used to constraint two bodies together in
+/// various fashions. Some joints also feature limits and motors.
+class b2Joint
+{
+public:
+
+	/// Get the type of the concrete joint.
+	b2JointType GetType() const;
+
+	/// Get the first body attached to this joint.
+	b2Body* GetBody1();
+
+	/// Get the second body attached to this joint.
+	b2Body* GetBody2();
+
+	/// Get the anchor point on body1 in world coordinates.
+	virtual b2Vec2 GetAnchor1() const = 0;
+
+	/// Get the anchor point on body2 in world coordinates.
+	virtual b2Vec2 GetAnchor2() const = 0;
+
+	/// Get the reaction force on body2 at the joint anchor.
+	virtual b2Vec2 GetReactionForce() const = 0;
+
+	/// Get the reaction torque on body2.
+	virtual float32 GetReactionTorque() const = 0;
+
+	/// Get the next joint the world joint list.
+	b2Joint* GetNext();
+
+	/// Get the user data pointer.
+	void* GetUserData();
+
+	/// Set the user data pointer.
+	void SetUserData(void* data);
+
+	//--------------- Internals Below -------------------
+protected:
+	friend class b2World;
+	friend class b2Body;
+	friend class b2Island;
+
+	static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
+	static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
+
+	b2Joint(const b2JointDef* def);
+	virtual ~b2Joint() {}
+
+	virtual void InitVelocityConstraints(const b2TimeStep& step) = 0;
+	virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
+
+	// This returns true if the position errors are within tolerance.
+	virtual void InitPositionConstraints() {}
+	virtual bool SolvePositionConstraints() = 0;
+
+	b2JointType m_type;
+	b2Joint* m_prev;
+	b2Joint* m_next;
+	b2JointEdge m_node1;
+	b2JointEdge m_node2;
+	b2Body* m_body1;
+	b2Body* m_body2;
+
+	float32 m_inv_dt;
+
+	bool m_islandFlag;
+	
+
+	void* m_userData;
+public:
+	bool m_collideConnected;
+};
+
+inline void b2Jacobian::SetZero()
+{
+	linear1.SetZero(); angular1 = 0.0f;
+	linear2.SetZero(); angular2 = 0.0f;
+}
+
+inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+	linear1 = x1; angular1 = a1;
+	linear2 = x2; angular2 = a2;
+}
+
+inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
+{
+	return b2Dot(linear1, x1) + angular1 * a1 + b2Dot(linear2, x2) + angular2 * a2;
+}
+
+inline b2JointType b2Joint::GetType() const
+{
+	return m_type;
+}
+
+inline b2Body* b2Joint::GetBody1()
+{
+	return m_body1;
+}
+
+inline b2Body* b2Joint::GetBody2()
+{
+	return m_body2;
+}
+
+inline b2Joint* b2Joint::GetNext()
+{
+	return m_next;
+}
+
+inline void* b2Joint::GetUserData()
+{
+	return m_userData;
+}
+
+inline void b2Joint::SetUserData(void* data)
+{
+	m_userData = data;
+}
+
+#endif

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

@@ -0,0 +1,146 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2MouseJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// p = attached point, m = mouse point
+// C = p - m
+// Cdot = v
+//      = v + cross(w, r)
+// J = [I r_skew]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
+: b2Joint(def)
+{
+	m_target = def->target;
+	m_localAnchor = b2MulT(m_body2->GetXForm(), m_target);
+
+	m_maxForce = B2FORCE_INV_SCALE(def->maxForce);
+	m_impulse.SetZero();
+
+	float32 mass = m_body2->m_mass;
+
+	// Frequency
+	float32 omega = 2.0f * b2_pi * def->frequencyHz;
+
+	// Damping coefficient
+	float32 d = 2.0f * mass * def->dampingRatio * omega;
+
+	// Spring stiffness
+	float32 k = (def->timeStep * mass) * (omega * omega);
+
+	// magic formulas
+	b2Assert(d + k > B2_FLT_EPSILON);
+	m_gamma = 1.0f / (d + k);
+	m_beta = k / (d + k);
+}
+
+void b2MouseJoint::SetTarget(const b2Vec2& target)
+{
+	if (m_body2->IsSleeping())
+	{
+		m_body2->WakeUp();
+	}
+	m_target = target;
+}
+
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_body2;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
+
+	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	float32 invMass = b->m_invMass;
+	float32 invI = b->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = invMass;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;		K1.col2.y = invMass;
+
+	b2Mat22 K2;
+	K2.col1.x =  invI * r.y * r.y;	K2.col2.x = -invI * r.x * r.y;
+	K2.col1.y = -invI * r.x * r.y;	K2.col2.y =  invI * r.x * r.x;
+
+	b2Mat22 K = K1 + K2;
+	K.col1.x += m_gamma;
+	K.col2.y += m_gamma;
+
+	m_mass = K.Invert();
+
+	m_C = b->m_sweep.c + r - m_target;
+
+	// Cheat with some damping
+	b->m_angularVelocity *= 0.98f;
+
+	// Warm starting.
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * m_impulse;
+	b->m_linearVelocity += invMass * P;
+	b->m_angularVelocity += invI * b2Cross(r, P);
+}
+
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b = m_body2;
+
+	b2Vec2 r = b2Mul(b->GetXForm().R, m_localAnchor - b->GetLocalCenter());
+
+	// Cdot = v + cross(w, r)
+	b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+	b2Vec2 force = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_mass, Cdot + (m_beta * step.inv_dt) * m_C + B2FORCE_SCALE(step.dt) * (m_gamma * m_impulse));
+
+	b2Vec2 oldForce = m_impulse;
+	m_impulse += force;
+	float32 forceMagnitude = m_impulse.Length();
+	if (forceMagnitude > m_maxForce)
+	{
+		m_impulse *= m_maxForce / forceMagnitude;
+	}
+	force = m_impulse - oldForce;
+
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * force;
+	b->m_linearVelocity += b->m_invMass * P;
+	b->m_angularVelocity += b->m_invI * b2Cross(r, P);
+}
+
+b2Vec2 b2MouseJoint::GetAnchor1() const
+{
+	return m_target;
+}
+
+b2Vec2 b2MouseJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor);
+}
+
+b2Vec2 b2MouseJoint::GetReactionForce() const
+{
+	return B2FORCE_SCALE(float32(1.0))*m_impulse;
+}
+
+float32 b2MouseJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}

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

@@ -0,0 +1,102 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_MOUSE_JOINT_H
+#define B2_MOUSE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Mouse joint definition. This requires a world target point,
+/// tuning parameters, and the time step.
+struct b2MouseJointDef : public b2JointDef
+{
+	b2MouseJointDef()
+	{
+		type = e_mouseJoint;
+		target.Set(0.0f, 0.0f);
+		maxForce = 0.0f;
+		frequencyHz = 5.0f;
+		dampingRatio = 0.7f;
+		timeStep = 1.0f / 60.0f;
+	}
+
+	/// The initial world target point. This is assumed
+	/// to coincide with the body anchor initially.
+	b2Vec2 target;
+
+	/// The maximum constraint force that can be exerted
+	/// to move the candidate body. Usually you will express
+	/// as some multiple of the weight (multiplier * mass * gravity).
+	float32 maxForce;
+
+	/// The response speed.
+	float32 frequencyHz;
+
+	/// The damping ratio. 0 = no damping, 1 = critical damping.
+	float32 dampingRatio;
+
+	/// The time step used in the simulation.
+	float32 timeStep;
+};
+
+/// A mouse joint is used to make a point on a body track a
+/// specified world point. This a soft constraint with a maximum
+/// force. This allows the constraint to stretch and without
+/// applying huge forces.
+class b2MouseJoint : public b2Joint
+{
+public:
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchor1() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetAnchor2() const;
+
+	/// Implements b2Joint.
+	b2Vec2 GetReactionForce() const;
+
+	/// Implements b2Joint.
+	float32 GetReactionTorque() const;
+
+	/// Use this to update the target point.
+	void SetTarget(const b2Vec2& target);
+
+	//--------------- Internals Below -------------------
+
+	b2MouseJoint(const b2MouseJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints()
+	{
+		return true;
+	}
+
+	b2Vec2 m_localAnchor;
+	b2Vec2 m_target;
+	b2Vec2 m_impulse;
+
+	b2Mat22 m_mass;		// effective mass for point-to-point constraint.
+	b2Vec2 m_C;				// position error
+	float32 m_maxForce;
+	float32 m_beta;			// bias factor
+	float32 m_gamma;		// softness
+};
+
+#endif

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

@@ -0,0 +1,478 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PrismaticJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Linear constraint (point-to-line)
+// d = p2 - p1 = x2 + r2 - x1 - r1
+// C = dot(ay1, d)
+// Cdot = dot(d, cross(w1, ay1)) + dot(ay1, v2 + cross(w2, r2) - v1 - cross(w1, r1))
+//      = -dot(ay1, v1) - dot(cross(d + r1, ay1), w1) + dot(ay1, v2) + dot(cross(r2, ay1), v2)
+// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+//
+// Angular constraint
+// C = a2 - a1 + a_initial
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+
+// Motor/Limit linear constraint
+// C = dot(ax1, d)
+// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
+// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
+
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	body1 = b1;
+	body2 = b2;
+	localAnchor1 = body1->GetLocalPoint(anchor);
+	localAnchor2 = body2->GetLocalPoint(anchor);
+	localAxis1 = body1->GetLocalVector(axis);
+	referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	m_localXAxis1 = def->localAxis1;
+	m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+	m_refAngle = def->referenceAngle;
+
+	m_linearJacobian.SetZero();
+	m_linearMass = 0.0f;
+	m_force = 0.0f;
+
+	m_angularMass = 0.0f;
+	m_torque = 0.0f;
+
+	m_motorJacobian.SetZero();
+	m_motorMass = 0.0;
+	m_motorForce = 0.0f;
+	m_limitForce = 0.0f;
+	m_limitPositionImpulse = 0.0f;
+
+	m_lowerTranslation = def->lowerTranslation;
+	m_upperTranslation = def->upperTranslation;
+	m_maxMotorForce = B2FORCE_INV_SCALE(def->maxMotorForce);
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+}
+
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	// Compute the effective masses.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	// Compute point to line constraint effective mass.
+	// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
+	b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+	b2Vec2 e = b2->m_sweep.c + r2 - b1->m_sweep.c;	// e = d + r1
+
+	m_linearJacobian.Set(-ay1, -b2Cross(e, ay1), ay1, b2Cross(r2, ay1));
+	m_linearMass =	invMass1 + invI1 * m_linearJacobian.angular1 * m_linearJacobian.angular1 +
+					invMass2 + invI2 * m_linearJacobian.angular2 * m_linearJacobian.angular2;
+	b2Assert(m_linearMass > B2_FLT_EPSILON);
+	m_linearMass = 1.0f / m_linearMass;
+
+	// Compute angular constraint effective mass.
+	m_angularMass = invI1 + invI2;
+	if (m_angularMass > B2_FLT_EPSILON)
+	{
+		m_angularMass = 1.0f / m_angularMass;
+	}
+
+	// Compute motor and limit terms.
+	if (m_enableLimit || m_enableMotor)
+	{
+		// The motor and limit share a Jacobian and effective mass.
+		b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+		m_motorJacobian.Set(-ax1, -b2Cross(e, ax1), ax1, b2Cross(r2, ax1));
+		m_motorMass =	invMass1 + invI1 * m_motorJacobian.angular1 * m_motorJacobian.angular1 +
+						invMass2 + invI2 * m_motorJacobian.angular2 * m_motorJacobian.angular2;
+		b2Assert(m_motorMass > B2_FLT_EPSILON);
+		m_motorMass = 1.0f / m_motorMass;
+
+		if (m_enableLimit)
+		{
+			b2Vec2 d = e - r1;	// p2 - p1
+			float32 jointTranslation = b2Dot(ax1, d);
+			if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+			{
+				m_limitState = e_equalLimits;
+			}
+			else if (jointTranslation <= m_lowerTranslation)
+			{
+				if (m_limitState != e_atLowerLimit)
+				{
+					m_limitForce = 0.0f;
+				}
+				m_limitState = e_atLowerLimit;
+			}
+			else if (jointTranslation >= m_upperTranslation)
+			{
+				if (m_limitState != e_atUpperLimit)
+				{
+					m_limitForce = 0.0f;
+				}
+				m_limitState = e_atUpperLimit;
+			}
+			else
+			{
+				m_limitState = e_inactiveLimit;
+				m_limitForce = 0.0f;
+			}
+		}
+	}
+
+	if (m_enableMotor == false)
+	{
+		m_motorForce = 0.0f;
+	}
+
+	if (m_enableLimit == false)
+	{
+		m_limitForce = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear1 + (m_motorForce + m_limitForce) * m_motorJacobian.linear1);
+		b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.linear2 + (m_motorForce + m_limitForce) * m_motorJacobian.linear2);
+		float32 L1 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular1 - m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular1);
+		float32 L2 = B2FORCE_SCALE(step.dt) * (m_force * m_linearJacobian.angular2 + m_torque + (m_motorForce + m_limitForce) * m_motorJacobian.angular2);
+
+		b1->m_linearVelocity += invMass1 * P1;
+		b1->m_angularVelocity += invI1 * L1;
+
+		b2->m_linearVelocity += invMass2 * P2;
+		b2->m_angularVelocity += invI2 * L2;
+	}
+	else
+	{
+		m_force = 0.0f;
+		m_torque = 0.0f;
+		m_limitForce = 0.0f;
+		m_motorForce = 0.0f;
+	}
+
+	m_limitPositionImpulse = 0.0f;
+}
+
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	// Solve linear constraint.
+	float32 linearCdot = m_linearJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+	float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_linearMass * linearCdot;
+	m_force += force;
+
+	float32 P = B2FORCE_SCALE(step.dt) * force;
+	b1->m_linearVelocity += (invMass1 * P) * m_linearJacobian.linear1;
+	b1->m_angularVelocity += invI1 * P * m_linearJacobian.angular1;
+
+	b2->m_linearVelocity += (invMass2 * P) * m_linearJacobian.linear2;
+	b2->m_angularVelocity += invI2 * P * m_linearJacobian.angular2;
+
+	// Solve angular constraint.
+	float32 angularCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+	float32 torque = -B2FORCE_INV_SCALE(step.inv_dt) * m_angularMass * angularCdot;
+	m_torque += torque;
+
+	float32 L = B2FORCE_SCALE(step.dt) * torque;
+	b1->m_angularVelocity -= invI1 * L;
+	b2->m_angularVelocity += invI2 * L;
+
+	// Solve linear motor constraint.
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 motorCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity) - m_motorSpeed;
+		float32 motorForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * motorCdot;
+		float32 oldMotorForce = m_motorForce;
+		m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorForce, m_maxMotorForce);
+		motorForce = m_motorForce - oldMotorForce;
+
+		float32 P = B2FORCE_SCALE(step.dt) * motorForce;
+		b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+		b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+		b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+		b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+	}
+
+	// Solve linear limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 limitCdot = m_motorJacobian.Compute(b1->m_linearVelocity, b1->m_angularVelocity, b2->m_linearVelocity, b2->m_angularVelocity);
+		float32 limitForce = -B2FORCE_INV_SCALE(step.inv_dt) * m_motorMass * limitCdot;
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_limitForce += limitForce;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+
+		float32 P = B2FORCE_SCALE(step.dt) * limitForce;
+
+		b1->m_linearVelocity += (invMass1 * P) * m_motorJacobian.linear1;
+		b1->m_angularVelocity += invI1 * P * m_motorJacobian.angular1;
+
+		b2->m_linearVelocity += (invMass2 * P) * m_motorJacobian.linear2;
+		b2->m_angularVelocity += invI2 * P * m_motorJacobian.angular2;
+	}
+}
+
+bool b2PrismaticJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 ay1 = b2Mul(b1->GetXForm().R, m_localYAxis1);
+
+	// Solve linear (point-to-line) constraint.
+	float32 linearC = b2Dot(ay1, d);
+	// Prevent overly large corrections.
+	linearC = b2Clamp(linearC, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+	float32 linearImpulse = -m_linearMass * linearC;
+
+	b1->m_sweep.c += (invMass1 * linearImpulse) * m_linearJacobian.linear1;
+	b1->m_sweep.a += invI1 * linearImpulse * m_linearJacobian.angular1;
+	//b1->SynchronizeTransform(); // updated by angular constraint
+	b2->m_sweep.c += (invMass2 * linearImpulse) * m_linearJacobian.linear2;
+	b2->m_sweep.a += invI2 * linearImpulse * m_linearJacobian.angular2;
+	//b2->SynchronizeTransform(); // updated by angular constraint
+
+	float32 positionError = b2Abs(linearC);
+
+	// Solve angular constraint.
+	float32 angularC = b2->m_sweep.a - b1->m_sweep.a - m_refAngle;
+	// Prevent overly large corrections.
+	angularC = b2Clamp(angularC, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+	float32 angularImpulse = -m_angularMass * angularC;
+
+	b1->m_sweep.a -= b1->m_invI * angularImpulse;
+	b2->m_sweep.a += b2->m_invI * angularImpulse;
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	float32 angularError = b2Abs(angularC);
+
+	// Solve linear limit constraint.
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+		b2Vec2 d = p2 - p1;
+		b2Vec2 ax1 = b2Mul(b1->GetXForm().R, m_localXAxis1);
+
+		float32 translation = b2Dot(ax1, d);
+		float32 limitImpulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			// Prevent large angular corrections
+			float32 limitC = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			positionError = b2Max(positionError, b2Abs(angularC));
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 limitC = translation - m_lowerTranslation;
+			positionError = b2Max(positionError, -limitC);
+
+			// Prevent large linear corrections and allow some slop.
+			limitC = b2Clamp(limitC + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 limitC = translation - m_upperTranslation;
+			positionError = b2Max(positionError, limitC);
+
+			// Prevent large linear corrections and allow some slop.
+			limitC = b2Clamp(limitC - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+
+		b1->m_sweep.c += (invMass1 * limitImpulse) * m_motorJacobian.linear1;
+		b1->m_sweep.a += invI1 * limitImpulse * m_motorJacobian.angular1;
+		b2->m_sweep.c += (invMass2 * limitImpulse) * m_motorJacobian.linear2;
+		b2->m_sweep.a += invI2 * limitImpulse * m_motorJacobian.angular2;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PrismaticJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PrismaticJoint::GetReactionForce() const
+{
+	b2Vec2 ax1 = b2Mul(m_body1->GetXForm().R, m_localXAxis1);
+	b2Vec2 ay1 = b2Mul(m_body1->GetXForm().R, m_localYAxis1);
+
+	return B2FORCE_SCALE(float32(1.0))*(m_limitForce * ax1 + m_force * ay1);
+}
+
+float32 b2PrismaticJoint::GetReactionTorque() const
+{
+	return B2FORCE_SCALE(m_torque);
+}
+
+float32 b2PrismaticJoint::GetJointTranslation() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+	b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	float32 translation = b2Dot(d, axis);
+	return translation;
+}
+
+float32 b2PrismaticJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 d = p2 - p1;
+	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+	b2Vec2 v1 = b1->m_linearVelocity;
+	b2Vec2 v2 = b2->m_linearVelocity;
+	float32 w1 = b1->m_angularVelocity;
+	float32 w2 = b2->m_angularVelocity;
+
+	float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+	return speed;
+}
+
+bool b2PrismaticJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2PrismaticJoint::EnableLimit(bool flag)
+{
+	m_enableLimit = flag;
+}
+
+float32 b2PrismaticJoint::GetLowerLimit() const
+{
+	return m_lowerTranslation;
+}
+
+float32 b2PrismaticJoint::GetUpperLimit() const
+{
+	return m_upperTranslation;
+}
+
+void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_lowerTranslation = lower;
+	m_upperTranslation = upper;
+}
+
+bool b2PrismaticJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2PrismaticJoint::EnableMotor(bool flag)
+{
+	m_enableMotor = flag;
+}
+
+void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+{
+	m_motorSpeed = speed;
+}
+
+void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+{
+	m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
+}
+
+float32 b2PrismaticJoint::GetMotorForce() const
+{
+	return m_motorForce;
+}
+
+
+

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

@@ -0,0 +1,176 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_PRISMATIC_JOINT_H
+#define B2_PRISMATIC_JOINT_H
+
+#include "b2Joint.h"
+
+/// Prismatic joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space. Using local
+/// anchors and a local axis helps when saving and loading a game.
+struct b2PrismaticJointDef : public b2JointDef
+{
+	b2PrismaticJointDef()
+	{
+		type = e_prismaticJoint;
+		localAnchor1.SetZero();
+		localAnchor2.SetZero();
+		localAxis1.Set(1.0f, 0.0f);
+		referenceAngle = 0.0f;
+		enableLimit = false;
+		lowerTranslation = 0.0f;
+		upperTranslation = 0.0f;
+		enableMotor = false;
+		maxMotorForce = 0.0f;
+		motorSpeed = 0.0f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The local translation axis in body1.
+	b2Vec2 localAxis1;
+
+	/// The constrained angle between the bodies: body2_angle - body1_angle.
+	float32 referenceAngle;
+
+	/// Enable/disable the joint limit.
+	bool enableLimit;
+
+	/// The lower translation limit, usually in meters.
+	float32 lowerTranslation;
+
+	/// The upper translation limit, usually in meters.
+	float32 upperTranslation;
+
+	/// Enable/disable the joint motor.
+	bool enableMotor;
+
+	/// The maximum motor torque, usually in N-m.
+	float32 maxMotorForce;
+
+	/// The desired motor speed in radians per second.
+	float32 motorSpeed;
+};
+
+/// A prismatic joint. This joint provides one degree of freedom: translation
+/// along an axis fixed in body1. Relative rotation is prevented. You can
+/// use a joint limit to restrict the range of motion and a joint motor to
+/// drive the motion or to model joint friction.
+class b2PrismaticJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the current joint translation, usually in meters.
+	float32 GetJointTranslation() const;
+
+	/// Get the current joint translation speed, usually in meters per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	/// Enable/disable the joint limit.
+	void EnableLimit(bool flag);
+
+	/// Get the lower joint limit, usually in meters.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit, usually in meters.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits, usually in meters.
+	void SetLimits(float32 lower, float32 upper);
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed, usually in meters per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed, usually in meters per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set the maximum motor force, usually in N.
+	void SetMaxMotorForce(float32 force);
+
+	/// Get the current motor force, usually in N.
+	float32 GetMotorForce() const;
+
+	//--------------- Internals Below -------------------
+
+	b2PrismaticJoint(const b2PrismaticJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_localXAxis1;
+	b2Vec2 m_localYAxis1;
+	float32 m_refAngle;
+
+	b2Jacobian m_linearJacobian;
+	float32 m_linearMass;				// effective mass for point-to-line constraint.
+	float32 m_force;
+	
+	float32 m_angularMass;			// effective mass for angular constraint.
+	float32 m_torque;
+
+	b2Jacobian m_motorJacobian;
+	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
+	float32 m_motorForce;
+	float32 m_limitForce;
+	float32 m_limitPositionImpulse;
+
+	float32 m_lowerTranslation;
+	float32 m_upperTranslation;
+	float32 m_maxMotorForce;
+	float32 m_motorSpeed;
+	
+	bool m_enableLimit;
+	bool m_enableMotor;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2PrismaticJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif

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

@@ -0,0 +1,430 @@
+/*
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2PulleyJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+// Pulley:
+// length1 = norm(p1 - s1)
+// length2 = norm(p2 - s2)
+// C0 = (length1 + ratio * length2)_initial
+// C = C0 - (length1 + ratio * length2) >= 0
+// u1 = (p1 - s1) / norm(p1 - s1)
+// u2 = (p2 - s2) / norm(p2 - s2)
+// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
+// J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
+// K = J * invM * JT
+//   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
+//
+// Limit:
+// C = maxLength - length
+// u = (p - s) / norm(p - s)
+// Cdot = -dot(u, v + cross(w, r))
+// K = invMass + invI * cross(r, u)^2
+// 0 <= impulse
+
+void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
+				const b2Vec2& ga1, const b2Vec2& ga2,
+				const b2Vec2& anchor1, const b2Vec2& anchor2,
+				float32 r)
+{
+	body1 = b1;
+	body2 = b2;
+	groundAnchor1 = ga1;
+	groundAnchor2 = ga2;
+	localAnchor1 = body1->GetLocalPoint(anchor1);
+	localAnchor2 = body2->GetLocalPoint(anchor2);
+	b2Vec2 d1 = anchor1 - ga1;
+	length1 = d1.Length();
+	b2Vec2 d2 = anchor2 - ga2;
+	length2 = d2.Length();
+	ratio = r;
+	b2Assert(ratio > B2_FLT_EPSILON);
+	float32 C = length1 + ratio * length2;
+	maxLength1 = C - ratio * b2_minPulleyLength;
+	maxLength2 = (C - b2_minPulleyLength) / ratio;
+}
+
+b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
+: b2Joint(def)
+{
+	m_ground = m_body1->GetWorld()->GetGroundBody();
+	m_groundAnchor1 = def->groundAnchor1 - m_ground->GetXForm().position;
+	m_groundAnchor2 = def->groundAnchor2 - m_ground->GetXForm().position;
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+
+	b2Assert(def->ratio != 0.0f);
+	m_ratio = def->ratio;
+
+	m_constant = def->length1 + m_ratio * def->length2;
+
+	m_maxLength1 = b2Min(def->maxLength1, m_constant - m_ratio * b2_minPulleyLength);
+	m_maxLength2 = b2Min(def->maxLength2, (m_constant - b2_minPulleyLength) / m_ratio);
+
+	m_force = 0.0f;
+	m_limitForce1 = 0.0f;
+	m_limitForce2 = 0.0f;
+}
+
+void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+
+	b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
+	b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+
+	// Get the pulley axes.
+	m_u1 = p1 - s1;
+	m_u2 = p2 - s2;
+
+	float32 length1 = m_u1.Length();
+	float32 length2 = m_u2.Length();
+
+	if (length1 > b2_linearSlop)
+	{
+		m_u1 *= 1.0f / length1;
+	}
+	else
+	{
+		m_u1.SetZero();
+	}
+
+	if (length2 > b2_linearSlop)
+	{
+		m_u2 *= 1.0f / length2;
+	}
+	else
+	{
+		m_u2.SetZero();
+	}
+
+	float32 C = m_constant - length1 - m_ratio * length2;
+	if (C > 0.0f)
+	{
+		m_state = e_inactiveLimit;
+		m_force = 0.0f;
+	}
+	else
+	{
+		m_state = e_atUpperLimit;
+		m_positionImpulse = 0.0f;
+	}
+
+	if (length1 < m_maxLength1)
+	{
+		m_limitState1 = e_inactiveLimit;
+		m_limitForce1 = 0.0f;
+	}
+	else
+	{
+		m_limitState1 = e_atUpperLimit;
+		m_limitPositionImpulse1 = 0.0f;
+	}
+
+	if (length2 < m_maxLength2)
+	{
+		m_limitState2 = e_inactiveLimit;
+		m_limitForce2 = 0.0f;
+	}
+	else
+	{
+		m_limitState2 = e_atUpperLimit;
+		m_limitPositionImpulse2 = 0.0f;
+	}
+
+	// Compute effective mass.
+	float32 cr1u1 = b2Cross(r1, m_u1);
+	float32 cr2u2 = b2Cross(r2, m_u2);
+
+	m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
+	m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
+	m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
+	b2Assert(m_limitMass1 > B2_FLT_EPSILON);
+	b2Assert(m_limitMass2 > B2_FLT_EPSILON);
+	b2Assert(m_pulleyMass > B2_FLT_EPSILON);
+	m_limitMass1 = 1.0f / m_limitMass1;
+	m_limitMass2 = 1.0f / m_limitMass2;
+	m_pulleyMass = 1.0f / m_pulleyMass;
+
+	if (step.warmStarting)
+	{
+		// Warm starting.
+		b2Vec2 P1 = B2FORCE_SCALE(step.dt) * (-m_force - m_limitForce1) * m_u1;
+		b2Vec2 P2 = B2FORCE_SCALE(step.dt) * (-m_ratio * m_force - m_limitForce2) * m_u2;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+	else
+	{
+		m_force = 0.0f;
+		m_limitForce1 = 0.0f;
+		m_limitForce2 = 0.0f;
+	}
+}
+
+void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	if (m_state == e_atUpperLimit)
+	{
+		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+
+		float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
+		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_pulleyMass * Cdot;
+		float32 oldForce = m_force;
+		m_force = b2Max(0.0f, m_force + force);
+		force = m_force - oldForce;
+
+		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * m_ratio * force * m_u2;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+
+	if (m_limitState1 == e_atUpperLimit)
+	{
+		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+
+		float32 Cdot = -b2Dot(m_u1, v1);
+		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass1 * Cdot;
+		float32 oldForce = m_limitForce1;
+		m_limitForce1 = b2Max(0.0f, m_limitForce1 + force);
+		force = m_limitForce1 - oldForce;
+
+		b2Vec2 P1 = -B2FORCE_SCALE(step.dt) * force * m_u1;
+		b1->m_linearVelocity += b1->m_invMass * P1;
+		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+	}
+
+	if (m_limitState2 == e_atUpperLimit)
+	{
+		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+
+		float32 Cdot = -b2Dot(m_u2, v2);
+		float32 force = -B2FORCE_INV_SCALE(step.inv_dt) * m_limitMass2 * Cdot;
+		float32 oldForce = m_limitForce2;
+		m_limitForce2 = b2Max(0.0f, m_limitForce2 + force);
+		force = m_limitForce2 - oldForce;
+
+		b2Vec2 P2 = -B2FORCE_SCALE(step.dt) * force * m_u2;
+		b2->m_linearVelocity += b2->m_invMass * P2;
+		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+	}
+}
+
+bool b2PulleyJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 s1 = m_ground->GetXForm().position + m_groundAnchor1;
+	b2Vec2 s2 = m_ground->GetXForm().position + m_groundAnchor2;
+
+	float32 linearError = 0.0f;
+
+	if (m_state == e_atUpperLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+
+		// Get the pulley axes.
+		m_u1 = p1 - s1;
+		m_u2 = p2 - s2;
+
+		float32 length1 = m_u1.Length();
+		float32 length2 = m_u2.Length();
+
+		if (length1 > b2_linearSlop)
+		{
+			m_u1 *= 1.0f / length1;
+		}
+		else
+		{
+			m_u1.SetZero();
+		}
+
+		if (length2 > b2_linearSlop)
+		{
+			m_u2 *= 1.0f / length2;
+		}
+		else
+		{
+			m_u2.SetZero();
+		}
+
+		float32 C = m_constant - length1 - m_ratio * length2;
+		linearError = b2Max(linearError, -C);
+
+		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+		float32 impulse = -m_pulleyMass * C;
+		float32 oldImpulse = m_positionImpulse;
+		m_positionImpulse = b2Max(0.0f, m_positionImpulse + impulse);
+		impulse = m_positionImpulse - oldImpulse;
+
+		b2Vec2 P1 = -impulse * m_u1;
+		b2Vec2 P2 = -m_ratio * impulse * m_u2;
+
+		b1->m_sweep.c += b1->m_invMass * P1;
+		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+		b2->m_sweep.c += b2->m_invMass * P2;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	if (m_limitState1 == e_atUpperLimit)
+	{
+		b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+		b2Vec2 p1 = b1->m_sweep.c + r1;
+
+		m_u1 = p1 - s1;
+		float32 length1 = m_u1.Length();
+
+		if (length1 > b2_linearSlop)
+		{
+			m_u1 *= 1.0f / length1;
+		}
+		else
+		{
+			m_u1.SetZero();
+		}
+
+		float32 C = m_maxLength1 - length1;
+		linearError = b2Max(linearError, -C);
+		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+		float32 impulse = -m_limitMass1 * C;
+		float32 oldLimitPositionImpulse = m_limitPositionImpulse1;
+		m_limitPositionImpulse1 = b2Max(0.0f, m_limitPositionImpulse1 + impulse);
+		impulse = m_limitPositionImpulse1 - oldLimitPositionImpulse;
+
+		b2Vec2 P1 = -impulse * m_u1;
+		b1->m_sweep.c += b1->m_invMass * P1;
+		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+
+		b1->SynchronizeTransform();
+	}
+
+	if (m_limitState2 == e_atUpperLimit)
+	{
+		b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+		b2Vec2 p2 = b2->m_sweep.c + r2;
+
+		m_u2 = p2 - s2;
+		float32 length2 = m_u2.Length();
+
+		if (length2 > b2_linearSlop)
+		{
+			m_u2 *= 1.0f / length2;
+		}
+		else
+		{
+			m_u2.SetZero();
+		}
+
+		float32 C = m_maxLength2 - length2;
+		linearError = b2Max(linearError, -C);
+		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+		float32 impulse = -m_limitMass2 * C;
+		float32 oldLimitPositionImpulse = m_limitPositionImpulse2;
+		m_limitPositionImpulse2 = b2Max(0.0f, m_limitPositionImpulse2 + impulse);
+		impulse = m_limitPositionImpulse2 - oldLimitPositionImpulse;
+
+		b2Vec2 P2 = -impulse * m_u2;
+		b2->m_sweep.c += b2->m_invMass * P2;
+		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+		b2->SynchronizeTransform();
+	}
+
+	return linearError < b2_linearSlop;
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2PulleyJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2PulleyJoint::GetReactionForce() const
+{
+	b2Vec2 F = B2FORCE_SCALE(m_force) * m_u2;
+	return F;
+}
+
+float32 b2PulleyJoint::GetReactionTorque() const
+{
+	return 0.0f;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor1() const
+{
+	return m_ground->GetXForm().position + m_groundAnchor1;
+}
+
+b2Vec2 b2PulleyJoint::GetGroundAnchor2() const
+{
+	return m_ground->GetXForm().position + m_groundAnchor2;
+}
+
+float32 b2PulleyJoint::GetLength1() const
+{
+	b2Vec2 p = m_body1->GetWorldPoint(m_localAnchor1);
+	b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor1;
+	b2Vec2 d = p - s;
+	return d.Length();
+}
+
+float32 b2PulleyJoint::GetLength2() const
+{
+	b2Vec2 p = m_body2->GetWorldPoint(m_localAnchor2);
+	b2Vec2 s = m_ground->GetXForm().position + m_groundAnchor2;
+	b2Vec2 d = p - s;
+	return d.Length();
+}
+
+float32 b2PulleyJoint::GetRatio() const
+{
+	return m_ratio;
+}

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

@@ -0,0 +1,153 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_PULLEY_JOINT_H
+#define B2_PULLEY_JOINT_H
+
+#include "b2Joint.h"
+
+const float32 b2_minPulleyLength = 2.0f;
+
+/// Pulley joint definition. This requires two ground anchors,
+/// two dynamic body anchor points, max lengths for each side,
+/// and a pulley ratio.
+struct b2PulleyJointDef : public b2JointDef
+{
+	b2PulleyJointDef()
+	{
+		type = e_pulleyJoint;
+		groundAnchor1.Set(-1.0f, 1.0f);
+		groundAnchor2.Set(1.0f, 1.0f);
+		localAnchor1.Set(-1.0f, 0.0f);
+		localAnchor2.Set(1.0f, 0.0f);
+		length1 = 0.0f;
+		maxLength1 = 0.0f;
+		length2 = 0.0f;
+		maxLength2 = 0.0f;
+		ratio = 1.0f;
+		collideConnected = true;
+	}
+
+	/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
+	void Initialize(b2Body* body1, b2Body* body2,
+					const b2Vec2& groundAnchor1, const b2Vec2& groundAnchor2,
+					const b2Vec2& anchor1, const b2Vec2& anchor2,
+					float32 ratio);
+
+	/// The first ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchor1;
+
+	/// The second ground anchor in world coordinates. This point never moves.
+	b2Vec2 groundAnchor2;
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The a reference length for the segment attached to body1.
+	float32 length1;
+
+	/// The maximum length of the segment attached to body1.
+	float32 maxLength1;
+
+	/// The a reference length for the segment attached to body2.
+	float32 length2;
+
+	/// The maximum length of the segment attached to body2.
+	float32 maxLength2;
+
+	/// The pulley ratio, used to simulate a block-and-tackle.
+	float32 ratio;
+};
+
+/// The pulley joint is connected to two bodies and two fixed ground points.
+/// The pulley supports a ratio such that:
+/// length1 + ratio * length2 <= constant
+/// Yes, the force transmitted is scaled by the ratio.
+/// The pulley also enforces a maximum length limit on both sides. This is
+/// useful to prevent one side of the pulley hitting the top.
+class b2PulleyJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the first ground anchor.
+	b2Vec2 GetGroundAnchor1() const;
+
+	/// Get the second ground anchor.
+	b2Vec2 GetGroundAnchor2() const;
+
+	/// Get the current length of the segment attached to body1.
+	float32 GetLength1() const;
+
+	/// Get the current length of the segment attached to body2.
+	float32 GetLength2() const;
+
+	/// Get the pulley ratio.
+	float32 GetRatio() const;
+
+	//--------------- Internals Below -------------------
+
+	b2PulleyJoint(const b2PulleyJointDef* data);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+	bool SolvePositionConstraints();
+
+	b2Body* m_ground;
+	b2Vec2 m_groundAnchor1;
+	b2Vec2 m_groundAnchor2;
+	b2Vec2 m_localAnchor1;
+	b2Vec2 m_localAnchor2;
+
+	b2Vec2 m_u1;
+	b2Vec2 m_u2;
+	
+	float32 m_constant;
+	float32 m_ratio;
+	
+	float32 m_maxLength1;
+	float32 m_maxLength2;
+
+	// Effective masses
+	float32 m_pulleyMass;
+	float32 m_limitMass1;
+	float32 m_limitMass2;
+
+	// Impulses for accumulation/warm starting.
+	float32 m_force;
+	float32 m_limitForce1;
+	float32 m_limitForce2;
+
+	// Position impulses for accumulation.
+	float32 m_positionImpulse;
+	float32 m_limitPositionImpulse1;
+	float32 m_limitPositionImpulse2;
+
+	b2LimitState m_state;
+	b2LimitState m_limitState1;
+	b2LimitState m_limitState2;
+};
+
+#endif

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

@@ -0,0 +1,399 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b2RevoluteJoint.h"
+#include "../b2Body.h"
+#include "../b2World.h"
+
+#include "../b2Island.h"
+
+// Point-to-point constraint
+// C = p2 - p1
+// Cdot = v2 - v1
+//      = v2 + cross(w2, r2) - v1 - cross(w1, r1)
+// J = [-I -r1_skew I r2_skew ]
+// Identity used:
+// w k % (rx i + ry j) = w * (-ry i + rx j)
+
+// Motor constraint
+// Cdot = w2 - w1
+// J = [0 0 -1 0 0 1]
+// K = invI1 + invI2
+
+void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
+{
+	body1 = b1;
+	body2 = b2;
+	localAnchor1 = body1->GetLocalPoint(anchor);
+	localAnchor2 = body2->GetLocalPoint(anchor);
+	referenceAngle = body2->GetAngle() - body1->GetAngle();
+}
+
+b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchor1 = def->localAnchor1;
+	m_localAnchor2 = def->localAnchor2;
+	m_referenceAngle = def->referenceAngle;
+
+	m_pivotForce.Set(0.0f, 0.0f);
+	m_motorForce = 0.0f;
+	m_limitForce = 0.0f;
+	m_limitPositionImpulse = 0.0f;
+
+	m_lowerAngle = def->lowerAngle;
+	m_upperAngle = def->upperAngle;
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableLimit = def->enableLimit;
+	m_enableMotor = def->enableMotor;
+}
+
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	// Compute the effective mass matrix.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
+	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
+	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;
+
+	b2Mat22 K2;
+	K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
+	K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;
+
+	b2Mat22 K3;
+	K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
+	K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;
+
+	b2Mat22 K = K1 + K2 + K3;
+	m_pivotMass = K.Invert();
+
+	m_motorMass = 1.0f / (invI1 + invI2);
+
+	if (m_enableMotor == false)
+	{
+		m_motorForce = 0.0f;
+	}
+
+	if (m_enableLimit)
+	{
+		float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
+		{
+			m_limitState = e_equalLimits;
+		}
+		else if (jointAngle <= m_lowerAngle)
+		{
+			if (m_limitState != e_atLowerLimit)
+			{
+				m_limitForce = 0.0f;
+			}
+			m_limitState = e_atLowerLimit;
+		}
+		else if (jointAngle >= m_upperAngle)
+		{
+			if (m_limitState != e_atUpperLimit)
+			{
+				m_limitForce = 0.0f;
+			}
+			m_limitState = e_atUpperLimit;
+		}
+		else
+		{
+			m_limitState = e_inactiveLimit;
+			m_limitForce = 0.0f;
+		}
+	}
+	else
+	{
+		m_limitForce = 0.0f;
+	}
+
+	if (step.warmStarting)
+	{
+		b1->m_linearVelocity -= B2FORCE_SCALE(step.dt) * invMass1 * m_pivotForce;
+		b1->m_angularVelocity -= B2FORCE_SCALE(step.dt) * invI1 * (b2Cross(r1, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+
+		b2->m_linearVelocity += B2FORCE_SCALE(step.dt) * invMass2 * m_pivotForce;
+		b2->m_angularVelocity += B2FORCE_SCALE(step.dt) * invI2 * (b2Cross(r2, m_pivotForce) + B2FORCE_INV_SCALE(m_motorForce + m_limitForce));
+	}
+	else
+	{
+		m_pivotForce.SetZero();
+		m_motorForce = 0.0f;
+		m_limitForce = 0.0f;
+	}
+
+	m_limitPositionImpulse = 0.0f;
+}
+
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	// Solve point-to-point constraint
+	b2Vec2 pivotCdot = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2) - b1->m_linearVelocity - b2Cross(b1->m_angularVelocity, r1);
+	b2Vec2 pivotForce = -B2FORCE_INV_SCALE(step.inv_dt) * b2Mul(m_pivotMass, pivotCdot);
+	m_pivotForce += pivotForce;
+
+	b2Vec2 P = B2FORCE_SCALE(step.dt) * pivotForce;
+	b1->m_linearVelocity -= b1->m_invMass * P;
+	b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+
+	b2->m_linearVelocity += b2->m_invMass * P;
+	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+
+	if (m_enableMotor && m_limitState != e_equalLimits)
+	{
+		float32 motorCdot = b2->m_angularVelocity - b1->m_angularVelocity - m_motorSpeed;
+		float32 motorForce = -step.inv_dt * m_motorMass * motorCdot;
+		float32 oldMotorForce = m_motorForce;
+		m_motorForce = b2Clamp(m_motorForce + motorForce, -m_maxMotorTorque, m_maxMotorTorque);
+		motorForce = m_motorForce - oldMotorForce;
+
+		float32 P = step.dt * motorForce;
+		b1->m_angularVelocity -= b1->m_invI * P;
+		b2->m_angularVelocity += b2->m_invI * P;
+	}
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 limitCdot = b2->m_angularVelocity - b1->m_angularVelocity;
+		float32 limitForce = -step.inv_dt * m_motorMass * limitCdot;
+
+		if (m_limitState == e_equalLimits)
+		{
+			m_limitForce += limitForce;
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Max(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 oldLimitForce = m_limitForce;
+			m_limitForce = b2Min(m_limitForce + limitForce, 0.0f);
+			limitForce = m_limitForce - oldLimitForce;
+		}
+
+		float32 P = step.dt * limitForce;
+		b1->m_angularVelocity -= b1->m_invI * P;
+		b2->m_angularVelocity += b2->m_invI * P;
+	}
+}
+
+bool b2RevoluteJoint::SolvePositionConstraints()
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+
+	float32 positionError = 0.0f;
+
+	// Solve point-to-point position error.
+	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
+	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());
+
+	b2Vec2 p1 = b1->m_sweep.c + r1;
+	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 ptpC = p2 - p1;
+
+	positionError = ptpC.Length();
+
+	// Prevent overly large corrections.
+	//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
+	//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
+
+	float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+	float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+
+	b2Mat22 K1;
+	K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
+	K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;
+
+	b2Mat22 K2;
+	K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
+	K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;
+
+	b2Mat22 K3;
+	K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
+	K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;
+
+	b2Mat22 K = K1 + K2 + K3;
+	b2Vec2 impulse = K.Solve(-ptpC);
+
+	b1->m_sweep.c -= b1->m_invMass * impulse;
+	b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
+
+	b2->m_sweep.c += b2->m_invMass * impulse;
+	b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
+
+	b1->SynchronizeTransform();
+	b2->SynchronizeTransform();
+
+	// Handle limits.
+	float32 angularError = 0.0f;
+
+	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	{
+		float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		float32 limitImpulse = 0.0f;
+
+		if (m_limitState == e_equalLimits)
+		{
+			// Prevent large angular corrections
+			float32 limitC = b2Clamp(angle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			angularError = b2Abs(limitC);
+		}
+		else if (m_limitState == e_atLowerLimit)
+		{
+			float32 limitC = angle - m_lowerAngle;
+			angularError = b2Max(0.0f, -limitC);
+
+			// Prevent large angular corrections and allow some slop.
+			limitC = b2Clamp(limitC + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Max(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+		else if (m_limitState == e_atUpperLimit)
+		{
+			float32 limitC = angle - m_upperAngle;
+			angularError = b2Max(0.0f, limitC);
+
+			// Prevent large angular corrections and allow some slop.
+			limitC = b2Clamp(limitC - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
+			limitImpulse = -m_motorMass * limitC;
+			float32 oldLimitImpulse = m_limitPositionImpulse;
+			m_limitPositionImpulse = b2Min(m_limitPositionImpulse + limitImpulse, 0.0f);
+			limitImpulse = m_limitPositionImpulse - oldLimitImpulse;
+		}
+
+		b1->m_sweep.a -= b1->m_invI * limitImpulse;
+		b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+		b1->SynchronizeTransform();
+		b2->SynchronizeTransform();
+	}
+
+	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor1() const
+{
+	return m_body1->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2RevoluteJoint::GetAnchor2() const
+{
+	return m_body2->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2RevoluteJoint::GetReactionForce() const
+{
+	return B2FORCE_SCALE(float32(1.0))*m_pivotForce;
+}
+
+float32 b2RevoluteJoint::GetReactionTorque() const
+{
+	return m_limitForce;
+}
+
+float32 b2RevoluteJoint::GetJointAngle() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+	return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+}
+
+float32 b2RevoluteJoint::GetJointSpeed() const
+{
+	b2Body* b1 = m_body1;
+	b2Body* b2 = m_body2;
+	return b2->m_angularVelocity - b1->m_angularVelocity;
+}
+
+bool b2RevoluteJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2RevoluteJoint::EnableMotor(bool flag)
+{
+	m_enableMotor = flag;
+}
+
+float32 b2RevoluteJoint::GetMotorTorque() const
+{
+	return m_motorForce;
+}
+
+void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+{
+	m_motorSpeed = speed;
+}
+
+void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+{
+	m_maxMotorTorque = torque;
+}
+
+bool b2RevoluteJoint::IsLimitEnabled() const
+{
+	return m_enableLimit;
+}
+
+void b2RevoluteJoint::EnableLimit(bool flag)
+{
+	m_enableLimit = flag;
+}
+
+float32 b2RevoluteJoint::GetLowerLimit() const
+{
+	return m_lowerAngle;
+}
+
+float32 b2RevoluteJoint::GetUpperLimit() const
+{
+	return m_upperAngle;
+}
+
+void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
+{
+	b2Assert(lower <= upper);
+	m_lowerAngle = lower;
+	m_upperAngle = upper;
+}

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

@@ -0,0 +1,172 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B2_REVOLUTE_JOINT_H
+#define B2_REVOLUTE_JOINT_H
+
+#include "b2Joint.h"
+
+/// Revolute joint definition. This requires defining an
+/// anchor point where the bodies are joined. The definition
+/// uses local anchor points so that the initial configuration
+/// can violate the constraint slightly. You also need to
+/// specify the initial relative angle for joint limits. This
+/// helps when saving and loading a game.
+/// The local anchor points are measured from the body's origin
+/// rather than the center of mass because:
+/// 1. you might not know where the center of mass will be.
+/// 2. if you add/remove shapes from a body and recompute the mass,
+///    the joints will be broken.
+struct b2RevoluteJointDef : public b2JointDef
+{
+	b2RevoluteJointDef()
+	{
+		type = e_revoluteJoint;
+		localAnchor1.Set(0.0f, 0.0f);
+		localAnchor2.Set(0.0f, 0.0f);
+		referenceAngle = 0.0f;
+		lowerAngle = 0.0f;
+		upperAngle = 0.0f;
+		maxMotorTorque = 0.0f;
+		motorSpeed = 0.0f;
+		enableLimit = false;
+		enableMotor = false;
+	}
+
+	/// Initialize the bodies, anchors, and reference angle using the world
+	/// anchor.
+	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchor1;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchor2;
+
+	/// The body2 angle minus body1 angle in the reference state (radians).
+	float32 referenceAngle;
+
+	/// A flag to enable joint limits.
+	bool enableLimit;
+
+	/// The lower angle for the joint limit (radians).
+	float32 lowerAngle;
+
+	/// The upper angle for the joint limit (radians).
+	float32 upperAngle;
+
+	/// A flag to enable the joint motor.
+	bool enableMotor;
+
+	/// The desired motor speed. Usually in radians per second.
+	float32 motorSpeed;
+
+	/// The maximum motor torque used to achieve the desired motor speed.
+	/// Usually in N-m.
+	float32 maxMotorTorque;
+};
+
+/// A revolute joint constrains to bodies to share a common point while they
+/// are free to rotate about the point. The relative rotation about the shared
+/// point is the joint angle. You can limit the relative rotation with
+/// a joint limit that specifies a lower and upper angle. You can use a motor
+/// to drive the relative rotation about the shared point. A maximum motor torque
+/// is provided so that infinite forces are not generated.
+class b2RevoluteJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchor1() const;
+	b2Vec2 GetAnchor2() const;
+
+	b2Vec2 GetReactionForce() const;
+	float32 GetReactionTorque() const;
+
+	/// Get the current joint angle in radians.
+	float32 GetJointAngle() const;
+
+	/// Get the current joint angle speed in radians per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint limit enabled?
+	bool IsLimitEnabled() const;
+
+	/// Enable/disable the joint limit.
+	void EnableLimit(bool flag);
+
+	/// Get the lower joint limit in radians.
+	float32 GetLowerLimit() const;
+
+	/// Get the upper joint limit in radians.
+	float32 GetUpperLimit() const;
+
+	/// Set the joint limits in radians.
+	void SetLimits(float32 lower, float32 upper);
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed in radians per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed in radians per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set the maximum motor torque, usually in N-m.
+	void SetMaxMotorTorque(float32 torque);
+
+	/// Get the current motor torque, usually in N-m.
+	float32 GetMotorTorque() const;
+
+	//--------------- Internals Below -------------------
+	b2RevoluteJoint(const b2RevoluteJointDef* def);
+
+	void InitVelocityConstraints(const b2TimeStep& step);
+	void SolveVelocityConstraints(const b2TimeStep& step);
+
+	bool SolvePositionConstraints();
+
+	b2Vec2 m_localAnchor1;	// relative
+	b2Vec2 m_localAnchor2;
+	b2Vec2 m_pivotForce;
+	float32 m_motorForce;
+	float32 m_limitForce;
+	float32 m_limitPositionImpulse;
+
+	b2Mat22 m_pivotMass;		// effective mass for point-to-point constraint.
+	float32 m_motorMass;	// effective mass for motor/limit angular constraint.
+	
+	bool m_enableMotor;
+	float32 m_maxMotorTorque;
+	float32 m_motorSpeed;
+
+	bool m_enableLimit;
+	float32 m_referenceAngle;
+	float32 m_lowerAngle;
+	float32 m_upperAngle;
+	b2LimitState m_limitState;
+};
+
+inline float32 b2RevoluteJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+#endif