Updated box2d from 2.2.1 to 2.3.0

--HG--
branch : box2d-2.3

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp

@@ -23,7 +23,6 @@
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp

@@ -23,7 +23,6 @@
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2CircleContact.cpp

@@ -24,7 +24,6 @@
 #include <Box2D/Collision/b2TimeOfImpact.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2Contact.cpp

@@ -102,14 +102,19 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
 {
 	b2Assert(s_initialized == true);
 
-	if (contact->m_manifold.pointCount > 0)
+	b2Fixture* fixtureA = contact->m_fixtureA;
+	b2Fixture* fixtureB = contact->m_fixtureB;
+
+	if (contact->m_manifold.pointCount > 0 &&
+		fixtureA->IsSensor() == false &&
+		fixtureB->IsSensor() == false)
 	{
-		contact->GetFixtureA()->GetBody()->SetAwake(true);
-		contact->GetFixtureB()->GetBody()->SetAwake(true);
+		fixtureA->GetBody()->SetAwake(true);
+		fixtureB->GetBody()->SetAwake(true);
 	}
 
-	b2Shape::Type typeA = contact->GetFixtureA()->GetType();
-	b2Shape::Type typeB = contact->GetFixtureB()->GetType();
+	b2Shape::Type typeA = fixtureA->GetType();
+	b2Shape::Type typeB = fixtureB->GetType();
 
 	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
 	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
@@ -147,6 +152,8 @@ b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
 
 	m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
 	m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
+
+	m_tangentSpeed = 0.0f;
 }
 
 // Update the contact manifold and touching status.

src/libraries/Box2D/Dynamics/Contacts/b2Contact.h

@@ -36,7 +36,7 @@ class b2ContactListener;
 /// For example, anything slides on ice.
 inline float32 b2MixFriction(float32 friction1, float32 friction2)
 {
-	return std::sqrt(friction1 * friction2);
+	return b2Sqrt(friction1 * friction2);
 }
 
 /// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
@@ -135,6 +135,12 @@ public:
 	/// Reset the restitution to the default value.
 	void ResetRestitution();
 
+	/// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
+	void SetTangentSpeed(float32 speed);
+
+	/// Get the desired tangent speed. In meters per second.
+	float32 GetTangentSpeed() const;
+
 	/// Evaluate this contact with your own manifold and transforms.
 	virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
 
@@ -209,6 +215,8 @@ protected:
 
 	float32 m_friction;
 	float32 m_restitution;
+
+	float32 m_tangentSpeed;
 };
 
 inline b2Manifold* b2Contact::GetManifold()
@@ -328,4 +336,14 @@ inline void b2Contact::ResetRestitution()
 	m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
 }
 
+inline void b2Contact::SetTangentSpeed(float32 speed)
+{
+	m_tangentSpeed = speed;
+}
+
+inline float32 b2Contact::GetTangentSpeed() const
+{
+	return m_tangentSpeed;
+}
+
 #endif

src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.cpp

@@ -73,6 +73,7 @@ b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
 		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
 		vc->friction = contact->m_friction;
 		vc->restitution = contact->m_restitution;
+		vc->tangentSpeed = contact->m_tangentSpeed;
 		vc->indexA = bodyA->m_islandIndex;
 		vc->indexB = bodyB->m_islandIndex;
 		vc->invMassA = bodyA->m_invMass;
@@ -320,7 +321,7 @@ void b2ContactSolver::SolveVelocityConstraints()
 			b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
 
 			// Compute tangent force
-			float32 vt = b2Dot(dv, tangent);
+			float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
 			float32 lambda = vcp->tangentMass * (-vt);
 
 			// b2Clamp the accumulated force
@@ -763,8 +764,8 @@ bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiInde
 			iA = pc->invIA;
 		}
 
-		float32 mB = pc->invMassB;
-		float32 iB = pc->invIB;
+		float32 mB = 0.0f;
+		float32 iB = 0.;
 		if (indexB == toiIndexA || indexB == toiIndexB)
 		{
 			mB = pc->invMassB;

src/libraries/Box2D/Dynamics/Contacts/b2ContactSolver.h

@@ -51,6 +51,7 @@ struct b2ContactVelocityConstraint
 	float32 invIA, invIB;
 	float32 friction;
 	float32 restitution;
+	float32 tangentSpeed;
 	int32 pointCount;
 	int32 contactIndex;
 };

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp

@@ -21,7 +21,6 @@
 #include <Box2D/Dynamics/b2Fixture.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp

@@ -21,7 +21,6 @@
 #include <Box2D/Dynamics/b2Fixture.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp

@@ -21,7 +21,6 @@
 #include <Box2D/Dynamics/b2Fixture.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Contacts/b2PolygonContact.cpp

@@ -24,7 +24,6 @@
 #include <Box2D/Dynamics/b2WorldCallbacks.h>
 
 #include <new>
-using namespace std;
 
 b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {

src/libraries/Box2D/Dynamics/Joints/b2GearJoint.cpp

@@ -147,22 +147,18 @@ void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_iC = m_bodyC->m_invI;
 	m_iD = m_bodyD->m_invI;
 
-	b2Vec2 cA = data.positions[m_indexA].c;
 	float32 aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
 	float32 wA = data.velocities[m_indexA].w;
 
-	b2Vec2 cB = data.positions[m_indexB].c;
 	float32 aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
 	float32 wB = data.velocities[m_indexB].w;
 
-	b2Vec2 cC = data.positions[m_indexC].c;
 	float32 aC = data.positions[m_indexC].a;
 	b2Vec2 vC = data.velocities[m_indexC].v;
 	float32 wC = data.velocities[m_indexC].w;
 
-	b2Vec2 cD = data.positions[m_indexD].c;
 	float32 aD = data.positions[m_indexD].a;
 	b2Vec2 vD = data.velocities[m_indexD].v;
 	float32 wD = data.velocities[m_indexD].w;

src/libraries/Box2D/Dynamics/Joints/b2Joint.cpp

@@ -27,6 +27,7 @@
 #include <Box2D/Dynamics/Joints/b2WeldJoint.h>
 #include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
 #include <Box2D/Dynamics/Joints/b2RopeJoint.h>
+#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2World.h>
 #include <Box2D/Common/b2BlockAllocator.h>
@@ -42,70 +43,77 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
 	case e_distanceJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
-			joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
+			joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
 		}
 		break;
 
 	case e_mouseJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2MouseJoint));
-			joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
+			joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
 		}
 		break;
 
 	case e_prismaticJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
-			joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
+			joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
 		}
 		break;
 
 	case e_revoluteJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
-			joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
+			joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
 		}
 		break;
 
 	case e_pulleyJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
-			joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
+			joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
 		}
 		break;
 
 	case e_gearJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2GearJoint));
-			joint = new (mem) b2GearJoint((b2GearJointDef*)def);
+			joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
 		}
 		break;
 
 	case e_wheelJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2WheelJoint));
-			joint = new (mem) b2WheelJoint((b2WheelJointDef*)def);
+			joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
 		}
 		break;
 
 	case e_weldJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2WeldJoint));
-			joint = new (mem) b2WeldJoint((b2WeldJointDef*)def);
+			joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
 		}
 		break;
         
 	case e_frictionJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
-			joint = new (mem) b2FrictionJoint((b2FrictionJointDef*)def);
+			joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
 		}
 		break;
 
 	case e_ropeJoint:
 		{
 			void* mem = allocator->Allocate(sizeof(b2RopeJoint));
-			joint = new (mem) b2RopeJoint((b2RopeJointDef*)def);
+			joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));
+		}
+		break;
+
+	case e_motorJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2MotorJoint));
+			joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
 		}
 		break;
 
@@ -162,6 +170,10 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
 		allocator->Free(joint, sizeof(b2RopeJoint));
 		break;
 
+	case e_motorJoint:
+		allocator->Free(joint, sizeof(b2MotorJoint));
+		break;
+
 	default:
 		b2Assert(false);
 		break;

src/libraries/Box2D/Dynamics/Joints/b2Joint.h

@@ -38,7 +38,8 @@ enum b2JointType
 	e_wheelJoint,
     e_weldJoint,
 	e_frictionJoint,
-	e_ropeJoint
+	e_ropeJoint,
+	e_motorJoint
 };
 
 enum b2LimitState
@@ -145,6 +146,9 @@ public:
 	/// Dump this joint to the log file.
 	virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }
 
+	/// Shift the origin for any points stored in world coordinates.
+	virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin);  }
+
 protected:
 	friend class b2World;
 	friend class b2Body;

src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.cpp

@@ -0,0 +1,304 @@
+/*
+* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
+*
+* 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/b2MotorJoint.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 b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)
+{
+	bodyA = bA;
+	bodyB = bB;
+	b2Vec2 xB = bodyB->GetPosition();
+	linearOffset = bodyA->GetLocalPoint(xB);
+
+	float32 angleA = bodyA->GetAngle();
+	float32 angleB = bodyB->GetAngle();
+	angularOffset = angleB - angleA;
+}
+
+b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)
+: b2Joint(def)
+{
+	m_linearOffset = def->linearOffset;
+	m_angularOffset = def->angularOffset;
+
+	m_linearImpulse.SetZero();
+	m_angularImpulse = 0.0f;
+
+	m_maxForce = def->maxForce;
+	m_maxTorque = def->maxTorque;
+	m_correctionFactor = def->correctionFactor;
+}
+
+void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
+{
+	m_indexA = m_bodyA->m_islandIndex;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_localCenterA = m_bodyA->m_sweep.localCenter;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	m_invMassA = m_bodyA->m_invMass;
+	m_invMassB = m_bodyB->m_invMass;
+	m_invIA = m_bodyA->m_invI;
+	m_invIB = m_bodyB->m_invI;
+
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float32 wA = data.velocities[m_indexA].w;
+
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float32 wB = data.velocities[m_indexB].w;
+
+	b2Rot qA(aA), qB(aB);
+
+	// Compute the effective mass matrix.
+	m_rA = b2Mul(qA, -m_localCenterA);
+	m_rB = b2Mul(qB, -m_localCenterB);
+
+	// 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 = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
+
+	b2Mat22 K;
+	K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
+	K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
+	K.ey.x = K.ex.y;
+	K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
+
+	m_linearMass = K.GetInverse();
+
+	m_angularMass = iA + iB;
+	if (m_angularMass > 0.0f)
+	{
+		m_angularMass = 1.0f / m_angularMass;
+	}
+
+	m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
+	m_angularError = aB - aA - m_angularOffset;
+
+	if (data.step.warmStarting)
+	{
+		// Scale impulses to support a variable time step.
+		m_linearImpulse *= data.step.dtRatio;
+		m_angularImpulse *= data.step.dtRatio;
+
+		b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
+	}
+	else
+	{
+		m_linearImpulse.SetZero();
+		m_angularImpulse = 0.0f;
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float32 wA = data.velocities[m_indexA].w;
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float32 wB = data.velocities[m_indexB].w;
+
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
+
+	float32 h = data.step.dt;
+	float32 inv_h = data.step.inv_dt;
+
+	// Solve angular friction
+	{
+		float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
+		float32 impulse = -m_angularMass * Cdot;
+
+		float32 oldImpulse = m_angularImpulse;
+		float32 maxImpulse = h * 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, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
+
+		b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
+		b2Vec2 oldImpulse = m_linearImpulse;
+		m_linearImpulse += impulse;
+
+		float32 maxImpulse = h * m_maxForce;
+
+		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
+		{
+			m_linearImpulse.Normalize();
+			m_linearImpulse *= maxImpulse;
+		}
+
+		impulse = m_linearImpulse - oldImpulse;
+
+		vA -= mA * impulse;
+		wA -= iA * b2Cross(m_rA, impulse);
+
+		vB += mB * impulse;
+		wB += iB * b2Cross(m_rB, impulse);
+	}
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	B2_NOT_USED(data);
+
+	return true;
+}
+
+b2Vec2 b2MotorJoint::GetAnchorA() const
+{
+	return m_bodyA->GetPosition();
+}
+
+b2Vec2 b2MotorJoint::GetAnchorB() const
+{
+	return m_bodyB->GetPosition();
+}
+
+b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * m_linearImpulse;
+}
+
+float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_angularImpulse;
+}
+
+void b2MotorJoint::SetMaxForce(float32 force)
+{
+	b2Assert(b2IsValid(force) && force >= 0.0f);
+	m_maxForce = force;
+}
+
+float32 b2MotorJoint::GetMaxForce() const
+{
+	return m_maxForce;
+}
+
+void b2MotorJoint::SetMaxTorque(float32 torque)
+{
+	b2Assert(b2IsValid(torque) && torque >= 0.0f);
+	m_maxTorque = torque;
+}
+
+float32 b2MotorJoint::GetMaxTorque() const
+{
+	return m_maxTorque;
+}
+
+void b2MotorJoint::SetCorrectionFactor(float32 factor)
+{
+	b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
+	m_correctionFactor = factor;
+}
+
+float32 b2MotorJoint::GetCorrectionFactor() const
+{
+	return m_correctionFactor;
+}
+
+void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset)
+{
+	if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_linearOffset = linearOffset;
+	}
+}
+
+const b2Vec2& b2MotorJoint::GetLinearOffset() const
+{
+	return m_linearOffset;
+}
+
+void b2MotorJoint::SetAngularOffset(float32 angularOffset)
+{
+	if (angularOffset != m_angularOffset)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_angularOffset = angularOffset;
+	}
+}
+
+float32 b2MotorJoint::GetAngularOffset() const
+{
+	return m_angularOffset;
+}
+
+void b2MotorJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2MotorJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);
+	b2Log("  jd.angularOffset = %.15lef;\n", m_angularOffset);
+	b2Log("  jd.maxForce = %.15lef;\n", m_maxForce);
+	b2Log("  jd.maxTorque = %.15lef;\n", m_maxTorque);
+	b2Log("  jd.correctionFactor = %.15lef;\n", m_correctionFactor);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

src/libraries/Box2D/Dynamics/Joints/b2MotorJoint.h

@@ -0,0 +1,133 @@
+/*
+* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
+*
+* 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_MOTOR_JOINT_H
+#define B2_MOTOR_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Motor joint definition.
+struct b2MotorJointDef : public b2JointDef
+{
+	b2MotorJointDef()
+	{
+		type = e_motorJoint;
+		linearOffset.SetZero();
+		angularOffset = 0.0f;
+		maxForce = 1.0f;
+		maxTorque = 1.0f;
+		correctionFactor = 0.3f;
+	}
+
+	/// Initialize the bodies and offsets using the current transforms.
+	void Initialize(b2Body* bodyA, b2Body* bodyB);
+
+	/// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
+	b2Vec2 linearOffset;
+
+	/// The bodyB angle minus bodyA angle in radians.
+	float32 angularOffset;
+	
+	/// The maximum motor force in N.
+	float32 maxForce;
+
+	/// The maximum motor torque in N-m.
+	float32 maxTorque;
+
+	/// Position correction factor in the range [0,1].
+	float32 correctionFactor;
+};
+
+/// A motor joint is used to control the relative motion
+/// between two bodies. A typical usage is to control the movement
+/// of a dynamic body with respect to the ground.
+class b2MotorJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Set/get the target linear offset, in frame A, in meters.
+	void SetLinearOffset(const b2Vec2& linearOffset);
+	const b2Vec2& GetLinearOffset() const;
+
+	/// Set/get the target angular offset, in radians.
+	void SetAngularOffset(float32 angularOffset);
+	float32 GetAngularOffset() 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;
+
+	/// Set the position correction factor in the range [0,1].
+	void SetCorrectionFactor(float32 factor);
+
+	/// Get the position correction factor in the range [0,1].
+	float32 GetCorrectionFactor() const;
+
+	/// Dump to b2Log
+	void Dump();
+
+protected:
+
+	friend class b2Joint;
+
+	b2MotorJoint(const b2MotorJointDef* def);
+
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
+
+	// Solver shared
+	b2Vec2 m_linearOffset;
+	float32 m_angularOffset;
+	b2Vec2 m_linearImpulse;
+	float32 m_angularImpulse;
+	float32 m_maxForce;
+	float32 m_maxTorque;
+	float32 m_correctionFactor;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	b2Vec2 m_linearError;
+	float32 m_angularError;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	b2Mat22 m_linearMass;
+	float32 m_angularMass;
+};
+
+#endif

src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.cpp

@@ -215,3 +215,8 @@ float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
 {
 	return inv_dt * 0.0f;
 }
+
+void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	m_targetA -= newOrigin;
+}

src/libraries/Box2D/Dynamics/Joints/b2MouseJoint.h

@@ -92,6 +92,9 @@ public:
 	/// The mouse joint does not support dumping.
 	void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }
 
+	/// Implement b2Joint::ShiftOrigin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
 protected:
 	friend class b2Joint;
 

src/libraries/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp

@@ -108,7 +108,7 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
 	m_referenceAngle = def->referenceAngle;
 
 	m_impulse.SetZero();
-	m_motorMass = 0.0;
+	m_motorMass = 0.0f;
 	m_motorImpulse = 0.0f;
 
 	m_lowerTranslation = def->lowerTranslation;
@@ -349,17 +349,6 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
 
 		vB += mB * P;
 		wB += iB * LB;
-
-		b2Vec2 Cdot10 = Cdot1;
-
-		Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
-		Cdot1.y = wB - wA;
-
-		if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f)
-		{
-			b2Vec2 test = b2Mul22(m_K, df);
-			Cdot1.x += 0.0f;
-		}
 	}
 
 	data.velocities[m_indexA].v = vA;

src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.cpp

@@ -292,6 +292,21 @@ b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
 }
 
 float32 b2PulleyJoint::GetLengthA() const
+{
+	return m_lengthA;
+}
+
+float32 b2PulleyJoint::GetLengthB() const
+{
+	return m_lengthB;
+}
+
+float32 b2PulleyJoint::GetRatio() const
+{
+	return m_ratio;
+}
+
+float32 b2PulleyJoint::GetCurrentLengthA() const
 {
 	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
 	b2Vec2 s = m_groundAnchorA;
@@ -299,7 +314,7 @@ float32 b2PulleyJoint::GetLengthA() const
 	return d.Length();
 }
 
-float32 b2PulleyJoint::GetLengthB() const
+float32 b2PulleyJoint::GetCurrentLengthB() const
 {
 	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
 	b2Vec2 s = m_groundAnchorB;
@@ -307,11 +322,6 @@ float32 b2PulleyJoint::GetLengthB() const
 	return d.Length();
 }
 
-float32 b2PulleyJoint::GetRatio() const
-{
-	return m_ratio;
-}
-
 void b2PulleyJoint::Dump()
 {
 	int32 indexA = m_bodyA->m_islandIndex;
@@ -330,3 +340,9 @@ void b2PulleyJoint::Dump()
 	b2Log("  jd.ratio = %.15lef;\n", m_ratio);
 	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
 }
+
+void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	m_groundAnchorA -= newOrigin;
+	m_groundAnchorB -= newOrigin;
+}

src/libraries/Box2D/Dynamics/Joints/b2PulleyJoint.h

@@ -100,9 +100,18 @@ public:
 	/// Get the pulley ratio.
 	float32 GetRatio() const;
 
+	/// Get the current length of the segment attached to bodyA.
+	float32 GetCurrentLengthA() const;
+
+	/// Get the current length of the segment attached to bodyB.
+	float32 GetCurrentLengthB() const;
+
 	/// Dump joint to dmLog
 	void Dump();
 
+	/// Implement b2Joint::ShiftOrigin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
 protected:
 
 	friend class b2Joint;

src/libraries/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp

@@ -72,12 +72,10 @@ void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIA = m_bodyA->m_invI;
 	m_invIB = m_bodyB->m_invI;
 
-	b2Vec2 cA = data.positions[m_indexA].c;
 	float32 aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
 	float32 wA = data.velocities[m_indexA].w;
 
-	b2Vec2 cB = data.positions[m_indexB].c;
 	float32 aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
 	float32 wB = data.velocities[m_indexB].w;

src/libraries/Box2D/Dynamics/Joints/b2WeldJoint.cpp

@@ -66,12 +66,10 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 	m_invIA = m_bodyA->m_invI;
 	m_invIB = m_bodyB->m_invI;
 
-	b2Vec2 cA = data.positions[m_indexA].c;
 	float32 aA = data.positions[m_indexA].a;
 	b2Vec2 vA = data.velocities[m_indexA].v;
 	float32 wA = data.velocities[m_indexA].w;
 
-	b2Vec2 cB = data.positions[m_indexB].c;
 	float32 aB = data.positions[m_indexB].a;
 	b2Vec2 vB = data.velocities[m_indexB].v;
 	float32 wB = data.velocities[m_indexB].w;

src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.cpp

@@ -55,7 +55,7 @@ b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
 
 	m_mass = 0.0f;
 	m_impulse = 0.0f;
-	m_motorMass = 0.0;
+	m_motorMass = 0.0f;
 	m_motorImpulse = 0.0f;
 	m_springMass = 0.0f;
 	m_springImpulse = 0.0f;

src/libraries/Box2D/Dynamics/Joints/b2WheelJoint.h

@@ -79,8 +79,6 @@ struct b2WheelJointDef : public b2JointDef
 class b2WheelJoint : public b2Joint
 {
 public:
-	void GetDefinition(b2WheelJointDef* def) const;
-
 	b2Vec2 GetAnchorA() const;
 	b2Vec2 GetAnchorB() const;
 

src/libraries/Box2D/Dynamics/b2Body.cpp

@@ -141,10 +141,25 @@ void b2Body::SetType(b2BodyType type)
 	m_force.SetZero();
 	m_torque = 0.0f;
 
-	// Since the body type changed, we need to flag contacts for filtering.
+	// Delete the attached contacts.
+	b2ContactEdge* ce = m_contactList;
+	while (ce)
+	{
+		b2ContactEdge* ce0 = ce;
+		ce = ce->next;
+		m_world->m_contactManager.Destroy(ce0->contact);
+	}
+	m_contactList = NULL;
+
+	// Touch the proxies so that new contacts will be created (when appropriate)
+	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 	{
-		f->Refilter();
+		int32 proxyCount = f->m_proxyCount;
+		for (int32 i = 0; i < proxyCount; ++i)
+		{
+			broadPhase->TouchProxy(f->m_proxies[i].proxyId);
+		}
 	}
 }
 
@@ -421,8 +436,6 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle)
 	{
 		f->Synchronize(broadPhase, m_xf, m_xf);
 	}
-
-	m_world->m_contactManager.FindNewContacts();
 }
 
 void b2Body::SynchronizeFixtures()
@@ -483,6 +496,28 @@ void b2Body::SetActive(bool flag)
 	}
 }
 
+void b2Body::SetFixedRotation(bool flag)
+{
+	bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
+	if (status == flag)
+	{
+		return;
+	}
+
+	if (flag)
+	{
+		m_flags |= e_fixedRotationFlag;
+	}
+	else
+	{
+		m_flags &= ~e_fixedRotationFlag;
+	}
+
+	m_angularVelocity = 0.0f;
+
+	ResetMassData();
+}
+
 void b2Body::Dump()
 {
 	int32 bodyIndex = m_islandIndex;
@@ -511,4 +546,4 @@ void b2Body::Dump()
 		b2Log("  }\n");
 	}
 	b2Log("}\n");
-}
+}

src/libraries/Box2D/Dynamics/b2Body.h

@@ -154,8 +154,8 @@ public:
 	void DestroyFixture(b2Fixture* fixture);
 
 	/// Set the position of the body's origin and rotation.
-	/// This breaks any contacts and wakes the other bodies.
 	/// Manipulating a body's transform may cause non-physical behavior.
+	/// Note: contacts are updated on the next call to b2World::Step.
 	/// @param position the world position of the body's local origin.
 	/// @param angle the world rotation in radians.
 	void SetTransform(const b2Vec2& position, float32 angle);
@@ -184,7 +184,7 @@ public:
 
 	/// Get the linear velocity of the center of mass.
 	/// @return the linear velocity of the center of mass.
-	b2Vec2 GetLinearVelocity() const;
+	const b2Vec2& GetLinearVelocity() const;
 
 	/// Set the angular velocity.
 	/// @param omega the new angular velocity in radians/second.
@@ -199,28 +199,33 @@ public:
 	/// affect the angular velocity. This wakes up the body.
 	/// @param force the world force vector, usually in Newtons (N).
 	/// @param point the world position of the point of application.
-	void ApplyForce(const b2Vec2& force, const b2Vec2& point);
+	/// @param wake also wake up the body
+	void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake);
 
 	/// Apply a force to the center of mass. This wakes up the body.
 	/// @param force the world force vector, usually in Newtons (N).
-	void ApplyForceToCenter(const b2Vec2& force);
+	/// @param wake also wake up the body
+	void ApplyForceToCenter(const b2Vec2& force, bool wake);
 
 	/// Apply a torque. This affects the angular velocity
 	/// without affecting the linear velocity of the center of mass.
 	/// This wakes up the body.
 	/// @param torque about the z-axis (out of the screen), usually in N-m.
-	void ApplyTorque(float32 torque);
+	/// @param wake also wake up the body
+	void ApplyTorque(float32 torque, bool wake);
 
 	/// Apply an impulse at a point. This immediately modifies the velocity.
 	/// It also modifies the angular velocity if the point of application
 	/// is not at the center of mass. This wakes up the body.
 	/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
 	/// @param point the world position of the point of application.
-	void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point);
+	/// @param wake also wake up the body
+	void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake);
 
 	/// Apply an angular impulse.
 	/// @param impulse the angular impulse in units of kg*m*m/s
-	void ApplyAngularImpulse(float32 impulse);
+	/// @param wake also wake up the body
+	void ApplyAngularImpulse(float32 impulse, bool wake);
 
 	/// Get the total mass of the body.
 	/// @return the mass, usually in kilograms (kg).
@@ -315,11 +320,11 @@ public:
 
 	/// Set the sleep state of the body. A sleeping body has very
 	/// low CPU cost.
-	/// @param flag set to true to put body to sleep, false to wake it.
+	/// @param flag set to true to wake the body, false to put it to sleep.
 	void SetAwake(bool flag);
 
 	/// Get the sleeping state of this body.
-	/// @return true if the body is sleeping.
+	/// @return true if the body is awake.
 	bool IsAwake() const;
 
 	/// Set the active state of the body. An inactive body is not
@@ -387,15 +392,16 @@ private:
 	friend class b2Contact;
 	
 	friend class b2DistanceJoint;
+	friend class b2FrictionJoint;
 	friend class b2GearJoint;
-	friend class b2WheelJoint;
+	friend class b2MotorJoint;
 	friend class b2MouseJoint;
 	friend class b2PrismaticJoint;
 	friend class b2PulleyJoint;
 	friend class b2RevoluteJoint;
-	friend class b2WeldJoint;
-	friend class b2FrictionJoint;
 	friend class b2RopeJoint;
+	friend class b2WeldJoint;
+	friend class b2WheelJoint;
 
 	// m_flags
 	enum
@@ -505,7 +511,7 @@ inline void b2Body::SetLinearVelocity(const b2Vec2& v)
 	m_linearVelocity = v;
 }
 
-inline b2Vec2 b2Body::GetLinearVelocity() const
+inline const b2Vec2& b2Body::GetLinearVelocity() const
 {
 	return m_linearVelocity;
 }
@@ -655,20 +661,6 @@ inline bool b2Body::IsActive() const
 	return (m_flags & e_activeFlag) == e_activeFlag;
 }
 
-inline void b2Body::SetFixedRotation(bool flag)
-{
-	if (flag)
-	{
-		m_flags |= e_fixedRotationFlag;
-	}
-	else
-	{
-		m_flags &= ~e_fixedRotationFlag;
-	}
-
-	ResetMassData();
-}
-
 inline bool b2Body::IsFixedRotation() const
 {
 	return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
@@ -742,79 +734,101 @@ inline void* b2Body::GetUserData() const
 	return m_userData;
 }
 
-inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
+inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake)
 {
 	if (m_type != b2_dynamicBody)
 	{
 		return;
 	}
 
-	if (IsAwake() == false)
+	if (wake && (m_flags & e_awakeFlag) == 0)
 	{
 		SetAwake(true);
 	}
 
-	m_force += force;
-	m_torque += b2Cross(point - m_sweep.c, force);
+	// Don't accumulate a force if the body is sleeping.
+	if (m_flags & e_awakeFlag)
+	{
+		m_force += force;
+		m_torque += b2Cross(point - m_sweep.c, force);
+	}
 }
 
-inline void b2Body::ApplyForceToCenter(const b2Vec2& force)
+inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake)
 {
 	if (m_type != b2_dynamicBody)
 	{
 		return;
 	}
 
-	if (IsAwake() == false)
+	if (wake && (m_flags & e_awakeFlag) == 0)
 	{
 		SetAwake(true);
 	}
 
-	m_force += force;
+	// Don't accumulate a force if the body is sleeping
+	if (m_flags & e_awakeFlag)
+	{
+		m_force += force;
+	}
 }
 
-inline void b2Body::ApplyTorque(float32 torque)
+inline void b2Body::ApplyTorque(float32 torque, bool wake)
 {
 	if (m_type != b2_dynamicBody)
 	{
 		return;
 	}
 
-	if (IsAwake() == false)
+	if (wake && (m_flags & e_awakeFlag) == 0)
 	{
 		SetAwake(true);
 	}
 
-	m_torque += torque;
+	// Don't accumulate a force if the body is sleeping
+	if (m_flags & e_awakeFlag)
+	{
+		m_torque += torque;
+	}
 }
 
-inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point)
+inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake)
 {
 	if (m_type != b2_dynamicBody)
 	{
 		return;
 	}
 
-	if (IsAwake() == false)
+	if (wake && (m_flags & e_awakeFlag) == 0)
 	{
 		SetAwake(true);
 	}
-	m_linearVelocity += m_invMass * impulse;
-	m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
+
+	// Don't accumulate velocity if the body is sleeping
+	if (m_flags & e_awakeFlag)
+	{
+		m_linearVelocity += m_invMass * impulse;
+		m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
+	}
 }
 
-inline void b2Body::ApplyAngularImpulse(float32 impulse)
+inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake)
 {
 	if (m_type != b2_dynamicBody)
 	{
 		return;
 	}
 
-	if (IsAwake() == false)
+	if (wake && (m_flags & e_awakeFlag) == 0)
 	{
 		SetAwake(true);
 	}
-	m_angularVelocity += m_invI * impulse;
+
+	// Don't accumulate velocity if the body is sleeping
+	if (m_flags & e_awakeFlag)
+	{
+		m_angularVelocity += m_invI * impulse;
+	}
 }
 
 inline void b2Body::SynchronizeTransform()

src/libraries/Box2D/Dynamics/b2ContactManager.cpp

@@ -286,8 +286,11 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	bodyB->m_contactList = &c->m_nodeB;
 
 	// Wake up the bodies
-	bodyA->SetAwake(true);
-	bodyB->SetAwake(true);
+	if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)
+	{
+		bodyA->SetAwake(true);
+		bodyB->SetAwake(true);
+	}
 
 	++m_contactCount;
 }

src/libraries/Box2D/Dynamics/b2Fixture.cpp

@@ -267,11 +267,11 @@ void b2Fixture::Dump(int32 bodyIndex)
 			b2PolygonShape* s = (b2PolygonShape*)m_shape;
 			b2Log("    b2PolygonShape shape;\n");
 			b2Log("    b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
-			for (int32 i = 0; i < s->m_vertexCount; ++i)
+			for (int32 i = 0; i < s->m_count; ++i)
 			{
 				b2Log("    vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
 			}
-			b2Log("    shape.Set(vs, %d);\n", s->m_vertexCount);
+			b2Log("    shape.Set(vs, %d);\n", s->m_count);
 		}
 		break;
 

src/libraries/Box2D/Dynamics/b2Island.cpp

@@ -211,10 +211,10 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
 			// Solution: v(t) = v0 * exp(-c * t)
 			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
 			// v2 = exp(-c * dt) * v1
-			// Taylor expansion:
-			// v2 = (1.0f - c * dt) * v1
-			v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f);
-			w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f);
+			// Pade approximation:
+			// v2 = v1 * 1 / (1 + c * dt)
+			v *= 1.0f / (1.0f + h * b->m_linearDamping);
+			w *= 1.0f / (1.0f + h * b->m_angularDamping);
 		}
 
 		m_positions[i].c = c;

src/libraries/Box2D/Dynamics/b2World.cpp

@@ -1073,7 +1073,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 	case b2Shape::e_polygon:
 		{
 			b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
-			int32 vertexCount = poly->m_vertexCount;
+			int32 vertexCount = poly->m_count;
 			b2Assert(vertexCount <= b2_maxPolygonVertices);
 			b2Vec2 vertices[b2_maxPolygonVertices];
 
@@ -1256,6 +1256,29 @@ float32 b2World::GetTreeQuality() const
 	return m_contactManager.m_broadPhase.GetTreeQuality();
 }
 
+void b2World::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	b2Assert((m_flags & e_locked) == 0);
+	if ((m_flags & e_locked) == e_locked)
+	{
+		return;
+	}
+
+	for (b2Body* b = m_bodyList; b; b = b->m_next)
+	{
+		b->m_xf.p -= newOrigin;
+		b->m_sweep.c0 -= newOrigin;
+		b->m_sweep.c -= newOrigin;
+	}
+
+	for (b2Joint* j = m_jointList; j; j = j->m_next)
+	{
+		j->ShiftOrigin(newOrigin);
+	}
+
+	m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
+}
+
 void b2World::Dump()
 {
 	if ((m_flags & e_locked) == e_locked)

src/libraries/Box2D/Dynamics/b2World.h

@@ -104,7 +104,7 @@ public:
 	/// @see SetAutoClearForces
 	void ClearForces();
 
-	/// Call this to draw shapes and other debug draw data.
+	/// Call this to draw shapes and other debug draw data. This is intentionally non-const.
 	void DrawDebugData();
 
 	/// Query the world for all fixtures that potentially overlap the
@@ -194,6 +194,11 @@ public:
 	/// Get the flag that controls automatic clearing of forces after each time step.
 	bool GetAutoClearForces() const;
 
+	/// Shift the world origin. Useful for large worlds.
+	/// The body shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
 	/// Get the contact manager for testing.
 	const b2ContactManager& GetContactManager() const;