Updated box2d from 2.2.1 to 2.3.0

--HG--
branch : box2d-2.3

src/libraries/Box2D/Box2D.h

@@ -56,12 +56,13 @@ For discussion please visit http://box2d.org/forum
 #include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
 #include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
 #include <Box2D/Dynamics/Joints/b2GearJoint.h>
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
+#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
 #include <Box2D/Dynamics/Joints/b2MouseJoint.h>
 #include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
 #include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
 #include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
 #include <Box2D/Dynamics/Joints/b2RopeJoint.h>
 #include <Box2D/Dynamics/Joints/b2WeldJoint.h>
+#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
 
 #endif

src/libraries/Box2D/Collision/Shapes/b2ChainShape.cpp

@@ -19,8 +19,7 @@
 #include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 #include <new>
-#include <cstring>
-using namespace std;
+#include <memory.h>
 
 b2ChainShape::~b2ChainShape()
 {
@@ -33,6 +32,14 @@ void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
 {
 	b2Assert(m_vertices == NULL && m_count == 0);
 	b2Assert(count >= 3);
+	for (int32 i = 1; i < count; ++i)
+	{
+		b2Vec2 v1 = vertices[i-1];
+		b2Vec2 v2 = vertices[i];
+		// If the code crashes here, it means your vertices are too close together.
+		b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
+	}
+
 	m_count = count + 1;
 	m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
 	memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
@@ -47,11 +54,23 @@ void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
 {
 	b2Assert(m_vertices == NULL && m_count == 0);
 	b2Assert(count >= 2);
+	for (int32 i = 1; i < count; ++i)
+	{
+		b2Vec2 v1 = vertices[i-1];
+		b2Vec2 v2 = vertices[i];
+		// If the code crashes here, it means your vertices are too close together.
+		b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
+	}
+
 	m_count = count;
 	m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
 	memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
+
 	m_hasPrevVertex = false;
 	m_hasNextVertex = false;
+
+	m_prevVertex.SetZero();
+	m_nextVertex.SetZero();
 }
 
 void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)

src/libraries/Box2D/Collision/Shapes/b2ChainShape.h

@@ -14,9 +14,6 @@
 * 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.
-*
-*
-* THIS FILE HAS BEEN MODIFIED BY THE LÖVE DEVELOPMENT TEAM
 */
 
 #ifndef B2_CHAIN_SHAPE_H

src/libraries/Box2D/Collision/Shapes/b2CircleShape.cpp

@@ -18,7 +18,6 @@
 
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <new>
-using namespace std;
 
 b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
 {

src/libraries/Box2D/Collision/Shapes/b2EdgeShape.cpp

@@ -18,7 +18,6 @@
 
 #include <Box2D/Collision/Shapes/b2EdgeShape.h>
 #include <new>
-using namespace std;
 
 void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
 {
@@ -105,11 +104,11 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
 	output->fraction = t;
 	if (numerator > 0.0f)
 	{
-		output->normal = -normal;
+		output->normal = -b2Mul(xf.q, normal);
 	}
 	else
 	{
-		output->normal = normal;
+		output->normal = b2Mul(xf.q, normal);
 	}
 	return true;
 }

src/libraries/Box2D/Collision/Shapes/b2PolygonShape.cpp

@@ -29,7 +29,7 @@ b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
 
 void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
 {
-	m_vertexCount = 4;
+	m_count = 4;
 	m_vertices[0].Set(-hx, -hy);
 	m_vertices[1].Set( hx, -hy);
 	m_vertices[2].Set( hx,  hy);
@@ -43,7 +43,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
 
 void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
 {
-	m_vertexCount = 4;
+	m_count = 4;
 	m_vertices[0].Set(-hx, -hy);
 	m_vertices[1].Set( hx, -hy);
 	m_vertices[2].Set( hx,  hy);
@@ -59,7 +59,7 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa
 	xf.q.Set(angle);
 
 	// Transform vertices and normals.
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
 		m_vertices[i] = b2Mul(xf, m_vertices[i]);
 		m_normals[i] = b2Mul(xf.q, m_normals[i]);
@@ -120,61 +120,131 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 {
 	b2Assert(3 <= count && count <= b2_maxPolygonVertices);
-	m_vertexCount = count;
+	if (count < 3)
+	{
+		SetAsBox(1.0f, 1.0f);
+		return;
+	}
+	
+	int32 n = b2Min(count, b2_maxPolygonVertices);
+
+	// Perform welding and copy vertices into local buffer.
+	b2Vec2 ps[b2_maxPolygonVertices];
+	int32 tempCount = 0;
+	for (int32 i = 0; i < n; ++i)
+	{
+		b2Vec2 v = vertices[i];
+
+		bool unique = true;
+		for (int32 j = 0; j < tempCount; ++j)
+		{
+			if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
+			{
+				unique = false;
+				break;
+			}
+		}
+
+		if (unique)
+		{
+			ps[tempCount++] = v;
+		}
+	}
+
+	n = tempCount;
+	if (n < 3)
+	{
+		// Polygon is degenerate.
+		b2Assert(false);
+		SetAsBox(1.0f, 1.0f);
+		return;
+	}
+
+	// Create the convex hull using the Gift wrapping algorithm
+	// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
+
+	// Find the right most point on the hull
+	int32 i0 = 0;
+	float32 x0 = ps[0].x;
+	for (int32 i = 1; i < n; ++i)
+	{
+		float32 x = ps[i].x;
+		if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
+		{
+			i0 = i;
+			x0 = x;
+		}
+	}
+
+	int32 hull[b2_maxPolygonVertices];
+	int32 m = 0;
+	int32 ih = i0;
+
+	for (;;)
+	{
+		hull[m] = ih;
+
+		int32 ie = 0;
+		for (int32 j = 1; j < n; ++j)
+		{
+			if (ie == ih)
+			{
+				ie = j;
+				continue;
+			}
+
+			b2Vec2 r = ps[ie] - ps[hull[m]];
+			b2Vec2 v = ps[j] - ps[hull[m]];
+			float32 c = b2Cross(r, v);
+			if (c < 0.0f)
+			{
+				ie = j;
+			}
+
+			// Collinearity check
+			if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
+			{
+				ie = j;
+			}
+		}
+
+		++m;
+		ih = ie;
+
+		if (ie == i0)
+		{
+			break;
+		}
+	}
+	
+	m_count = m;
 
 	// Copy vertices.
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m; ++i)
 	{
-		m_vertices[i] = vertices[i];
+		m_vertices[i] = ps[hull[i]];
 	}
 
 	// Compute normals. Ensure the edges have non-zero length.
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m; ++i)
 	{
 		int32 i1 = i;
-		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+		int32 i2 = i + 1 < m ? i + 1 : 0;
 		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
 		b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
 		m_normals[i] = b2Cross(edge, 1.0f);
 		m_normals[i].Normalize();
 	}
 
-#ifdef _DEBUG
-	// Ensure the polygon is convex and the interior
-	// is to the left of each edge.
-	for (int32 i = 0; i < m_vertexCount; ++i)
-	{
-		int32 i1 = i;
-		int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
-		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
-
-		for (int32 j = 0; j < m_vertexCount; ++j)
-		{
-			// Don't check vertices on the current edge.
-			if (j == i1 || j == i2)
-			{
-				continue;
-			}
-			
-			b2Vec2 r = m_vertices[j] - m_vertices[i1];
-
-			// If this crashes, your polygon is non-convex, has colinear edges,
-			// or the winding order is wrong.
-			float32 s = b2Cross(edge, r);
-			b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order");
-		}
-	}
-#endif
-
 	// Compute the polygon centroid.
-	m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
+	m_centroid = ComputeCentroid(m_vertices, m);
 }
 
 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
 {
 	b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
 
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
 		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
 		if (dot > 0.0f)
@@ -200,7 +270,7 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu
 
 	int32 index = -1;
 
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
 		// p = p1 + a * d
 		// dot(normal, p - v) = 0
@@ -265,7 +335,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil
 	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
 	b2Vec2 upper = lower;
 
-	for (int32 i = 1; i < m_vertexCount; ++i)
+	for (int32 i = 1; i < m_count; ++i)
 	{
 		b2Vec2 v = b2Mul(xf, m_vertices[i]);
 		lower = b2Min(lower, v);
@@ -303,7 +373,7 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 	//
 	// The rest of the derivation is handled by computer algebra.
 
-	b2Assert(m_vertexCount >= 3);
+	b2Assert(m_count >= 3);
 
 	b2Vec2 center; center.Set(0.0f, 0.0f);
 	float32 area = 0.0f;
@@ -314,19 +384,19 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 	b2Vec2 s(0.0f, 0.0f);
 
 	// This code would put the reference point inside the polygon.
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
 		s += m_vertices[i];
 	}
-	s *= 1.0f / m_vertexCount;
+	s *= 1.0f / m_count;
 
 	const float32 k_inv3 = 1.0f / 3.0f;
 
-	for (int32 i = 0; i < m_vertexCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
 		// Triangle vertices.
 		b2Vec2 e1 = m_vertices[i] - s;
-		b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s;
+		b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
 
 		float32 D = b2Cross(e1, e2);
 
@@ -359,3 +429,31 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 	// Shift to center of mass then to original body origin.
 	massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
 }
+
+bool b2PolygonShape::Validate() const
+{
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		int32 i1 = i;
+		int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
+		b2Vec2 p = m_vertices[i1];
+		b2Vec2 e = m_vertices[i2] - p;
+
+		for (int32 j = 0; j < m_count; ++j)
+		{
+			if (j == i1 || j == i2)
+			{
+				continue;
+			}
+
+			b2Vec2 v = m_vertices[j] - p;
+			float32 c = b2Cross(e, v);
+			if (c < 0.0f)
+			{
+				return false;
+			}
+		}
+	}
+
+	return true;
+}

src/libraries/Box2D/Collision/Shapes/b2PolygonShape.h

@@ -36,12 +36,14 @@ public:
 	/// @see b2Shape::GetChildCount
 	int32 GetChildCount() const;
 
-	/// Copy vertices. This assumes the vertices define a convex polygon.
-	/// It is assumed that the exterior is the the right of each edge.
+	/// Create a convex hull from the given array of local points.
 	/// The count must be in the range [3, b2_maxPolygonVertices].
-	void Set(const b2Vec2* vertices, int32 vertexCount);
+	/// @warning the points may be re-ordered, even if they form a convex polygon
+	/// @warning collinear points are handled but not removed. Collinear points
+	/// may lead to poor stacking behavior.
+	void Set(const b2Vec2* points, int32 count);
 
-	/// Build vertices to represent an axis-aligned box.
+	/// Build vertices to represent an axis-aligned box centered on the local origin.
 	/// @param hx the half-width.
 	/// @param hy the half-height.
 	void SetAsBox(float32 hx, float32 hy);
@@ -67,28 +69,32 @@ public:
 	void ComputeMass(b2MassData* massData, float32 density) const;
 
 	/// Get the vertex count.
-	int32 GetVertexCount() const { return m_vertexCount; }
+	int32 GetVertexCount() const { return m_count; }
 
 	/// Get a vertex by index.
 	const b2Vec2& GetVertex(int32 index) const;
 
+	/// Validate convexity. This is a very time consuming operation.
+	/// @returns true if valid
+	bool Validate() const;
+
 	b2Vec2 m_centroid;
 	b2Vec2 m_vertices[b2_maxPolygonVertices];
 	b2Vec2 m_normals[b2_maxPolygonVertices];
-	int32 m_vertexCount;
+	int32 m_count;
 };
 
 inline b2PolygonShape::b2PolygonShape()
 {
 	m_type = e_polygon;
 	m_radius = b2_polygonRadius;
-	m_vertexCount = 0;
+	m_count = 0;
 	m_centroid.SetZero();
 }
 
 inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
 {
-	b2Assert(0 <= index && index < m_vertexCount);
+	b2Assert(0 <= index && index < m_count);
 	return m_vertices[index];
 }
 

src/libraries/Box2D/Collision/b2BroadPhase.cpp

@@ -17,8 +17,7 @@
 */
 
 #include <Box2D/Collision/b2BroadPhase.h>
-#include <cstring>
-using namespace std;
+#include <memory.h>
 
 b2BroadPhase::b2BroadPhase()
 {
@@ -90,7 +89,6 @@ void b2BroadPhase::UnBufferMove(int32 proxyId)
 		if (m_moveBuffer[i] == proxyId)
 		{
 			m_moveBuffer[i] = e_nullProxy;
-			return;
 		}
 	}
 }

src/libraries/Box2D/Collision/b2BroadPhase.h

@@ -28,7 +28,6 @@ struct b2Pair
 {
 	int32 proxyIdA;
 	int32 proxyIdB;
-	int32 next;
 };
 
 /// The broad-phase is used for computing pairs and performing volume queries and ray casts.
@@ -100,6 +99,11 @@ public:
 	/// Get the quality metric of the embedded tree.
 	float32 GetTreeQuality() const;
 
+	/// Shift the world origin. Useful for large worlds.
+	/// The shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
 private:
 
 	friend class b2DynamicTree;
@@ -245,4 +249,9 @@ inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) cons
 	m_tree.RayCast(callback, input);
 }
 
+inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	m_tree.ShiftOrigin(newOrigin);
+}
+
 #endif

src/libraries/Box2D/Collision/b2CollideCircle.cpp

@@ -63,7 +63,7 @@ void b2CollidePolygonAndCircle(
 	int32 normalIndex = 0;
 	float32 separation = -b2_maxFloat;
 	float32 radius = polygonA->m_radius + circleB->m_radius;
-	int32 vertexCount = polygonA->m_vertexCount;
+	int32 vertexCount = polygonA->m_count;
 	const b2Vec2* vertices = polygonA->m_vertices;
 	const b2Vec2* normals = polygonA->m_normals;
 

src/libraries/Box2D/Collision/b2CollideEdge.cpp

@@ -425,8 +425,8 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const
 	}
 	
 	// Get polygonB in frameA
-	m_polygonB.count = polygonB->m_vertexCount;
-	for (int32 i = 0; i < polygonB->m_vertexCount; ++i)
+	m_polygonB.count = polygonB->m_count;
+	for (int32 i = 0; i < polygonB->m_count; ++i)
 	{
 		m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
 		m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
@@ -497,13 +497,13 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const
 		
 		ie[0].v = m_polygonB.vertices[i1];
 		ie[0].id.cf.indexA = 0;
-		ie[0].id.cf.indexB = i1;
+		ie[0].id.cf.indexB = static_cast<uint8>(i1);
 		ie[0].id.cf.typeA = b2ContactFeature::e_face;
 		ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
 		
 		ie[1].v = m_polygonB.vertices[i2];
 		ie[1].id.cf.indexA = 0;
-		ie[1].id.cf.indexB = i2;
+		ie[1].id.cf.indexB = static_cast<uint8>(i2);
 		ie[1].id.cf.typeA = b2ContactFeature::e_face;
 		ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
 		
@@ -530,13 +530,13 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const
 		
 		ie[0].v = m_v1;
 		ie[0].id.cf.indexA = 0;
-		ie[0].id.cf.indexB = primaryAxis.index;
+		ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
 		ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
 		ie[0].id.cf.typeB = b2ContactFeature::e_face;
 		
 		ie[1].v = m_v2;
 		ie[1].id.cf.indexA = 0;
-		ie[1].id.cf.indexB = primaryAxis.index;		
+		ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);		
 		ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
 		ie[1].id.cf.typeB = b2ContactFeature::e_face;
 		

src/libraries/Box2D/Collision/b2CollidePolygon.cpp

@@ -19,123 +19,46 @@
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
-// Find the separation between poly1 and poly2 for a give edge normal on poly1.
-static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
-							  const b2PolygonShape* poly2, const b2Transform& xf2)
-{
-	const b2Vec2* vertices1 = poly1->m_vertices;
-	const b2Vec2* normals1 = poly1->m_normals;
-
-	int32 count2 = poly2->m_vertexCount;
-	const b2Vec2* vertices2 = poly2->m_vertices;
-
-	b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
-
-	// Convert normal from poly1's frame into poly2's frame.
-	b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]);
-	b2Vec2 normal1 = b2MulT(xf2.q, normal1World);
-
-	// Find support vertex on poly2 for -normal.
-	int32 index = 0;
-	float32 minDot = b2_maxFloat;
-
-	for (int32 i = 0; i < count2; ++i)
-	{
-		float32 dot = b2Dot(vertices2[i], normal1);
-		if (dot < minDot)
-		{
-			minDot = dot;
-			index = i;
-		}
-	}
-
-	b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
-	b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
-	float32 separation = b2Dot(v2 - v1, normal1World);
-	return separation;
-}
-
 // Find the max separation between poly1 and poly2 using edge normals from poly1.
 static float32 b2FindMaxSeparation(int32* edgeIndex,
 								 const b2PolygonShape* poly1, const b2Transform& xf1,
 								 const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->m_vertexCount;
-	const b2Vec2* normals1 = poly1->m_normals;
+	int32 count1 = poly1->m_count;
+	int32 count2 = poly2->m_count;
+	const b2Vec2* n1s = poly1->m_normals;
+	const b2Vec2* v1s = poly1->m_vertices;
+	const b2Vec2* v2s = poly2->m_vertices;
+	b2Transform xf = b2MulT(xf2, xf1);
 
-	// Vector pointing from the centroid of poly1 to the centroid of poly2.
-	b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
-	b2Vec2 dLocal1 = b2MulT(xf1.q, d);
-
-	// Find edge normal on poly1 that has the largest projection onto d.
-	int32 edge = 0;
-	float32 maxDot = -b2_maxFloat;
+	int32 bestIndex = 0;
+	float32 maxSeparation = -b2_maxFloat;
 	for (int32 i = 0; i < count1; ++i)
 	{
-		float32 dot = b2Dot(normals1[i], dLocal1);
-		if (dot > maxDot)
+		// Get poly1 normal in frame2.
+		b2Vec2 n = b2Mul(xf.q, n1s[i]);
+		b2Vec2 v1 = b2Mul(xf, v1s[i]);
+
+		// Find deepest point for normal i.
+		float32 si = b2_maxFloat;
+		for (int32 j = 0; j < count2; ++j)
 		{
-			maxDot = dot;
-			edge = i;
+			float32 sij = b2Dot(n, v2s[j] - v1);
+			if (sij < si)
+			{
+				si = sij;
+			}
+		}
+
+		if (si > maxSeparation)
+		{
+			maxSeparation = si;
+			bestIndex = i;
 		}
 	}
 
-	// Get the separation for the edge normal.
-	float32 s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
-
-	// Check the separation for the previous edge normal.
-	int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
-	float32 sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
-
-	// Check the separation for the next edge normal.
-	int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
-	float32 sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
-
-	// Find the best edge and the search direction.
-	int32 bestEdge;
-	float32 bestSeparation;
-	int32 increment;
-	if (sPrev > s && sPrev > sNext)
-	{
-		increment = -1;
-		bestEdge = prevEdge;
-		bestSeparation = sPrev;
-	}
-	else if (sNext > s)
-	{
-		increment = 1;
-		bestEdge = nextEdge;
-		bestSeparation = sNext;
-	}
-	else
-	{
-		*edgeIndex = edge;
-		return s;
-	}
-
-	// Perform a local search for the best edge normal.
-	for ( ; ; )
-	{
-		if (increment == -1)
-			edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
-		else
-			edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
-
-		s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
-
-		if (s > bestSeparation)
-		{
-			bestEdge = edge;
-			bestSeparation = s;
-		}
-		else
-		{
-			break;
-		}
-	}
-
-	*edgeIndex = bestEdge;
-	return bestSeparation;
+	*edgeIndex = bestIndex;
+	return maxSeparation;
 }
 
 static void b2FindIncidentEdge(b2ClipVertex c[2],
@@ -144,11 +67,11 @@ static void b2FindIncidentEdge(b2ClipVertex c[2],
 {
 	const b2Vec2* normals1 = poly1->m_normals;
 
-	int32 count2 = poly2->m_vertexCount;
+	int32 count2 = poly2->m_count;
 	const b2Vec2* vertices2 = poly2->m_vertices;
 	const b2Vec2* normals2 = poly2->m_normals;
 
-	b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
+	b2Assert(0 <= edge1 && edge1 < poly1->m_count);
 
 	// Get the normal of the reference edge in poly2's frame.
 	b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
@@ -210,12 +133,11 @@ void b2CollidePolygons(b2Manifold* manifold,
 	const b2PolygonShape* poly1;	// reference polygon
 	const b2PolygonShape* poly2;	// incident polygon
 	b2Transform xf1, xf2;
-	int32 edge1;		// reference edge
+	int32 edge1;					// reference edge
 	uint8 flip;
-	const float32 k_relativeTol = 0.98f;
-	const float32 k_absoluteTol = 0.001f;
+	const float32 k_tol = 0.1f * b2_linearSlop;
 
-	if (separationB > k_relativeTol * separationA + k_absoluteTol)
+	if (separationB > separationA + k_tol)
 	{
 		poly1 = polyB;
 		poly2 = polyA;
@@ -239,7 +161,7 @@ void b2CollidePolygons(b2Manifold* manifold,
 	b2ClipVertex incidentEdge[2];
 	b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
 
-	int32 count1 = poly1->m_vertexCount;
+	int32 count1 = poly1->m_count;
 	const b2Vec2* vertices1 = poly1->m_vertices;
 
 	int32 iv1 = edge1;

src/libraries/Box2D/Collision/b2Collision.cpp

@@ -44,6 +44,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
 			b2Vec2 cA = pointA + radiusA * normal;
 			b2Vec2 cB = pointB - radiusB * normal;
 			points[0] = 0.5f * (cA + cB);
+			separations[0] = b2Dot(cB - cA, normal);
 		}
 		break;
 
@@ -58,6 +59,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
 				b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
 				b2Vec2 cB = clipPoint - radiusB * normal;
 				points[i] = 0.5f * (cA + cB);
+				separations[i] = b2Dot(cB - cA, normal);
 			}
 		}
 		break;
@@ -73,6 +75,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
 				b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
 				b2Vec2 cA = clipPoint - radiusA * normal;
 				points[i] = 0.5f * (cA + cB);
+				separations[i] = b2Dot(cA - cB, normal);
 			}
 
 			// Ensure normal points from A to B.
@@ -217,7 +220,7 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
 		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
 
 		// VertexA is hitting edgeB.
-		vOut[numOut].id.cf.indexA = vertexIndexA;
+		vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
 		vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
 		vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
 		vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;

src/libraries/Box2D/Collision/b2Collision.h

@@ -20,7 +20,7 @@
 #define B2_COLLISION_H
 
 #include <Box2D/Common/b2Math.h>
-#include <climits>
+#include <limits.h>
 
 /// @file
 /// Structures and functions used for computing contact points, distance
@@ -117,8 +117,9 @@ struct b2WorldManifold
 					const b2Transform& xfA, float32 radiusA,
 					const b2Transform& xfB, float32 radiusB);
 
-	b2Vec2 normal;							///< world vector pointing from A to B
-	b2Vec2 points[b2_maxManifoldPoints];	///< world contact point (point of intersection)
+	b2Vec2 normal;								///< world vector pointing from A to B
+	b2Vec2 points[b2_maxManifoldPoints];		///< world contact point (point of intersection)
+	float32 separations[b2_maxManifoldPoints];	///< a negative value indicates overlap, in meters
 };
 
 /// This is used for determining the state of contact points.

src/libraries/Box2D/Collision/b2Distance.cpp

@@ -31,7 +31,7 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 	{
 	case b2Shape::e_circle:
 		{
-			const b2CircleShape* circle = (b2CircleShape*)shape;
+			const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
 			m_vertices = &circle->m_p;
 			m_count = 1;
 			m_radius = circle->m_radius;
@@ -40,16 +40,16 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 
 	case b2Shape::e_polygon:
 		{
-			const b2PolygonShape* polygon = (b2PolygonShape*)shape;
+			const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
 			m_vertices = polygon->m_vertices;
-			m_count = polygon->m_vertexCount;
+			m_count = polygon->m_count;
 			m_radius = polygon->m_radius;
 		}
 		break;
 
 	case b2Shape::e_chain:
 		{
-			const b2ChainShape* chain = (b2ChainShape*)shape;
+			const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
 			b2Assert(0 <= index && index < chain->m_count);
 
 			m_buffer[0] = chain->m_vertices[index];
@@ -70,7 +70,7 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 
 	case b2Shape::e_edge:
 		{
-			const b2EdgeShape* edge = (b2EdgeShape*)shape;
+			const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
 			m_vertices = &edge->m_vertex1;
 			m_count = 2;
 			m_radius = edge->m_radius;
@@ -141,6 +141,7 @@ struct b2Simplex
 			v->wA = b2Mul(transformA, wALocal);
 			v->wB = b2Mul(transformB, wBLocal);
 			v->w = v->wB - v->wA;
+			v->a = 1.0f;
 			m_count = 1;
 		}
 	}
@@ -244,7 +245,7 @@ struct b2Simplex
 		{
 		case 0:
 			b2Assert(false);
-			return 0.0;
+			return 0.0f;
 
 		case 1:
 			return 0.0f;
@@ -465,8 +466,7 @@ void b2Distance(b2DistanceOutput* output,
 	int32 saveA[3], saveB[3];
 	int32 saveCount = 0;
 
-	b2Vec2 closestPoint = simplex.GetClosestPoint();
-	float32 distanceSqr1 = closestPoint.LengthSquared();
+	float32 distanceSqr1 = b2_maxFloat;
 	float32 distanceSqr2 = distanceSqr1;
 
 	// Main iteration loop.

src/libraries/Box2D/Collision/b2DynamicTree.cpp

@@ -17,10 +17,7 @@
 */
 
 #include <Box2D/Collision/b2DynamicTree.h>
-#include <cstring>
-#include <cfloat>
-using namespace std;
-
+#include <memory.h>
 
 b2DynamicTree::b2DynamicTree()
 {
@@ -769,3 +766,13 @@ void b2DynamicTree::RebuildBottomUp()
 
 	Validate();
 }
+
+void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
+{
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		m_nodes[i].aabb.lowerBound -= newOrigin;
+		m_nodes[i].aabb.upperBound -= newOrigin;
+	}
+}

src/libraries/Box2D/Collision/b2DynamicTree.h

@@ -118,6 +118,11 @@ public:
 	/// Build an optimal tree. Very expensive. For testing.
 	void RebuildBottomUp();
 
+	/// Shift the world origin. Useful for large worlds.
+	/// The shift formula is: position -= newOrigin
+	/// @param newOrigin the new origin with respect to the old origin
+	void ShiftOrigin(const b2Vec2& newOrigin);
+
 private:
 
 	int32 AllocateNode();

src/libraries/Box2D/Collision/b2TimeOfImpact.cpp

@@ -21,13 +21,15 @@
 #include <Box2D/Collision/b2TimeOfImpact.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
+#include <Box2D/Common/b2Timer.h>
 
-#include <cstdio>
-using namespace std;
+#include <stdio.h>
 
+float32 b2_toiTime, b2_toiMaxTime;
 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
 int32 b2_toiRootIters, b2_toiMaxRootIters;
 
+//
 struct b2SeparationFunction
 {
 	enum Type
@@ -119,6 +121,7 @@ struct b2SeparationFunction
 		}
 	}
 
+	//
 	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
 	{
 		b2Transform xfA, xfB;
@@ -187,6 +190,7 @@ struct b2SeparationFunction
 		}
 	}
 
+	//
 	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
 	{
 		b2Transform xfA, xfB;
@@ -197,9 +201,6 @@ struct b2SeparationFunction
 		{
 		case e_points:
 			{
-				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
-				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
-
 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
 
@@ -215,8 +216,6 @@ struct b2SeparationFunction
 				b2Vec2 normal = b2Mul(xfA.q, m_axis);
 				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
 
-				b2Vec2 axisB = b2MulT(xfB.q, -normal);
-
 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
 
@@ -229,8 +228,6 @@ struct b2SeparationFunction
 				b2Vec2 normal = b2Mul(xfB.q, m_axis);
 				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
 
-				b2Vec2 axisA = b2MulT(xfA.q, -normal);
-
 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
 
@@ -256,6 +253,8 @@ struct b2SeparationFunction
 // by computing the largest time at which separation is maintained.
 void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 {
+	b2Timer timer;
+
 	++b2_toiCalls;
 
 	output->state = b2TOIOutput::e_unknown;
@@ -422,6 +421,9 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 					t = 0.5f * (a1 + a2);
 				}
 
+				++rootIterCount;
+				++b2_toiRootIters;
+
 				float32 s = fcn.Evaluate(indexA, indexB, t);
 
 				if (b2Abs(s - target) < tolerance)
@@ -442,10 +444,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 					a2 = t;
 					s2 = s;
 				}
-
-				++rootIterCount;
-				++b2_toiRootIters;
-
+				
 				if (rootIterCount == 50)
 				{
 					break;
@@ -480,4 +479,8 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 	}
 
 	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
+
+	float32 time = timer.GetMilliseconds();
+	b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
+	b2_toiTime += time;
 }

src/libraries/Box2D/Common/b2BlockAllocator.cpp

@@ -17,11 +17,9 @@
 */
 
 #include <Box2D/Common/b2BlockAllocator.h>
-#include <cstdlib>
-#include <climits>
-#include <cstring>
-#include <memory>
-using namespace std;
+#include <limits.h>
+#include <memory.h>
+#include <stddef.h>
 
 int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
 {

src/libraries/Box2D/Common/b2Draw.h

@@ -16,6 +16,9 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
+#ifndef B2_DRAW_H
+#define B2_DRAW_H
+
 #include <Box2D/Common/b2Math.h>
 
 /// Color for debug drawing. Each value has the range [0,1].
@@ -79,3 +82,5 @@ public:
 protected:
 	uint32 m_drawFlags;
 };
+
+#endif

src/libraries/Box2D/Common/b2GrowableStack.h

@@ -19,7 +19,7 @@
 #ifndef B2_GROWABLE_STACK_H
 #define B2_GROWABLE_STACK_H
 #include <Box2D/Common/b2Settings.h>
-#include <cstring>
+#include <memory.h>
 
 /// This is a growable LIFO stack with an initial capacity of N.
 /// If the stack size exceeds the initial capacity, the heap is used
@@ -51,7 +51,7 @@ public:
 			T* old = m_stack;
 			m_capacity *= 2;
 			m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
-			std::memcpy(m_stack, old, m_count * sizeof(T));
+			memcpy(m_stack, old, m_count * sizeof(T));
 			if (old != m_array)
 			{
 				b2Free(old);

src/libraries/Box2D/Common/b2Math.h

@@ -20,24 +20,14 @@
 #define B2_MATH_H
 
 #include <Box2D/Common/b2Settings.h>
+#include <math.h>
+#include <float.h>
 
-#include <cmath>
-#include <cfloat>
-#include <cstddef>
-#include <limits>
-
-/// This function is used to ensure that a floating point number is
-/// not a NaN or infinity.
+/// This function is used to ensure that a floating point number is not a NaN or infinity.
 inline bool b2IsValid(float32 x)
 {
-	if (x != x)
-	{
-		// NaN.
-		return false;
-	}
-
-	float32 infinity = std::numeric_limits<float32>::infinity();
-	return -infinity < x && x < infinity;
+	int32 ix = *reinterpret_cast<int32*>(&x);
+	return (ix & 0x7f800000) != 0x7f800000;
 }
 
 /// This is a approximate yet fast inverse square-root.
@@ -57,8 +47,8 @@ inline float32 b2InvSqrt(float32 x)
 	return x;
 }
 
-#define	b2Sqrt(x)	std::sqrt(x)
-#define	b2Atan2(y, x)	std::atan2(y, x)
+#define	b2Sqrt(x)	sqrtf(x)
+#define	b2Atan2(y, x)	atan2f(y, x)
 
 /// A 2D column vector.
 struct b2Vec2
@@ -714,8 +704,8 @@ inline void b2Sweep::Advance(float32 alpha)
 {
 	b2Assert(alpha0 < 1.0f);
 	float32 beta = (alpha - alpha0) / (1.0f - alpha0);
-	c0 = (1.0f - beta) * c0 + beta * c;
-	a0 = (1.0f - beta) * a0 + beta * a;
+	c0 += beta * (c - c0);
+	a0 += beta * (a - a0);
 	alpha0 = alpha;
 }
 

src/libraries/Box2D/Common/b2Settings.cpp

@@ -17,13 +17,13 @@
 */
 
 #include <Box2D/Common/b2Settings.h>
-#include <cstdlib>
-#include <cstdio>
-#include <cstdarg>
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdlib.h>
 
 #include "common/Exception.h"
 
-b2Version b2_version = {2, 2, 1};
+b2Version b2_version = {2, 3, 0};
 
 // Memory allocators. Modify these to use your own allocator.
 void* b2Alloc(int32 size)
@@ -48,5 +48,5 @@ void b2Log(const char* string, ...)
 void loveAssert(bool test, const char *teststr)
 {
 	if (!test)
-		throw love::Exception("Box2D error: %s", teststr);
+		throw love::Exception("Box2D assertion failed: %s", teststr);
 }

src/libraries/Box2D/Common/b2Settings.h

@@ -19,8 +19,9 @@
 #ifndef B2_SETTINGS_H
 #define B2_SETTINGS_H
 
-#include <cassert>
-#include <cmath>
+#include <stddef.h>
+#include <assert.h>
+#include <float.h>
 
 void loveAssert(bool test, const char *teststr);
 

src/libraries/Box2D/Common/b2Timer.cpp

@@ -22,6 +22,7 @@
 
 float64 b2Timer::s_invFrequency = 0.0f;
 
+#define WIN32_LEAN_AND_MEAN
 #include <windows.h>
 
 b2Timer::b2Timer()
@@ -72,14 +73,14 @@ void b2Timer::Reset()
     timeval t;
     gettimeofday(&t, 0);
     m_start_sec = t.tv_sec;
-    m_start_msec = t.tv_usec * 0.001f;
+    m_start_usec = t.tv_usec;
 }
 
 float32 b2Timer::GetMilliseconds() const
 {
     timeval t;
     gettimeofday(&t, 0);
-    return (t.tv_sec - m_start_sec) * 1000 + t.tv_usec * 0.001f - m_start_msec;
+    return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);
 }
 
 #else

src/libraries/Box2D/Common/b2Timer.h

@@ -16,6 +16,9 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
+#ifndef B2_TIMER_H
+#define B2_TIMER_H
+
 #include <Box2D/Common/b2Settings.h>
 
 /// Timer for profiling. This has platform specific code and may
@@ -40,6 +43,8 @@ private:
 	static float64 s_invFrequency;
 #elif defined(__linux__) || defined (__APPLE__)
 	unsigned long m_start_sec;
-	unsigned long m_start_msec;
+	unsigned long m_start_usec;
 #endif
 };
+
+#endif

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;