Updated to Box2D 2.2, added new joints, some other updates to match the API changes (still plenty more to go though)

--HG--
branch : box2d-update

src/modules/physics/Joint.cpp

@@ -47,8 +47,9 @@ namespace physics
 		{"pulley", Joint::JOINT_PULLEY},
 		{"gear", Joint::JOINT_GEAR},
 		{"friction", Joint::JOINT_FRICTION},
-		{"line", Joint::JOINT_LINE},
 		{"weld", Joint::JOINT_WELD},
+		{"wheel", Joint::JOINT_WHEEL},
+		{"rope", Joint::JOINT_ROPE},
 	};
 
 	StringMap<Joint::Type, Joint::JOINT_MAX_ENUM> Joint::types(Joint::typeEntries, sizeof(Joint::typeEntries));

src/modules/physics/Joint.h

@@ -43,8 +43,9 @@ namespace physics
 			JOINT_PULLEY,
 			JOINT_GEAR,
 			JOINT_FRICTION,
-			JOINT_LINE,
 			JOINT_WELD,
+			JOINT_WHEEL,
+			JOINT_ROPE,
 			JOINT_MAX_ENUM
 		};
 

src/modules/physics/box2d/Box2D/Box2D.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -32,8 +32,12 @@ For discussion please visit http://box2d.org/forum
 // These include files constitute the main Box2D API
 
 #include <Box2D/Common/b2Settings.h>
+#include <Box2D/Common/b2Draw.h>
+#include <Box2D/Common/b2Timer.h>
 
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
 #include <Box2D/Collision/b2BroadPhase.h>
@@ -52,11 +56,14 @@ 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/b2LineJoint.h>
+#include <Box2D/Dynamics/Joints/b2WheelJoint.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/Rope/b2Rope.h>
+
 #endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.cpp

@@ -0,0 +1,171 @@
+/*
+* Copyright (c) 2006-2010 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/Collision/Shapes/b2ChainShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <new>
+#include <cstring>
+using namespace std;
+
+b2ChainShape::~b2ChainShape()
+{
+	b2Free(m_vertices);
+	m_vertices = NULL;
+	m_count = 0;
+}
+
+void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(m_vertices == NULL && m_count == 0);
+	b2Assert(count >= 3);
+	m_count = count + 1;
+	m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+	memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
+	m_vertices[count] = m_vertices[0];
+	m_prevVertex = m_vertices[m_count - 2];
+	m_nextVertex = m_vertices[1];
+	m_hasPrevVertex = true;
+	m_hasNextVertex = true;
+}
+
+void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
+{
+	b2Assert(m_vertices == NULL && m_count == 0);
+	b2Assert(count >= 2);
+	m_count = count;
+	m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
+	memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
+	m_hasPrevVertex = false;
+	m_hasNextVertex = false;
+}
+
+void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
+{
+	m_prevVertex = prevVertex;
+	m_hasPrevVertex = true;
+}
+
+void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
+{
+	m_nextVertex = nextVertex;
+	m_hasNextVertex = true;
+}
+
+b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainShape));
+	b2ChainShape* clone = new (mem) b2ChainShape;
+	clone->CreateChain(m_vertices, m_count);
+	clone->m_prevVertex = m_prevVertex;
+	clone->m_nextVertex = m_nextVertex;
+	clone->m_hasPrevVertex = m_hasPrevVertex;
+	clone->m_hasNextVertex = m_hasNextVertex;
+	return clone;
+}
+
+int32 b2ChainShape::GetChildCount() const
+{
+	// edge count = vertex count - 1
+	return m_count - 1;
+}
+
+void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
+{
+	b2Assert(0 <= index && index < m_count - 1);
+	edge->m_type = b2Shape::e_edge;
+	edge->m_radius = m_radius;
+
+	edge->m_vertex1 = m_vertices[index + 0];
+	edge->m_vertex2 = m_vertices[index + 1];
+
+	if (index > 0)
+	{
+		edge->m_vertex0 = m_vertices[index - 1];
+		edge->m_hasVertex0 = true;
+	}
+	else
+	{
+		edge->m_vertex0 = m_prevVertex;
+		edge->m_hasVertex0 = m_hasPrevVertex;
+	}
+
+	if (index < m_count - 2)
+	{
+		edge->m_vertex3 = m_vertices[index + 2];
+		edge->m_hasVertex3 = true;
+	}
+	else
+	{
+		edge->m_vertex3 = m_nextVertex;
+		edge->m_hasVertex3 = m_hasNextVertex;
+	}
+}
+
+bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	B2_NOT_USED(xf);
+	B2_NOT_USED(p);
+	return false;
+}
+
+bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& xf, int32 childIndex) const
+{
+	b2Assert(childIndex < m_count);
+
+	b2EdgeShape edgeShape;
+
+	int32 i1 = childIndex;
+	int32 i2 = childIndex + 1;
+	if (i2 == m_count)
+	{
+		i2 = 0;
+	}
+
+	edgeShape.m_vertex1 = m_vertices[i1];
+	edgeShape.m_vertex2 = m_vertices[i2];
+
+	return edgeShape.RayCast(output, input, xf, 0);
+}
+
+void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+	b2Assert(childIndex < m_count);
+
+	int32 i1 = childIndex;
+	int32 i2 = childIndex + 1;
+	if (i2 == m_count)
+	{
+		i2 = 0;
+	}
+
+	b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
+	b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
+
+	aabb->lowerBound = b2Min(v1, v2);
+	aabb->upperBound = b2Max(v1, v2);
+}
+
+void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	B2_NOT_USED(density);
+
+	massData->mass = 0.0f;
+	massData->center.SetZero();
+	massData->I = 0.0f;
+}

src/modules/physics/box2d/Box2D/Collision/Shapes/b2ChainShape.h

@@ -0,0 +1,117 @@
+/*
+* Copyright (c) 2006-2010 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_CHAIN_SHAPE_H
+#define B2_CHAIN_SHAPE_H
+
+#include <Box2D/Collision/Shapes/b2Shape.h>
+
+class b2EdgeShape;
+
+/// A chain shape is a free form sequence of line segments.
+/// The chain has two-sided collision, so you can use inside and outside collision.
+/// Therefore, you may use any winding order.
+/// Since there may be many vertices, they are allocated using b2Alloc.
+/// Connectivity information is used to create smooth collisions.
+/// WARNING: The chain will not collide properly if there are self-intersections.
+class b2ChainShape : public b2Shape
+{
+public:
+	b2ChainShape();
+
+	/// The destructor frees the vertices using b2Free.
+	~b2ChainShape();
+
+	/// Create a loop. This automatically adjusts connectivity.
+	/// @param vertices an array of vertices, these are copied
+	/// @param count the vertex count
+	void CreateLoop(const b2Vec2* vertices, int32 count);
+
+	/// Create a chain with isolated end vertices.
+	/// @param vertices an array of vertices, these are copied
+	/// @param count the vertex count
+	void CreateChain(const b2Vec2* vertices, int32 count);
+
+	/// Establish connectivity to a vertex that precedes the first vertex.
+	/// Don't call this for loops.
+	void SetPrevVertex(const b2Vec2& prevVertex);
+
+	/// Establish connectivity to a vertex that follows the last vertex.
+	/// Don't call this for loops.
+	void SetNextVertex(const b2Vec2& nextVertex);
+
+	/// Implement b2Shape. Vertices are cloned using b2Alloc.
+	b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const;
+
+	/// Get a child edge.
+	void GetChildEdge(b2EdgeShape* edge, int32 index) const;
+
+	/// This always return false.
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+					const b2Transform& transform, int32 childIndex) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+	/// Chains have zero mass.
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+
+	/// Get the number of vertices.
+	int32 GetVertexCount() const { return m_count; }
+
+	/// Get the vertices (read-only).
+	const b2Vec2& GetVertex(int32 index) const
+	{
+		b2Assert(0 <= index && index < m_count);
+		return m_vertices[index];
+	}
+
+	/// Get the vertices (read-only).
+	const b2Vec2* GetVertices() const { return m_vertices; }
+
+protected:
+
+	/// The vertices. Owned by this class.
+	b2Vec2* m_vertices;
+
+	/// The vertex count.
+	int32 m_count;
+
+	b2Vec2 m_prevVertex, m_nextVertex;
+	bool m_hasPrevVertex, m_hasNextVertex;
+};
+
+inline b2ChainShape::b2ChainShape()
+{
+	m_type = e_chain;
+	m_radius = b2_polygonRadius;
+	m_vertices = NULL;
+	m_count = 0;
+	m_hasPrevVertex = NULL;
+	m_hasNextVertex = NULL;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -18,6 +18,7 @@
 
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
 #include <new>
+using namespace std;
 
 b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
 {
@@ -27,9 +28,14 @@ b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
 	return clone;
 }
 
+int32 b2CircleShape::GetChildCount() const
+{
+	return 1;
+}
+
 bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
 {
-	b2Vec2 center = transform.position + b2Mul(transform.R, m_p);
+	b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
 	b2Vec2 d = p - center;
 	return b2Dot(d, d) <= m_radius * m_radius;
 }
@@ -38,9 +44,12 @@ bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) con
 // From Section 3.1.2
 // x = s + a * r
 // norm(x) = radius
-bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const
+bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& transform, int32 childIndex) const
 {
-	b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
 	b2Vec2 s = input.p1 - position;
 	float32 b = b2Dot(s, s) - m_radius * m_radius;
 
@@ -72,9 +81,11 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input
 	return false;
 }
 
-void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform) const
+void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const
 {
-	b2Vec2 p = transform.position + b2Mul(transform.R, m_p);
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
 	aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
 	aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
 }

src/modules/physics/box2d/Box2D/Collision/Shapes/b2CircleShape.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -30,14 +30,18 @@ public:
 	/// Implement b2Shape.
 	b2Shape* Clone(b2BlockAllocator* allocator) const;
 
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const;
+
 	/// Implement b2Shape.
 	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
 
 	/// Implement b2Shape.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+				const b2Transform& transform, int32 childIndex) const;
 
 	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
 
 	/// @see b2Shape::ComputeMass
 	void ComputeMass(b2MassData* massData, float32 density) const;

src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.cpp

@@ -0,0 +1,139 @@
+/*
+* Copyright (c) 2006-2010 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/Collision/Shapes/b2EdgeShape.h>
+#include <new>
+using namespace std;
+
+void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
+{
+	m_vertex1 = v1;
+	m_vertex2 = v2;
+	m_hasVertex0 = false;
+	m_hasVertex3 = false;
+}
+
+b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeShape));
+	b2EdgeShape* clone = new (mem) b2EdgeShape;
+	*clone = *this;
+	return clone;
+}
+
+int32 b2EdgeShape::GetChildCount() const
+{
+	return 1;
+}
+
+bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+	B2_NOT_USED(xf);
+	B2_NOT_USED(p);
+	return false;
+}
+
+// p = p1 + t * d
+// v = v1 + s * e
+// p1 + t * d = v1 + s * e
+// s * e - t * d = p1 - v1
+bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+							const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	// Put the ray into the edge's frame of reference.
+	b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+	b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
+	b2Vec2 d = p2 - p1;
+
+	b2Vec2 v1 = m_vertex1;
+	b2Vec2 v2 = m_vertex2;
+	b2Vec2 e = v2 - v1;
+	b2Vec2 normal(e.y, -e.x);
+	normal.Normalize();
+
+	// q = p1 + t * d
+	// dot(normal, q - v1) = 0
+	// dot(normal, p1 - v1) + t * dot(normal, d) = 0
+	float32 numerator = b2Dot(normal, v1 - p1);
+	float32 denominator = b2Dot(normal, d);
+
+	if (denominator == 0.0f)
+	{
+		return false;
+	}
+
+	float32 t = numerator / denominator;
+	if (t < 0.0f || input.maxFraction < t)
+	{
+		return false;
+	}
+
+	b2Vec2 q = p1 + t * d;
+
+	// q = v1 + s * r
+	// s = dot(q - v1, r) / dot(r, r)
+	b2Vec2 r = v2 - v1;
+	float32 rr = b2Dot(r, r);
+	if (rr == 0.0f)
+	{
+		return false;
+	}
+
+	float32 s = b2Dot(q - v1, r) / rr;
+	if (s < 0.0f || 1.0f < s)
+	{
+		return false;
+	}
+
+	output->fraction = t;
+	if (numerator > 0.0f)
+	{
+		output->normal = -normal;
+	}
+	else
+	{
+		output->normal = normal;
+	}
+	return true;
+}
+
+void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
+{
+	B2_NOT_USED(childIndex);
+
+	b2Vec2 v1 = b2Mul(xf, m_vertex1);
+	b2Vec2 v2 = b2Mul(xf, m_vertex2);
+
+	b2Vec2 lower = b2Min(v1, v2);
+	b2Vec2 upper = b2Max(v1, v2);
+
+	b2Vec2 r(m_radius, m_radius);
+	aabb->lowerBound = lower - r;
+	aabb->upperBound = upper + r;
+}
+
+void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
+{
+	B2_NOT_USED(density);
+
+	massData->mass = 0.0f;
+	massData->center = 0.5f * (m_vertex1 + m_vertex2);
+	massData->I = 0.0f;
+}

src/modules/physics/box2d/Box2D/Collision/Shapes/b2EdgeShape.h

@@ -0,0 +1,70 @@
+/*
+* Copyright (c) 2006-2010 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_EDGE_SHAPE_H
+#define B2_EDGE_SHAPE_H
+
+#include <Box2D/Collision/Shapes/b2Shape.h>
+
+/// A line segment (edge) shape. These can be connected in chains or loops
+/// to other edge shapes. The connectivity information is used to ensure
+/// correct contact normals.
+class b2EdgeShape : public b2Shape
+{
+public:
+	b2EdgeShape();
+
+	/// Set this as an isolated edge.
+	void Set(const b2Vec2& v1, const b2Vec2& v2);
+
+	/// Implement b2Shape.
+	b2Shape* Clone(b2BlockAllocator* allocator) const;
+
+	/// @see b2Shape::GetChildCount
+	int32 GetChildCount() const;
+
+	/// @see b2Shape::TestPoint
+	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
+
+	/// Implement b2Shape.
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+				const b2Transform& transform, int32 childIndex) const;
+
+	/// @see b2Shape::ComputeAABB
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
+
+	/// @see b2Shape::ComputeMass
+	void ComputeMass(b2MassData* massData, float32 density) const;
+	
+	/// These are the edge vertices
+	b2Vec2 m_vertex1, m_vertex2;
+
+	/// Optional adjacent vertices. These are used for smooth collision.
+	b2Vec2 m_vertex0, m_vertex3;
+	bool m_hasVertex0, m_hasVertex3;
+};
+
+inline b2EdgeShape::b2EdgeShape()
+{
+	m_type = e_edge;
+	m_radius = b2_polygonRadius;
+	m_hasVertex0 = false;
+	m_hasVertex3 = false;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -55,41 +55,29 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa
 	m_centroid = center;
 
 	b2Transform xf;
-	xf.position = center;
-	xf.R.Set(angle);
+	xf.p = center;
+	xf.q.Set(angle);
 
 	// Transform vertices and normals.
 	for (int32 i = 0; i < m_vertexCount; ++i)
 	{
 		m_vertices[i] = b2Mul(xf, m_vertices[i]);
-		m_normals[i] = b2Mul(xf.R, m_normals[i]);
+		m_normals[i] = b2Mul(xf.q, m_normals[i]);
 	}
 }
 
-void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)
+int32 b2PolygonShape::GetChildCount() const
 {
-	m_vertexCount = 2;
-	m_vertices[0] = v1;
-	m_vertices[1] = v2;
-	m_centroid = 0.5f * (v1 + v2);
-	m_normals[0] = b2Cross(v2 - v1, 1.0f);
-	m_normals[0].Normalize();
-	m_normals[1] = -m_normals[0];
+	return 1;
 }
 
 static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 {
-	b2Assert(count >= 2);
+	b2Assert(count >= 3);
 
 	b2Vec2 c; c.Set(0.0f, 0.0f);
 	float32 area = 0.0f;
 
-	if (count == 2)
-	{
-		c = 0.5f * (vs[0] + vs[1]);
-		return c;
-	}
-
 	// pRef is the reference point for forming triangles.
 	// It's location doesn't change the result (except for rounding error).
 	b2Vec2 pRef(0.0f, 0.0f);
@@ -131,7 +119,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
 
 void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 {
-	b2Assert(2 <= count && count <= b2_maxPolygonVertices);
+	b2Assert(3 <= count && count <= b2_maxPolygonVertices);
 	m_vertexCount = count;
 
 	// Copy vertices.
@@ -170,10 +158,10 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 			
 			b2Vec2 r = m_vertices[j] - m_vertices[i1];
 
-			// Your polygon is non-convex (it has an indentation) or
-			// has colinear edges.
+			// 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);
+			b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order");
 		}
 	}
 #endif
@@ -184,7 +172,7 @@ void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
 
 bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
 {
-	b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
+	b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
 
 	for (int32 i = 0; i < m_vertexCount; ++i)
 	{
@@ -198,131 +186,82 @@ bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
 	return true;
 }
 
-bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const
+bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+								const b2Transform& xf, int32 childIndex) const
 {
+	B2_NOT_USED(childIndex);
+
 	// Put the ray into the polygon's frame of reference.
-	b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
-	b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
+	b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
+	b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
 	b2Vec2 d = p2 - p1;
 
-	if (m_vertexCount == 2)
-	{
-		b2Vec2 v1 = m_vertices[0];
-		b2Vec2 v2 = m_vertices[1];
-		b2Vec2 normal = m_normals[0];
+	float32 lower = 0.0f, upper = input.maxFraction;
 
-		// q = p1 + t * d
-		// dot(normal, q - v1) = 0
-		// dot(normal, p1 - v1) + t * dot(normal, d) = 0
-		float32 numerator = b2Dot(normal, v1 - p1);
-		float32 denominator = b2Dot(normal, d);
+	int32 index = -1;
+
+	for (int32 i = 0; i < m_vertexCount; ++i)
+	{
+		// p = p1 + a * d
+		// dot(normal, p - v) = 0
+		// dot(normal, p1 - v) + a * dot(normal, d) = 0
+		float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+		float32 denominator = b2Dot(m_normals[i], d);
 
 		if (denominator == 0.0f)
-		{
-			return false;
-		}
-	
-		float32 t = numerator / denominator;
-		if (t < 0.0f || 1.0f < t)
-		{
-			return false;
-		}
-
-		b2Vec2 q = p1 + t * d;
-
-		// q = v1 + s * r
-		// s = dot(q - v1, r) / dot(r, r)
-		b2Vec2 r = v2 - v1;
-		float32 rr = b2Dot(r, r);
-		if (rr == 0.0f)
-		{
-			return false;
-		}
-
-		float32 s = b2Dot(q - v1, r) / rr;
-		if (s < 0.0f || 1.0f < s)
-		{
-			return false;
-		}
-
-		output->fraction = t;
-		if (numerator > 0.0f)
-		{
-			output->normal = -normal;
-		}
-		else
-		{
-			output->normal = normal;
-		}
-		return true;
-	}
-	else
-	{
-		float32 lower = 0.0f, upper = input.maxFraction;
-
-		int32 index = -1;
-
-		for (int32 i = 0; i < m_vertexCount; ++i)
-		{
-			// p = p1 + a * d
-			// dot(normal, p - v) = 0
-			// dot(normal, p1 - v) + a * dot(normal, d) = 0
-			float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
-			float32 denominator = b2Dot(m_normals[i], d);
-
-			if (denominator == 0.0f)
-			{	
-				if (numerator < 0.0f)
-				{
-					return false;
-				}
-			}
-			else
-			{
-				// Note: we want this predicate without division:
-				// lower < numerator / denominator, where denominator < 0
-				// Since denominator < 0, we have to flip the inequality:
-				// lower < numerator / denominator <==> denominator * lower > numerator.
-				if (denominator < 0.0f && numerator < lower * denominator)
-				{
-					// Increase lower.
-					// The segment enters this half-space.
-					lower = numerator / denominator;
-					index = i;
-				}
-				else if (denominator > 0.0f && numerator < upper * denominator)
-				{
-					// Decrease upper.
-					// The segment exits this half-space.
-					upper = numerator / denominator;
-				}
-			}
-
-			// The use of epsilon here causes the assert on lower to trip
-			// in some cases. Apparently the use of epsilon was to make edge
-			// shapes work, but now those are handled separately.
-			//if (upper < lower - b2_epsilon)
-			if (upper < lower)
+		{	
+			if (numerator < 0.0f)
 			{
 				return false;
 			}
 		}
-
-		b2Assert(0.0f <= lower && lower <= input.maxFraction);
-
-		if (index >= 0)
+		else
 		{
-			output->fraction = lower;
-			output->normal = b2Mul(xf.R, m_normals[index]);
-			return true;
+			// Note: we want this predicate without division:
+			// lower < numerator / denominator, where denominator < 0
+			// Since denominator < 0, we have to flip the inequality:
+			// lower < numerator / denominator <==> denominator * lower > numerator.
+			if (denominator < 0.0f && numerator < lower * denominator)
+			{
+				// Increase lower.
+				// The segment enters this half-space.
+				lower = numerator / denominator;
+				index = i;
+			}
+			else if (denominator > 0.0f && numerator < upper * denominator)
+			{
+				// Decrease upper.
+				// The segment exits this half-space.
+				upper = numerator / denominator;
+			}
 		}
+
+		// The use of epsilon here causes the assert on lower to trip
+		// in some cases. Apparently the use of epsilon was to make edge
+		// shapes work, but now those are handled separately.
+		//if (upper < lower - b2_epsilon)
+		if (upper < lower)
+		{
+			return false;
+		}
+	}
+
+	b2Assert(0.0f <= lower && lower <= input.maxFraction);
+
+	if (index >= 0)
+	{
+		output->fraction = lower;
+		output->normal = b2Mul(xf.q, m_normals[index]);
+		return true;
 	}
 
 	return false;
 }
 
-void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf) const
+void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
 {
+	B2_NOT_USED(childIndex);
+
 	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
 	b2Vec2 upper = lower;
 
@@ -364,44 +303,30 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 	//
 	// The rest of the derivation is handled by computer algebra.
 
-	b2Assert(m_vertexCount >= 2);
-
-	// A line segment has zero mass.
-	if (m_vertexCount == 2)
-	{
-		massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
-		massData->mass = 0.0f;
-		massData->I = 0.0f;
-		return;
-	}
+	b2Assert(m_vertexCount >= 3);
 
 	b2Vec2 center; center.Set(0.0f, 0.0f);
 	float32 area = 0.0f;
 	float32 I = 0.0f;
 
-	// pRef is the reference point for forming triangles.
+	// s is the reference point for forming triangles.
 	// It's location doesn't change the result (except for rounding error).
-	b2Vec2 pRef(0.0f, 0.0f);
-#if 0
+	b2Vec2 s(0.0f, 0.0f);
+
 	// This code would put the reference point inside the polygon.
 	for (int32 i = 0; i < m_vertexCount; ++i)
 	{
-		pRef += m_vertices[i];
+		s += m_vertices[i];
 	}
-	pRef *= 1.0f / count;
-#endif
+	s *= 1.0f / m_vertexCount;
 
 	const float32 k_inv3 = 1.0f / 3.0f;
 
 	for (int32 i = 0; i < m_vertexCount; ++i)
 	{
 		// Triangle vertices.
-		b2Vec2 p1 = pRef;
-		b2Vec2 p2 = m_vertices[i];
-		b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
-
-		b2Vec2 e1 = p2 - p1;
-		b2Vec2 e2 = p3 - p1;
+		b2Vec2 e1 = m_vertices[i] - s;
+		b2Vec2 e2 = i + 1 < m_vertexCount ? m_vertices[i+1] - s : m_vertices[0] - s;
 
 		float32 D = b2Cross(e1, e2);
 
@@ -409,16 +334,15 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 		area += triangleArea;
 
 		// Area weighted centroid
-		center += triangleArea * k_inv3 * (p1 + p2 + p3);
+		center += triangleArea * k_inv3 * (e1 + e2);
 
-		float32 px = p1.x, py = p1.y;
 		float32 ex1 = e1.x, ey1 = e1.y;
 		float32 ex2 = e2.x, ey2 = e2.y;
 
-		float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
-		float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
+		float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
+		float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
 
-		I += D * (intx2 + inty2);
+		I += (0.25f * k_inv3 * D) * (intx2 + inty2);
 	}
 
 	// Total mass
@@ -427,8 +351,11 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
 	// Center of mass
 	b2Assert(area > b2_epsilon);
 	center *= 1.0f / area;
-	massData->center = center;
+	massData->center = center + s;
 
-	// Inertia tensor relative to the local origin.
+	// Inertia tensor relative to the local origin (point s).
 	massData->I = density * I;
+	
+	// Shift to center of mass then to original body origin.
+	massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
 }

src/modules/physics/box2d/Box2D/Collision/Shapes/b2PolygonShape.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -23,6 +23,8 @@
 
 /// A convex polygon. It is assumed that the interior of the polygon is to
 /// the left of each edge.
+/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
+/// In most cases you should not need many vertices for a convex polygon.
 class b2PolygonShape : public b2Shape
 {
 public:
@@ -31,8 +33,12 @@ public:
 	/// Implement b2Shape.
 	b2Shape* Clone(b2BlockAllocator* allocator) const;
 
+	/// @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.
+	/// The count must be in the range [3, b2_maxPolygonVertices].
 	void Set(const b2Vec2* vertices, int32 vertexCount);
 
 	/// Build vertices to represent an axis-aligned box.
@@ -47,27 +53,19 @@ public:
 	/// @param angle the rotation of the box in local coordinates.
 	void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
 
-	/// Set this as a single edge.
-	void SetAsEdge(const b2Vec2& v1, const b2Vec2& v2);
-
 	/// @see b2Shape::TestPoint
 	bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
 
 	/// Implement b2Shape.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const;
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+					const b2Transform& transform, int32 childIndex) const;
 
 	/// @see b2Shape::ComputeAABB
-	void ComputeAABB(b2AABB* aabb, const b2Transform& transform) const;
+	void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
 
 	/// @see b2Shape::ComputeMass
 	void ComputeMass(b2MassData* massData, float32 density) const;
 
-	/// Get the supporting vertex index in the given direction.
-	int32 GetSupport(const b2Vec2& d) const;
-
-	/// Get the supporting vertex in the given direction.
-	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
-
 	/// Get the vertex count.
 	int32 GetVertexCount() const { return m_vertexCount; }
 
@@ -88,40 +86,6 @@ inline b2PolygonShape::b2PolygonShape()
 	m_centroid.SetZero();
 }
 
-inline int32 b2PolygonShape::GetSupport(const b2Vec2& d) const
-{
-	int32 bestIndex = 0;
-	float32 bestValue = b2Dot(m_vertices[0], d);
-	for (int32 i = 1; i < m_vertexCount; ++i)
-	{
-		float32 value = b2Dot(m_vertices[i], d);
-		if (value > bestValue)
-		{
-			bestIndex = i;
-			bestValue = value;
-		}
-	}
-
-	return bestIndex;
-}
-
-inline const b2Vec2& b2PolygonShape::GetSupportVertex(const b2Vec2& d) const
-{
-	int32 bestIndex = 0;
-	float32 bestValue = b2Dot(m_vertices[0], d);
-	for (int32 i = 1; i < m_vertexCount; ++i)
-	{
-		float32 value = b2Dot(m_vertices[i], d);
-		if (value > bestValue)
-		{
-			bestIndex = i;
-			bestValue = value;
-		}
-	}
-
-	return m_vertices[bestIndex];
-}
-
 inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
 {
 	b2Assert(0 <= index && index < m_vertexCount);

src/modules/physics/box2d/Box2D/Collision/Shapes/b2Shape.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -38,20 +38,20 @@ struct b2MassData
 
 /// A shape is used for collision detection. You can create a shape however you like.
 /// Shapes used for simulation in b2World are created automatically when a b2Fixture
-/// is created.
+/// is created. Shapes may encapsulate a one or more child shapes.
 class b2Shape
 {
 public:
 	
 	enum Type
 	{
-		e_unknown= -1,
 		e_circle = 0,
-		e_polygon = 1,
-		e_typeCount = 2,
+		e_edge = 1,
+		e_polygon = 2,
+		e_chain = 3,
+		e_typeCount = 4
 	};
 
-	b2Shape() { m_type = e_unknown; }
 	virtual ~b2Shape() {}
 
 	/// Clone the concrete shape using the provided allocator.
@@ -61,21 +61,27 @@ public:
 	/// @return the shape type.
 	Type GetType() const;
 
+	/// Get the number of child primitives.
+	virtual int32 GetChildCount() const = 0;
+
 	/// Test a point for containment in this shape. This only works for convex shapes.
 	/// @param xf the shape world transform.
 	/// @param p a point in world coordinates.
 	virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
 
-	/// Cast a ray against this shape.
+	/// Cast a ray against a child shape.
 	/// @param output the ray-cast results.
 	/// @param input the ray-cast input parameters.
 	/// @param transform the transform to be applied to the shape.
-	virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const = 0;
+	/// @param childIndex the child shape index
+	virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
+						const b2Transform& transform, int32 childIndex) const = 0;
 
-	/// Given a transform, compute the associated axis aligned bounding box for this shape.
+	/// Given a transform, compute the associated axis aligned bounding box for a child shape.
 	/// @param aabb returns the axis aligned box.
 	/// @param xf the world transform of the shape.
-	virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf) const = 0;
+	/// @param childIndex the child shape
+	virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
 
 	/// Compute the mass properties of this shape using its dimensions and density.
 	/// The inertia tensor is computed about the local origin.

src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -18,6 +18,7 @@
 
 #include <Box2D/Collision/b2BroadPhase.h>
 #include <cstring>
+using namespace std;
 
 b2BroadPhase::b2BroadPhase()
 {
@@ -62,6 +63,11 @@ void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& di
 	}
 }
 
+void b2BroadPhase::TouchProxy(int32 proxyId)
+{
+	BufferMove(proxyId);
+}
+
 void b2BroadPhase::BufferMove(int32 proxyId)
 {
 	if (m_moveCount == m_moveCapacity)

src/modules/physics/box2d/Box2D/Collision/b2BroadPhase.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -40,7 +40,7 @@ public:
 
 	enum
 	{
-		e_nullProxy = -1,
+		e_nullProxy = -1
 	};
 
 	b2BroadPhase();
@@ -57,6 +57,9 @@ public:
 	/// call UpdatePairs to finalized the proxy pairs (for your time step).
 	void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
 
+	/// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
+	void TouchProxy(int32 proxyId);
+
 	/// Get the fat AABB for a proxy.
 	const b2AABB& GetFatAABB(int32 proxyId) const;
 
@@ -88,8 +91,14 @@ public:
 	template <typename T>
 	void RayCast(T* callback, const b2RayCastInput& input) const;
 
-	/// Compute the height of the embedded tree.
-	int32 ComputeHeight() const;
+	/// Get the height of the embedded tree.
+	int32 GetTreeHeight() const;
+
+	/// Get the balance of the embedded tree.
+	int32 GetTreeBalance() const;
+
+	/// Get the quality metric of the embedded tree.
+	float32 GetTreeQuality() const;
 
 private:
 
@@ -153,9 +162,19 @@ inline int32 b2BroadPhase::GetProxyCount() const
 	return m_proxyCount;
 }
 
-inline int32 b2BroadPhase::ComputeHeight() const
+inline int32 b2BroadPhase::GetTreeHeight() const
 {
-	return m_tree.ComputeHeight();
+	return m_tree.GetHeight();
+}
+
+inline int32 b2BroadPhase::GetTreeBalance() const
+{
+	return m_tree.GetMaxBalance();
+}
+
+inline float32 b2BroadPhase::GetTreeQuality() const
+{
+	return m_tree.GetAreaRatio();
 }
 
 template <typename T>
@@ -211,7 +230,7 @@ void b2BroadPhase::UpdatePairs(T* callback)
 	}
 
 	// Try to keep the tree balanced.
-	m_tree.Rebalance(4);
+	//m_tree.Rebalance(4);
 }
 
 template <typename T>

src/modules/physics/box2d/Box2D/Collision/b2CollideCircle.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2009 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

src/modules/physics/box2d/Box2D/Collision/b2CollideEdge.cpp

@@ -0,0 +1,698 @@
+/*
+ * Copyright (c) 2007-2009 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/Collision/b2Collision.h>
+#include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+
+
+// Compute contact points for edge versus circle.
+// This accounts for edge connectivity.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							const b2EdgeShape* edgeA, const b2Transform& xfA,
+							const b2CircleShape* circleB, const b2Transform& xfB)
+{
+	manifold->pointCount = 0;
+	
+	// Compute circle in frame of edge
+	b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
+	
+	b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
+	b2Vec2 e = B - A;
+	
+	// Barycentric coordinates
+	float32 u = b2Dot(e, B - Q);
+	float32 v = b2Dot(e, Q - A);
+	
+	float32 radius = edgeA->m_radius + circleB->m_radius;
+	
+	b2ContactFeature cf;
+	cf.indexB = 0;
+	cf.typeB = b2ContactFeature::e_vertex;
+	
+	// Region A
+	if (v <= 0.0f)
+	{
+		b2Vec2 P = A;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to A?
+		if (edgeA->m_hasVertex0)
+		{
+			b2Vec2 A1 = edgeA->m_vertex0;
+			b2Vec2 B1 = A;
+			b2Vec2 e1 = B1 - A1;
+			float32 u1 = b2Dot(e1, B1 - Q);
+			
+			// Is the circle in Region AB of the previous edge?
+			if (u1 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 0;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region B
+	if (u <= 0.0f)
+	{
+		b2Vec2 P = B;
+		b2Vec2 d = Q - P;
+		float32 dd = b2Dot(d, d);
+		if (dd > radius * radius)
+		{
+			return;
+		}
+		
+		// Is there an edge connected to B?
+		if (edgeA->m_hasVertex3)
+		{
+			b2Vec2 B2 = edgeA->m_vertex3;
+			b2Vec2 A2 = B;
+			b2Vec2 e2 = B2 - A2;
+			float32 v2 = b2Dot(e2, Q - A2);
+			
+			// Is the circle in Region AB of the next edge?
+			if (v2 > 0.0f)
+			{
+				return;
+			}
+		}
+		
+		cf.indexA = 1;
+		cf.typeA = b2ContactFeature::e_vertex;
+		manifold->pointCount = 1;
+		manifold->type = b2Manifold::e_circles;
+		manifold->localNormal.SetZero();
+		manifold->localPoint = P;
+		manifold->points[0].id.key = 0;
+		manifold->points[0].id.cf = cf;
+		manifold->points[0].localPoint = circleB->m_p;
+		return;
+	}
+	
+	// Region AB
+	float32 den = b2Dot(e, e);
+	b2Assert(den > 0.0f);
+	b2Vec2 P = (1.0f / den) * (u * A + v * B);
+	b2Vec2 d = Q - P;
+	float32 dd = b2Dot(d, d);
+	if (dd > radius * radius)
+	{
+		return;
+	}
+	
+	b2Vec2 n(-e.y, e.x);
+	if (b2Dot(n, Q - A) < 0.0f)
+	{
+		n.Set(-n.x, -n.y);
+	}
+	n.Normalize();
+	
+	cf.indexA = 0;
+	cf.typeA = b2ContactFeature::e_face;
+	manifold->pointCount = 1;
+	manifold->type = b2Manifold::e_faceA;
+	manifold->localNormal = n;
+	manifold->localPoint = A;
+	manifold->points[0].id.key = 0;
+	manifold->points[0].id.cf = cf;
+	manifold->points[0].localPoint = circleB->m_p;
+}
+
+// This structure is used to keep track of the best separating axis.
+struct b2EPAxis
+{
+	enum Type
+	{
+		e_unknown,
+		e_edgeA,
+		e_edgeB
+	};
+	
+	Type type;
+	int32 index;
+	float32 separation;
+};
+
+// This holds polygon B expressed in frame A.
+struct b2TempPolygon
+{
+	b2Vec2 vertices[b2_maxPolygonVertices];
+	b2Vec2 normals[b2_maxPolygonVertices];
+	int32 count;
+};
+
+// Reference face used for clipping
+struct b2ReferenceFace
+{
+	int32 i1, i2;
+	
+	b2Vec2 v1, v2;
+	
+	b2Vec2 normal;
+	
+	b2Vec2 sideNormal1;
+	float32 sideOffset1;
+	
+	b2Vec2 sideNormal2;
+	float32 sideOffset2;
+};
+
+// This class collides and edge and a polygon, taking into account edge adjacency.
+struct b2EPCollider
+{
+	void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+				 const b2PolygonShape* polygonB, const b2Transform& xfB);
+	b2EPAxis ComputeEdgeSeparation();
+	b2EPAxis ComputePolygonSeparation();
+	
+	enum VertexType
+	{
+		e_isolated,
+		e_concave,
+		e_convex
+	};
+	
+	b2TempPolygon m_polygonB;
+	
+	b2Transform m_xf;
+	b2Vec2 m_centroidB;
+	b2Vec2 m_v0, m_v1, m_v2, m_v3;
+	b2Vec2 m_normal0, m_normal1, m_normal2;
+	b2Vec2 m_normal;
+	VertexType m_type1, m_type2;
+	b2Vec2 m_lowerLimit, m_upperLimit;
+	float32 m_radius;
+	bool m_front;
+};
+
+// Algorithm:
+// 1. Classify v1 and v2
+// 2. Classify polygon centroid as front or back
+// 3. Flip normal if necessary
+// 4. Initialize normal range to [-pi, pi] about face normal
+// 5. Adjust normal range according to adjacent edges
+// 6. Visit each separating axes, only accept axes within the range
+// 7. Return if _any_ axis indicates separation
+// 8. Clip
+void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
+						   const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	m_xf = b2MulT(xfA, xfB);
+	
+	m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
+	
+	m_v0 = edgeA->m_vertex0;
+	m_v1 = edgeA->m_vertex1;
+	m_v2 = edgeA->m_vertex2;
+	m_v3 = edgeA->m_vertex3;
+	
+	bool hasVertex0 = edgeA->m_hasVertex0;
+	bool hasVertex3 = edgeA->m_hasVertex3;
+	
+	b2Vec2 edge1 = m_v2 - m_v1;
+	edge1.Normalize();
+	m_normal1.Set(edge1.y, -edge1.x);
+	float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
+	float32 offset0 = 0.0f, offset2 = 0.0f;
+	bool convex1 = false, convex2 = false;
+	
+	// Is there a preceding edge?
+	if (hasVertex0)
+	{
+		b2Vec2 edge0 = m_v1 - m_v0;
+		edge0.Normalize();
+		m_normal0.Set(edge0.y, -edge0.x);
+		convex1 = b2Cross(edge0, edge1) >= 0.0f;
+		offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
+	}
+	
+	// Is there a following edge?
+	if (hasVertex3)
+	{
+		b2Vec2 edge2 = m_v3 - m_v2;
+		edge2.Normalize();
+		m_normal2.Set(edge2.y, -edge2.x);
+		convex2 = b2Cross(edge1, edge2) > 0.0f;
+		offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
+	}
+	
+	// Determine front or back collision. Determine collision normal limits.
+	if (hasVertex0 && hasVertex3)
+	{
+		if (convex1 && convex2)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex1)
+		{
+			m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else if (convex2)
+		{
+			m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex0)
+	{
+		if (convex1)
+		{
+			m_front = offset0 >= 0.0f || offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal0;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset0 >= 0.0f && offset1 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = m_normal1;
+				m_upperLimit = -m_normal0;
+			}
+		}
+	}
+	else if (hasVertex3)
+	{
+		if (convex2)
+		{
+			m_front = offset1 >= 0.0f || offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal2;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+		}
+		else
+		{
+			m_front = offset1 >= 0.0f && offset2 >= 0.0f;
+			if (m_front)
+			{
+				m_normal = m_normal1;
+				m_lowerLimit = -m_normal1;
+				m_upperLimit = m_normal1;
+			}
+			else
+			{
+				m_normal = -m_normal1;
+				m_lowerLimit = -m_normal2;
+				m_upperLimit = m_normal1;
+			}
+		}		
+	}
+	else
+	{
+		m_front = offset1 >= 0.0f;
+		if (m_front)
+		{
+			m_normal = m_normal1;
+			m_lowerLimit = -m_normal1;
+			m_upperLimit = -m_normal1;
+		}
+		else
+		{
+			m_normal = -m_normal1;
+			m_lowerLimit = m_normal1;
+			m_upperLimit = m_normal1;
+		}
+	}
+	
+	// Get polygonB in frameA
+	m_polygonB.count = polygonB->m_vertexCount;
+	for (int32 i = 0; i < polygonB->m_vertexCount; ++i)
+	{
+		m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
+		m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
+	}
+	
+	m_radius = 2.0f * b2_polygonRadius;
+	
+	manifold->pointCount = 0;
+	
+	b2EPAxis edgeAxis = ComputeEdgeSeparation();
+	
+	// If no valid normal can be found than this edge should not collide.
+	if (edgeAxis.type == b2EPAxis::e_unknown)
+	{
+		return;
+	}
+	
+	if (edgeAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	b2EPAxis polygonAxis = ComputePolygonSeparation();
+	if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
+	{
+		return;
+	}
+	
+	// Use hysteresis for jitter reduction.
+	const float32 k_relativeTol = 0.98f;
+	const float32 k_absoluteTol = 0.001f;
+	
+	b2EPAxis primaryAxis;
+	if (polygonAxis.type == b2EPAxis::e_unknown)
+	{
+		primaryAxis = edgeAxis;
+	}
+	else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
+	{
+		primaryAxis = polygonAxis;
+	}
+	else
+	{
+		primaryAxis = edgeAxis;
+	}
+	
+	b2ClipVertex ie[2];
+	b2ReferenceFace rf;
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->type = b2Manifold::e_faceA;
+		
+		// Search for the polygon normal that is most anti-parallel to the edge normal.
+		int32 bestIndex = 0;
+		float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
+		for (int32 i = 1; i < m_polygonB.count; ++i)
+		{
+			float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
+			if (value < bestValue)
+			{
+				bestValue = value;
+				bestIndex = i;
+			}
+		}
+		
+		int32 i1 = bestIndex;
+		int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
+		
+		ie[0].v = m_polygonB.vertices[i1];
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = 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.typeA = b2ContactFeature::e_face;
+		ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
+		
+		if (m_front)
+		{
+			rf.i1 = 0;
+			rf.i2 = 1;
+			rf.v1 = m_v1;
+			rf.v2 = m_v2;
+			rf.normal = m_normal1;
+		}
+		else
+		{
+			rf.i1 = 1;
+			rf.i2 = 0;
+			rf.v1 = m_v2;
+			rf.v2 = m_v1;
+			rf.normal = -m_normal1;
+		}		
+	}
+	else
+	{
+		manifold->type = b2Manifold::e_faceB;
+		
+		ie[0].v = m_v1;
+		ie[0].id.cf.indexA = 0;
+		ie[0].id.cf.indexB = 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.typeA = b2ContactFeature::e_vertex;
+		ie[1].id.cf.typeB = b2ContactFeature::e_face;
+		
+		rf.i1 = primaryAxis.index;
+		rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
+		rf.v1 = m_polygonB.vertices[rf.i1];
+		rf.v2 = m_polygonB.vertices[rf.i2];
+		rf.normal = m_polygonB.normals[rf.i1];
+	}
+	
+	rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
+	rf.sideNormal2 = -rf.sideNormal1;
+	rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
+	rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
+	
+	// Clip incident edge against extruded edge1 side edges.
+	b2ClipVertex clipPoints1[2];
+	b2ClipVertex clipPoints2[2];
+	int32 np;
+	
+	// Clip to box side 1
+	np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Clip to negative box side 1
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
+	
+	if (np < b2_maxManifoldPoints)
+	{
+		return;
+	}
+	
+	// Now clipPoints2 contains the clipped points.
+	if (primaryAxis.type == b2EPAxis::e_edgeA)
+	{
+		manifold->localNormal = rf.normal;
+		manifold->localPoint = rf.v1;
+	}
+	else
+	{
+		manifold->localNormal = polygonB->m_normals[rf.i1];
+		manifold->localPoint = polygonB->m_vertices[rf.i1];
+	}
+	
+	int32 pointCount = 0;
+	for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
+	{
+		float32 separation;
+		
+		separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
+		
+		if (separation <= m_radius)
+		{
+			b2ManifoldPoint* cp = manifold->points + pointCount;
+			
+			if (primaryAxis.type == b2EPAxis::e_edgeA)
+			{
+				cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
+				cp->id = clipPoints2[i].id;
+			}
+			else
+			{
+				cp->localPoint = clipPoints2[i].v;
+				cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
+				cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
+				cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
+				cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
+			}
+			
+			++pointCount;
+		}
+	}
+	
+	manifold->pointCount = pointCount;
+}
+
+b2EPAxis b2EPCollider::ComputeEdgeSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_edgeA;
+	axis.index = m_front ? 0 : 1;
+	axis.separation = FLT_MAX;
+	
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
+		if (s < axis.separation)
+		{
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+b2EPAxis b2EPCollider::ComputePolygonSeparation()
+{
+	b2EPAxis axis;
+	axis.type = b2EPAxis::e_unknown;
+	axis.index = -1;
+	axis.separation = -FLT_MAX;
+
+	b2Vec2 perp(-m_normal.y, m_normal.x);
+
+	for (int32 i = 0; i < m_polygonB.count; ++i)
+	{
+		b2Vec2 n = -m_polygonB.normals[i];
+		
+		float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
+		float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
+		float32 s = b2Min(s1, s2);
+		
+		if (s > m_radius)
+		{
+			// No collision
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+			return axis;
+		}
+		
+		// Adjacency
+		if (b2Dot(n, perp) >= 0.0f)
+		{
+			if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		else
+		{
+			if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
+			{
+				continue;
+			}
+		}
+		
+		if (s > axis.separation)
+		{
+			axis.type = b2EPAxis::e_edgeB;
+			axis.index = i;
+			axis.separation = s;
+		}
+	}
+	
+	return axis;
+}
+
+void b2CollideEdgeAndPolygon(	b2Manifold* manifold,
+							 const b2EdgeShape* edgeA, const b2Transform& xfA,
+							 const b2PolygonShape* polygonB, const b2Transform& xfB)
+{
+	b2EPCollider collider;
+	collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
+}

src/modules/physics/box2d/Box2D/Collision/b2CollidePolygon.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -23,18 +23,17 @@
 static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
 							  const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->m_vertexCount;
 	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 < count1);
+	b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
 
 	// Convert normal from poly1's frame into poly2's frame.
-	b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
-	b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
+	b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]);
+	b2Vec2 normal1 = b2MulT(xf2.q, normal1World);
 
 	// Find support vertex on poly2 for -normal.
 	int32 index = 0;
@@ -66,7 +65,7 @@ static float32 b2FindMaxSeparation(int32* edgeIndex,
 
 	// 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.R, d);
+	b2Vec2 dLocal1 = b2MulT(xf1.q, d);
 
 	// Find edge normal on poly1 that has the largest projection onto d.
 	int32 edge = 0;
@@ -143,17 +142,16 @@ static void b2FindIncidentEdge(b2ClipVertex c[2],
 							 const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
 							 const b2PolygonShape* poly2, const b2Transform& xf2)
 {
-	int32 count1 = poly1->m_vertexCount;
 	const b2Vec2* normals1 = poly1->m_normals;
 
 	int32 count2 = poly2->m_vertexCount;
 	const b2Vec2* vertices2 = poly2->m_vertices;
 	const b2Vec2* normals2 = poly2->m_normals;
 
-	b2Assert(0 <= edge1 && edge1 < count1);
+	b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
 
 	// Get the normal of the reference edge in poly2's frame.
-	b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
+	b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
 
 	// Find the incident edge on poly2.
 	int32 index = 0;
@@ -173,14 +171,16 @@ static void b2FindIncidentEdge(b2ClipVertex c[2],
 	int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
 
 	c[0].v = b2Mul(xf2, vertices2[i1]);
-	c[0].id.features.referenceEdge = (uint8)edge1;
-	c[0].id.features.incidentEdge = (uint8)i1;
-	c[0].id.features.incidentVertex = 0;
+	c[0].id.cf.indexA = (uint8)edge1;
+	c[0].id.cf.indexB = (uint8)i1;
+	c[0].id.cf.typeA = b2ContactFeature::e_face;
+	c[0].id.cf.typeB = b2ContactFeature::e_vertex;
 
 	c[1].v = b2Mul(xf2, vertices2[i2]);
-	c[1].id.features.referenceEdge = (uint8)edge1;
-	c[1].id.features.incidentEdge = (uint8)i2;
-	c[1].id.features.incidentVertex = 1;
+	c[1].id.cf.indexA = (uint8)edge1;
+	c[1].id.cf.indexB = (uint8)i2;
+	c[1].id.cf.typeA = b2ContactFeature::e_face;
+	c[1].id.cf.typeB = b2ContactFeature::e_vertex;
 }
 
 // Find edge normal of max separation on A - return if separating axis is found
@@ -242,8 +242,11 @@ void b2CollidePolygons(b2Manifold* manifold,
 	int32 count1 = poly1->m_vertexCount;
 	const b2Vec2* vertices1 = poly1->m_vertices;
 
-	b2Vec2 v11 = vertices1[edge1];
-	b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
+	int32 iv1 = edge1;
+	int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+	b2Vec2 v11 = vertices1[iv1];
+	b2Vec2 v12 = vertices1[iv2];
 
 	b2Vec2 localTangent = v12 - v11;
 	localTangent.Normalize();
@@ -251,7 +254,7 @@ void b2CollidePolygons(b2Manifold* manifold,
 	b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
 	b2Vec2 planePoint = 0.5f * (v11 + v12);
 
-	b2Vec2 tangent = b2Mul(xf1.R, localTangent);
+	b2Vec2 tangent = b2Mul(xf1.q, localTangent);
 	b2Vec2 normal = b2Cross(tangent, 1.0f);
 	
 	v11 = b2Mul(xf1, v11);
@@ -270,13 +273,13 @@ void b2CollidePolygons(b2Manifold* manifold,
 	int np;
 
 	// Clip to box side 1
-	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
+	np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
 
 	if (np < 2)
 		return;
 
 	// Clip to negative box side 1
-	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2);
+	np = b2ClipSegmentToLine(clipPoints2, clipPoints1,  tangent, sideOffset2, iv2);
 
 	if (np < 2)
 	{
@@ -297,7 +300,15 @@ void b2CollidePolygons(b2Manifold* manifold,
 			b2ManifoldPoint* cp = manifold->points + pointCount;
 			cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
 			cp->id = clipPoints2[i].id;
-			cp->id.features.flip = flip;
+			if (flip)
+			{
+				// Swap features
+				b2ContactFeature cf = cp->id.cf;
+				cp->id.cf.indexA = cf.indexB;
+				cp->id.cf.indexB = cf.indexA;
+				cp->id.cf.typeA = cf.typeB;
+				cp->id.cf.typeB = cf.typeA;
+			}
 			++pointCount;
 		}
 	}

src/modules/physics/box2d/Box2D/Collision/b2Collision.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2009 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
@@ -49,7 +49,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
 
 	case b2Manifold::e_faceA:
 		{
-			normal = b2Mul(xfA.R, manifold->localNormal);
+			normal = b2Mul(xfA.q, manifold->localNormal);
 			b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
 			
 			for (int32 i = 0; i < manifold->pointCount; ++i)
@@ -64,7 +64,7 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
 
 	case b2Manifold::e_faceB:
 		{
-			normal = b2Mul(xfB.R, manifold->localNormal);
+			normal = b2Mul(xfB.q, manifold->localNormal);
 			b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
 
 			for (int32 i = 0; i < manifold->pointCount; ++i)
@@ -196,7 +196,7 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
 
 // Sutherland-Hodgman clipping.
 int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
-						const b2Vec2& normal, float32 offset)
+						const b2Vec2& normal, float32 offset, int32 vertexIndexA)
 {
 	// Start with no output points
 	int32 numOut = 0;
@@ -215,26 +215,25 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
 		// Find intersection point of edge and plane
 		float32 interp = distance0 / (distance0 - distance1);
 		vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
-		if (distance0 > 0.0f)
-		{
-			vOut[numOut].id = vIn[0].id;
-		}
-		else
-		{
-			vOut[numOut].id = vIn[1].id;
-		}
+
+		// VertexA is hitting edgeB.
+		vOut[numOut].id.cf.indexA = 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;
 		++numOut;
 	}
 
 	return numOut;
 }
 
-bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
-				   const b2Transform& xfA, const b2Transform& xfB)
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					const b2Transform& xfA, const b2Transform& xfB)
 {
 	b2DistanceInput input;
-	input.proxyA.Set(shapeA);
-	input.proxyB.Set(shapeB);
+	input.proxyA.Set(shapeA, indexA);
+	input.proxyB.Set(shapeB, indexB);
 	input.transformA = xfA;
 	input.transformB = xfB;
 	input.useRadii = true;

src/modules/physics/box2d/Box2D/Collision/b2Collision.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -28,21 +28,31 @@
 
 class b2Shape;
 class b2CircleShape;
+class b2EdgeShape;
 class b2PolygonShape;
 
 const uint8 b2_nullFeature = UCHAR_MAX;
 
+/// The features that intersect to form the contact point
+/// This must be 4 bytes or less.
+struct b2ContactFeature
+{
+	enum Type
+	{
+		e_vertex = 0,
+		e_face = 1
+	};
+
+	uint8 indexA;		///< Feature index on shapeA
+	uint8 indexB;		///< Feature index on shapeB
+	uint8 typeA;		///< The feature type on shapeA
+	uint8 typeB;		///< The feature type on shapeB
+};
+
 /// Contact ids to facilitate warm starting.
 union b2ContactID
 {
-	/// The features that intersect to form the contact point
-	struct Features
-	{
-		uint8 referenceEdge;	///< The edge that defines the outward contact normal.
-		uint8 incidentEdge;		///< The edge most anti-parallel to the reference edge.
-		uint8 incidentVertex;	///< The vertex (0 or 1) on the incident edge that was clipped.
-		uint8 flip;				///< A value of 1 indicates that the reference edge is on shape2.
-	} features;
+	b2ContactFeature cf;
 	uint32 key;					///< Used to quickly compare contact ids.
 };
 
@@ -107,7 +117,7 @@ struct b2WorldManifold
 					const b2Transform& xfA, float32 radiusA,
 					const b2Transform& xfB, float32 radiusB);
 
-	b2Vec2 normal;						///< world vector pointing from A to B
+	b2Vec2 normal;							///< world vector pointing from A to B
 	b2Vec2 points[b2_maxManifoldPoints];	///< world contact point (point of intersection)
 };
 
@@ -165,6 +175,21 @@ struct b2AABB
 		return 0.5f * (upperBound - lowerBound);
 	}
 
+	/// Get the perimeter length
+	float32 GetPerimeter() const
+	{
+		float32 wx = upperBound.x - lowerBound.x;
+		float32 wy = upperBound.y - lowerBound.y;
+		return 2.0f * (wx + wy);
+	}
+
+	/// Combine an AABB into this one.
+	void Combine(const b2AABB& aabb)
+	{
+		lowerBound = b2Min(lowerBound, aabb.lowerBound);
+		upperBound = b2Max(upperBound, aabb.upperBound);
+	}
+
 	/// Combine two AABBs into this one.
 	void Combine(const b2AABB& aabb1, const b2AABB& aabb2)
 	{
@@ -191,26 +216,37 @@ struct b2AABB
 
 /// Compute the collision manifold between two circles.
 void b2CollideCircles(b2Manifold* manifold,
-					  const b2CircleShape* circle1, const b2Transform& xf1,
-					  const b2CircleShape* circle2, const b2Transform& xf2);
+					  const b2CircleShape* circleA, const b2Transform& xfA,
+					  const b2CircleShape* circleB, const b2Transform& xfB);
 
 /// Compute the collision manifold between a polygon and a circle.
 void b2CollidePolygonAndCircle(b2Manifold* manifold,
-							   const b2PolygonShape* polygon, const b2Transform& xf1,
-							   const b2CircleShape* circle, const b2Transform& xf2);
+							   const b2PolygonShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
 
 /// Compute the collision manifold between two polygons.
 void b2CollidePolygons(b2Manifold* manifold,
-					   const b2PolygonShape* polygon1, const b2Transform& xf1,
-					   const b2PolygonShape* polygon2, const b2Transform& xf2);
+					   const b2PolygonShape* polygonA, const b2Transform& xfA,
+					   const b2PolygonShape* polygonB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndCircle(b2Manifold* manifold,
+							   const b2EdgeShape* polygonA, const b2Transform& xfA,
+							   const b2CircleShape* circleB, const b2Transform& xfB);
+
+/// Compute the collision manifold between an edge and a circle.
+void b2CollideEdgeAndPolygon(b2Manifold* manifold,
+							   const b2EdgeShape* edgeA, const b2Transform& xfA,
+							   const b2PolygonShape* circleB, const b2Transform& xfB);
 
 /// Clipping for contact manifolds.
 int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
-							const b2Vec2& normal, float32 offset);
+							const b2Vec2& normal, float32 offset, int32 vertexIndexA);
 
 /// Determine if two generic shapes overlap.
-bool b2TestOverlap(const b2Shape* shapeA, const b2Shape* shapeB,
-				   const b2Transform& xfA, const b2Transform& xfB);
+bool b2TestOverlap(	const b2Shape* shapeA, int32 indexA,
+					const b2Shape* shapeB, int32 indexB,
+					const b2Transform& xfA, const b2Transform& xfB);
 
 // ---------------- Inline Functions ------------------------------------------
 

src/modules/physics/box2d/Box2D/Collision/b2Distance.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2009 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
@@ -18,12 +18,14 @@
 
 #include <Box2D/Collision/b2Distance.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
 // GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
 int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
 
-void b2DistanceProxy::Set(const b2Shape* shape)
+void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 {
 	switch (shape->GetType())
 	{
@@ -45,6 +47,36 @@ void b2DistanceProxy::Set(const b2Shape* shape)
 		}
 		break;
 
+	case b2Shape::e_chain:
+		{
+			const b2ChainShape* chain = (b2ChainShape*)shape;
+			b2Assert(0 <= index && index < chain->GetVertexCount());
+
+			m_buffer[0] = chain->GetVertex(index);
+			if (index + 1 < chain->GetVertexCount())
+			{
+				m_buffer[1] = chain->GetVertex(index + 1);
+			}
+			else
+			{
+				m_buffer[1] = chain->GetVertex(0);
+			}
+
+			m_vertices = m_buffer;
+			m_count = 2;
+			m_radius = chain->m_radius;
+		}
+		break;
+
+	case b2Shape::e_edge:
+		{
+			const b2EdgeShape* edge = (b2EdgeShape*)shape;
+			m_vertices = &edge->m_vertex1;
+			m_count = 2;
+			m_radius = edge->m_radius;
+		}
+		break;
+
 	default:
 		b2Assert(false);
 	}
@@ -500,10 +532,10 @@ void b2Distance(b2DistanceOutput* output,
 
 		// Compute a tentative new simplex vertex using support points.
 		b2SimplexVertex* vertex = vertices + simplex.m_count;
-		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.R, -d));
+		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
 		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
 		b2Vec2 wBLocal;
-		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, d));
+		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
 		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
 		vertex->w = vertex->wB - vertex->wA;
 

src/modules/physics/box2d/Box2D/Collision/b2Distance.h

@@ -1,6 +1,6 @@
 
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -21,7 +21,6 @@
 #define B2_DISTANCE_H
 
 #include <Box2D/Common/b2Math.h>
-#include <climits>
 
 class b2Shape;
 
@@ -33,7 +32,7 @@ struct b2DistanceProxy
 
 	/// Initialize the proxy using the given shape. The shape
 	/// must remain in scope while the proxy is in use.
-	void Set(const b2Shape* shape);
+	void Set(const b2Shape* shape, int32 index);
 
 	/// Get the supporting vertex index in the given direction.
 	int32 GetSupport(const b2Vec2& d) const;
@@ -47,6 +46,7 @@ struct b2DistanceProxy
 	/// Get a vertex by index. Used by b2Distance.
 	const b2Vec2& GetVertex(int32 index) const;
 
+	b2Vec2 m_buffer[2];
 	const b2Vec2* m_vertices;
 	int32 m_count;
 	float32 m_radius;

src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2009 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
@@ -19,6 +19,8 @@
 #include <Box2D/Collision/b2DynamicTree.h>
 #include <cstring>
 #include <cfloat>
+using namespace std;
+
 
 b2DynamicTree::b2DynamicTree()
 {
@@ -26,15 +28,17 @@ b2DynamicTree::b2DynamicTree()
 
 	m_nodeCapacity = 16;
 	m_nodeCount = 0;
-	m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
-	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
+	m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
 
 	// Build a linked list for the free list.
 	for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
 	{
 		m_nodes[i].next = i + 1;
+		m_nodes[i].height = -1;
 	}
 	m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+	m_nodes[m_nodeCapacity-1].height = -1;
 	m_freeList = 0;
 
 	m_path = 0;
@@ -57,10 +61,10 @@ int32 b2DynamicTree::AllocateNode()
 		b2Assert(m_nodeCount == m_nodeCapacity);
 
 		// The free list is empty. Rebuild a bigger pool.
-		b2DynamicTreeNode* oldNodes = m_nodes;
+		b2TreeNode* oldNodes = m_nodes;
 		m_nodeCapacity *= 2;
-		m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
-		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
+		m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
+		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
 		b2Free(oldNodes);
 
 		// Build a linked list for the free list. The parent
@@ -68,8 +72,10 @@ int32 b2DynamicTree::AllocateNode()
 		for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
 		{
 			m_nodes[i].next = i + 1;
+			m_nodes[i].height = -1;
 		}
 		m_nodes[m_nodeCapacity-1].next = b2_nullNode;
+		m_nodes[m_nodeCapacity-1].height = -1;
 		m_freeList = m_nodeCount;
 	}
 
@@ -79,6 +85,8 @@ int32 b2DynamicTree::AllocateNode()
 	m_nodes[nodeId].parent = b2_nullNode;
 	m_nodes[nodeId].child1 = b2_nullNode;
 	m_nodes[nodeId].child2 = b2_nullNode;
+	m_nodes[nodeId].height = 0;
+	m_nodes[nodeId].userData = NULL;
 	++m_nodeCount;
 	return nodeId;
 }
@@ -89,6 +97,7 @@ void b2DynamicTree::FreeNode(int32 nodeId)
 	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
 	b2Assert(0 < m_nodeCount);
 	m_nodes[nodeId].next = m_freeList;
+	m_nodes[nodeId].height = -1;
 	m_freeList = nodeId;
 	--m_nodeCount;
 }
@@ -105,20 +114,10 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
 	m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
 	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
 	m_nodes[proxyId].userData = userData;
+	m_nodes[proxyId].height = 0;
 
 	InsertLeaf(proxyId);
 
-	// Rebalance if necessary.
-	int32 iterationCount = m_nodeCount >> 4;
-	int32 tryCount = 0;
-	int32 height = ComputeHeight();
-	while (height > 64 && tryCount < 10)
-	{
-		Rebalance(iterationCount);
-		height = ComputeHeight();
-		++tryCount;
-	}
-
 	return proxyId;
 }
 
@@ -188,79 +187,133 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
 		return;
 	}
 
-	// Find the best sibling for this node.
-	b2Vec2 center = m_nodes[leaf].aabb.GetCenter();
-	int32 sibling = m_root;
-	if (m_nodes[sibling].IsLeaf() == false)
+	// Find the best sibling for this node
+	b2AABB leafAABB = m_nodes[leaf].aabb;
+	int32 index = m_root;
+	while (m_nodes[index].IsLeaf() == false)
 	{
-		do 
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		float32 area = m_nodes[index].aabb.GetPerimeter();
+
+		b2AABB combinedAABB;
+		combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
+		float32 combinedArea = combinedAABB.GetPerimeter();
+
+		// Cost of creating a new parent for this node and the new leaf
+		float32 cost = 2.0f * combinedArea;
+
+		// Minimum cost of pushing the leaf further down the tree
+		float32 inheritanceCost = 2.0f * (combinedArea - area);
+
+		// Cost of descending into child1
+		float32 cost1;
+		if (m_nodes[child1].IsLeaf())
 		{
-			int32 child1 = m_nodes[sibling].child1;
-			int32 child2 = m_nodes[sibling].child2;
-
-			b2Vec2 delta1 = b2Abs(m_nodes[child1].aabb.GetCenter() - center);
-			b2Vec2 delta2 = b2Abs(m_nodes[child2].aabb.GetCenter() - center);
-
-			float32 norm1 = delta1.x + delta1.y;
-			float32 norm2 = delta2.x + delta2.y;
-
-			if (norm1 < norm2)
-			{
-				sibling = child1;
-			}
-			else
-			{
-				sibling = child2;
-			}
-
-		}
-		while(m_nodes[sibling].IsLeaf() == false);
-	}
-
-	// Create a parent for the siblings.
-	int32 node1 = m_nodes[sibling].parent;
-	int32 node2 = AllocateNode();
-	m_nodes[node2].parent = node1;
-	m_nodes[node2].userData = NULL;
-	m_nodes[node2].aabb.Combine(m_nodes[leaf].aabb, m_nodes[sibling].aabb);
-
-	if (node1 != b2_nullNode)
-	{
-		if (m_nodes[m_nodes[sibling].parent].child1 == sibling)
-		{
-			m_nodes[node1].child1 = node2;
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			cost1 = aabb.GetPerimeter() + inheritanceCost;
 		}
 		else
 		{
-			m_nodes[node1].child2 = node2;
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child1].aabb);
+			float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost1 = (newArea - oldArea) + inheritanceCost;
 		}
 
-		m_nodes[node2].child1 = sibling;
-		m_nodes[node2].child2 = leaf;
-		m_nodes[sibling].parent = node2;
-		m_nodes[leaf].parent = node2;
-
-		do 
+		// Cost of descending into child2
+		float32 cost2;
+		if (m_nodes[child2].IsLeaf())
 		{
-			if (m_nodes[node1].aabb.Contains(m_nodes[node2].aabb))
-			{
-				break;
-			}
-
-			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
-			node2 = node1;
-			node1 = m_nodes[node1].parent;
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			cost2 = aabb.GetPerimeter() + inheritanceCost;
 		}
-		while(node1 != b2_nullNode);
+		else
+		{
+			b2AABB aabb;
+			aabb.Combine(leafAABB, m_nodes[child2].aabb);
+			float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
+			float32 newArea = aabb.GetPerimeter();
+			cost2 = newArea - oldArea + inheritanceCost;
+		}
+
+		// Descend according to the minimum cost.
+		if (cost < cost1 && cost < cost2)
+		{
+			break;
+		}
+
+		// Descend
+		if (cost1 < cost2)
+		{
+			index = child1;
+		}
+		else
+		{
+			index = child2;
+		}
+	}
+
+	int32 sibling = index;
+
+	// Create a new parent.
+	int32 oldParent = m_nodes[sibling].parent;
+	int32 newParent = AllocateNode();
+	m_nodes[newParent].parent = oldParent;
+	m_nodes[newParent].userData = NULL;
+	m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
+	m_nodes[newParent].height = m_nodes[sibling].height + 1;
+
+	if (oldParent != b2_nullNode)
+	{
+		// The sibling was not the root.
+		if (m_nodes[oldParent].child1 == sibling)
+		{
+			m_nodes[oldParent].child1 = newParent;
+		}
+		else
+		{
+			m_nodes[oldParent].child2 = newParent;
+		}
+
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
 	}
 	else
 	{
-		m_nodes[node2].child1 = sibling;
-		m_nodes[node2].child2 = leaf;
-		m_nodes[sibling].parent = node2;
-		m_nodes[leaf].parent = node2;
-		m_root = node2;
+		// The sibling was the root.
+		m_nodes[newParent].child1 = sibling;
+		m_nodes[newParent].child2 = leaf;
+		m_nodes[sibling].parent = newParent;
+		m_nodes[leaf].parent = newParent;
+		m_root = newParent;
 	}
+
+	// Walk back up the tree fixing heights and AABBs
+	index = m_nodes[leaf].parent;
+	while (index != b2_nullNode)
+	{
+		index = Balance(index);
+
+		int32 child1 = m_nodes[index].child1;
+		int32 child2 = m_nodes[index].child2;
+
+		b2Assert(child1 != b2_nullNode);
+		b2Assert(child2 != b2_nullNode);
+
+		m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+		m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+		index = m_nodes[index].parent;
+	}
+
+	//Validate();
 }
 
 void b2DynamicTree::RemoveLeaf(int32 leaf)
@@ -271,89 +324,250 @@ void b2DynamicTree::RemoveLeaf(int32 leaf)
 		return;
 	}
 
-	int32 node2 = m_nodes[leaf].parent;
-	int32 node1 = m_nodes[node2].parent;
+	int32 parent = m_nodes[leaf].parent;
+	int32 grandParent = m_nodes[parent].parent;
 	int32 sibling;
-	if (m_nodes[node2].child1 == leaf)
+	if (m_nodes[parent].child1 == leaf)
 	{
-		sibling = m_nodes[node2].child2;
+		sibling = m_nodes[parent].child2;
 	}
 	else
 	{
-		sibling = m_nodes[node2].child1;
+		sibling = m_nodes[parent].child1;
 	}
 
-	if (node1 != b2_nullNode)
+	if (grandParent != b2_nullNode)
 	{
-		// Destroy node2 and connect node1 to sibling.
-		if (m_nodes[node1].child1 == node2)
+		// Destroy parent and connect sibling to grandParent.
+		if (m_nodes[grandParent].child1 == parent)
 		{
-			m_nodes[node1].child1 = sibling;
+			m_nodes[grandParent].child1 = sibling;
 		}
 		else
 		{
-			m_nodes[node1].child2 = sibling;
+			m_nodes[grandParent].child2 = sibling;
 		}
-		m_nodes[sibling].parent = node1;
-		FreeNode(node2);
+		m_nodes[sibling].parent = grandParent;
+		FreeNode(parent);
 
 		// Adjust ancestor bounds.
-		while (node1 != b2_nullNode)
+		int32 index = grandParent;
+		while (index != b2_nullNode)
 		{
-			b2AABB oldAABB = m_nodes[node1].aabb;
-			m_nodes[node1].aabb.Combine(m_nodes[m_nodes[node1].child1].aabb, m_nodes[m_nodes[node1].child2].aabb);
+			index = Balance(index);
 
-			if (oldAABB.Contains(m_nodes[node1].aabb))
-			{
-				break;
-			}
+			int32 child1 = m_nodes[index].child1;
+			int32 child2 = m_nodes[index].child2;
 
-			node1 = m_nodes[node1].parent;
+			m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+			m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
+
+			index = m_nodes[index].parent;
 		}
 	}
 	else
 	{
 		m_root = sibling;
 		m_nodes[sibling].parent = b2_nullNode;
-		FreeNode(node2);
+		FreeNode(parent);
 	}
+
+	//Validate();
 }
 
-void b2DynamicTree::Rebalance(int32 iterations)
+// Perform a left or right rotation if node A is imbalanced.
+// Returns the new root index.
+int32 b2DynamicTree::Balance(int32 iA)
+{
+	b2Assert(iA != b2_nullNode);
+
+	b2TreeNode* A = m_nodes + iA;
+	if (A->IsLeaf() || A->height < 2)
+	{
+		return iA;
+	}
+
+	int32 iB = A->child1;
+	int32 iC = A->child2;
+	b2Assert(0 <= iB && iB < m_nodeCapacity);
+	b2Assert(0 <= iC && iC < m_nodeCapacity);
+
+	b2TreeNode* B = m_nodes + iB;
+	b2TreeNode* C = m_nodes + iC;
+
+	int32 balance = C->height - B->height;
+
+	// Rotate C up
+	if (balance > 1)
+	{
+		int32 iF = C->child1;
+		int32 iG = C->child2;
+		b2TreeNode* F = m_nodes + iF;
+		b2TreeNode* G = m_nodes + iG;
+		b2Assert(0 <= iF && iF < m_nodeCapacity);
+		b2Assert(0 <= iG && iG < m_nodeCapacity);
+
+		// Swap A and C
+		C->child1 = iA;
+		C->parent = A->parent;
+		A->parent = iC;
+
+		// A's old parent should point to C
+		if (C->parent != b2_nullNode)
+		{
+			if (m_nodes[C->parent].child1 == iA)
+			{
+				m_nodes[C->parent].child1 = iC;
+			}
+			else
+			{
+				b2Assert(m_nodes[C->parent].child2 == iA);
+				m_nodes[C->parent].child2 = iC;
+			}
+		}
+		else
+		{
+			m_root = iC;
+		}
+
+		// Rotate
+		if (F->height > G->height)
+		{
+			C->child2 = iF;
+			A->child2 = iG;
+			G->parent = iA;
+			A->aabb.Combine(B->aabb, G->aabb);
+			C->aabb.Combine(A->aabb, F->aabb);
+
+			A->height = 1 + b2Max(B->height, G->height);
+			C->height = 1 + b2Max(A->height, F->height);
+		}
+		else
+		{
+			C->child2 = iG;
+			A->child2 = iF;
+			F->parent = iA;
+			A->aabb.Combine(B->aabb, F->aabb);
+			C->aabb.Combine(A->aabb, G->aabb);
+
+			A->height = 1 + b2Max(B->height, F->height);
+			C->height = 1 + b2Max(A->height, G->height);
+		}
+
+		return iC;
+	}
+	
+	// Rotate B up
+	if (balance < -1)
+	{
+		int32 iD = B->child1;
+		int32 iE = B->child2;
+		b2TreeNode* D = m_nodes + iD;
+		b2TreeNode* E = m_nodes + iE;
+		b2Assert(0 <= iD && iD < m_nodeCapacity);
+		b2Assert(0 <= iE && iE < m_nodeCapacity);
+
+		// Swap A and B
+		B->child1 = iA;
+		B->parent = A->parent;
+		A->parent = iB;
+
+		// A's old parent should point to B
+		if (B->parent != b2_nullNode)
+		{
+			if (m_nodes[B->parent].child1 == iA)
+			{
+				m_nodes[B->parent].child1 = iB;
+			}
+			else
+			{
+				b2Assert(m_nodes[B->parent].child2 == iA);
+				m_nodes[B->parent].child2 = iB;
+			}
+		}
+		else
+		{
+			m_root = iB;
+		}
+
+		// Rotate
+		if (D->height > E->height)
+		{
+			B->child2 = iD;
+			A->child1 = iE;
+			E->parent = iA;
+			A->aabb.Combine(C->aabb, E->aabb);
+			B->aabb.Combine(A->aabb, D->aabb);
+
+			A->height = 1 + b2Max(C->height, E->height);
+			B->height = 1 + b2Max(A->height, D->height);
+		}
+		else
+		{
+			B->child2 = iE;
+			A->child1 = iD;
+			D->parent = iA;
+			A->aabb.Combine(C->aabb, D->aabb);
+			B->aabb.Combine(A->aabb, E->aabb);
+
+			A->height = 1 + b2Max(C->height, D->height);
+			B->height = 1 + b2Max(A->height, E->height);
+		}
+
+		return iB;
+	}
+
+	return iA;
+}
+
+int32 b2DynamicTree::GetHeight() const
 {
 	if (m_root == b2_nullNode)
 	{
-		return;
+		return 0;
 	}
 
-	for (int32 i = 0; i < iterations; ++i)
+	return m_nodes[m_root].height;
+}
+
+//
+float32 b2DynamicTree::GetAreaRatio() const
+{
+	if (m_root == b2_nullNode)
 	{
-		int32 node = m_root;
-
-		uint32 bit = 0;
-		while (m_nodes[node].IsLeaf() == false)
-		{
-			int32* children = &m_nodes[node].child1;
-			node = children[(m_path >> bit) & 1];
-			bit = (bit + 1) & (8* sizeof(uint32) - 1);
-		}
-		++m_path;
-
-		RemoveLeaf(node);
-		InsertLeaf(node);
+		return 0.0f;
 	}
+
+	const b2TreeNode* root = m_nodes + m_root;
+	float32 rootArea = root->aabb.GetPerimeter();
+
+	float32 totalArea = 0.0f;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height < 0)
+		{
+			// Free node in pool
+			continue;
+		}
+
+		totalArea += node->aabb.GetPerimeter();
+	}
+
+	return totalArea / rootArea;
 }
 
 // Compute the height of a sub-tree.
 int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
 {
-	if (nodeId == b2_nullNode)
+	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+	b2TreeNode* node = m_nodes + nodeId;
+
+	if (node->IsLeaf())
 	{
 		return 0;
 	}
 
-	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
-	b2DynamicTreeNode* node = m_nodes + nodeId;
 	int32 height1 = ComputeHeight(node->child1);
 	int32 height2 = ComputeHeight(node->child2);
 	return 1 + b2Max(height1, height2);
@@ -361,5 +575,197 @@ int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
 
 int32 b2DynamicTree::ComputeHeight() const
 {
-	return ComputeHeight(m_root);
+	int32 height = ComputeHeight(m_root);
+	return height;
+}
+
+void b2DynamicTree::ValidateStructure(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	if (index == m_root)
+	{
+		b2Assert(m_nodes[index].parent == b2_nullNode);
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	b2Assert(m_nodes[child1].parent == index);
+	b2Assert(m_nodes[child2].parent == index);
+
+	ValidateStructure(child1);
+	ValidateStructure(child2);
+}
+
+void b2DynamicTree::ValidateMetrics(int32 index) const
+{
+	if (index == b2_nullNode)
+	{
+		return;
+	}
+
+	const b2TreeNode* node = m_nodes + index;
+
+	int32 child1 = node->child1;
+	int32 child2 = node->child2;
+
+	if (node->IsLeaf())
+	{
+		b2Assert(child1 == b2_nullNode);
+		b2Assert(child2 == b2_nullNode);
+		b2Assert(node->height == 0);
+		return;
+	}
+
+	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
+	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
+
+	int32 height1 = m_nodes[child1].height;
+	int32 height2 = m_nodes[child2].height;
+	int32 height;
+	height = 1 + b2Max(height1, height2);
+	b2Assert(node->height == height);
+
+	b2AABB aabb;
+	aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
+
+	b2Assert(aabb.lowerBound == node->aabb.lowerBound);
+	b2Assert(aabb.upperBound == node->aabb.upperBound);
+
+	ValidateMetrics(child1);
+	ValidateMetrics(child2);
+}
+
+void b2DynamicTree::Validate() const
+{
+	ValidateStructure(m_root);
+	ValidateMetrics(m_root);
+
+	int32 freeCount = 0;
+	int32 freeIndex = m_freeList;
+	while (freeIndex != b2_nullNode)
+	{
+		b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
+		freeIndex = m_nodes[freeIndex].next;
+		++freeCount;
+	}
+
+	b2Assert(GetHeight() == ComputeHeight());
+
+	b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
+}
+
+int32 b2DynamicTree::GetMaxBalance() const
+{
+	int32 maxBalance = 0;
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		const b2TreeNode* node = m_nodes + i;
+		if (node->height <= 1)
+		{
+			continue;
+		}
+
+		b2Assert(node->IsLeaf() == false);
+
+		int32 child1 = node->child1;
+		int32 child2 = node->child2;
+		int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
+		maxBalance = b2Max(maxBalance, balance);
+	}
+
+	return maxBalance;
+}
+
+void b2DynamicTree::RebuildBottomUp()
+{
+	int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
+	int32 count = 0;
+
+	// Build array of leaves. Free the rest.
+	for (int32 i = 0; i < m_nodeCapacity; ++i)
+	{
+		if (m_nodes[i].height < 0)
+		{
+			// free node in pool
+			continue;
+		}
+
+		if (m_nodes[i].IsLeaf())
+		{
+			m_nodes[i].parent = b2_nullNode;
+			nodes[count] = i;
+			++count;
+		}
+		else
+		{
+			FreeNode(i);
+		}
+	}
+
+	while (count > 1)
+	{
+		float32 minCost = b2_maxFloat;
+		int32 iMin = -1, jMin = -1;
+		for (int32 i = 0; i < count; ++i)
+		{
+			b2AABB aabbi = m_nodes[nodes[i]].aabb;
+
+			for (int32 j = i + 1; j < count; ++j)
+			{
+				b2AABB aabbj = m_nodes[nodes[j]].aabb;
+				b2AABB b;
+				b.Combine(aabbi, aabbj);
+				float32 cost = b.GetPerimeter();
+				if (cost < minCost)
+				{
+					iMin = i;
+					jMin = j;
+					minCost = cost;
+				}
+			}
+		}
+
+		int32 index1 = nodes[iMin];
+		int32 index2 = nodes[jMin];
+		b2TreeNode* child1 = m_nodes + index1;
+		b2TreeNode* child2 = m_nodes + index2;
+
+		int32 parentIndex = AllocateNode();
+		b2TreeNode* parent = m_nodes + parentIndex;
+		parent->child1 = index1;
+		parent->child2 = index2;
+		parent->height = 1 + b2Max(child1->height, child2->height);
+		parent->aabb.Combine(child1->aabb, child2->aabb);
+		parent->parent = b2_nullNode;
+
+		child1->parent = parentIndex;
+		child2->parent = parentIndex;
+
+		nodes[jMin] = nodes[count-1];
+		nodes[iMin] = parentIndex;
+		--count;
+	}
+
+	m_root = nodes[0];
+	b2Free(nodes);
+
+	Validate();
 }

src/modules/physics/box2d/Box2D/Collision/b2DynamicTree.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2009 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
@@ -20,23 +20,21 @@
 #define B2_DYNAMIC_TREE_H
 
 #include <Box2D/Collision/b2Collision.h>
-
-/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+#include <Box2D/Common/b2GrowableStack.h>
 
 #define b2_nullNode (-1)
 
 /// A node in the dynamic tree. The client does not interact with this directly.
-struct b2DynamicTreeNode
+struct b2TreeNode
 {
 	bool IsLeaf() const
 	{
 		return child1 == b2_nullNode;
 	}
 
-	/// This is the fattened AABB.
+	/// Enlarged AABB
 	b2AABB aabb;
 
-	//int32 userData;
 	void* userData;
 
 	union
@@ -47,8 +45,12 @@ struct b2DynamicTreeNode
 
 	int32 child1;
 	int32 child2;
+
+	// leaf = 0, free node = -1
+	int32 height;
 };
 
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
 /// A dynamic tree arranges data in a binary tree to accelerate
 /// queries such as volume queries and ray casts. Leafs are proxies
 /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
@@ -59,7 +61,6 @@ struct b2DynamicTreeNode
 class b2DynamicTree
 {
 public:
-
 	/// Constructing the tree initializes the node pool.
 	b2DynamicTree();
 
@@ -78,9 +79,6 @@ public:
 	/// @return true if the proxy was re-inserted.
 	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
 
-	/// Perform some iterations to re-balance the tree.
-	void Rebalance(int32 iterations);
-
 	/// Get proxy user data.
 	/// @return the proxy user data or 0 if the id is invalid.
 	void* GetUserData(int32 proxyId) const;
@@ -88,9 +86,6 @@ public:
 	/// Get the fat AABB for a proxy.
 	const b2AABB& GetFatAABB(int32 proxyId) const;
 
-	/// Compute the height of the tree.
-	int32 ComputeHeight() const;
-
 	/// Query an AABB for overlapping proxies. The callback class
 	/// is called for each proxy that overlaps the supplied AABB.
 	template <typename T>
@@ -106,6 +101,23 @@ public:
 	template <typename T>
 	void RayCast(T* callback, const b2RayCastInput& input) const;
 
+	/// Validate this tree. For testing.
+	void Validate() const;
+
+	/// Compute the height of the binary tree in O(N) time. Should not be
+	/// called often.
+	int32 GetHeight() const;
+
+	/// Get the maximum balance of an node in the tree. The balance is the difference
+	/// in height of the two children of a node.
+	int32 GetMaxBalance() const;
+
+	/// Get the ratio of the sum of the node areas to the root area.
+	float32 GetAreaRatio() const;
+
+	/// Build an optimal tree. Very expensive. For testing.
+	void RebuildBottomUp();
+
 private:
 
 	int32 AllocateNode();
@@ -114,17 +126,23 @@ private:
 	void InsertLeaf(int32 node);
 	void RemoveLeaf(int32 node);
 
+	int32 Balance(int32 index);
+
+	int32 ComputeHeight() const;
 	int32 ComputeHeight(int32 nodeId) const;
 
+	void ValidateStructure(int32 index) const;
+	void ValidateMetrics(int32 index) const;
+
 	int32 m_root;
 
-	b2DynamicTreeNode* m_nodes;
+	b2TreeNode* m_nodes;
 	int32 m_nodeCount;
 	int32 m_nodeCapacity;
 
 	int32 m_freeList;
 
-	/// This is used incrementally traverse the tree for re-balancing.
+	/// This is used to incrementally traverse the tree for re-balancing.
 	uint32 m_path;
 
 	int32 m_insertionCount;
@@ -145,21 +163,18 @@ inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
 template <typename T>
 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
 {
-	const int32 k_stackSize = 128;
-	int32 stack[k_stackSize];
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
 
-	int32 count = 0;
-	stack[count++] = m_root;
-
-	while (count > 0)
+	while (stack.GetCount() > 0)
 	{
-		int32 nodeId = stack[--count];
+		int32 nodeId = stack.Pop();
 		if (nodeId == b2_nullNode)
 		{
 			continue;
 		}
 
-		const b2DynamicTreeNode* node = m_nodes + nodeId;
+		const b2TreeNode* node = m_nodes + nodeId;
 
 		if (b2TestOverlap(node->aabb, aabb))
 		{
@@ -173,15 +188,8 @@ inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
 			}
 			else
 			{
-				if (count < k_stackSize)
-				{
-					stack[count++] = node->child1;
-				}
-
-				if (count < k_stackSize)
-				{
-					stack[count++] = node->child2;
-				}
+				stack.Push(node->child1);
+				stack.Push(node->child2);
 			}
 		}
 	}
@@ -213,21 +221,18 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
 		segmentAABB.upperBound = b2Max(p1, t);
 	}
 
-	const int32 k_stackSize = 128;
-	int32 stack[k_stackSize];
+	b2GrowableStack<int32, 256> stack;
+	stack.Push(m_root);
 
-	int32 count = 0;
-	stack[count++] = m_root;
-
-	while (count > 0)
+	while (stack.GetCount() > 0)
 	{
-		int32 nodeId = stack[--count];
+		int32 nodeId = stack.Pop();
 		if (nodeId == b2_nullNode)
 		{
 			continue;
 		}
 
-		const b2DynamicTreeNode* node = m_nodes + nodeId;
+		const b2TreeNode* node = m_nodes + nodeId;
 
 		if (b2TestOverlap(node->aabb, segmentAABB) == false)
 		{
@@ -270,15 +275,8 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
 		}
 		else
 		{
-			if (count < k_stackSize)
-			{
-				stack[count++] = node->child1;
-			}
-
-			if (count < k_stackSize)
-			{
-				stack[count++] = node->child2;
-			}
+			stack.Push(node->child1);
+			stack.Push(node->child2);
 		}
 	}
 }

src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2009 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
@@ -23,12 +23,11 @@
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 
 #include <cstdio>
+using namespace std;
 
 int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
 int32 b2_toiRootIters, b2_toiMaxRootIters;
 
-int32 b2_toiMaxOptIters;
-
 struct b2SeparationFunction
 {
 	enum Type
@@ -42,7 +41,8 @@ struct b2SeparationFunction
 
 	float32 Initialize(const b2SimplexCache* cache,
 		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
-		const b2DistanceProxy* proxyB, const b2Sweep& sweepB)
+		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
+		float32 t1)
 	{
 		m_proxyA = proxyA;
 		m_proxyB = proxyB;
@@ -53,8 +53,8 @@ struct b2SeparationFunction
 		m_sweepB = sweepB;
 
 		b2Transform xfA, xfB;
-		m_sweepA.GetTransform(&xfA, 0.0f);
-		m_sweepB.GetTransform(&xfB, 0.0f);
+		m_sweepA.GetTransform(&xfA, t1);
+		m_sweepB.GetTransform(&xfB, t1);
 
 		if (count == 1)
 		{
@@ -76,7 +76,7 @@ struct b2SeparationFunction
 
 			m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
 			m_axis.Normalize();
-			b2Vec2 normal = b2Mul(xfB.R, m_axis);
+			b2Vec2 normal = b2Mul(xfB.q, m_axis);
 
 			m_localPoint = 0.5f * (localPointB1 + localPointB2);
 			b2Vec2 pointB = b2Mul(xfB, m_localPoint);
@@ -101,7 +101,7 @@ struct b2SeparationFunction
 			
 			m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
 			m_axis.Normalize();
-			b2Vec2 normal = b2Mul(xfA.R, m_axis);
+			b2Vec2 normal = b2Mul(xfA.q, m_axis);
 
 			m_localPoint = 0.5f * (localPointA1 + localPointA2);
 			b2Vec2 pointA = b2Mul(xfA, m_localPoint);
@@ -129,8 +129,8 @@ struct b2SeparationFunction
 		{
 		case e_points:
 			{
-				b2Vec2 axisA = b2MulT(xfA.R,  m_axis);
-				b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
+				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
+				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
 
 				*indexA = m_proxyA->GetSupport(axisA);
 				*indexB = m_proxyB->GetSupport(axisB);
@@ -147,10 +147,10 @@ struct b2SeparationFunction
 
 		case e_faceA:
 			{
-				b2Vec2 normal = b2Mul(xfA.R, m_axis);
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
 				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
 
-				b2Vec2 axisB = b2MulT(xfB.R, -normal);
+				b2Vec2 axisB = b2MulT(xfB.q, -normal);
 				
 				*indexA = -1;
 				*indexB = m_proxyB->GetSupport(axisB);
@@ -164,10 +164,10 @@ struct b2SeparationFunction
 
 		case e_faceB:
 			{
-				b2Vec2 normal = b2Mul(xfB.R, m_axis);
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
 				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
 
-				b2Vec2 axisA = b2MulT(xfA.R, -normal);
+				b2Vec2 axisA = b2MulT(xfA.q, -normal);
 
 				*indexB = -1;
 				*indexA = m_proxyA->GetSupport(axisA);
@@ -197,8 +197,8 @@ struct b2SeparationFunction
 		{
 		case e_points:
 			{
-				b2Vec2 axisA = b2MulT(xfA.R,  m_axis);
-				b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
+				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);
@@ -212,10 +212,10 @@ struct b2SeparationFunction
 
 		case e_faceA:
 			{
-				b2Vec2 normal = b2Mul(xfA.R, m_axis);
+				b2Vec2 normal = b2Mul(xfA.q, m_axis);
 				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
 
-				b2Vec2 axisB = b2MulT(xfB.R, -normal);
+				b2Vec2 axisB = b2MulT(xfB.q, -normal);
 
 				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
 				b2Vec2 pointB = b2Mul(xfB, localPointB);
@@ -226,10 +226,10 @@ struct b2SeparationFunction
 
 		case e_faceB:
 			{
-				b2Vec2 normal = b2Mul(xfB.R, m_axis);
+				b2Vec2 normal = b2Mul(xfB.q, m_axis);
 				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
 
-				b2Vec2 axisA = b2MulT(xfA.R, -normal);
+				b2Vec2 axisA = b2MulT(xfA.q, -normal);
 
 				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
 				b2Vec2 pointA = b2Mul(xfA, localPointA);
@@ -325,7 +325,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
 
 		// Initialize the separating axis.
 		b2SeparationFunction fcn;
-		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB);
+		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
 #if 0
 		// Dump the curve seen by the root finder
 		{

src/modules/physics/box2d/Box2D/Collision/b2TimeOfImpact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -21,7 +21,6 @@
 
 #include <Box2D/Common/b2Math.h>
 #include <Box2D/Collision/b2Distance.h>
-#include <climits>
 
 /// Input parameters for b2TimeOfImpact
 struct b2TOIInput

src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -21,6 +21,7 @@
 #include <climits>
 #include <cstring>
 #include <memory>
+using namespace std;
 
 int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] = 
 {
@@ -100,7 +101,12 @@ void* b2BlockAllocator::Allocate(int32 size)
 	if (size == 0)
 		return NULL;
 
-	b2Assert(0 < size && size <= b2_maxBlockSize);
+	b2Assert(0 < size);
+
+	if (size > b2_maxBlockSize)
+	{
+		return b2Alloc(size);
+	}
 
 	int32 index = s_blockSizeLookup[size];
 	b2Assert(0 <= index && index < b2_blockSizes);
@@ -155,7 +161,13 @@ void b2BlockAllocator::Free(void* p, int32 size)
 		return;
 	}
 
-	b2Assert(0 < size && size <= b2_maxBlockSize);
+	b2Assert(0 < size);
+
+	if (size > b2_maxBlockSize)
+	{
+		b2Free(p);
+		return;
+	}
 
 	int32 index = s_blockSizeLookup[size];
 	b2Assert(0 <= index && index < b2_blockSizes);

src/modules/physics/box2d/Box2D/Common/b2BlockAllocator.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -21,7 +21,7 @@
 
 #include <Box2D/Common/b2Settings.h>
 
-const int32 b2_chunkSize = 4096;
+const int32 b2_chunkSize = 16 * 1024;
 const int32 b2_maxBlockSize = 640;
 const int32 b2_blockSizes = 14;
 const int32 b2_chunkArrayIncrement = 128;
@@ -29,16 +29,19 @@ const int32 b2_chunkArrayIncrement = 128;
 struct b2Block;
 struct b2Chunk;
 
-// This is a small object allocator used for allocating small
-// objects that persist for more than one time step.
-// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
+/// This is a small object allocator used for allocating small
+/// objects that persist for more than one time step.
+/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
 class b2BlockAllocator
 {
 public:
 	b2BlockAllocator();
 	~b2BlockAllocator();
 
+	/// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
 	void* Allocate(int32 size);
+
+	/// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
 	void Free(void* p, int32 size);
 
 	void Clear();

src/modules/physics/box2d/Box2D/Common/b2Draw.cpp

@@ -0,0 +1,44 @@
+/*
+* Copyright (c) 2011 Erin Catto http://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/Common/b2Draw.h>
+
+b2Draw::b2Draw()
+{
+	m_drawFlags = 0;
+}
+
+void b2Draw::SetFlags(uint32 flags)
+{
+	m_drawFlags = flags;
+}
+
+uint32 b2Draw::GetFlags() const
+{
+	return m_drawFlags;
+}
+
+void b2Draw::AppendFlags(uint32 flags)
+{
+	m_drawFlags |= flags;
+}
+
+void b2Draw::ClearFlags(uint32 flags)
+{
+	m_drawFlags &= ~flags;
+}

src/modules/physics/box2d/Box2D/Common/b2Draw.h

@@ -0,0 +1,81 @@
+/*
+* Copyright (c) 2011 Erin Catto http://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/Common/b2Math.h>
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+	b2Color() {}
+	b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
+	void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }
+	float32 r, g, b;
+};
+
+/// Implement and register this class with a b2World to provide debug drawing of physics
+/// entities in your game.
+class b2Draw
+{
+public:
+	b2Draw();
+
+	virtual ~b2Draw() {}
+
+	enum
+	{
+		e_shapeBit				= 0x0001,	///< draw shapes
+		e_jointBit				= 0x0002,	///< draw joint connections
+		e_aabbBit				= 0x0004,	///< draw axis aligned bounding boxes
+		e_pairBit				= 0x0008,	///< draw broad-phase pairs
+		e_centerOfMassBit		= 0x0010	///< draw center of mass frame
+	};
+
+	/// Set the drawing flags.
+	void SetFlags(uint32 flags);
+
+	/// Get the drawing flags.
+	uint32 GetFlags() const;
+	
+	/// Append flags to the current flags.
+	void AppendFlags(uint32 flags);
+
+	/// Clear flags from the current flags.
+	void ClearFlags(uint32 flags);
+
+	/// Draw a closed polygon provided in CCW order.
+	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a solid closed polygon provided in CCW order.
+	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
+
+	/// Draw a circle.
+	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
+	
+	/// Draw a solid circle.
+	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
+	
+	/// Draw a line segment.
+	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
+
+	/// Draw a transform. Choose your own length scale.
+	/// @param xf a transform.
+	virtual void DrawTransform(const b2Transform& xf) = 0;
+
+protected:
+	uint32 m_drawFlags;
+};

src/modules/physics/box2d/Box2D/Common/b2GrowableStack.h

@@ -0,0 +1,85 @@
+/*
+* Copyright (c) 2010 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_GROWABLE_STACK_H
+#define B2_GROWABLE_STACK_H
+#include <Box2D/Common/b2Settings.h>
+#include <cstring>
+
+/// This is a growable LIFO stack with an initial capacity of N.
+/// If the stack size exceeds the initial capacity, the heap is used
+/// to increase the size of the stack.
+template <typename T, int32 N>
+class b2GrowableStack
+{
+public:
+	b2GrowableStack()
+	{
+		m_stack = m_array;
+		m_count = 0;
+		m_capacity = N;
+	}
+
+	~b2GrowableStack()
+	{
+		if (m_stack != m_array)
+		{
+			b2Free(m_stack);
+			m_stack = NULL;
+		}
+	}
+
+	void Push(const T& element)
+	{
+		if (m_count == m_capacity)
+		{
+			T* old = m_stack;
+			m_capacity *= 2;
+			m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
+			std::memcpy(m_stack, old, m_count * sizeof(T));
+			if (old != m_array)
+			{
+				b2Free(old);
+			}
+		}
+
+		m_stack[m_count] = element;
+		++m_count;
+	}
+
+	T Pop()
+	{
+		b2Assert(m_count > 0);
+		--m_count;
+		return m_stack[m_count];
+	}
+
+	int32 GetCount()
+	{
+		return m_count;
+	}
+
+private:
+	T* m_stack;
+	T m_array[N];
+	int32 m_count;
+	int32 m_capacity;
+};
+
+
+#endif

src/modules/physics/box2d/Box2D/Common/b2Math.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2009 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
@@ -19,22 +19,20 @@
 #include <Box2D/Common/b2Math.h>
 
 const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
-const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f);
-const b2Transform b2Transform_identity(b2Vec2_zero, b2Mat22_identity);
 
 /// Solve A * x = b, where b is a column vector. This is more efficient
 /// than computing the inverse in one-shot cases.
 b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
 {
-	float32 det = b2Dot(col1, b2Cross(col2, col3));
+	float32 det = b2Dot(ex, b2Cross(ey, ez));
 	if (det != 0.0f)
 	{
 		det = 1.0f / det;
 	}
 	b2Vec3 x;
-	x.x = det * b2Dot(b, b2Cross(col2, col3));
-	x.y = det * b2Dot(col1, b2Cross(b, col3));
-	x.z = det * b2Dot(col1, b2Cross(col2, b));
+	x.x = det * b2Dot(b, b2Cross(ey, ez));
+	x.y = det * b2Dot(ex, b2Cross(b, ez));
+	x.z = det * b2Dot(ex, b2Cross(ey, b));
 	return x;
 }
 
@@ -42,7 +40,7 @@ b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
 /// than computing the inverse in one-shot cases.
 b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
 {
-	float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+	float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
 	float32 det = a11 * a22 - a12 * a21;
 	if (det != 0.0f)
 	{

src/modules/physics/box2d/Box2D/Common/b2Math.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -57,13 +57,8 @@ inline float32 b2InvSqrt(float32 x)
 	return x;
 }
 
-#define	b2Sqrt(x)	sqrtf(x)
-#define	b2Atan2(y, x)	atan2f(y, x)
-
-inline float32 b2Abs(float32 a)
-{
-	return a > 0.0f ? a : -a;
-}
+#define	b2Sqrt(x)	std::sqrt(x)
+#define	b2Atan2(y, x)	std::atan2(y, x)
 
 /// A 2D column vector.
 struct b2Vec2
@@ -147,6 +142,12 @@ struct b2Vec2
 		return b2IsValid(x) && b2IsValid(y);
 	}
 
+	/// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
+	b2Vec2 Skew() const
+	{
+		return b2Vec2(-y, x);
+	}
+
 	float32 x, y;
 };
 
@@ -198,75 +199,49 @@ struct b2Mat22
 	/// Construct this matrix using columns.
 	b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
 	{
-		col1 = c1;
-		col2 = c2;
+		ex = c1;
+		ey = c2;
 	}
 
 	/// Construct this matrix using scalars.
 	b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
 	{
-		col1.x = a11; col1.y = a21;
-		col2.x = a12; col2.y = a22;
-	}
-
-	/// Construct this matrix using an angle. This matrix becomes
-	/// an orthonormal rotation matrix.
-	explicit b2Mat22(float32 angle)
-	{
-		// TODO_ERIN compute sin+cos together.
-		float32 c = cosf(angle), s = sinf(angle);
-		col1.x = c; col2.x = -s;
-		col1.y = s; col2.y = c;
+		ex.x = a11; ex.y = a21;
+		ey.x = a12; ey.y = a22;
 	}
 
 	/// Initialize this matrix using columns.
 	void Set(const b2Vec2& c1, const b2Vec2& c2)
 	{
-		col1 = c1;
-		col2 = c2;
-	}
-
-	/// Initialize this matrix using an angle. This matrix becomes
-	/// an orthonormal rotation matrix.
-	void Set(float32 angle)
-	{
-		float32 c = cosf(angle), s = sinf(angle);
-		col1.x = c; col2.x = -s;
-		col1.y = s; col2.y = c;
+		ex = c1;
+		ey = c2;
 	}
 
 	/// Set this to the identity matrix.
 	void SetIdentity()
 	{
-		col1.x = 1.0f; col2.x = 0.0f;
-		col1.y = 0.0f; col2.y = 1.0f;
+		ex.x = 1.0f; ey.x = 0.0f;
+		ex.y = 0.0f; ey.y = 1.0f;
 	}
 
 	/// Set this matrix to all zeros.
 	void SetZero()
 	{
-		col1.x = 0.0f; col2.x = 0.0f;
-		col1.y = 0.0f; col2.y = 0.0f;
-	}
-
-	/// Extract the angle from this matrix (assumed to be
-	/// a rotation matrix).
-	float32 GetAngle() const
-	{
-		return b2Atan2(col1.y, col1.x);
+		ex.x = 0.0f; ey.x = 0.0f;
+		ex.y = 0.0f; ey.y = 0.0f;
 	}
 
 	b2Mat22 GetInverse() const
 	{
-		float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
+		float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
 		b2Mat22 B;
 		float32 det = a * d - b * c;
 		if (det != 0.0f)
 		{
 			det = 1.0f / det;
 		}
-		B.col1.x =  det * d;	B.col2.x = -det * b;
-		B.col1.y = -det * c;	B.col2.y =  det * a;
+		B.ex.x =  det * d;	B.ey.x = -det * b;
+		B.ex.y = -det * c;	B.ey.y =  det * a;
 		return B;
 	}
 
@@ -274,7 +249,7 @@ struct b2Mat22
 	/// than computing the inverse in one-shot cases.
 	b2Vec2 Solve(const b2Vec2& b) const
 	{
-		float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+		float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
 		float32 det = a11 * a22 - a12 * a21;
 		if (det != 0.0f)
 		{
@@ -286,7 +261,7 @@ struct b2Mat22
 		return x;
 	}
 
-	b2Vec2 col1, col2;
+	b2Vec2 ex, ey;
 };
 
 /// A 3-by-3 matrix. Stored in column-major order.
@@ -298,17 +273,17 @@ struct b2Mat33
 	/// Construct this matrix using columns.
 	b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
 	{
-		col1 = c1;
-		col2 = c2;
-		col3 = c3;
+		ex = c1;
+		ey = c2;
+		ez = c3;
 	}
 
 	/// Set this matrix to all zeros.
 	void SetZero()
 	{
-		col1.SetZero();
-		col2.SetZero();
-		col3.SetZero();
+		ex.SetZero();
+		ey.SetZero();
+		ez.SetZero();
 	}
 
 	/// Solve A * x = b, where b is a column vector. This is more efficient
@@ -320,41 +295,85 @@ struct b2Mat33
 	/// 2-by-2 matrix equation.
 	b2Vec2 Solve22(const b2Vec2& b) const;
 
-	b2Vec3 col1, col2, col3;
+	b2Vec3 ex, ey, ez;
+};
+
+/// Rotation
+struct b2Rot
+{
+	b2Rot() {}
+
+	/// Initialize from an angle in radians
+	explicit b2Rot(float32 angle)
+	{
+		/// TODO_ERIN optimize
+		s = sinf(angle);
+		c = cosf(angle);
+	}
+
+	/// Set using an angle in radians.
+	void Set(float32 angle)
+	{
+		/// TODO_ERIN optimize
+		s = sinf(angle);
+		c = cosf(angle);
+	}
+
+	/// Set to the identity rotation
+	void SetIdentity()
+	{
+		s = 0.0f;
+		c = 1.0f;
+	}
+
+	/// Get the angle in radians
+	float32 GetAngle() const
+	{
+		return b2Atan2(s, c);
+	}
+
+	/// Get the x-axis
+	b2Vec2 GetXAxis() const
+	{
+		return b2Vec2(c, s);
+	}
+
+	/// Get the u-axis
+	b2Vec2 GetYAxis() const
+	{
+		return b2Vec2(-s, c);
+	}
+
+	/// Sine and cosine
+	float32 s, c;
 };
 
 /// A transform contains translation and rotation. It is used to represent
 /// the position and orientation of rigid frames.
 struct b2Transform
 {
-	/// The default constructor does nothing (for performance).
+	/// The default constructor does nothing.
 	b2Transform() {}
 
-	/// Initialize using a position vector and a rotation matrix.
-	b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
+	/// Initialize using a position vector and a rotation.
+	b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
 
 	/// Set this to the identity transform.
 	void SetIdentity()
 	{
-		position.SetZero();
-		R.SetIdentity();
+		p.SetZero();
+		q.SetIdentity();
 	}
 
 	/// Set this based on the position and angle.
-	void Set(const b2Vec2& p, float32 angle)
+	void Set(const b2Vec2& position, float32 angle)
 	{
-		position = p;
-		R.Set(angle);
+		p = position;
+		q.Set(angle);
 	}
 
-	/// Calculate the angle that the rotation matrix represents.
-	float32 GetAngle() const
-	{
-		return b2Atan2(R.col1.y, R.col1.x);
-	}
-
-	b2Vec2 position;
-	b2Mat22 R;
+	b2Vec2 p;
+	b2Rot q;
 };
 
 /// This describes the motion of a body/shape for TOI computation.
@@ -364,12 +383,12 @@ struct b2Transform
 struct b2Sweep
 {
 	/// Get the interpolated transform at a specific time.
-	/// @param alpha is a factor in [0,1], where 0 indicates t0.
-	void GetTransform(b2Transform* xf, float32 alpha) const;
+	/// @param beta is a factor in [0,1], where 0 indicates alpha0.
+	void GetTransform(b2Transform* xfb, float32 beta) const;
 
 	/// Advance the sweep forward, yielding a new initial state.
-	/// @param t the new initial time.
-	void Advance(float32 t);
+	/// @param alpha the new initial time.
+	void Advance(float32 alpha);
 
 	/// Normalize the angles.
 	void Normalize();
@@ -377,12 +396,14 @@ struct b2Sweep
 	b2Vec2 localCenter;	///< local center of mass position
 	b2Vec2 c0, c;		///< center world positions
 	float32 a0, a;		///< world angles
+
+	/// Fraction of the current time step in the range [0,1]
+	/// c0 and a0 are the positions at alpha0.
+	float32 alpha0;
 };
 
-
+/// Useful constant
 extern const b2Vec2 b2Vec2_zero;
-extern const b2Mat22 b2Mat22_identity;
-extern const b2Transform b2Transform_identity;
 
 /// Perform the dot product on two vectors.
 inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
@@ -414,14 +435,14 @@ inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
 /// then this transforms the vector from one frame to another.
 inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
 {
-	return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
+	return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
 }
 
 /// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
 /// then this transforms the vector from one frame to another (inverse transform).
 inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
 {
-	return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
+	return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
 }
 
 /// Add two vectors component-wise.
@@ -489,40 +510,109 @@ inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
 
 inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
 {
-	return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
+	return b2Mat22(A.ex + B.ex, A.ey + B.ey);
 }
 
 // A * B
 inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
 {
-	return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
+	return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
 }
 
 // A^T * B
 inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
 {
-	b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
-	b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
+	b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
+	b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
 	return b2Mat22(c1, c2);
 }
 
 /// Multiply a matrix times a vector.
 inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
 {
-	return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
+	return v.x * A.ex + v.y * A.ey + v.z * A.ez;
+}
+
+/// Multiply two rotations: q * r
+inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
+{
+	// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
+	// [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
+	// s = qs * rc + qc * rs
+	// c = qc * rc - qs * rs
+	b2Rot qr;
+	qr.s = q.s * r.c + q.c * r.s;
+	qr.c = q.c * r.c - q.s * r.s;
+	return qr;
+}
+
+/// Transpose multiply two rotations: qT * r
+inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
+{
+	// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
+	// [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
+	// s = qc * rs - qs * rc
+	// c = qc * rc + qs * rs
+	b2Rot qr;
+	qr.s = q.c * r.s - q.s * r.c;
+	qr.c = q.c * r.c + q.s * r.s;
+	return qr;
+}
+
+/// Rotate a vector
+inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
+{
+	return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
+}
+
+/// Inverse rotate a vector
+inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
+{
+	return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
 }
 
 inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
 {
-	float32 x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
-	float32 y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.y;
+	float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
+	float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
 
 	return b2Vec2(x, y);
 }
 
 inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
 {
-	return b2MulT(T.R, v - T.position);
+	float32 px = v.x - T.p.x;
+	float32 py = v.y - T.p.y;
+	float32 x = (T.q.c * px + T.q.s * py);
+	float32 y = (-T.q.s * px + T.q.c * py);
+
+	return b2Vec2(x, y);
+}
+
+// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
+//    = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
+inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
+{
+	b2Transform C;
+	C.q = b2Mul(A.q, B.q);
+	C.p = b2Mul(A.q, B.p) + A.p;
+	return C;
+}
+
+// v2 = A.q' * (B.q * v1 + B.p - A.p)
+//    = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
+{
+	b2Transform C;
+	C.q = b2MulT(A.q, B.q);
+	C.p = b2MulT(A.q, B.p - A.p);
+	return C;
+}
+
+template <typename T>
+inline T b2Abs(T a)
+{
+	return a > T(0) ? a : -a;
 }
 
 inline b2Vec2 b2Abs(const b2Vec2& a)
@@ -532,7 +622,7 @@ inline b2Vec2 b2Abs(const b2Vec2& a)
 
 inline b2Mat22 b2Abs(const b2Mat22& A)
 {
-	return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
+	return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
 }
 
 template <typename T>
@@ -596,20 +686,23 @@ inline bool b2IsPowerOfTwo(uint32 x)
 	return result;
 }
 
-inline void b2Sweep::GetTransform(b2Transform* xf, float32 alpha) const
+inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
 {
-	xf->position = (1.0f - alpha) * c0 + alpha * c;
-	float32 angle = (1.0f - alpha) * a0 + alpha * a;
-	xf->R.Set(angle);
+	xf->p = (1.0f - beta) * c0 + beta * c;
+	float32 angle = (1.0f - beta) * a0 + beta * a;
+	xf->q.Set(angle);
 
 	// Shift to origin
-	xf->position -= b2Mul(xf->R, localCenter);
+	xf->p -= b2Mul(xf->q, localCenter);
 }
 
-inline void b2Sweep::Advance(float32 t)
+inline void b2Sweep::Advance(float32 alpha)
 {
-	c0 = (1.0f - t) * c0 + t * c;
-	a0 = (1.0f - t) * a0 + t * a;
+	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;
+	alpha0 = alpha;
 }
 
 /// Normalize an angle in radians to be between -pi and pi

src/modules/physics/box2d/Box2D/Common/b2Settings.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -19,7 +19,7 @@
 #include <Box2D/Common/b2Settings.h>
 #include <cstdlib>
 
-b2Version b2_version = {2, 1, 2};
+b2Version b2_version = {2, 2, 0};
 
 // Memory allocators. Modify these to use your own allocator.
 void* b2Alloc(int32 size)

src/modules/physics/box2d/Box2D/Common/b2Settings.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -32,6 +32,7 @@ typedef unsigned char uint8;
 typedef unsigned short uint16;
 typedef unsigned int uint32;
 typedef float float32;
+typedef double float64;
 
 #define	b2_maxFloat		FLT_MAX
 #define	b2_epsilon		FLT_EPSILON
@@ -43,10 +44,12 @@ typedef float float32;
 
 // Collision
 
-/// The maximum number of contact points between two convex shapes.
+/// The maximum number of contact points between two convex shapes. Do
+/// not change this value.
 #define b2_maxManifoldPoints	2
 
-/// The maximum number of vertices on a convex polygon.
+/// The maximum number of vertices on a convex polygon. You cannot increase
+/// this too much because b2BlockAllocator has a maximum object size.
 #define b2_maxPolygonVertices	8
 
 /// This is used to fatten AABBs in the dynamic tree. This allows proxies
@@ -72,6 +75,9 @@ typedef float float32;
 /// Making it larger may create artifacts for vertex collision.
 #define b2_polygonRadius		(2.0f * b2_linearSlop)
 
+/// Maximum number of sub-steps per contact in continuous physics simulation.
+#define b2_maxSubSteps			8
+
 
 // Dynamics
 
@@ -103,7 +109,9 @@ typedef float float32;
 /// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
 /// that overlap is removed in one time step. However using values close to 1 often lead
 /// to overshoot.
-#define b2_contactBaumgarte			0.2f
+#define b2_baumgarte				0.2f
+#define b2_toiBaugarte				0.75f
+
 
 // Sleep
 
@@ -136,16 +144,4 @@ struct b2Version
 /// Current version.
 extern b2Version b2_version;
 
-/// Friction mixing law. Feel free to customize this.
-inline float32 b2MixFriction(float32 friction1, float32 friction2)
-{
-	return sqrtf(friction1 * friction2);
-}
-
-/// Restitution mixing law. Feel free to customize this.
-inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
-{
-	return restitution1 > restitution2 ? restitution1 : restitution2;
-}
-
 #endif

src/modules/physics/box2d/Box2D/Common/b2StackAllocator.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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

src/modules/physics/box2d/Box2D/Common/b2StackAllocator.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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

src/modules/physics/box2d/Box2D/Common/b2Timer.cpp

@@ -0,0 +1,100 @@
+/*
+* Copyright (c) 2011 Erin Catto http://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/Common/b2Timer.h>
+
+#if defined(WIN32)
+
+float64 b2Timer::s_invFrequency = 0.0f;
+
+#include <Windows.h>
+
+b2Timer::b2Timer()
+{
+	LARGE_INTEGER largeInteger;
+
+	if (s_invFrequency == 0.0f)
+	{
+		QueryPerformanceFrequency(&largeInteger);
+		s_invFrequency = float64(largeInteger.QuadPart);
+		if (s_invFrequency > 0.0f)
+		{
+			s_invFrequency = 1000.0f / s_invFrequency;
+		}
+	}
+
+	QueryPerformanceCounter(&largeInteger);
+	m_start = float64(largeInteger.QuadPart);
+}
+
+void b2Timer::Reset()
+{
+	LARGE_INTEGER largeInteger;
+	QueryPerformanceCounter(&largeInteger);
+	m_start = float64(largeInteger.QuadPart);
+}
+
+float32 b2Timer::GetMilliseconds() const
+{
+	LARGE_INTEGER largeInteger;
+	QueryPerformanceCounter(&largeInteger);
+	float64 count = float64(largeInteger.QuadPart);
+	float32 ms = float32(s_invFrequency * (count - m_start));
+	return ms;
+}
+
+#elif defined(__linux__) || defined (__APPLE__)
+
+#include <sys/time.h>
+
+b2Timer::b2Timer()
+{
+    Reset();
+}
+
+void b2Timer::Reset()
+{
+    timeval t;
+    gettimeofday(&t, 0);
+    m_start_sec = t.tv_sec;
+    m_start_msec = t.tv_usec * 0.001f;
+}
+
+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;
+}
+
+#else
+
+b2Timer::b2Timer()
+{
+}
+
+void b2Timer::Reset()
+{
+}
+
+float32 b2Timer::GetMilliseconds() const
+{
+	return 0.0f;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Common/b2Timer.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+* Copyright (c) 2011 Erin Catto http://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
@@ -16,36 +16,30 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
-#ifndef B2_TOI_SOLVER_H
-#define B2_TOI_SOLVER_H
+#include <Box2D/Common/b2Settings.h>
 
-#include <Box2D/Common/b2Math.h>
-
-class b2Contact;
-class b2Body;
-struct b2TOIConstraint;
-class b2StackAllocator;
-
-/// This is a pure position solver for a single movable body in contact with
-/// multiple non-moving bodies.
-class b2TOISolver
+/// Timer for profiling. This has platform specific code and may
+/// not work on every platform.
+class b2Timer
 {
 public:
-	b2TOISolver(b2StackAllocator* allocator);
-	~b2TOISolver();
 
-	void Initialize(b2Contact** contacts, int32 contactCount, b2Body* toiBody);
-	void Clear();
+	/// Constructor
+	b2Timer();
 
-	// Perform one solver iteration. Returns true if converged.
-	bool Solve(float32 baumgarte);
+	/// Reset the timer.
+	void Reset();
+
+	/// Get the time since construction or the last reset.
+	float32 GetMilliseconds() const;
 
 private:
 
-	b2TOIConstraint* m_constraints;
-	int32 m_count;
-	b2Body* m_toiBody;
-	b2StackAllocator* m_allocator;
-};
-
+#if defined(WIN32)
+	float64 m_start;
+	static float64 s_invFrequency;
+#elif defined(__linux__) || defined (__APPLE__)
+	unsigned long m_start_sec;
+	unsigned long m_start_msec;
 #endif
+};

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp

@@ -0,0 +1,54 @@
+/*
+* Copyright (c) 2006-2010 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/Contacts/b2ChainAndCircleContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+
+#include <new>
+using namespace std;
+
+b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
+	return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
+	allocator->Free(contact, sizeof(b2ChainAndCircleContact));
+}
+
+b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+	b2EdgeShape edge;
+	chain->GetChildEdge(&edge, m_indexA);
+	b2CollideEdgeAndCircle(	manifold, &edge, xfA,
+							(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h

@@ -0,0 +1,39 @@
+/*
+* Copyright (c) 2006-2009 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_CHAIN_AND_CIRCLE_CONTACT_H
+#define B2_CHAIN_AND_CIRCLE_CONTACT_H
+
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+class b2BlockAllocator;
+
+class b2ChainAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+	~b2ChainAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp

@@ -0,0 +1,54 @@
+/*
+* Copyright (c) 2006-2010 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/Contacts/b2ChainAndPolygonContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+
+#include <new>
+using namespace std;
+
+b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
+	return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
+	allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
+}
+
+b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+	b2EdgeShape edge;
+	chain->GetChildEdge(&edge, m_indexA);
+	b2CollideEdgeAndPolygon(	manifold, &edge, xfA,
+								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h

@@ -0,0 +1,39 @@
+/*
+* Copyright (c) 2006-2009 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_CHAIN_AND_POLYGON_CONTACT_H
+#define B2_CHAIN_AND_POLYGON_CONTACT_H
+
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+class b2BlockAllocator;
+
+class b2ChainAndPolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+	~b2ChainAndPolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -24,8 +24,9 @@
 #include <Box2D/Collision/b2TimeOfImpact.h>
 
 #include <new>
+using namespace std;
 
-b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {
 	void* mem = allocator->Allocate(sizeof(b2CircleContact));
 	return new (mem) b2CircleContact(fixtureA, fixtureB);
@@ -38,7 +39,7 @@ void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
 }
 
 b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-	: b2Contact(fixtureA, fixtureB)
+	: b2Contact(fixtureA, 0, fixtureB, 0)
 {
 	b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
 	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2CircleContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -26,7 +26,8 @@ class b2BlockAllocator;
 class b2CircleContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
 	b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -20,6 +20,10 @@
 #include <Box2D/Dynamics/Contacts/b2CircleContact.h>
 #include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
 #include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
+#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
+#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>
 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
 
 #include <Box2D/Collision/b2Collision.h>
@@ -38,13 +42,17 @@ void b2Contact::InitializeRegisters()
 	AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
 	AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
 	AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
+	AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
+	AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
+	AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);
+	AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);
 }
 
 void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
 						b2Shape::Type type1, b2Shape::Type type2)
 {
-	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
-	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
 	
 	s_registers[type1][type2].createFcn = createFcn;
 	s_registers[type1][type2].destroyFcn = destoryFcn;
@@ -58,7 +66,7 @@ void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* dest
 	}
 }
 
-b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
 {
 	if (s_initialized == false)
 	{
@@ -69,19 +77,19 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAl
 	b2Shape::Type type1 = fixtureA->GetType();
 	b2Shape::Type type2 = fixtureB->GetType();
 
-	b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
-	b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
+	b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
+	b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
 	
 	b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
 	if (createFcn)
 	{
 		if (s_registers[type1][type2].primary)
 		{
-			return createFcn(fixtureA, fixtureB, allocator);
+			return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
 		}
 		else
 		{
-			return createFcn(fixtureB, fixtureA, allocator);
+			return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
 		}
 	}
 	else
@@ -103,20 +111,23 @@ void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
 	b2Shape::Type typeA = contact->GetFixtureA()->GetType();
 	b2Shape::Type typeB = contact->GetFixtureB()->GetType();
 
-	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
-	b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
+	b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
 
 	b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
 	destroyFcn(contact, allocator);
 }
 
-b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB)
+b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
 {
 	m_flags = e_enabledFlag;
 
 	m_fixtureA = fA;
 	m_fixtureB = fB;
 
+	m_indexA = indexA;
+	m_indexB = indexB;
+
 	m_manifold.pointCount = 0;
 
 	m_prev = NULL;
@@ -133,6 +144,9 @@ b2Contact::b2Contact(b2Fixture* fA, b2Fixture* fB)
 	m_nodeB.other = NULL;
 
 	m_toiCount = 0;
+
+	m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
+	m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
 }
 
 // Update the contact manifold and touching status.
@@ -161,7 +175,7 @@ void b2Contact::Update(b2ContactListener* listener)
 	{
 		const b2Shape* shapeA = m_fixtureA->GetShape();
 		const b2Shape* shapeB = m_fixtureB->GetShape();
-		touching = b2TestOverlap(shapeA, shapeB, xfA, xfB);
+		touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
 
 		// Sensors don't generate manifolds.
 		m_manifold.pointCount = 0;

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2Contact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -22,7 +22,6 @@
 #include <Box2D/Common/b2Math.h>
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Collision/Shapes/b2Shape.h>
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
 #include <Box2D/Dynamics/b2Fixture.h>
 
 class b2Body;
@@ -33,7 +32,23 @@ class b2BlockAllocator;
 class b2StackAllocator;
 class b2ContactListener;
 
-typedef b2Contact* b2ContactCreateFcn(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero.
+/// For example, anything slides on ice.
+inline float32 b2MixFriction(float32 friction1, float32 friction2)
+{
+	return std::sqrt(friction1 * friction2);
+}
+
+/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
+/// For example, a superball bounces on anything.
+inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
+{
+	return restitution1 > restitution2 ? restitution1 : restitution2;
+}
+
+typedef b2Contact* b2ContactCreateFcn(	b2Fixture* fixtureA, int32 indexA,
+										b2Fixture* fixtureB, int32 indexB,
+										b2BlockAllocator* allocator);
 typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
 
 struct b2ContactRegister
@@ -86,14 +101,40 @@ public:
 	b2Contact* GetNext();
 	const b2Contact* GetNext() const;
 
-	/// Get the first fixture in this contact.
+	/// Get fixture A in this contact.
 	b2Fixture* GetFixtureA();
 	const b2Fixture* GetFixtureA() const;
 
-	/// Get the second fixture in this contact.
+	/// Get the child primitive index for fixture A.
+	int32 GetChildIndexA() const;
+
+	/// Get fixture B in this contact.
 	b2Fixture* GetFixtureB();
 	const b2Fixture* GetFixtureB() const;
 
+	/// Get the child primitive index for fixture B.
+	int32 GetChildIndexB() const;
+
+	/// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
+	/// This value persists until set or reset.
+	void SetFriction(float32 friction);
+
+	/// Get the friction.
+	float32 GetFriction() const;
+
+	/// Reset the friction mixture to the default value.
+	void ResetFriction();
+
+	/// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
+	/// The value persists until you set or reset.
+	void SetRestitution(float32 restitution);
+
+	/// Get the restitution.
+	float32 GetRestitution() const;
+
+	/// Reset the restitution to the default value.
+	void ResetRestitution();
+
 	/// Evaluate this contact with your own manifold and transforms.
 	virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
 
@@ -122,6 +163,8 @@ protected:
 		// This bullet contact had a TOI event
 		e_bulletHitFlag		= 0x0010,
 
+		// This contact has a valid TOI in m_toi
+		e_toiFlag			= 0x0020
 	};
 
 	/// Flag this contact for filtering. Filtering will occur the next time step.
@@ -130,12 +173,12 @@ protected:
 	static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
 						b2Shape::Type typeA, b2Shape::Type typeB);
 	static void InitializeRegisters();
-	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
 	b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}
-	b2Contact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
 	virtual ~b2Contact() {}
 
 	void Update(b2ContactListener* listener);
@@ -156,10 +199,16 @@ protected:
 	b2Fixture* m_fixtureA;
 	b2Fixture* m_fixtureB;
 
+	int32 m_indexA;
+	int32 m_indexB;
+
 	b2Manifold m_manifold;
 
 	int32 m_toiCount;
-//	float32 m_toi;
+	float32 m_toi;
+
+	float32 m_friction;
+	float32 m_restitution;
 };
 
 inline b2Manifold* b2Contact::GetManifold()
@@ -229,14 +278,54 @@ inline b2Fixture* b2Contact::GetFixtureB()
 	return m_fixtureB;
 }
 
+inline int32 b2Contact::GetChildIndexA() const
+{
+	return m_indexA;
+}
+
 inline const b2Fixture* b2Contact::GetFixtureB() const
 {
 	return m_fixtureB;
 }
 
+inline int32 b2Contact::GetChildIndexB() const
+{
+	return m_indexB;
+}
+
 inline void b2Contact::FlagForFiltering()
 {
 	m_flags |= e_filterFlag;
 }
 
+inline void b2Contact::SetFriction(float32 friction)
+{
+	m_friction = friction;
+}
+
+inline float32 b2Contact::GetFriction() const
+{
+	return m_friction;
+}
+
+inline void b2Contact::ResetFriction()
+{
+	m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
+}
+
+inline void b2Contact::SetRestitution(float32 restitution)
+{
+	m_restitution = restitution;
+}
+
+inline float32 b2Contact::GetRestitution() const
+{
+	return m_restitution;
+}
+
+inline void b2Contact::ResetRestitution()
+{
+	m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
+}
+
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -17,6 +17,7 @@
 */
 
 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
+
 #include <Box2D/Dynamics/Contacts/b2Contact.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2Fixture.h>
@@ -25,17 +26,36 @@
 
 #define B2_DEBUG_SOLVER 0
 
-b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,
-								b2StackAllocator* allocator, float32 impulseRatio)
+struct b2ContactPositionConstraint
 {
-	m_allocator = allocator;
+	b2Vec2 localPoints[b2_maxManifoldPoints];
+	b2Vec2 localNormal;
+	b2Vec2 localPoint;
+	int32 indexA;
+	int32 indexB;
+	float32 invMassA, invMassB;
+	b2Vec2 localCenterA, localCenterB;
+	float32 invIA, invIB;
+	b2Manifold::Type type;
+	float32 radiusA, radiusB;
+	int32 pointCount;
+};
 
-	m_constraintCount = contactCount;
-	m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_constraintCount * sizeof(b2ContactConstraint));
+b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
+{
+	m_step = def->step;
+	m_allocator = def->allocator;
+	m_count = def->count;
+	m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
+	m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
+	m_positions = def->positions;
+	m_velocities = def->velocities;
+	m_contacts = def->contacts;
 
-	for (int32 i = 0; i < m_constraintCount; ++i)
+	// Initialize position independent portions of the constraints.
+	for (int32 i = 0; i < m_count; ++i)
 	{
-		b2Contact* contact = contacts[i];
+		b2Contact* contact = m_contacts[i];
 
 		b2Fixture* fixtureA = contact->m_fixtureA;
 		b2Fixture* fixtureB = contact->m_fixtureB;
@@ -47,222 +67,302 @@ b2ContactSolver::b2ContactSolver(b2Contact** contacts, int32 contactCount,
 		b2Body* bodyB = fixtureB->GetBody();
 		b2Manifold* manifold = contact->GetManifold();
 
-		float32 friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
-		float32 restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
+		int32 pointCount = manifold->pointCount;
+		b2Assert(pointCount > 0);
 
-		b2Vec2 vA = bodyA->m_linearVelocity;
-		b2Vec2 vB = bodyB->m_linearVelocity;
-		float32 wA = bodyA->m_angularVelocity;
-		float32 wB = bodyB->m_angularVelocity;
+		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+		vc->friction = contact->m_friction;
+		vc->restitution = contact->m_restitution;
+		vc->indexA = bodyA->m_islandIndex;
+		vc->indexB = bodyB->m_islandIndex;
+		vc->invMassA = bodyA->m_invMass;
+		vc->invMassB = bodyB->m_invMass;
+		vc->invIA = bodyA->m_invI;
+		vc->invIB = bodyB->m_invI;
+		vc->contactIndex = i;
+		vc->pointCount = pointCount;
+		vc->K.SetZero();
+		vc->normalMass.SetZero();
 
-		b2Assert(manifold->pointCount > 0);
+		b2ContactPositionConstraint* pc = m_positionConstraints + i;
+		pc->indexA = bodyA->m_islandIndex;
+		pc->indexB = bodyB->m_islandIndex;
+		pc->invMassA = bodyA->m_invMass;
+		pc->invMassB = bodyB->m_invMass;
+		pc->localCenterA = bodyA->m_sweep.localCenter;
+		pc->localCenterB = bodyB->m_sweep.localCenter;
+		pc->invIA = bodyA->m_invI;
+		pc->invIB = bodyB->m_invI;
+		pc->localNormal = manifold->localNormal;
+		pc->localPoint = manifold->localPoint;
+		pc->pointCount = pointCount;
+		pc->radiusA = radiusA;
+		pc->radiusB = radiusB;
+		pc->type = manifold->type;
 
-		b2WorldManifold worldManifold;
-		worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
-
-		b2ContactConstraint* cc = m_constraints + i;
-		cc->bodyA = bodyA;
-		cc->bodyB = bodyB;
-		cc->manifold = manifold;
-		cc->normal = worldManifold.normal;
-		cc->pointCount = manifold->pointCount;
-		cc->friction = friction;
-
-		cc->localNormal = manifold->localNormal;
-		cc->localPoint = manifold->localPoint;
-		cc->radius = radiusA + radiusB;
-		cc->type = manifold->type;
-
-		for (int32 j = 0; j < cc->pointCount; ++j)
+		for (int32 j = 0; j < pointCount; ++j)
 		{
 			b2ManifoldPoint* cp = manifold->points + j;
-			b2ContactConstraintPoint* ccp = cc->points + j;
-
-			ccp->normalImpulse = impulseRatio * cp->normalImpulse;
-			ccp->tangentImpulse = impulseRatio * cp->tangentImpulse;
-
-			ccp->localPoint = cp->localPoint;
-
-			ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;
-			ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;
-
-			float32 rnA = b2Cross(ccp->rA, cc->normal);
-			float32 rnB = b2Cross(ccp->rB, cc->normal);
-			rnA *= rnA;
-			rnB *= rnB;
-
-			float32 kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
-
-			b2Assert(kNormal > b2_epsilon);
-			ccp->normalMass = 1.0f / kNormal;
-
-			b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
-
-			float32 rtA = b2Cross(ccp->rA, tangent);
-			float32 rtB = b2Cross(ccp->rB, tangent);
-			rtA *= rtA;
-			rtB *= rtB;
-
-			float32 kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
-
-			b2Assert(kTangent > b2_epsilon);
-			ccp->tangentMass = 1.0f /  kTangent;
-
-			// Setup a velocity bias for restitution.
-			ccp->velocityBias = 0.0f;
-			float32 vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
-			if (vRel < -b2_velocityThreshold)
+			b2VelocityConstraintPoint* vcp = vc->points + j;
+	
+			if (m_step.warmStarting)
 			{
-				ccp->velocityBias = -restitution * vRel;
-			}
-		}
-
-		// If we have two points, then prepare the block solver.
-		if (cc->pointCount == 2)
-		{
-			b2ContactConstraintPoint* ccp1 = cc->points + 0;
-			b2ContactConstraintPoint* ccp2 = cc->points + 1;
-			
-			float32 invMassA = bodyA->m_invMass;
-			float32 invIA = bodyA->m_invI;
-			float32 invMassB = bodyB->m_invMass;
-			float32 invIB = bodyB->m_invI;
-
-			float32 rn1A = b2Cross(ccp1->rA, cc->normal);
-			float32 rn1B = b2Cross(ccp1->rB, cc->normal);
-			float32 rn2A = b2Cross(ccp2->rA, cc->normal);
-			float32 rn2B = b2Cross(ccp2->rB, cc->normal);
-
-			float32 k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
-			float32 k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
-			float32 k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
-
-			// Ensure a reasonable condition number.
-			const float32 k_maxConditionNumber = 100.0f;
-			if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
-			{
-				// K is safe to invert.
-				cc->K.col1.Set(k11, k12);
-				cc->K.col2.Set(k12, k22);
-				cc->normalMass = cc->K.GetInverse();
+				vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
+				vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
 			}
 			else
 			{
-				// The constraints are redundant, just use one.
-				// TODO_ERIN use deepest?
-				cc->pointCount = 1;
+				vcp->normalImpulse = 0.0f;
+				vcp->tangentImpulse = 0.0f;
 			}
+
+			vcp->rA.SetZero();
+			vcp->rB.SetZero();
+			vcp->normalMass = 0.0f;
+			vcp->tangentMass = 0.0f;
+			vcp->velocityBias = 0.0f;
+
+			pc->localPoints[j] = cp->localPoint;
 		}
 	}
 }
 
 b2ContactSolver::~b2ContactSolver()
 {
-	m_allocator->Free(m_constraints);
+	m_allocator->Free(m_velocityConstraints);
+	m_allocator->Free(m_positionConstraints);
+}
+
+// Initialize position dependent portions of the velocity constraints.
+void b2ContactSolver::InitializeVelocityConstraints()
+{
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b2ContactPositionConstraint* pc = m_positionConstraints + i;
+
+		float32 radiusA = pc->radiusA;
+		float32 radiusB = pc->radiusB;
+		b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
+
+		int32 indexA = vc->indexA;
+		int32 indexB = vc->indexB;
+
+		float32 mA = vc->invMassA;
+		float32 mB = vc->invMassB;
+		float32 iA = vc->invIA;
+		float32 iB = vc->invIB;
+		b2Vec2 localCenterA = pc->localCenterA;
+		b2Vec2 localCenterB = pc->localCenterB;
+
+		b2Vec2 cA = m_positions[indexA].c;
+		float32 aA = m_positions[indexA].a;
+		b2Vec2 vA = m_velocities[indexA].v;
+		float32 wA = m_velocities[indexA].w;
+
+		b2Vec2 cB = m_positions[indexB].c;
+		float32 aB = m_positions[indexB].a;
+		b2Vec2 vB = m_velocities[indexB].v;
+		float32 wB = m_velocities[indexB].w;
+
+		b2Assert(manifold->pointCount > 0);
+
+		b2Transform xfA, xfB;
+		xfA.q.Set(aA);
+		xfB.q.Set(aB);
+		xfA.p = cA - b2Mul(xfA.q, localCenterA);
+		xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
+		b2WorldManifold worldManifold;
+		worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
+
+		vc->normal = worldManifold.normal;
+
+		int32 pointCount = vc->pointCount;
+		for (int32 j = 0; j < pointCount; ++j)
+		{
+			b2VelocityConstraintPoint* vcp = vc->points + j;
+
+			vcp->rA = worldManifold.points[j] - cA;
+			vcp->rB = worldManifold.points[j] - cB;
+
+			float32 rnA = b2Cross(vcp->rA, vc->normal);
+			float32 rnB = b2Cross(vcp->rB, vc->normal);
+
+			float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+			vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+
+			b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
+
+			float32 rtA = b2Cross(vcp->rA, tangent);
+			float32 rtB = b2Cross(vcp->rB, tangent);
+
+			float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+
+			vcp->tangentMass = kTangent > 0.0f ? 1.0f /  kTangent : 0.0f;
+
+			// Setup a velocity bias for restitution.
+			vcp->velocityBias = 0.0f;
+			float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
+			if (vRel < -b2_velocityThreshold)
+			{
+				vcp->velocityBias = -vc->restitution * vRel;
+			}
+		}
+
+		// If we have two points, then prepare the block solver.
+		if (vc->pointCount == 2)
+		{
+			b2VelocityConstraintPoint* vcp1 = vc->points + 0;
+			b2VelocityConstraintPoint* vcp2 = vc->points + 1;
+
+			float32 rn1A = b2Cross(vcp1->rA, vc->normal);
+			float32 rn1B = b2Cross(vcp1->rB, vc->normal);
+			float32 rn2A = b2Cross(vcp2->rA, vc->normal);
+			float32 rn2B = b2Cross(vcp2->rB, vc->normal);
+
+			float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
+			float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
+			float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
+
+			// Ensure a reasonable condition number.
+			const float32 k_maxConditionNumber = 1000.0f;
+			if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
+			{
+				// K is safe to invert.
+				vc->K.ex.Set(k11, k12);
+				vc->K.ey.Set(k12, k22);
+				vc->normalMass = vc->K.GetInverse();
+			}
+			else
+			{
+				// The constraints are redundant, just use one.
+				// TODO_ERIN use deepest?
+				vc->pointCount = 1;
+			}
+		}
+	}
 }
 
 void b2ContactSolver::WarmStart()
 {
 	// Warm start.
-	for (int32 i = 0; i < m_constraintCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
-		b2ContactConstraint* c = m_constraints + i;
+		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
 
-		b2Body* bodyA = c->bodyA;
-		b2Body* bodyB = c->bodyB;
-		float32 invMassA = bodyA->m_invMass;
-		float32 invIA = bodyA->m_invI;
-		float32 invMassB = bodyB->m_invMass;
-		float32 invIB = bodyB->m_invI;
-		b2Vec2 normal = c->normal;
+		int32 indexA = vc->indexA;
+		int32 indexB = vc->indexB;
+		float32 mA = vc->invMassA;
+		float32 iA = vc->invIA;
+		float32 mB = vc->invMassB;
+		float32 iB = vc->invIB;
+		int32 pointCount = vc->pointCount;
+
+		b2Vec2 vA = m_velocities[indexA].v;
+		float32 wA = m_velocities[indexA].w;
+		b2Vec2 vB = m_velocities[indexB].v;
+		float32 wB = m_velocities[indexB].w;
+
+		b2Vec2 normal = vc->normal;
 		b2Vec2 tangent = b2Cross(normal, 1.0f);
 
-		for (int32 j = 0; j < c->pointCount; ++j)
+		for (int32 j = 0; j < pointCount; ++j)
 		{
-			b2ContactConstraintPoint* ccp = c->points + j;
-			b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
-			bodyA->m_angularVelocity -= invIA * b2Cross(ccp->rA, P);
-			bodyA->m_linearVelocity -= invMassA * P;
-			bodyB->m_angularVelocity += invIB * b2Cross(ccp->rB, P);
-			bodyB->m_linearVelocity += invMassB * P;
+			b2VelocityConstraintPoint* vcp = vc->points + j;
+			b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
+			wA -= iA * b2Cross(vcp->rA, P);
+			vA -= mA * P;
+			wB += iB * b2Cross(vcp->rB, P);
+			vB += mB * P;
 		}
+
+		m_velocities[indexA].v = vA;
+		m_velocities[indexA].w = wA;
+		m_velocities[indexB].v = vB;
+		m_velocities[indexB].w = wB;
 	}
 }
 
 void b2ContactSolver::SolveVelocityConstraints()
 {
-	for (int32 i = 0; i < m_constraintCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Body* bodyA = c->bodyA;
-		b2Body* bodyB = c->bodyB;
-		float32 wA = bodyA->m_angularVelocity;
-		float32 wB = bodyB->m_angularVelocity;
-		b2Vec2 vA = bodyA->m_linearVelocity;
-		b2Vec2 vB = bodyB->m_linearVelocity;
-		float32 invMassA = bodyA->m_invMass;
-		float32 invIA = bodyA->m_invI;
-		float32 invMassB = bodyB->m_invMass;
-		float32 invIB = bodyB->m_invI;
-		b2Vec2 normal = c->normal;
+		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+
+		int32 indexA = vc->indexA;
+		int32 indexB = vc->indexB;
+		float32 mA = vc->invMassA;
+		float32 iA = vc->invIA;
+		float32 mB = vc->invMassB;
+		float32 iB = vc->invIB;
+		int32 pointCount = vc->pointCount;
+
+		b2Vec2 vA = m_velocities[indexA].v;
+		float32 wA = m_velocities[indexA].w;
+		b2Vec2 vB = m_velocities[indexB].v;
+		float32 wB = m_velocities[indexB].w;
+
+		b2Vec2 normal = vc->normal;
 		b2Vec2 tangent = b2Cross(normal, 1.0f);
-		float32 friction = c->friction;
+		float32 friction = vc->friction;
 
-		b2Assert(c->pointCount == 1 || c->pointCount == 2);
+		b2Assert(pointCount == 1 || pointCount == 2);
 
-		// Solve tangent constraints
-		for (int32 j = 0; j < c->pointCount; ++j)
+		// Solve tangent constraints first because non-penetration is more important
+		// than friction.
+		for (int32 j = 0; j < pointCount; ++j)
 		{
-			b2ContactConstraintPoint* ccp = c->points + j;
+			b2VelocityConstraintPoint* vcp = vc->points + j;
 
 			// Relative velocity at contact
-			b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
+			b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
 
 			// Compute tangent force
 			float32 vt = b2Dot(dv, tangent);
-			float32 lambda = ccp->tangentMass * (-vt);
+			float32 lambda = vcp->tangentMass * (-vt);
 
 			// b2Clamp the accumulated force
-			float32 maxFriction = friction * ccp->normalImpulse;
-			float32 newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
-			lambda = newImpulse - ccp->tangentImpulse;
+			float32 maxFriction = friction * vcp->normalImpulse;
+			float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
+			lambda = newImpulse - vcp->tangentImpulse;
+			vcp->tangentImpulse = newImpulse;
 
 			// Apply contact impulse
 			b2Vec2 P = lambda * tangent;
 
-			vA -= invMassA * P;
-			wA -= invIA * b2Cross(ccp->rA, P);
+			vA -= mA * P;
+			wA -= iA * b2Cross(vcp->rA, P);
 
-			vB += invMassB * P;
-			wB += invIB * b2Cross(ccp->rB, P);
-
-			ccp->tangentImpulse = newImpulse;
+			vB += mB * P;
+			wB += iB * b2Cross(vcp->rB, P);
 		}
 
 		// Solve normal constraints
-		if (c->pointCount == 1)
+		if (vc->pointCount == 1)
 		{
-			b2ContactConstraintPoint* ccp = c->points + 0;
+			b2VelocityConstraintPoint* vcp = vc->points + 0;
 
 			// Relative velocity at contact
-			b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
+			b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
 
 			// Compute normal impulse
 			float32 vn = b2Dot(dv, normal);
-			float32 lambda = -ccp->normalMass * (vn - ccp->velocityBias);
+			float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
 
 			// b2Clamp the accumulated impulse
-			float32 newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
-			lambda = newImpulse - ccp->normalImpulse;
+			float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
+			lambda = newImpulse - vcp->normalImpulse;
+			vcp->normalImpulse = newImpulse;
 
 			// Apply contact impulse
 			b2Vec2 P = lambda * normal;
-			vA -= invMassA * P;
-			wA -= invIA * b2Cross(ccp->rA, P);
+			vA -= mA * P;
+			wA -= iA * b2Cross(vcp->rA, P);
 
-			vB += invMassB * P;
-			wB += invIB * b2Cross(ccp->rB, P);
-			ccp->normalImpulse = newImpulse;
+			vB += mB * P;
+			wB += iB * b2Cross(vcp->rB, P);
 		}
 		else
 		{
@@ -272,7 +372,7 @@ void b2ContactSolver::SolveVelocityConstraints()
 			// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
 			//
 			// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
-			// b = vn_0 - velocityBias
+			// b = vn0 - velocityBias
 			//
 			// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
 			// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
@@ -284,18 +384,23 @@ void b2ContactSolver::SolveVelocityConstraints()
 			//
 			// Substitute:
 			// 
-			// x = x' - a
+			// x = a + d
 			// 
-			// Plug into above equation:
+			// a := old total impulse
+			// x := new total impulse
+			// d := incremental impulse 
 			//
-			// vn = A * x + b
-			//    = A * (x' - a) + b
-			//    = A * x' + b - A * a
-			//    = A * x' + b'
+			// For the current iteration we extend the formula for the incremental impulse
+			// to compute the new total impulse:
+			//
+			// vn = A * d + b
+			//    = A * (x - a) + b
+			//    = A * x + b - A * a
+			//    = A * x + b'
 			// b' = b - A * a;
 
-			b2ContactConstraintPoint* cp1 = c->points + 0;
-			b2ContactConstraintPoint* cp2 = c->points + 1;
+			b2VelocityConstraintPoint* cp1 = vc->points + 0;
+			b2VelocityConstraintPoint* cp2 = vc->points + 1;
 
 			b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
 			b2Assert(a.x >= 0.0f && a.y >= 0.0f);
@@ -311,7 +416,9 @@ void b2ContactSolver::SolveVelocityConstraints()
 			b2Vec2 b;
 			b.x = vn1 - cp1->velocityBias;
 			b.y = vn2 - cp2->velocityBias;
-			b -= b2Mul(c->K, a);
+
+			// Compute b'
+			b -= b2Mul(vc->K, a);
 
 			const float32 k_errorTol = 1e-3f;
 			B2_NOT_USED(k_errorTol);
@@ -321,27 +428,27 @@ void b2ContactSolver::SolveVelocityConstraints()
 				//
 				// Case 1: vn = 0
 				//
-				// 0 = A * x' + b'
+				// 0 = A * x + b'
 				//
-				// Solve for x':
+				// Solve for x:
 				//
-				// x' = - inv(A) * b'
+				// x = - inv(A) * b'
 				//
-				b2Vec2 x = - b2Mul(c->normalMass, b);
+				b2Vec2 x = - b2Mul(vc->normalMass, b);
 
 				if (x.x >= 0.0f && x.y >= 0.0f)
 				{
-					// Resubstitute for the incremental impulse
+					// Get the incremental impulse
 					b2Vec2 d = x - a;
 
 					// Apply incremental impulse
 					b2Vec2 P1 = d.x * normal;
 					b2Vec2 P2 = d.y * normal;
-					vA -= invMassA * (P1 + P2);
-					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+					vA -= mA * (P1 + P2);
+					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
 
-					vB += invMassB * (P1 + P2);
-					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+					vB += mB * (P1 + P2);
+					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
 
 					// Accumulate
 					cp1->normalImpulse = x.x;
@@ -365,27 +472,27 @@ void b2ContactSolver::SolveVelocityConstraints()
 				//
 				// Case 2: vn1 = 0 and x2 = 0
 				//
-				//   0 = a11 * x1' + a12 * 0 + b1' 
-				// vn2 = a21 * x1' + a22 * 0 + b2'
+				//   0 = a11 * x1 + a12 * 0 + b1' 
+				// vn2 = a21 * x1 + a22 * 0 + b2'
 				//
 				x.x = - cp1->normalMass * b.x;
 				x.y = 0.0f;
 				vn1 = 0.0f;
-				vn2 = c->K.col1.y * x.x + b.y;
+				vn2 = vc->K.ex.y * x.x + b.y;
 
 				if (x.x >= 0.0f && vn2 >= 0.0f)
 				{
-					// Resubstitute for the incremental impulse
+					// Get the incremental impulse
 					b2Vec2 d = x - a;
 
 					// Apply incremental impulse
 					b2Vec2 P1 = d.x * normal;
 					b2Vec2 P2 = d.y * normal;
-					vA -= invMassA * (P1 + P2);
-					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+					vA -= mA * (P1 + P2);
+					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
 
-					vB += invMassB * (P1 + P2);
-					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+					vB += mB * (P1 + P2);
+					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
 
 					// Accumulate
 					cp1->normalImpulse = x.x;
@@ -407,12 +514,12 @@ void b2ContactSolver::SolveVelocityConstraints()
 				//
 				// Case 3: vn2 = 0 and x1 = 0
 				//
-				// vn1 = a11 * 0 + a12 * x2' + b1' 
-				//   0 = a21 * 0 + a22 * x2' + b2'
+				// vn1 = a11 * 0 + a12 * x2 + b1' 
+				//   0 = a21 * 0 + a22 * x2 + b2'
 				//
 				x.x = 0.0f;
 				x.y = - cp2->normalMass * b.y;
-				vn1 = c->K.col2.x * x.y + b.x;
+				vn1 = vc->K.ey.x * x.y + b.x;
 				vn2 = 0.0f;
 
 				if (x.y >= 0.0f && vn1 >= 0.0f)
@@ -423,11 +530,11 @@ void b2ContactSolver::SolveVelocityConstraints()
 					// Apply incremental impulse
 					b2Vec2 P1 = d.x * normal;
 					b2Vec2 P2 = d.y * normal;
-					vA -= invMassA * (P1 + P2);
-					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+					vA -= mA * (P1 + P2);
+					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
 
-					vB += invMassB * (P1 + P2);
-					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+					vB += mB * (P1 + P2);
+					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
 
 					// Accumulate
 					cp1->normalImpulse = x.x;
@@ -463,11 +570,11 @@ void b2ContactSolver::SolveVelocityConstraints()
 					// Apply incremental impulse
 					b2Vec2 P1 = d.x * normal;
 					b2Vec2 P2 = d.y * normal;
-					vA -= invMassA * (P1 + P2);
-					wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
+					vA -= mA * (P1 + P2);
+					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
 
-					vB += invMassB * (P1 + P2);
-					wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+					vB += mB * (P1 + P2);
+					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
 
 					// Accumulate
 					cp1->normalImpulse = x.x;
@@ -481,73 +588,65 @@ void b2ContactSolver::SolveVelocityConstraints()
 			}
 		}
 
-		bodyA->m_linearVelocity = vA;
-		bodyA->m_angularVelocity = wA;
-		bodyB->m_linearVelocity = vB;
-		bodyB->m_angularVelocity = wB;
+		m_velocities[indexA].v = vA;
+		m_velocities[indexA].w = wA;
+		m_velocities[indexB].v = vB;
+		m_velocities[indexB].w = wB;
 	}
 }
 
 void b2ContactSolver::StoreImpulses()
 {
-	for (int32 i = 0; i < m_constraintCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Manifold* m = c->manifold;
+		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
+		b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
 
-		for (int32 j = 0; j < c->pointCount; ++j)
+		for (int32 j = 0; j < vc->pointCount; ++j)
 		{
-			m->points[j].normalImpulse = c->points[j].normalImpulse;
-			m->points[j].tangentImpulse = c->points[j].tangentImpulse;
+			manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
+			manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
 		}
 	}
 }
 
 struct b2PositionSolverManifold
 {
-	void Initialize(b2ContactConstraint* cc, int32 index)
+	void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
 	{
-		b2Assert(cc->pointCount > 0);
+		b2Assert(pc->pointCount > 0);
 
-		switch (cc->type)
+		switch (pc->type)
 		{
 		case b2Manifold::e_circles:
 			{
-				b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
-				b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint);
-				if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
-				{
-					normal = pointB - pointA;
-					normal.Normalize();
-				}
-				else
-				{
-					normal.Set(1.0f, 0.0f);
-				}
-
+				b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
+				b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
+				normal = pointB - pointA;
+				normal.Normalize();
 				point = 0.5f * (pointA + pointB);
-				separation = b2Dot(pointB - pointA, normal) - cc->radius;
+				separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
 			}
 			break;
 
 		case b2Manifold::e_faceA:
 			{
-				normal = cc->bodyA->GetWorldVector(cc->localNormal);
-				b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
+				normal = b2Mul(xfA.q, pc->localNormal);
+				b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
 
-				b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint);
-				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
 				point = clipPoint;
 			}
 			break;
 
 		case b2Manifold::e_faceB:
 			{
-				normal = cc->bodyB->GetWorldVector(cc->localNormal);
-				b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
+				normal = b2Mul(xfB.q, pc->localNormal);
+				b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
 
-				b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint);
-				separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
+				b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
+				separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
 				point = clipPoint;
 
 				// Ensure normal points from A to B
@@ -563,58 +662,168 @@ struct b2PositionSolverManifold
 };
 
 // Sequential solver.
-bool b2ContactSolver::SolvePositionConstraints(float32 baumgarte)
+bool b2ContactSolver::SolvePositionConstraints()
 {
 	float32 minSeparation = 0.0f;
 
-	for (int32 i = 0; i < m_constraintCount; ++i)
+	for (int32 i = 0; i < m_count; ++i)
 	{
-		b2ContactConstraint* c = m_constraints + i;
-		b2Body* bodyA = c->bodyA;
-		b2Body* bodyB = c->bodyB;
+		b2ContactPositionConstraint* pc = m_positionConstraints + i;
 
-		float32 invMassA = bodyA->m_mass * bodyA->m_invMass;
-		float32 invIA = bodyA->m_mass * bodyA->m_invI;
-		float32 invMassB = bodyB->m_mass * bodyB->m_invMass;
-		float32 invIB = bodyB->m_mass * bodyB->m_invI;
+		int32 indexA = pc->indexA;
+		int32 indexB = pc->indexB;
+		b2Vec2 localCenterA = pc->localCenterA;
+		float32 mA = pc->invMassA;
+		float32 iA = pc->invIA;
+		b2Vec2 localCenterB = pc->localCenterB;
+		float32 mB = pc->invMassB;
+		float32 iB = pc->invIB;
+		int32 pointCount = pc->pointCount;
+
+		b2Vec2 cA = m_positions[indexA].c;
+		float32 aA = m_positions[indexA].a;
+
+		b2Vec2 cB = m_positions[indexB].c;
+		float32 aB = m_positions[indexB].a;
 
 		// Solve normal constraints
-		for (int32 j = 0; j < c->pointCount; ++j)
+		for (int32 j = 0; j < pointCount; ++j)
 		{
+			b2Transform xfA, xfB;
+			xfA.q.Set(aA);
+			xfB.q.Set(aB);
+			xfA.p = cA - b2Mul(xfA.q, localCenterA);
+			xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
 			b2PositionSolverManifold psm;
-			psm.Initialize(c, j);
+			psm.Initialize(pc, xfA, xfB, j);
 			b2Vec2 normal = psm.normal;
 
 			b2Vec2 point = psm.point;
 			float32 separation = psm.separation;
 
-			b2Vec2 rA = point - bodyA->m_sweep.c;
-			b2Vec2 rB = point - bodyB->m_sweep.c;
+			b2Vec2 rA = point - cA;
+			b2Vec2 rB = point - cB;
 
 			// Track max constraint error.
 			minSeparation = b2Min(minSeparation, separation);
 
 			// Prevent large corrections and allow slop.
-			float32 C = b2Clamp(baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+			float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
 
 			// Compute the effective mass.
 			float32 rnA = b2Cross(rA, normal);
 			float32 rnB = b2Cross(rB, normal);
-			float32 K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
+			float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
 
 			// Compute normal impulse
 			float32 impulse = K > 0.0f ? - C / K : 0.0f;
 
 			b2Vec2 P = impulse * normal;
 
-			bodyA->m_sweep.c -= invMassA * P;
-			bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
-			bodyA->SynchronizeTransform();
+			cA -= mA * P;
+			aA -= iA * b2Cross(rA, P);
 
-			bodyB->m_sweep.c += invMassB * P;
-			bodyB->m_sweep.a += invIB * b2Cross(rB, P);
-			bodyB->SynchronizeTransform();
+			cB += mB * P;
+			aB += iB * b2Cross(rB, P);
 		}
+
+		m_positions[indexA].c = cA;
+		m_positions[indexA].a = aA;
+
+		m_positions[indexB].c = cB;
+		m_positions[indexB].a = aB;
+	}
+
+	// We can't expect minSpeparation >= -b2_linearSlop because we don't
+	// push the separation above -b2_linearSlop.
+	return minSeparation >= -3.0f * b2_linearSlop;
+}
+
+// Sequential position solver for position constraints.
+bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
+{
+	float32 minSeparation = 0.0f;
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		b2ContactPositionConstraint* pc = m_positionConstraints + i;
+
+		int32 indexA = pc->indexA;
+		int32 indexB = pc->indexB;
+		b2Vec2 localCenterA = pc->localCenterA;
+		b2Vec2 localCenterB = pc->localCenterB;
+		int32 pointCount = pc->pointCount;
+
+		float32 mA = 0.0f;
+		float32 iA = 0.0f;
+		if (indexA == toiIndexA || indexA == toiIndexB)
+		{
+			mA = pc->invMassA;
+			iA = pc->invIA;
+		}
+
+		float32 mB = pc->invMassB;
+		float32 iB = pc->invIB;
+		if (indexB == toiIndexA || indexB == toiIndexB)
+		{
+			mB = pc->invMassB;
+			iB = pc->invIB;
+		}
+
+		b2Vec2 cA = m_positions[indexA].c;
+		float32 aA = m_positions[indexA].a;
+
+		b2Vec2 cB = m_positions[indexB].c;
+		float32 aB = m_positions[indexB].a;
+
+		// Solve normal constraints
+		for (int32 j = 0; j < pointCount; ++j)
+		{
+			b2Transform xfA, xfB;
+			xfA.q.Set(aA);
+			xfB.q.Set(aB);
+			xfA.p = cA - b2Mul(xfA.q, localCenterA);
+			xfB.p = cB - b2Mul(xfB.q, localCenterB);
+
+			b2PositionSolverManifold psm;
+			psm.Initialize(pc, xfA, xfB, j);
+			b2Vec2 normal = psm.normal;
+
+			b2Vec2 point = psm.point;
+			float32 separation = psm.separation;
+
+			b2Vec2 rA = point - cA;
+			b2Vec2 rB = point - cB;
+
+			// Track max constraint error.
+			minSeparation = b2Min(minSeparation, separation);
+
+			// Prevent large corrections and allow slop.
+			float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
+
+			// Compute the effective mass.
+			float32 rnA = b2Cross(rA, normal);
+			float32 rnB = b2Cross(rB, normal);
+			float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+
+			// Compute normal impulse
+			float32 impulse = K > 0.0f ? - C / K : 0.0f;
+
+			b2Vec2 P = impulse * normal;
+
+			cA -= mA * P;
+			aA -= iA * b2Cross(rA, P);
+
+			cB += mB * P;
+			aB += iB * b2Cross(rB, P);
+		}
+
+		m_positions[indexA].c = cA;
+		m_positions[indexA].a = aA;
+
+		m_positions[indexB].c = cB;
+		m_positions[indexB].a = aB;
 	}
 
 	// We can't expect minSpeparation >= -b2_linearSlop because we don't

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2ContactSolver.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -21,15 +21,15 @@
 
 #include <Box2D/Common/b2Math.h>
 #include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Dynamics/b2Island.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 class b2Contact;
 class b2Body;
 class b2StackAllocator;
+struct b2ContactPositionConstraint;
 
-struct b2ContactConstraintPoint
+struct b2VelocityConstraintPoint
 {
-	b2Vec2 localPoint;
 	b2Vec2 rA;
 	b2Vec2 rB;
 	float32 normalImpulse;
@@ -39,40 +39,56 @@ struct b2ContactConstraintPoint
 	float32 velocityBias;
 };
 
-struct b2ContactConstraint
+struct b2ContactVelocityConstraint
 {
-	b2ContactConstraintPoint points[b2_maxManifoldPoints];
-	b2Vec2 localNormal;
-	b2Vec2 localPoint;
+	b2VelocityConstraintPoint points[b2_maxManifoldPoints];
 	b2Vec2 normal;
 	b2Mat22 normalMass;
 	b2Mat22 K;
-	b2Body* bodyA;
-	b2Body* bodyB;
-	b2Manifold::Type type;
-	float32 radius;
+	int32 indexA;
+	int32 indexB;
+	float32 invMassA, invMassB;
+	float32 invIA, invIB;
 	float32 friction;
+	float32 restitution;
 	int32 pointCount;
-	b2Manifold* manifold;
+	int32 contactIndex;
+};
+
+struct b2ContactSolverDef
+{
+	b2TimeStep step;
+	b2Contact** contacts;
+	int32 count;
+	b2Position* positions;
+	b2Velocity* velocities;
+	b2StackAllocator* allocator;
 };
 
 class b2ContactSolver
 {
 public:
-	b2ContactSolver(b2Contact** contacts, int32 contactCount,
-					b2StackAllocator* allocator, float32 impulseRatio);
-
+	b2ContactSolver(b2ContactSolverDef* def);
 	~b2ContactSolver();
 
+	void InitializeVelocityConstraints();
+
 	void WarmStart();
 	void SolveVelocityConstraints();
 	void StoreImpulses();
 
-	bool SolvePositionConstraints(float32 baumgarte);
+	bool SolvePositionConstraints();
+	bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
 
+	b2TimeStep m_step;
+	b2Position* m_positions;
+	b2Velocity* m_velocities;
 	b2StackAllocator* m_allocator;
-	b2ContactConstraint* m_constraints;
-	int m_constraintCount;
+	b2ContactPositionConstraint* m_positionConstraints;
+	b2ContactVelocityConstraint* m_velocityConstraints;
+	b2Contact** m_contacts;
+	int m_count;
 };
 
 #endif
+

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp

@@ -0,0 +1,50 @@
+/*
+* Copyright (c) 2006-2010 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/Contacts/b2EdgeAndCircleContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+
+#include <new>
+using namespace std;
+
+b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
+	return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
+	allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
+}
+
+b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideEdgeAndCircle(	manifold,
+								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+								(b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h

@@ -0,0 +1,39 @@
+/*
+* Copyright (c) 2006-2009 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_EDGE_AND_CIRCLE_CONTACT_H
+#define B2_EDGE_AND_CIRCLE_CONTACT_H
+
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+class b2BlockAllocator;
+
+class b2EdgeAndCircleContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2EdgeAndCircleContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp

@@ -0,0 +1,50 @@
+/*
+* Copyright (c) 2006-2010 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/Contacts/b2EdgeAndPolygonContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+
+#include <new>
+using namespace std;
+
+b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
+{
+	void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
+	return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
+}
+
+void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+	((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
+	allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
+}
+
+b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
+{
+	b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
+	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
+}
+
+void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+	b2CollideEdgeAndPolygon(	manifold,
+								(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
+								(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
+}

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h

@@ -0,0 +1,39 @@
+/*
+* Copyright (c) 2006-2009 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_EDGE_AND_POLYGON_CONTACT_H
+#define B2_EDGE_AND_POLYGON_CONTACT_H
+
+#include <Box2D/Dynamics/Contacts/b2Contact.h>
+
+class b2BlockAllocator;
+
+class b2EdgeAndPolygonContact : public b2Contact
+{
+public:
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
+	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
+
+	b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
+	~b2EdgeAndPolygonContact() {}
+
+	void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -18,14 +18,12 @@
 
 #include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
 #include <Box2D/Common/b2BlockAllocator.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
 
 #include <new>
+using namespace std;
 
-b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {
 	void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
 	return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
@@ -38,7 +36,7 @@ void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* al
 }
 
 b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-: b2Contact(fixtureA, fixtureB)
+: b2Contact(fixtureA, 0, fixtureB, 0)
 {
 	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
 	b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -26,7 +26,7 @@ class b2BlockAllocator;
 class b2PolygonAndCircleContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
 	b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -24,8 +24,9 @@
 #include <Box2D/Dynamics/b2WorldCallbacks.h>
 
 #include <new>
+using namespace std;
 
-b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator)
+b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
 {
 	void* mem = allocator->Allocate(sizeof(b2PolygonContact));
 	return new (mem) b2PolygonContact(fixtureA, fixtureB);
@@ -38,7 +39,7 @@ void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
 }
 
 b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
-	: b2Contact(fixtureA, fixtureB)
+	: b2Contact(fixtureA, 0, fixtureB, 0)
 {
 	b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
 	b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2PolygonContact.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -26,7 +26,8 @@ class b2BlockAllocator;
 class b2PolygonContact : public b2Contact
 {
 public:
-	static b2Contact* Create(b2Fixture* fixtureA, b2Fixture* fixtureB, b2BlockAllocator* allocator);
+	static b2Contact* Create(	b2Fixture* fixtureA, int32 indexA,
+								b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
 	static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
 
 	b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);

src/modules/physics/box2d/Box2D/Dynamics/Contacts/b2TOISolver.cpp

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

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -46,12 +46,11 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
 	length = d.Length();
 }
 
-
 b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
 : b2Joint(def)
 {
-	m_localAnchor1 = def->localAnchorA;
-	m_localAnchor2 = def->localAnchorB;
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
 	m_length = def->length;
 	m_frequencyHz = def->frequencyHz;
 	m_dampingRatio = def->dampingRatio;
@@ -60,15 +59,32 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
 	m_bias = 0.0f;
 }
 
-void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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;
 
-	// Compute the effective mass matrix.
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-	m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+	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);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	m_u = cB + m_rB - cA - m_rA;
 
 	// Handle singularity.
 	float32 length = m_u.Length();
@@ -81,10 +97,11 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
 		m_u.Set(0.0f, 0.0f);
 	}
 
-	float32 cr1u = b2Cross(r1, m_u);
-	float32 cr2u = b2Cross(r2, m_u);
-	float32 invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u;
+	float32 crAu = b2Cross(m_rA, m_u);
+	float32 crBu = b2Cross(m_rB, m_u);
+	float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
 
+	// Compute the effective mass matrix.
 	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
 
 	if (m_frequencyHz > 0.0f)
@@ -101,101 +118,111 @@ void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
 		float32 k = m_mass * omega * omega;
 
 		// magic formulas
-		m_gamma = step.dt * (d + step.dt * k);
+		float32 h = data.step.dt;
+		m_gamma = h * (d + h * k);
 		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
-		m_bias = C * step.dt * k * m_gamma;
+		m_bias = C * h * k * m_gamma;
 
 		m_mass = invMass + m_gamma;
 		m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
 	}
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
 		// Scale the impulse to support a variable time step.
-		m_impulse *= step.dtRatio;
+		m_impulse *= data.step.dtRatio;
 
 		b2Vec2 P = m_impulse * m_u;
-		b1->m_linearVelocity -= b1->m_invMass * P;
-		b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
-		b2->m_linearVelocity += b2->m_invMass * P;
-		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+		vA -= m_invMassA * P;
+		wA -= m_invIA * b2Cross(m_rA, P);
+		vB += m_invMassB * P;
+		wB += m_invIB * b2Cross(m_rB, P);
 	}
 	else
 	{
 		m_impulse = 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 b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(step);
-
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
-
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	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;
 
 	// Cdot = dot(u, v + cross(w, r))
-	b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
-	b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
-	float32 Cdot = b2Dot(m_u, v2 - v1);
+	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
+	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
+	float32 Cdot = b2Dot(m_u, vpB - vpA);
 
 	float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
 	m_impulse += impulse;
 
 	b2Vec2 P = impulse * m_u;
-	b1->m_linearVelocity -= b1->m_invMass * P;
-	b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
-	b2->m_linearVelocity += b2->m_invMass * P;
-	b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
+	vA -= m_invMassA * P;
+	wA -= m_invIA * b2Cross(m_rA, P);
+	vB += m_invMassB * P;
+	wB += m_invIB * b2Cross(m_rB, P);
+
+	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 b2DistanceJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
-
 	if (m_frequencyHz > 0.0f)
 	{
 		// There is no position correction for soft distance constraints.
 		return true;
 	}
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
 
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	b2Rot qA(aA), qB(aB);
 
-	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 u = cB + rB - cA - rA;
 
-	float32 length = d.Normalize();
+	float32 length = u.Normalize();
 	float32 C = length - m_length;
 	C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
 
 	float32 impulse = -m_mass * C;
-	m_u = d;
-	b2Vec2 P = impulse * m_u;
+	b2Vec2 P = impulse * u;
 
-	b1->m_sweep.c -= b1->m_invMass * P;
-	b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
-	b2->m_sweep.c += b2->m_invMass * P;
-	b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);
+	cA -= m_invMassA * P;
+	aA -= m_invIA * b2Cross(rA, P);
+	cB += m_invMassB * P;
+	aB += m_invIB * b2Cross(rB, P);
 
-	b1->SynchronizeTransform();
-	b2->SynchronizeTransform();
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
 
 	return b2Abs(C) < b2_linearSlop;
 }
 
 b2Vec2 b2DistanceJoint::GetAnchorA() const
 {
-	return m_bodyA->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
 }
 
 b2Vec2 b2DistanceJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -70,7 +70,12 @@ public:
 	b2Vec2 GetAnchorA() const;
 	b2Vec2 GetAnchorB() const;
 
+	/// Get the reaction force given the inverse time step.
+	/// Unit is N.
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
+
+	/// Get the reaction torque given the inverse time step.
+	/// Unit is N*m. This is always zero for a distance joint.
 	float32 GetReactionTorque(float32 inv_dt) const;
 
 	/// Set/get the natural length.
@@ -91,20 +96,34 @@ protected:
 	friend class b2Joint;
 	b2DistanceJoint(const b2DistanceJointDef* data);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	b2Vec2 m_localAnchor1;
-	b2Vec2 m_localAnchor2;
-	b2Vec2 m_u;
 	float32 m_frequencyHz;
 	float32 m_dampingRatio;
-	float32 m_gamma;
 	float32 m_bias;
+
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	float32 m_gamma;
 	float32 m_impulse;
-	float32 m_mass;
 	float32 m_length;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_u;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	float32 m_mass;
 };
 
 inline void b2DistanceJoint::SetLength(float32 length)

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -53,14 +53,30 @@ b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
 	m_maxTorque = def->maxTorque;
 }
 
-void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
+	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;
+
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 vA = data.velocities[m_indexA].v;
+	float32 wA = data.velocities[m_indexA].w;
+
+	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.
-	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
-	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
 	// J = [-I -r1_skew I r2_skew]
 	//     [ 0       -1 0       1]
@@ -71,22 +87,15 @@ void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
 	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
 	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
 
-	float32 mA = bA->m_invMass, mB = bB->m_invMass;
-	float32 iA = bA->m_invI, iB = bB->m_invI;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Mat22 K1;
-	K1.col1.x = mA + mB;	K1.col2.x = 0.0f;
-	K1.col1.y = 0.0f;		K1.col2.y = mA + mB;
+	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;
 
-	b2Mat22 K2;
-	K2.col1.x =  iA * rA.y * rA.y;	K2.col2.x = -iA * rA.x * rA.y;
-	K2.col1.y = -iA * rA.x * rA.y;	K2.col2.y =  iA * rA.x * rA.x;
-
-	b2Mat22 K3;
-	K3.col1.x =  iB * rB.y * rB.y;	K3.col2.x = -iB * rB.x * rB.y;
-	K3.col1.y = -iB * rB.x * rB.y;	K3.col2.y =  iB * rB.x * rB.x;
-
-	b2Mat22 K = K1 + K2 + K3;
 	m_linearMass = K.GetInverse();
 
 	m_angularMass = iA + iB;
@@ -95,44 +104,41 @@ void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
 		m_angularMass = 1.0f / m_angularMass;
 	}
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
 		// Scale impulses to support a variable time step.
-		m_linearImpulse *= step.dtRatio;
-		m_angularImpulse *= step.dtRatio;
+		m_linearImpulse *= data.step.dtRatio;
+		m_angularImpulse *= data.step.dtRatio;
 
 		b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
-
-		bA->m_linearVelocity -= mA * P;
-		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
-
-		bB->m_linearVelocity += mB * P;
-		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_angularImpulse);
+		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 b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(step);
+	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;
 
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Vec2 vA = bA->m_linearVelocity;
-	float32 wA = bA->m_angularVelocity;
-	b2Vec2 vB = bB->m_linearVelocity;
-	float32 wB = bB->m_angularVelocity;
-
-	float32 mA = bA->m_invMass, mB = bB->m_invMass;
-	float32 iA = bA->m_invI, iB = bB->m_invI;
-
-	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
-	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+	float32 h = data.step.dt;
 
 	// Solve angular friction
 	{
@@ -140,7 +146,7 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		float32 impulse = -m_angularMass * Cdot;
 
 		float32 oldImpulse = m_angularImpulse;
-		float32 maxImpulse = step.dt * m_maxTorque;
+		float32 maxImpulse = h * m_maxTorque;
 		m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = m_angularImpulse - oldImpulse;
 
@@ -150,13 +156,13 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
 
 	// Solve linear friction
 	{
-		b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+		b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
 
 		b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
 		b2Vec2 oldImpulse = m_linearImpulse;
 		m_linearImpulse += impulse;
 
-		float32 maxImpulse = step.dt * m_maxForce;
+		float32 maxImpulse = h * m_maxForce;
 
 		if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
 		{
@@ -167,21 +173,21 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		impulse = m_linearImpulse - oldImpulse;
 
 		vA -= mA * impulse;
-		wA -= iA * b2Cross(rA, impulse);
+		wA -= iA * b2Cross(m_rA, impulse);
 
 		vB += mB * impulse;
-		wB += iB * b2Cross(rB, impulse);
+		wB += iB * b2Cross(m_rB, impulse);
 	}
 
-	bA->m_linearVelocity = vA;
-	bA->m_angularVelocity = wA;
-	bB->m_linearVelocity = vB;
-	bB->m_angularVelocity = wB;
+	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 b2FrictionJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
+	B2_NOT_USED(data);
 
 	return true;
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2FrictionJoint.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -79,21 +79,32 @@ protected:
 
 	b2FrictionJoint(const b2FrictionJointDef* def);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
 	b2Vec2 m_localAnchorA;
 	b2Vec2 m_localAnchorB;
 
-	b2Mat22 m_linearMass;
-	float32 m_angularMass;
-
+	// Solver shared
 	b2Vec2 m_linearImpulse;
 	float32 m_angularImpulse;
-
 	float32 m_maxForce;
 	float32 m_maxTorque;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	b2Mat22 m_linearMass;
+	float32 m_angularMass;
 };
 
 #endif

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007-2011 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
@@ -24,9 +24,8 @@
 
 // Gear Joint:
 // C0 = (coordinate1 + ratio * coordinate2)_initial
-// C = C0 - (cordinate1 + ratio * coordinate2) = 0
-// Cdot = -(Cdot1 + ratio * Cdot2)
-// J = -[J1 ratio * J2]
+// C = (coordinate1 + ratio * coordinate2) - C0 = 0
+// J = [J1 ratio * J2]
 // K = J * invM * JT
 //   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
 //
@@ -45,177 +44,323 @@
 b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 : b2Joint(def)
 {
-	b2JointType type1 = def->joint1->GetType();
-	b2JointType type2 = def->joint2->GetType();
+	m_typeA = def->joint1->GetType();
+	m_typeB = def->joint2->GetType();
 
-	b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
-	b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
-	b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody);
-	b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
+	b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
+	b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
 
-	m_revolute1 = NULL;
-	m_prismatic1 = NULL;
-	m_revolute2 = NULL;
-	m_prismatic2 = NULL;
+	float32 coordinateA, coordinateB;
 
-	float32 coordinate1, coordinate2;
-
-	m_ground1 = def->joint1->GetBodyA();
+	m_bodyC = def->joint1->GetBodyA();
 	m_bodyA = def->joint1->GetBodyB();
-	if (type1 == e_revoluteJoint)
+
+	// Get geometry of joint1
+	b2Transform xfA = m_bodyA->m_xf;
+	float32 aA = m_bodyA->m_sweep.a;
+	b2Transform xfC = m_bodyC->m_xf;
+	float32 aC = m_bodyC->m_sweep.a;
+
+	if (m_typeA == e_revoluteJoint)
 	{
-		m_revolute1 = (b2RevoluteJoint*)def->joint1;
-		m_groundAnchor1 = m_revolute1->m_localAnchor1;
-		m_localAnchor1 = m_revolute1->m_localAnchor2;
-		coordinate1 = m_revolute1->GetJointAngle();
+		b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
+		m_localAnchorC = revolute->m_localAnchorA;
+		m_localAnchorA = revolute->m_localAnchorB;
+		m_referenceAngleA = revolute->m_referenceAngle;
+		m_localAxisC.SetZero();
+
+		coordinateA = aA - aC - m_referenceAngleA;
 	}
 	else
 	{
-		m_prismatic1 = (b2PrismaticJoint*)def->joint1;
-		m_groundAnchor1 = m_prismatic1->m_localAnchor1;
-		m_localAnchor1 = m_prismatic1->m_localAnchor2;
-		coordinate1 = m_prismatic1->GetJointTranslation();
+		b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
+		m_localAnchorC = prismatic->m_localAnchorA;
+		m_localAnchorA = prismatic->m_localAnchorB;
+		m_referenceAngleA = prismatic->m_refAngle;
+		m_localAxisC = prismatic->m_localXAxisA;
+
+		b2Vec2 pC = m_localAnchorC;
+		b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
+		coordinateA = b2Dot(pA - pC, m_localAxisC);
 	}
 
-	m_ground2 = def->joint2->GetBodyA();
+	m_bodyD = def->joint2->GetBodyA();
 	m_bodyB = def->joint2->GetBodyB();
-	if (type2 == e_revoluteJoint)
+
+	// Get geometry of joint2
+	b2Transform xfB = m_bodyB->m_xf;
+	float32 aB = m_bodyB->m_sweep.a;
+	b2Transform xfD = m_bodyD->m_xf;
+	float32 aD = m_bodyD->m_sweep.a;
+
+	if (m_typeB == e_revoluteJoint)
 	{
-		m_revolute2 = (b2RevoluteJoint*)def->joint2;
-		m_groundAnchor2 = m_revolute2->m_localAnchor1;
-		m_localAnchor2 = m_revolute2->m_localAnchor2;
-		coordinate2 = m_revolute2->GetJointAngle();
+		b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
+		m_localAnchorD = revolute->m_localAnchorA;
+		m_localAnchorB = revolute->m_localAnchorB;
+		m_referenceAngleB = revolute->m_referenceAngle;
+		m_localAxisD.SetZero();
+
+		coordinateB = aB - aD - m_referenceAngleB;
 	}
 	else
 	{
-		m_prismatic2 = (b2PrismaticJoint*)def->joint2;
-		m_groundAnchor2 = m_prismatic2->m_localAnchor1;
-		m_localAnchor2 = m_prismatic2->m_localAnchor2;
-		coordinate2 = m_prismatic2->GetJointTranslation();
+		b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
+		m_localAnchorD = prismatic->m_localAnchorA;
+		m_localAnchorB = prismatic->m_localAnchorB;
+		m_referenceAngleB = prismatic->m_refAngle;
+		m_localAxisD = prismatic->m_localXAxisA;
+
+		b2Vec2 pD = m_localAnchorD;
+		b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
+		coordinateB = b2Dot(pB - pD, m_localAxisD);
 	}
 
 	m_ratio = def->ratio;
 
-	m_constant = coordinate1 + m_ratio * coordinate2;
+	m_constant = coordinateA + m_ratio * coordinateB;
 
 	m_impulse = 0.0f;
 }
 
-void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* g1 = m_ground1;
-	b2Body* g2 = m_ground2;
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	m_indexA = m_bodyA->m_islandIndex;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_indexC = m_bodyC->m_islandIndex;
+	m_indexD = m_bodyD->m_islandIndex;
+	m_lcA = m_bodyA->m_sweep.localCenter;
+	m_lcB = m_bodyB->m_sweep.localCenter;
+	m_lcC = m_bodyC->m_sweep.localCenter;
+	m_lcD = m_bodyD->m_sweep.localCenter;
+	m_mA = m_bodyA->m_invMass;
+	m_mB = m_bodyB->m_invMass;
+	m_mC = m_bodyC->m_invMass;
+	m_mD = m_bodyD->m_invMass;
+	m_iA = m_bodyA->m_invI;
+	m_iB = m_bodyB->m_invI;
+	m_iC = m_bodyC->m_invI;
+	m_iD = m_bodyD->m_invI;
 
-	float32 K = 0.0f;
-	m_J.SetZero();
+	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;
 
-	if (m_revolute1)
+	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;
+
+	b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
+
+	m_mass = 0.0f;
+
+	if (m_typeA == e_revoluteJoint)
 	{
-		m_J.angularA = -1.0f;
-		K += b1->m_invI;
+		m_JvAC.SetZero();
+		m_JwA = 1.0f;
+		m_JwC = 1.0f;
+		m_mass += m_iA + m_iC;
 	}
 	else
 	{
-		b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
-		b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		float32 crug = b2Cross(r, ug);
-		m_J.linearA = -ug;
-		m_J.angularA = -crug;
-		K += b1->m_invMass + b1->m_invI * crug * crug;
+		b2Vec2 u = b2Mul(qC, m_localAxisC);
+		b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
+		m_JvAC = u;
+		m_JwC = b2Cross(rC, u);
+		m_JwA = b2Cross(rA, u);
+		m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
 	}
 
-	if (m_revolute2)
+	if (m_typeB == e_revoluteJoint)
 	{
-		m_J.angularB = -m_ratio;
-		K += m_ratio * m_ratio * b2->m_invI;
+		m_JvBD.SetZero();
+		m_JwB = m_ratio;
+		m_JwD = m_ratio;
+		m_mass += m_ratio * m_ratio * (m_iB + m_iD);
 	}
 	else
 	{
-		b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
-		b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-		float32 crug = b2Cross(r, ug);
-		m_J.linearB = -m_ratio * ug;
-		m_J.angularB = -m_ratio * crug;
-		K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
+		b2Vec2 u = b2Mul(qD, m_localAxisD);
+		b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
+		m_JvBD = m_ratio * u;
+		m_JwD = m_ratio * b2Cross(rD, u);
+		m_JwB = m_ratio * b2Cross(rB, u);
+		m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
 	}
 
 	// Compute effective mass.
-	m_mass = K > 0.0f ? 1.0f / K : 0.0f;
+	m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
-		// Warm starting.
-		b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
-		b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
-		b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
-		b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
+		vA += (m_mA * m_impulse) * m_JvAC;
+		wA += m_iA * m_impulse * m_JwA;
+		vB += (m_mB * m_impulse) * m_JvBD;
+		wB += m_iB * m_impulse * m_JwB;
+		vC -= (m_mC * m_impulse) * m_JvAC;
+		wC -= m_iC * m_impulse * m_JwC;
+		vD -= (m_mD * m_impulse) * m_JvBD;
+		wD -= m_iD * m_impulse * m_JwD;
 	}
 	else
 	{
 		m_impulse = 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;
+	data.velocities[m_indexC].v = vC;
+	data.velocities[m_indexC].w = wC;
+	data.velocities[m_indexD].v = vD;
+	data.velocities[m_indexD].w = wD;
 }
 
-void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(step);
+	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;
+	b2Vec2 vC = data.velocities[m_indexC].v;
+	float32 wC = data.velocities[m_indexC].w;
+	b2Vec2 vD = data.velocities[m_indexD].v;
+	float32 wD = data.velocities[m_indexD].w;
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
+	Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
 
-	float32 Cdot = m_J.Compute(	b1->m_linearVelocity, b1->m_angularVelocity,
-								b2->m_linearVelocity, b2->m_angularVelocity);
-
-	float32 impulse = m_mass * (-Cdot);
+	float32 impulse = -m_mass * Cdot;
 	m_impulse += impulse;
 
-	b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
-	b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
-	b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
-	b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
+	vA += (m_mA * impulse) * m_JvAC;
+	wA += m_iA * impulse * m_JwA;
+	vB += (m_mB * impulse) * m_JvBD;
+	wB += m_iB * impulse * m_JwB;
+	vC -= (m_mC * impulse) * m_JvAC;
+	wC -= m_iC * impulse * m_JwC;
+	vD -= (m_mD * impulse) * m_JvBD;
+	wD -= m_iD * impulse * m_JwD;
+
+	data.velocities[m_indexA].v = vA;
+	data.velocities[m_indexA].w = wA;
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+	data.velocities[m_indexC].v = vC;
+	data.velocities[m_indexC].w = wC;
+	data.velocities[m_indexD].v = vD;
+	data.velocities[m_indexD].w = wD;
 }
 
-bool b2GearJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
-	
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
+	b2Vec2 cC = data.positions[m_indexC].c;
+	float32 aC = data.positions[m_indexC].a;
+	b2Vec2 cD = data.positions[m_indexD].c;
+	float32 aD = data.positions[m_indexD].a;
+
+	b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
+
 	float32 linearError = 0.0f;
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	float32 coordinateA, coordinateB;
 
-	float32 coordinate1, coordinate2;
-	if (m_revolute1)
+	b2Vec2 JvAC, JvBD;
+	float32 JwA, JwB, JwC, JwD;
+	float32 mass = 0.0f;
+
+	if (m_typeA == e_revoluteJoint)
 	{
-		coordinate1 = m_revolute1->GetJointAngle();
+		JvAC.SetZero();
+		JwA = 1.0f;
+		JwC = 1.0f;
+		mass += m_iA + m_iC;
+
+		coordinateA = aA - aC - m_referenceAngleA;
 	}
 	else
 	{
-		coordinate1 = m_prismatic1->GetJointTranslation();
+		b2Vec2 u = b2Mul(qC, m_localAxisC);
+		b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
+		JvAC = u;
+		JwC = b2Cross(rC, u);
+		JwA = b2Cross(rA, u);
+		mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
+
+		b2Vec2 pC = m_localAnchorC - m_lcC;
+		b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
+		coordinateA = b2Dot(pA - pC, m_localAxisC);
 	}
 
-	if (m_revolute2)
+	if (m_typeB == e_revoluteJoint)
 	{
-		coordinate2 = m_revolute2->GetJointAngle();
+		JvBD.SetZero();
+		JwB = 1.0f;
+		JwD = 1.0f;
+		mass += m_iB + m_iD;
+
+		coordinateB = aB - aD - m_referenceAngleB;
 	}
 	else
 	{
-		coordinate2 = m_prismatic2->GetJointTranslation();
+		b2Vec2 u = b2Mul(qD, m_localAxisD);
+		b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
+		JvBD = m_ratio * u;
+		JwD = m_ratio * b2Cross(rD, u);
+		JwB = m_ratio * b2Cross(rB, u);
+		mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
+
+		b2Vec2 pD = m_localAnchorD - m_lcD;
+		b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
+		coordinateB = b2Dot(pB - pD, m_localAxisD);
 	}
 
-	float32 C = m_constant - (coordinate1 + m_ratio * coordinate2);
+	float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
 
-	float32 impulse = m_mass * (-C);
+	float32 impulse = 0.0f;
+	if (mass > 0.0f)
+	{
+		impulse = -C / mass;
+	}
 
-	b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA;
-	b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
-	b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
-	b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
+	cA += m_mA * impulse * JvAC;
+	aA += m_iA * impulse * JwA;
+	cB += m_mB * impulse * JvBD;
+	aB += m_iB * impulse * JwB;
+	cC -= m_mC * impulse * JvAC;
+	aC -= m_iC * impulse * JwC;
+	cD -= m_mD * impulse * JvBD;
+	aD -= m_iD * impulse * JwD;
 
-	b1->SynchronizeTransform();
-	b2->SynchronizeTransform();
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+	data.positions[m_indexC].c = cC;
+	data.positions[m_indexC].a = aC;
+	data.positions[m_indexD].c = cD;
+	data.positions[m_indexD].a = aD;
 
 	// TODO_ERIN not implemented
 	return linearError < b2_linearSlop;
@@ -223,27 +368,23 @@ bool b2GearJoint::SolvePositionConstraints(float32 baumgarte)
 
 b2Vec2 b2GearJoint::GetAnchorA() const
 {
-	return m_bodyA->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
 }
 
 b2Vec2 b2GearJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
 {
-	// TODO_ERIN not tested
-	b2Vec2 P = m_impulse * m_J.linearB;
+	b2Vec2 P = m_impulse * m_JvAC;
 	return inv_dt * P;
 }
 
 float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
 {
-	// TODO_ERIN not tested
-	b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
-	b2Vec2 P = m_impulse * m_J.linearB;
-	float32 L = m_impulse * m_J.angularB - b2Cross(r, P);
+	float32 L = m_impulse * m_JwA;
 	return inv_dt * L;
 }
 

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -21,12 +21,8 @@
 
 #include <Box2D/Dynamics/Joints/b2Joint.h>
 
-class b2RevoluteJoint;
-class b2PrismaticJoint;
-
 /// Gear joint definition. This definition requires two existing
 /// revolute or prismatic joints (any combination will work).
-/// The provided joints must attach a dynamic body to a static body.
 struct b2GearJointDef : public b2JointDef
 {
 	b2GearJointDef()
@@ -55,8 +51,8 @@ struct b2GearJointDef : public b2JointDef
 /// The ratio can be negative or positive. If one joint is a revolute joint
 /// and the other joint is a prismatic joint, then the ratio will have units
 /// of length or units of 1/length.
-/// @warning The revolute and prismatic joints must be attached to
-/// fixed bodies (which must be body1 on those joints).
+/// @warning You have to manually destroy the gear joint if joint1 or joint2
+/// is destroyed.
 class b2GearJoint : public b2Joint
 {
 public:
@@ -75,37 +71,43 @@ protected:
 	friend class b2Joint;
 	b2GearJoint(const b2GearJointDef* data);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	b2Body* m_ground1;
-	b2Body* m_ground2;
+	b2JointType m_typeA;
+	b2JointType m_typeB;
 
-	// One of these is NULL.
-	b2RevoluteJoint* m_revolute1;
-	b2PrismaticJoint* m_prismatic1;
+	// Body A is connected to body C
+	// Body B is connected to body D
+	b2Body* m_bodyC;
+	b2Body* m_bodyD;
 
-	// One of these is NULL.
-	b2RevoluteJoint* m_revolute2;
-	b2PrismaticJoint* m_prismatic2;
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	b2Vec2 m_localAnchorC;
+	b2Vec2 m_localAnchorD;
 
-	b2Vec2 m_groundAnchor1;
-	b2Vec2 m_groundAnchor2;
+	b2Vec2 m_localAxisC;
+	b2Vec2 m_localAxisD;
 
-	b2Vec2 m_localAnchor1;
-	b2Vec2 m_localAnchor2;
-
-	b2Jacobian m_J;
+	float32 m_referenceAngleA;
+	float32 m_referenceAngleB;
 
 	float32 m_constant;
 	float32 m_ratio;
 
-	// Effective mass
-	float32 m_mass;
-
-	// Impulse for accumulation/warm starting.
 	float32 m_impulse;
+
+	// Solver temp
+	int32 m_indexA, m_indexB, m_indexC, m_indexD;
+	b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
+	float32 m_mA, m_mB, m_mC, m_mD;
+	float32 m_iA, m_iB, m_iC, m_iD;
+	b2Vec2 m_JvAC, m_JvBD;
+	float32 m_JwA, m_JwB, m_JwC, m_JwD;
+	float32 m_mass;
 };
 
 #endif

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -18,7 +18,7 @@
 
 #include <Box2D/Dynamics/Joints/b2Joint.h>
 #include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/Joints/b2LineJoint.h>
+#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
 #include <Box2D/Dynamics/Joints/b2MouseJoint.h>
 #include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
 #include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
@@ -26,6 +26,7 @@
 #include <Box2D/Dynamics/Joints/b2GearJoint.h>
 #include <Box2D/Dynamics/Joints/b2WeldJoint.h>
 #include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
+#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2World.h>
 #include <Box2D/Common/b2BlockAllocator.h>
@@ -80,10 +81,10 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
 		}
 		break;
 
-	case e_lineJoint:
+	case e_wheelJoint:
 		{
-			void* mem = allocator->Allocate(sizeof(b2LineJoint));
-			joint = new (mem) b2LineJoint((b2LineJointDef*)def);
+			void* mem = allocator->Allocate(sizeof(b2WheelJoint));
+			joint = new (mem) b2WheelJoint((b2WheelJointDef*)def);
 		}
 		break;
 
@@ -101,6 +102,13 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
 		}
 		break;
 
+	case e_ropeJoint:
+		{
+			void* mem = allocator->Allocate(sizeof(b2RopeJoint));
+			joint = new (mem) b2RopeJoint((b2RopeJointDef*)def);
+		}
+		break;
+
 	default:
 		b2Assert(false);
 		break;
@@ -138,8 +146,8 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
 		allocator->Free(joint, sizeof(b2GearJoint));
 		break;
 
-	case e_lineJoint:
-		allocator->Free(joint, sizeof(b2LineJoint));
+	case e_wheelJoint:
+		allocator->Free(joint, sizeof(b2WheelJoint));
 		break;
     
 	case e_weldJoint:
@@ -150,6 +158,10 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
 		allocator->Free(joint, sizeof(b2FrictionJoint));
 		break;
 
+	case e_ropeJoint:
+		allocator->Free(joint, sizeof(b2RopeJoint));
+		break;
+
 	default:
 		b2Assert(false);
 		break;

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -23,7 +23,7 @@
 
 class b2Body;
 class b2Joint;
-struct b2TimeStep;
+struct b2SolverData;
 class b2BlockAllocator;
 
 enum b2JointType
@@ -35,9 +35,10 @@ enum b2JointType
 	e_pulleyJoint,
 	e_mouseJoint,
 	e_gearJoint,
-	e_lineJoint,
+	e_wheelJoint,
     e_weldJoint,
 	e_frictionJoint,
+	e_ropeJoint
 };
 
 enum b2LimitState
@@ -50,14 +51,9 @@ enum b2LimitState
 
 struct b2Jacobian
 {
-	b2Vec2 linearA;
+	b2Vec2 linear;
 	float32 angularA;
-	b2Vec2 linearB;
 	float32 angularB;
-
-	void SetZero();
-	void Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
-	float32 Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2);
 };
 
 /// A joint edge is used to connect bodies and joints together
@@ -130,6 +126,7 @@ public:
 
 	/// Get the next joint the world joint list.
 	b2Joint* GetNext();
+	const b2Joint* GetNext() const;
 
 	/// Get the user data pointer.
 	void* GetUserData() const;
@@ -140,6 +137,11 @@ public:
 	/// Short-cut function to determine if either body is inactive.
 	bool IsActive() const;
 
+	/// Get collide connected.
+	/// Note: modifying the collide connect flag won't work correctly because
+	/// the flag is only checked when fixture AABBs begin to overlap.
+	bool GetCollideConnected() const;
+
 protected:
 	friend class b2World;
 	friend class b2Body;
@@ -151,11 +153,11 @@ protected:
 	b2Joint(const b2JointDef* def);
 	virtual ~b2Joint() {}
 
-	virtual void InitVelocityConstraints(const b2TimeStep& step) = 0;
-	virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
+	virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
+	virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
 
 	// This returns true if the position errors are within tolerance.
-	virtual bool SolvePositionConstraints(float32 baumgarte) = 0;
+	virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
 
 	b2JointType m_type;
 	b2Joint* m_prev;
@@ -169,30 +171,8 @@ protected:
 	bool m_collideConnected;
 
 	void* m_userData;
-
-	// Cache here per time step to reduce cache misses.
-	b2Vec2 m_localCenterA, m_localCenterB;
-	float32 m_invMassA, m_invIA;
-	float32 m_invMassB, m_invIB;
 };
 
-inline void b2Jacobian::SetZero()
-{
-	linearA.SetZero(); angularA = 0.0f;
-	linearB.SetZero(); angularB = 0.0f;
-}
-
-inline void b2Jacobian::Set(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
-{
-	linearA = x1; angularA = a1;
-	linearB = x2; angularB = a2;
-}
-
-inline float32 b2Jacobian::Compute(const b2Vec2& x1, float32 a1, const b2Vec2& x2, float32 a2)
-{
-	return b2Dot(linearA, x1) + angularA * a1 + b2Dot(linearB, x2) + angularB * a2;
-}
-
 inline b2JointType b2Joint::GetType() const
 {
 	return m_type;
@@ -213,6 +193,11 @@ inline b2Joint* b2Joint::GetNext()
 	return m_next;
 }
 
+inline const b2Joint* b2Joint::GetNext() const
+{
+	return m_next;
+}
+
 inline void* b2Joint::GetUserData() const
 {
 	return m_userData;
@@ -223,4 +208,9 @@ inline void b2Joint::SetUserData(void* data)
 	m_userData = data;
 }
 
+inline bool b2Joint::GetCollideConnected() const
+{
+	return m_collideConnected;
+}
+
 #endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.cpp

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

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2LineJoint.h

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

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -36,8 +36,8 @@ b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
 	b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
 	b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
 
-	m_target = def->target;
-	m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target);
+	m_targetA = def->target;
+	m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
 
 	m_maxForce = def->maxForce;
 	m_impulse.SetZero();
@@ -55,12 +55,12 @@ void b2MouseJoint::SetTarget(const b2Vec2& target)
 	{
 		m_bodyB->SetAwake(true);
 	}
-	m_target = target;
+	m_targetA = target;
 }
 
 const b2Vec2& b2MouseJoint::GetTarget() const
 {
-	return m_target;
+	return m_targetA;
 }
 
 void b2MouseJoint::SetMaxForce(float32 force)
@@ -93,11 +93,21 @@ float32 b2MouseJoint::GetDampingRatio() const
 	return m_dampingRatio;
 }
 
-void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b = m_bodyB;
+	m_indexB = m_bodyB->m_islandIndex;
+	m_localCenterB = m_bodyB->m_sweep.localCenter;
+	m_invMassB = m_bodyB->m_invMass;
+	m_invIB = m_bodyB->m_invI;
 
-	float32 mass = b->GetMass();
+	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 qB(aB);
+
+	float32 mass = m_bodyB->GetMass();
 
 	// Frequency
 	float32 omega = 2.0f * b2_pi * m_frequencyHz;
@@ -111,79 +121,89 @@ void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
 	// magic formulas
 	// gamma has units of inverse mass.
 	// beta has units of inverse time.
-	b2Assert(d + step.dt * k > b2_epsilon);
-	m_gamma = step.dt * (d + step.dt * k);
+	float32 h = data.step.dt;
+	b2Assert(d + h * k > b2_epsilon);
+	m_gamma = h * (d + h * k);
 	if (m_gamma != 0.0f)
 	{
 		m_gamma = 1.0f / m_gamma;
 	}
-	m_beta = step.dt * k * m_gamma;
+	m_beta = h * k * m_gamma;
 
 	// Compute the effective mass matrix.
-	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
 	// K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
 	//      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
 	//        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
-	float32 invMass = b->m_invMass;
-	float32 invI = b->m_invI;
-
-	b2Mat22 K1;
-	K1.col1.x = invMass;	K1.col2.x = 0.0f;
-	K1.col1.y = 0.0f;		K1.col2.y = invMass;
-
-	b2Mat22 K2;
-	K2.col1.x =  invI * r.y * r.y;	K2.col2.x = -invI * r.x * r.y;
-	K2.col1.y = -invI * r.x * r.y;	K2.col2.y =  invI * r.x * r.x;
-
-	b2Mat22 K = K1 + K2;
-	K.col1.x += m_gamma;
-	K.col2.y += m_gamma;
+	b2Mat22 K;
+	K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
+	K.ex.y = -m_invIB * m_rB.x * m_rB.y;
+	K.ey.x = K.ex.y;
+	K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
 
 	m_mass = K.GetInverse();
 
-	m_C = b->m_sweep.c + r - m_target;
+	m_C = cB + m_rB - m_targetA;
+	m_C *= m_beta;
 
 	// Cheat with some damping
-	b->m_angularVelocity *= 0.98f;
+	wB *= 0.98f;
 
-	// Warm starting.
-	m_impulse *= step.dtRatio;
-	b->m_linearVelocity += invMass * m_impulse;
-	b->m_angularVelocity += invI * b2Cross(r, m_impulse);
+	if (data.step.warmStarting)
+	{
+		m_impulse *= data.step.dtRatio;
+		vB += m_invMassB * m_impulse;
+		wB += m_invIB * b2Cross(m_rB, m_impulse);
+	}
+	else
+	{
+		m_impulse.SetZero();
+	}
+
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
 }
 
-void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b = m_bodyB;
-
-	b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
+	b2Vec2 vB = data.velocities[m_indexB].v;
+	float32 wB = data.velocities[m_indexB].w;
 
 	// Cdot = v + cross(w, r)
-	b2Vec2 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
-	b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
+	b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
+	b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
 
 	b2Vec2 oldImpulse = m_impulse;
 	m_impulse += impulse;
-	float32 maxImpulse = step.dt * m_maxForce;
+	float32 maxImpulse = data.step.dt * m_maxForce;
 	if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
 	{
 		m_impulse *= maxImpulse / m_impulse.Length();
 	}
 	impulse = m_impulse - oldImpulse;
 
-	b->m_linearVelocity += b->m_invMass * impulse;
-	b->m_angularVelocity += b->m_invI * b2Cross(r, impulse);
+	vB += m_invMassB * impulse;
+	wB += m_invIB * b2Cross(m_rB, impulse);
+
+	data.velocities[m_indexB].v = vB;
+	data.velocities[m_indexB].w = wB;
+}
+
+bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	B2_NOT_USED(data);
+	return true;
 }
 
 b2Vec2 b2MouseJoint::GetAnchorA() const
 {
-	return m_target;
+	return m_targetA;
 }
 
 b2Vec2 b2MouseJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -94,21 +94,30 @@ protected:
 
 	b2MouseJoint(const b2MouseJointDef* def);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte) { B2_NOT_USED(baumgarte); return true; }
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	b2Vec2 m_localAnchor;
-	b2Vec2 m_target;
-	b2Vec2 m_impulse;
-
-	b2Mat22 m_mass;		// effective mass for point-to-point constraint.
-	b2Vec2 m_C;				// position error
-	float32 m_maxForce;
+	b2Vec2 m_localAnchorB;
+	b2Vec2 m_targetA;
 	float32 m_frequencyHz;
 	float32 m_dampingRatio;
 	float32 m_beta;
+	
+	// Solver shared
+	b2Vec2 m_impulse;
+	float32 m_maxForce;
 	float32 m_gamma;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassB;
+	float32 m_invIB;
+	b2Mat22 m_mass;
+	b2Vec2 m_C;
 };
 
 #endif

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -87,23 +87,23 @@
 // Now compute impulse to be applied:
 // df = f2 - f1
 
-void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
 {
-	bodyA = b1;
-	bodyB = b2;
+	bodyA = bA;
+	bodyB = bB;
 	localAnchorA = bodyA->GetLocalPoint(anchor);
 	localAnchorB = bodyB->GetLocalPoint(anchor);
-	localAxis1 = bodyA->GetLocalVector(axis);
+	localAxisA = bodyA->GetLocalVector(axis);
 	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
 }
 
 b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
 : b2Joint(def)
 {
-	m_localAnchor1 = def->localAnchorA;
-	m_localAnchor2 = def->localAnchorB;
-	m_localXAxis1 = def->localAxis1;
-	m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_localXAxisA = def->localAxisA;
+	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
 	m_refAngle = def->referenceAngle;
 
 	m_impulse.SetZero();
@@ -122,35 +122,45 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
 	m_perp.SetZero();
 }
 
-void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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;
 
-	m_localCenterA = b1->GetLocalCenter();
-	m_localCenterB = b2->GetLocalCenter();
+	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;
 
-	b2Transform xf1 = b1->GetTransform();
-	b2Transform xf2 = b2->GetTransform();
+	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 masses.
-	b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
-	b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
-	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = (cB - cA) + rB - rA;
 
-	m_invMassA = b1->m_invMass;
-	m_invIA = b1->m_invI;
-	m_invMassB = b2->m_invMass;
-	m_invIB = b2->m_invI;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
 	// Compute motor Jacobian and effective mass.
 	{
-		m_axis = b2Mul(xf1.R, m_localXAxis1);
-		m_a1 = b2Cross(d + r1, m_axis);
-		m_a2 = b2Cross(r2, m_axis);
+		m_axis = b2Mul(qA, m_localXAxisA);
+		m_a1 = b2Cross(d + rA, m_axis);
+		m_a2 = b2Cross(rB, m_axis);
 
-		m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
-		if (m_motorMass > b2_epsilon)
+		m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
+		if (m_motorMass > 0.0f)
 		{
 			m_motorMass = 1.0f / m_motorMass;
 		}
@@ -158,24 +168,26 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
 
 	// Prismatic constraint.
 	{
-		m_perp = b2Mul(xf1.R, m_localYAxis1);
+		m_perp = b2Mul(qA, m_localYAxisA);
 
-		m_s1 = b2Cross(d + r1, m_perp);
-		m_s2 = b2Cross(r2, m_perp);
+		m_s1 = b2Cross(d + rA, m_perp);
+		m_s2 = b2Cross(rB, m_perp);
 
-		float32 m1 = m_invMassA, m2 = m_invMassB;
-		float32 i1 = m_invIA, i2 = m_invIB;
+		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
+		float32 k12 = iA * m_s1 + iB * m_s2;
+		float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
+		float32 k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			// For bodies with fixed rotation.
+			k22 = 1.0f;
+		}
+		float32 k23 = iA * m_a1 + iB * m_a2;
+		float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
 
-		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
-		float32 k12 = i1 * m_s1 + i2 * m_s2;
-		float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
-		float32 k22 = i1 + i2;
-		float32 k23 = i1 * m_a1 + i2 * m_a2;
-		float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
-
-		m_K.col1.Set(k11, k12, k13);
-		m_K.col2.Set(k12, k22, k23);
-		m_K.col3.Set(k13, k23, k33);
+		m_K.ex.Set(k11, k12, k13);
+		m_K.ey.Set(k12, k22, k23);
+		m_K.ez.Set(k13, k23, k33);
 	}
 
 	// Compute motor and limit terms.
@@ -219,69 +231,74 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
 		m_motorImpulse = 0.0f;
 	}
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
 		// Account for variable time step.
-		m_impulse *= step.dtRatio;
-		m_motorImpulse *= step.dtRatio;
+		m_impulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
 
 		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
-		float32 L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
-		float32 L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
+		float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
+		float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
 
-		b1->m_linearVelocity -= m_invMassA * P;
-		b1->m_angularVelocity -= m_invIA * L1;
+		vA -= mA * P;
+		wA -= iA * LA;
 
-		b2->m_linearVelocity += m_invMassB * P;
-		b2->m_angularVelocity += m_invIB * L2;
+		vB += mB * P;
+		wB += iB * LB;
 	}
 	else
 	{
 		m_impulse.SetZero();
 		m_motorImpulse = 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 b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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;
 
-	b2Vec2 v1 = b1->m_linearVelocity;
-	float32 w1 = b1->m_angularVelocity;
-	b2Vec2 v2 = b2->m_linearVelocity;
-	float32 w2 = b2->m_angularVelocity;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
 	// Solve linear motor constraint.
 	if (m_enableMotor && m_limitState != e_equalLimits)
 	{
-		float32 Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
 		float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
 		float32 oldImpulse = m_motorImpulse;
-		float32 maxImpulse = step.dt * m_maxMotorForce;
+		float32 maxImpulse = data.step.dt * m_maxMotorForce;
 		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = m_motorImpulse - oldImpulse;
 
 		b2Vec2 P = impulse * m_axis;
-		float32 L1 = impulse * m_a1;
-		float32 L2 = impulse * m_a2;
+		float32 LA = impulse * m_a1;
+		float32 LB = impulse * m_a2;
 
-		v1 -= m_invMassA * P;
-		w1 -= m_invIA * L1;
+		vA -= mA * P;
+		wA -= iA * LA;
 
-		v2 += m_invMassB * P;
-		w2 += m_invIB * L2;
+		vB += mB * P;
+		wB += iB * LB;
 	}
 
 	b2Vec2 Cdot1;
-	Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
-	Cdot1.y = w2 - w1;
+	Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
+	Cdot1.y = wB - wA;
 
 	if (m_enableLimit && m_limitState != e_inactiveLimit)
 	{
 		// Solve prismatic and limit constraint in block form.
 		float32 Cdot2;
-		Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+		Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
 		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
 
 		b2Vec3 f1 = m_impulse;
@@ -298,7 +315,7 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		}
 
 		// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
-		b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y);
+		b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
 		b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
 		m_impulse.x = f2r.x;
 		m_impulse.y = f2r.y;
@@ -306,14 +323,14 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		df = m_impulse - f1;
 
 		b2Vec2 P = df.x * m_perp + df.z * m_axis;
-		float32 L1 = df.x * m_s1 + df.y + df.z * m_a1;
-		float32 L2 = df.x * m_s2 + df.y + df.z * m_a2;
+		float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
+		float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
 
-		v1 -= m_invMassA * P;
-		w1 -= m_invIA * L1;
+		vA -= mA * P;
+		wA -= iA * LA;
 
-		v2 += m_invMassB * P;
-		w2 += m_invIB * L2;
+		vB += mB * P;
+		wB += iB * LB;
 	}
 	else
 	{
@@ -323,105 +340,100 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
 		m_impulse.y += df.y;
 
 		b2Vec2 P = df.x * m_perp;
-		float32 L1 = df.x * m_s1 + df.y;
-		float32 L2 = df.x * m_s2 + df.y;
+		float32 LA = df.x * m_s1 + df.y;
+		float32 LB = df.x * m_s2 + df.y;
 
-		v1 -= m_invMassA * P;
-		w1 -= m_invIA * L1;
+		vA -= mA * P;
+		wA -= iA * LA;
 
-		v2 += m_invMassB * P;
-		w2 += m_invIB * L2;
+		vB += mB * P;
+		wB += iB * LB;
 	}
 
-	b1->m_linearVelocity = v1;
-	b1->m_angularVelocity = w1;
-	b2->m_linearVelocity = v2;
-	b2->m_angularVelocity = w2;
+	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 b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Rot qA(aA), qB(aB);
 
-	b2Vec2 c1 = b1->m_sweep.c;
-	float32 a1 = b1->m_sweep.a;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Vec2 c2 = b2->m_sweep.c;
-	float32 a2 = b2->m_sweep.a;
+	// Compute fresh Jacobians
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = cB + rB - cA - rA;
+
+	b2Vec2 axis = b2Mul(qA, m_localXAxisA);
+	float32 a1 = b2Cross(d + rA, axis);
+	float32 a2 = b2Cross(rB, axis);
+	b2Vec2 perp = b2Mul(qA, m_localYAxisA);
+
+	float32 s1 = b2Cross(d + rA, perp);
+	float32 s2 = b2Cross(rB, perp);
+
+	b2Vec3 impulse;
+	b2Vec2 C1;
+	C1.x = b2Dot(perp, d);
+	C1.y = aB - aA - m_refAngle;
+
+	float32 linearError = b2Abs(C1.x);
+	float32 angularError = b2Abs(C1.y);
 
-	// Solve linear limit constraint.
-	float32 linearError = 0.0f, angularError = 0.0f;
 	bool active = false;
 	float32 C2 = 0.0f;
-
-	b2Mat22 R1(a1), R2(a2);
-
-	b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
-	b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
-	b2Vec2 d = c2 + r2 - c1 - r1;
-
 	if (m_enableLimit)
 	{
-		m_axis = b2Mul(R1, m_localXAxis1);
-
-		m_a1 = b2Cross(d + r1, m_axis);
-		m_a2 = b2Cross(r2, m_axis);
-
-		float32 translation = b2Dot(m_axis, d);
+		float32 translation = b2Dot(axis, d);
 		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
 		{
 			// Prevent large angular corrections
 			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
-			linearError = b2Abs(translation);
+			linearError = b2Max(linearError, b2Abs(translation));
 			active = true;
 		}
 		else if (translation <= m_lowerTranslation)
 		{
 			// Prevent large linear corrections and allow some slop.
 			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-			linearError = m_lowerTranslation - translation;
+			linearError = b2Max(linearError, m_lowerTranslation - translation);
 			active = true;
 		}
 		else if (translation >= m_upperTranslation)
 		{
 			// Prevent large linear corrections and allow some slop.
 			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
-			linearError = translation - m_upperTranslation;
+			linearError = b2Max(linearError, translation - m_upperTranslation);
 			active = true;
 		}
 	}
 
-	m_perp = b2Mul(R1, m_localYAxis1);
-
-	m_s1 = b2Cross(d + r1, m_perp);
-	m_s2 = b2Cross(r2, m_perp);
-
-	b2Vec3 impulse;
-	b2Vec2 C1;
-	C1.x = b2Dot(m_perp, d);
-	C1.y = a2 - a1 - m_refAngle;
-
-	linearError = b2Max(linearError, b2Abs(C1.x));
-	angularError = b2Abs(C1.y);
-
 	if (active)
 	{
-		float32 m1 = m_invMassA, m2 = m_invMassB;
-		float32 i1 = m_invIA, i2 = m_invIB;
+		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
+		float32 k12 = iA * m_s1 + iB * m_s2;
+		float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
+		float32 k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			// For fixed rotation
+			k22 = 1.0f;
+		}
+		float32 k23 = iA * m_a1 + iB * m_a2;
+		float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
 
-		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
-		float32 k12 = i1 * m_s1 + i2 * m_s2;
-		float32 k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
-		float32 k22 = i1 + i2;
-		float32 k23 = i1 * m_a1 + i2 * m_a2;
-		float32 k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
-
-		m_K.col1.Set(k11, k12, k13);
-		m_K.col2.Set(k12, k22, k23);
-		m_K.col3.Set(k13, k23, k33);
+		m_K.ex.Set(k11, k12, k13);
+		m_K.ey.Set(k12, k22, k23);
+		m_K.ez.Set(k13, k23, k33);
 
 		b2Vec3 C;
 		C.x = C1.x;
@@ -432,15 +444,16 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte)
 	}
 	else
 	{
-		float32 m1 = m_invMassA, m2 = m_invMassB;
-		float32 i1 = m_invIA, i2 = m_invIB;
+		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
+		float32 k12 = iA * m_s1 + iB * m_s2;
+		float32 k22 = iA + iB;
+		if (k22 == 0.0f)
+		{
+			k22 = 1.0f;
+		}
 
-		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
-		float32 k12 = i1 * m_s1 + i2 * m_s2;
-		float32 k22 = i1 + i2;
-
-		m_K.col1.Set(k11, k12, 0.0f);
-		m_K.col2.Set(k12, k22, 0.0f);
+		m_K.ex.Set(k11, k12, 0.0f);
+		m_K.ey.Set(k12, k22, 0.0f);
 
 		b2Vec2 impulse1 = m_K.Solve22(-C1);
 		impulse.x = impulse1.x;
@@ -448,34 +461,31 @@ bool b2PrismaticJoint::SolvePositionConstraints(float32 baumgarte)
 		impulse.z = 0.0f;
 	}
 
-	b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis;
-	float32 L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
-	float32 L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
+	b2Vec2 P = impulse.x * perp + impulse.z * axis;
+	float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
+	float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;
 
-	c1 -= m_invMassA * P;
-	a1 -= m_invIA * L1;
-	c2 += m_invMassB * P;
-	a2 += m_invIB * L2;
+	cA -= mA * P;
+	aA -= iA * LA;
+	cB += mB * P;
+	aB += iB * LB;
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
 
-	// TODO_ERIN remove need for this.
-	b1->m_sweep.c = c1;
-	b1->m_sweep.a = a1;
-	b2->m_sweep.c = c2;
-	b2->m_sweep.a = a2;
-	b1->SynchronizeTransform();
-	b2->SynchronizeTransform();
-	
 	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
 }
 
 b2Vec2 b2PrismaticJoint::GetAnchorA() const
 {
-	return m_bodyA->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
 }
 
 b2Vec2 b2PrismaticJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const
@@ -490,13 +500,10 @@ float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const
 
 float32 b2PrismaticJoint::GetJointTranslation() const
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
-
-	b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
-	b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
-	b2Vec2 d = p2 - p1;
-	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+	b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
+	b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
+	b2Vec2 d = pB - pA;
+	b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);
 
 	float32 translation = b2Dot(d, axis);
 	return translation;
@@ -504,22 +511,22 @@ float32 b2PrismaticJoint::GetJointTranslation() const
 
 float32 b2PrismaticJoint::GetJointSpeed() const
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
 
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-	b2Vec2 p1 = b1->m_sweep.c + r1;
-	b2Vec2 p2 = b2->m_sweep.c + r2;
+	b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
+	b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
+	b2Vec2 p1 = bA->m_sweep.c + rA;
+	b2Vec2 p2 = bB->m_sweep.c + rB;
 	b2Vec2 d = p2 - p1;
-	b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+	b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
 
-	b2Vec2 v1 = b1->m_linearVelocity;
-	b2Vec2 v2 = b2->m_linearVelocity;
-	float32 w1 = b1->m_angularVelocity;
-	float32 w2 = b2->m_angularVelocity;
+	b2Vec2 vA = bA->m_linearVelocity;
+	b2Vec2 vB = bB->m_linearVelocity;
+	float32 wA = bA->m_angularVelocity;
+	float32 wB = bB->m_angularVelocity;
 
-	float32 speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+	float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
 	return speed;
 }
 
@@ -530,9 +537,13 @@ bool b2PrismaticJoint::IsLimitEnabled() const
 
 void b2PrismaticJoint::EnableLimit(bool flag)
 {
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_enableLimit = flag;
+	if (flag != m_enableLimit)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableLimit = flag;
+		m_impulse.z = 0.0f;
+	}
 }
 
 float32 b2PrismaticJoint::GetLowerLimit() const
@@ -548,10 +559,14 @@ float32 b2PrismaticJoint::GetUpperLimit() const
 void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
 {
 	b2Assert(lower <= upper);
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_lowerTranslation = lower;
-	m_upperTranslation = upper;
+	if (lower != m_lowerTranslation || upper != m_upperTranslation)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_lowerTranslation = lower;
+		m_upperTranslation = upper;
+		m_impulse.z = 0.0f;
+	}
 }
 
 bool b2PrismaticJoint::IsMotorEnabled() const
@@ -580,7 +595,7 @@ void b2PrismaticJoint::SetMaxMotorForce(float32 force)
 	m_maxMotorForce = force;
 }
 
-float32 b2PrismaticJoint::GetMotorForce() const
+float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const
 {
-	return m_motorImpulse;
+	return inv_dt * m_motorImpulse;
 }

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -27,7 +27,6 @@
 /// can violate the constraint slightly. The joint translation is zero
 /// when the local anchor points coincide in world space. Using local
 /// anchors and a local axis helps when saving and loading a game.
-/// @warning at least one body should by dynamic with a non-fixed rotation.
 struct b2PrismaticJointDef : public b2JointDef
 {
 	b2PrismaticJointDef()
@@ -35,7 +34,7 @@ struct b2PrismaticJointDef : public b2JointDef
 		type = e_prismaticJoint;
 		localAnchorA.SetZero();
 		localAnchorB.SetZero();
-		localAxis1.Set(1.0f, 0.0f);
+		localAxisA.Set(1.0f, 0.0f);
 		referenceAngle = 0.0f;
 		enableLimit = false;
 		lowerTranslation = 0.0f;
@@ -56,9 +55,9 @@ struct b2PrismaticJointDef : public b2JointDef
 	b2Vec2 localAnchorB;
 
 	/// The local translation axis in body1.
-	b2Vec2 localAxis1;
+	b2Vec2 localAxisA;
 
-	/// The constrained angle between the bodies: body2_angle - body1_angle.
+	/// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
 	float32 referenceAngle;
 
 	/// Enable/disable the joint limit.
@@ -81,7 +80,7 @@ struct b2PrismaticJointDef : public b2JointDef
 };
 
 /// A prismatic joint. This joint provides one degree of freedom: translation
-/// along an axis fixed in body1. Relative rotation is prevented. You can
+/// along an axis fixed in bodyA. Relative rotation is prevented. You can
 /// use a joint limit to restrict the range of motion and a joint motor to
 /// drive the motion or to model joint friction.
 class b2PrismaticJoint : public b2Joint
@@ -129,42 +128,48 @@ public:
 	/// Set the maximum motor force, usually in N.
 	void SetMaxMotorForce(float32 force);
 
-	/// Get the current motor force, usually in N.
-	float32 GetMotorForce() const;
+	/// Get the current motor force given the inverse time step, usually in N.
+	float32 GetMotorForce(float32 inv_dt) const;
 
 protected:
 	friend class b2Joint;
 	friend class b2GearJoint;
 	b2PrismaticJoint(const b2PrismaticJointDef* def);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	b2Vec2 m_localAnchor1;
-	b2Vec2 m_localAnchor2;
-	b2Vec2 m_localXAxis1;
-	b2Vec2 m_localYAxis1;
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	b2Vec2 m_localXAxisA;
+	b2Vec2 m_localYAxisA;
 	float32 m_refAngle;
-
-	b2Vec2 m_axis, m_perp;
-	float32 m_s1, m_s2;
-	float32 m_a1, m_a2;
-
-	b2Mat33 m_K;
 	b2Vec3 m_impulse;
-
-	float32 m_motorMass;			// effective mass for motor/limit translational constraint.
 	float32 m_motorImpulse;
-
 	float32 m_lowerTranslation;
 	float32 m_upperTranslation;
 	float32 m_maxMotorForce;
 	float32 m_motorSpeed;
-	
 	bool m_enableLimit;
 	bool m_enableMotor;
 	b2LimitState m_limitState;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	b2Vec2 m_axis, m_perp;
+	float32 m_s1, m_s2;
+	float32 m_a1, m_a2;
+	b2Mat33 m_K;
+	float32 m_motorMass;
 };
 
 inline float32 b2PrismaticJoint::GetMotorSpeed() const

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2007 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
@@ -24,368 +24,251 @@
 // length1 = norm(p1 - s1)
 // length2 = norm(p2 - s2)
 // C0 = (length1 + ratio * length2)_initial
-// C = C0 - (length1 + ratio * length2) >= 0
+// C = C0 - (length1 + ratio * length2)
 // u1 = (p1 - s1) / norm(p1 - s1)
 // u2 = (p2 - s2) / norm(p2 - s2)
 // Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
 // J = -[u1 cross(r1, u1) ratio * u2  ratio * cross(r2, u2)]
 // K = J * invM * JT
 //   = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
-//
-// Limit:
-// C = maxLength - length
-// u = (p - s) / norm(p - s)
-// Cdot = -dot(u, v + cross(w, r))
-// K = invMass + invI * cross(r, u)^2
-// 0 <= impulse
 
-void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
-				const b2Vec2& ga1, const b2Vec2& ga2,
-				const b2Vec2& anchor1, const b2Vec2& anchor2,
+void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,
+				const b2Vec2& groundA, const b2Vec2& groundB,
+				const b2Vec2& anchorA, const b2Vec2& anchorB,
 				float32 r)
 {
-	bodyA = b1;
-	bodyB = b2;
-	groundAnchorA = ga1;
-	groundAnchorB = ga2;
-	localAnchorA = bodyA->GetLocalPoint(anchor1);
-	localAnchorB = bodyB->GetLocalPoint(anchor2);
-	b2Vec2 d1 = anchor1 - ga1;
-	lengthA = d1.Length();
-	b2Vec2 d2 = anchor2 - ga2;
-	lengthB = d2.Length();
+	bodyA = bA;
+	bodyB = bB;
+	groundAnchorA = groundA;
+	groundAnchorB = groundB;
+	localAnchorA = bodyA->GetLocalPoint(anchorA);
+	localAnchorB = bodyB->GetLocalPoint(anchorB);
+	b2Vec2 dA = anchorA - groundA;
+	lengthA = dA.Length();
+	b2Vec2 dB = anchorB - groundB;
+	lengthB = dB.Length();
 	ratio = r;
 	b2Assert(ratio > b2_epsilon);
-	float32 C = lengthA + ratio * lengthB;
-	maxLengthA = C - ratio * b2_minPulleyLength;
-	maxLengthB = (C - b2_minPulleyLength) / ratio;
 }
 
 b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
 : b2Joint(def)
 {
-	m_groundAnchor1 = def->groundAnchorA;
-	m_groundAnchor2 = def->groundAnchorB;
-	m_localAnchor1 = def->localAnchorA;
-	m_localAnchor2 = def->localAnchorB;
+	m_groundAnchorA = def->groundAnchorA;
+	m_groundAnchorB = def->groundAnchorB;
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
 
 	b2Assert(def->ratio != 0.0f);
 	m_ratio = def->ratio;
 
 	m_constant = def->lengthA + m_ratio * def->lengthB;
 
-	m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength);
-	m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio);
-
 	m_impulse = 0.0f;
-	m_limitImpulse1 = 0.0f;
-	m_limitImpulse2 = 0.0f;
 }
 
-void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	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 p1 = b1->m_sweep.c + r1;
-	b2Vec2 p2 = b2->m_sweep.c + r2;
+	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 s1 = m_groundAnchor1;
-	b2Vec2 s2 = m_groundAnchor2;
+	b2Rot qA(aA), qB(aB);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
 	// Get the pulley axes.
-	m_u1 = p1 - s1;
-	m_u2 = p2 - s2;
+	m_uA = cA + m_rA - m_groundAnchorA;
+	m_uB = cB + m_rB - m_groundAnchorB;
 
-	float32 length1 = m_u1.Length();
-	float32 length2 = m_u2.Length();
+	float32 lengthA = m_uA.Length();
+	float32 lengthB = m_uB.Length();
 
-	if (length1 > b2_linearSlop)
+	if (lengthA > 10.0f * b2_linearSlop)
 	{
-		m_u1 *= 1.0f / length1;
+		m_uA *= 1.0f / lengthA;
 	}
 	else
 	{
-		m_u1.SetZero();
+		m_uA.SetZero();
 	}
 
-	if (length2 > b2_linearSlop)
+	if (lengthB > 10.0f * b2_linearSlop)
 	{
-		m_u2 *= 1.0f / length2;
+		m_uB *= 1.0f / lengthB;
 	}
 	else
 	{
-		m_u2.SetZero();
-	}
-
-	float32 C = m_constant - length1 - m_ratio * length2;
-	if (C > 0.0f)
-	{
-		m_state = e_inactiveLimit;
-		m_impulse = 0.0f;
-	}
-	else
-	{
-		m_state = e_atUpperLimit;
-	}
-
-	if (length1 < m_maxLength1)
-	{
-		m_limitState1 = e_inactiveLimit;
-		m_limitImpulse1 = 0.0f;
-	}
-	else
-	{
-		m_limitState1 = e_atUpperLimit;
-	}
-
-	if (length2 < m_maxLength2)
-	{
-		m_limitState2 = e_inactiveLimit;
-		m_limitImpulse2 = 0.0f;
-	}
-	else
-	{
-		m_limitState2 = e_atUpperLimit;
+		m_uB.SetZero();
 	}
 
 	// Compute effective mass.
-	float32 cr1u1 = b2Cross(r1, m_u1);
-	float32 cr2u2 = b2Cross(r2, m_u2);
+	float32 ruA = b2Cross(m_rA, m_uA);
+	float32 ruB = b2Cross(m_rB, m_uB);
 
-	m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
-	m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
-	m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
-	b2Assert(m_limitMass1 > b2_epsilon);
-	b2Assert(m_limitMass2 > b2_epsilon);
-	b2Assert(m_pulleyMass > b2_epsilon);
-	m_limitMass1 = 1.0f / m_limitMass1;
-	m_limitMass2 = 1.0f / m_limitMass2;
-	m_pulleyMass = 1.0f / m_pulleyMass;
+	float32 mA = m_invMassA + m_invIA * ruA * ruA;
+	float32 mB = m_invMassB + m_invIB * ruB * ruB;
 
-	if (step.warmStarting)
+	m_mass = mA + m_ratio * m_ratio * mB;
+
+	if (m_mass > 0.0f)
+	{
+		m_mass = 1.0f / m_mass;
+	}
+
+	if (data.step.warmStarting)
 	{
 		// Scale impulses to support variable time steps.
-		m_impulse *= step.dtRatio;
-		m_limitImpulse1 *= step.dtRatio;
-		m_limitImpulse2 *= step.dtRatio;
+		m_impulse *= data.step.dtRatio;
 
 		// Warm starting.
-		b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;
-		b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;
-		b1->m_linearVelocity += b1->m_invMass * P1;
-		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
-		b2->m_linearVelocity += b2->m_invMass * P2;
-		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+		b2Vec2 PA = -(m_impulse) * m_uA;
+		b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
+
+		vA += m_invMassA * PA;
+		wA += m_invIA * b2Cross(m_rA, PA);
+		vB += m_invMassB * PB;
+		wB += m_invIB * b2Cross(m_rB, PB);
 	}
 	else
 	{
 		m_impulse = 0.0f;
-		m_limitImpulse1 = 0.0f;
-		m_limitImpulse2 = 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 b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(step);
+	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;
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
+	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
 
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
+	float32 impulse = -m_mass * Cdot;
+	m_impulse += impulse;
 
-	if (m_state == e_atUpperLimit)
-	{
-		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
-		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+	b2Vec2 PA = -impulse * m_uA;
+	b2Vec2 PB = -m_ratio * impulse * m_uB;
+	vA += m_invMassA * PA;
+	wA += m_invIA * b2Cross(m_rA, PA);
+	vB += m_invMassB * PB;
+	wB += m_invIB * b2Cross(m_rB, PB);
 
-		float32 Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
-		float32 impulse = m_pulleyMass * (-Cdot);
-		float32 oldImpulse = m_impulse;
-		m_impulse = b2Max(0.0f, m_impulse + impulse);
-		impulse = m_impulse - oldImpulse;
-
-		b2Vec2 P1 = -impulse * m_u1;
-		b2Vec2 P2 = -m_ratio * impulse * m_u2;
-		b1->m_linearVelocity += b1->m_invMass * P1;
-		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
-		b2->m_linearVelocity += b2->m_invMass * P2;
-		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
-	}
-
-	if (m_limitState1 == e_atUpperLimit)
-	{
-		b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
-
-		float32 Cdot = -b2Dot(m_u1, v1);
-		float32 impulse = -m_limitMass1 * Cdot;
-		float32 oldImpulse = m_limitImpulse1;
-		m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse);
-		impulse = m_limitImpulse1 - oldImpulse;
-
-		b2Vec2 P1 = -impulse * m_u1;
-		b1->m_linearVelocity += b1->m_invMass * P1;
-		b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
-	}
-
-	if (m_limitState2 == e_atUpperLimit)
-	{
-		b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
-
-		float32 Cdot = -b2Dot(m_u2, v2);
-		float32 impulse = -m_limitMass2 * Cdot;
-		float32 oldImpulse = m_limitImpulse2;
-		m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse);
-		impulse = m_limitImpulse2 - oldImpulse;
-
-		b2Vec2 P2 = -impulse * m_u2;
-		b2->m_linearVelocity += b2->m_invMass * P2;
-		b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
-	}
+	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 b2PulleyJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
 
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Rot qA(aA), qB(aB);
 
-	b2Vec2 s1 = m_groundAnchor1;
-	b2Vec2 s2 = m_groundAnchor2;
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
-	float32 linearError = 0.0f;
+	// Get the pulley axes.
+	b2Vec2 uA = cA + rA - m_groundAnchorA;
+	b2Vec2 uB = cB + rB - m_groundAnchorB;
 
-	if (m_state == e_atUpperLimit)
+	float32 lengthA = uA.Length();
+	float32 lengthB = uB.Length();
+
+	if (lengthA > 10.0f * b2_linearSlop)
 	{
-		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-
-		b2Vec2 p1 = b1->m_sweep.c + r1;
-		b2Vec2 p2 = b2->m_sweep.c + r2;
-
-		// Get the pulley axes.
-		m_u1 = p1 - s1;
-		m_u2 = p2 - s2;
-
-		float32 length1 = m_u1.Length();
-		float32 length2 = m_u2.Length();
-
-		if (length1 > b2_linearSlop)
-		{
-			m_u1 *= 1.0f / length1;
-		}
-		else
-		{
-			m_u1.SetZero();
-		}
-
-		if (length2 > b2_linearSlop)
-		{
-			m_u2 *= 1.0f / length2;
-		}
-		else
-		{
-			m_u2.SetZero();
-		}
-
-		float32 C = m_constant - length1 - m_ratio * length2;
-		linearError = b2Max(linearError, -C);
-
-		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-		float32 impulse = -m_pulleyMass * C;
-
-		b2Vec2 P1 = -impulse * m_u1;
-		b2Vec2 P2 = -m_ratio * impulse * m_u2;
-
-		b1->m_sweep.c += b1->m_invMass * P1;
-		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
-		b2->m_sweep.c += b2->m_invMass * P2;
-		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
-
-		b1->SynchronizeTransform();
-		b2->SynchronizeTransform();
+		uA *= 1.0f / lengthA;
+	}
+	else
+	{
+		uA.SetZero();
 	}
 
-	if (m_limitState1 == e_atUpperLimit)
+	if (lengthB > 10.0f * b2_linearSlop)
 	{
-		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 p1 = b1->m_sweep.c + r1;
-
-		m_u1 = p1 - s1;
-		float32 length1 = m_u1.Length();
-
-		if (length1 > b2_linearSlop)
-		{
-			m_u1 *= 1.0f / length1;
-		}
-		else
-		{
-			m_u1.SetZero();
-		}
-
-		float32 C = m_maxLength1 - length1;
-		linearError = b2Max(linearError, -C);
-		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-		float32 impulse = -m_limitMass1 * C;
-
-		b2Vec2 P1 = -impulse * m_u1;
-		b1->m_sweep.c += b1->m_invMass * P1;
-		b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
-
-		b1->SynchronizeTransform();
+		uB *= 1.0f / lengthB;
+	}
+	else
+	{
+		uB.SetZero();
 	}
 
-	if (m_limitState2 == e_atUpperLimit)
+	// Compute effective mass.
+	float32 ruA = b2Cross(rA, uA);
+	float32 ruB = b2Cross(rB, uB);
+
+	float32 mA = m_invMassA + m_invIA * ruA * ruA;
+	float32 mB = m_invMassB + m_invIB * ruB * ruB;
+
+	float32 mass = mA + m_ratio * m_ratio * mB;
+
+	if (mass > 0.0f)
 	{
-		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-		b2Vec2 p2 = b2->m_sweep.c + r2;
-
-		m_u2 = p2 - s2;
-		float32 length2 = m_u2.Length();
-
-		if (length2 > b2_linearSlop)
-		{
-			m_u2 *= 1.0f / length2;
-		}
-		else
-		{
-			m_u2.SetZero();
-		}
-
-		float32 C = m_maxLength2 - length2;
-		linearError = b2Max(linearError, -C);
-		C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
-		float32 impulse = -m_limitMass2 * C;
-
-		b2Vec2 P2 = -impulse * m_u2;
-		b2->m_sweep.c += b2->m_invMass * P2;
-		b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
-
-		b2->SynchronizeTransform();
+		mass = 1.0f / mass;
 	}
 
+	float32 C = m_constant - lengthA - m_ratio * lengthB;
+	float32 linearError = b2Abs(C);
+
+	float32 impulse = -mass * C;
+
+	b2Vec2 PA = -impulse * uA;
+	b2Vec2 PB = -m_ratio * impulse * uB;
+
+	cA += m_invMassA * PA;
+	aA += m_invIA * b2Cross(rA, PA);
+	cB += m_invMassB * PB;
+	aB += m_invIB * b2Cross(rB, PB);
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
 	return linearError < b2_linearSlop;
 }
 
 b2Vec2 b2PulleyJoint::GetAnchorA() const
 {
-	return m_bodyA->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
 }
 
 b2Vec2 b2PulleyJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
 {
-	b2Vec2 P = m_impulse * m_u2;
+	b2Vec2 P = m_impulse * m_uB;
 	return inv_dt * P;
 }
 
@@ -397,26 +280,26 @@ float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
 
 b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
 {
-	return m_groundAnchor1;
+	return m_groundAnchorA;
 }
 
 b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
 {
-	return m_groundAnchor2;
+	return m_groundAnchorB;
 }
 
-float32 b2PulleyJoint::GetLength1() const
+float32 b2PulleyJoint::GetLengthA() const
 {
-	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchor1);
-	b2Vec2 s = m_groundAnchor1;
+	b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
+	b2Vec2 s = m_groundAnchorA;
 	b2Vec2 d = p - s;
 	return d.Length();
 }
 
-float32 b2PulleyJoint::GetLength2() const
+float32 b2PulleyJoint::GetLengthB() const
 {
-	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchor2);
-	b2Vec2 s = m_groundAnchor2;
+	b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
+	b2Vec2 s = m_groundAnchorB;
 	b2Vec2 d = p - s;
 	return d.Length();
 }

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -24,8 +24,7 @@
 const float32 b2_minPulleyLength = 2.0f;
 
 /// Pulley joint definition. This requires two ground anchors,
-/// two dynamic body anchor points, max lengths for each side,
-/// and a pulley ratio.
+/// two dynamic body anchor points, and a pulley ratio.
 struct b2PulleyJointDef : public b2JointDef
 {
 	b2PulleyJointDef()
@@ -36,9 +35,7 @@ struct b2PulleyJointDef : public b2JointDef
 		localAnchorA.Set(-1.0f, 0.0f);
 		localAnchorB.Set(1.0f, 0.0f);
 		lengthA = 0.0f;
-		maxLengthA = 0.0f;
 		lengthB = 0.0f;
-		maxLengthB = 0.0f;
 		ratio = 1.0f;
 		collideConnected = true;
 	}
@@ -64,15 +61,9 @@ struct b2PulleyJointDef : public b2JointDef
 	/// The a reference length for the segment attached to bodyA.
 	float32 lengthA;
 
-	/// The maximum length of the segment attached to bodyA.
-	float32 maxLengthA;
-
 	/// The a reference length for the segment attached to bodyB.
 	float32 lengthB;
 
-	/// The maximum length of the segment attached to bodyB.
-	float32 maxLengthB;
-
 	/// The pulley ratio, used to simulate a block-and-tackle.
 	float32 ratio;
 };
@@ -81,8 +72,10 @@ struct b2PulleyJointDef : public b2JointDef
 /// The pulley supports a ratio such that:
 /// length1 + ratio * length2 <= constant
 /// Yes, the force transmitted is scaled by the ratio.
-/// The pulley also enforces a maximum length limit on both sides. This is
-/// useful to prevent one side of the pulley hitting the top.
+/// Warning: the pulley joint can get a bit squirrelly by itself. They often
+/// work better when combined with prismatic joints. You should also cover the
+/// the anchor points with static shapes to prevent one side from going to
+/// zero length.
 class b2PulleyJoint : public b2Joint
 {
 public:
@@ -98,11 +91,11 @@ public:
 	/// Get the second ground anchor.
 	b2Vec2 GetGroundAnchorB() const;
 
-	/// Get the current length of the segment attached to body1.
-	float32 GetLength1() const;
+	/// Get the current length of the segment attached to bodyA.
+	float32 GetLengthA() const;
 
-	/// Get the current length of the segment attached to body2.
-	float32 GetLength2() const;
+	/// Get the current length of the segment attached to bodyB.
+	float32 GetLengthB() const;
 
 	/// Get the pulley ratio.
 	float32 GetRatio() const;
@@ -112,37 +105,34 @@ protected:
 	friend class b2Joint;
 	b2PulleyJoint(const b2PulleyJointDef* data);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	b2Vec2 m_groundAnchor1;
-	b2Vec2 m_groundAnchor2;
-	b2Vec2 m_localAnchor1;
-	b2Vec2 m_localAnchor2;
-
-	b2Vec2 m_u1;
-	b2Vec2 m_u2;
+	b2Vec2 m_groundAnchorA;
+	b2Vec2 m_groundAnchorB;
 	
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
 	float32 m_constant;
 	float32 m_ratio;
-	
-	float32 m_maxLength1;
-	float32 m_maxLength2;
-
-	// Effective masses
-	float32 m_pulleyMass;
-	float32 m_limitMass1;
-	float32 m_limitMass2;
-
-	// Impulses for accumulation/warm starting.
 	float32 m_impulse;
-	float32 m_limitImpulse1;
-	float32 m_limitImpulse2;
 
-	b2LimitState m_state;
-	b2LimitState m_limitState1;
-	b2LimitState m_limitState2;
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_uA;
+	b2Vec2 m_uB;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	float32 m_mass;
 };
 
 #endif

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -33,10 +33,10 @@
 // J = [0 0 -1 0 0 1]
 // K = invI1 + invI2
 
-void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
+void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
 {
-	bodyA = b1;
-	bodyB = b2;
+	bodyA = bA;
+	bodyB = bB;
 	localAnchorA = bodyA->GetLocalPoint(anchor);
 	localAnchorB = bodyB->GetLocalPoint(anchor);
 	referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
@@ -45,8 +45,8 @@ void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor
 b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
 : b2Joint(def)
 {
-	m_localAnchor1 = def->localAnchorA;
-	m_localAnchor2 = def->localAnchorB;
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
 	m_referenceAngle = def->referenceAngle;
 
 	m_impulse.SetZero();
@@ -61,58 +61,70 @@ b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
 	m_limitState = e_inactiveLimit;
 }
 
-void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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;
 
-	if (m_enableMotor || m_enableLimit)
-	{
-		// You cannot create a rotation limit between bodies that
-		// both have fixed rotation.
-		b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f);
-	}
+	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;
 
-	// Compute the effective mass matrix.
-	b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-	b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+	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);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
 	// J = [-I -r1_skew I r2_skew]
 	//     [ 0       -1 0       1]
 	// r_skew = [-ry; rx]
 
 	// Matlab
-	// K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
-	//     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
-	//     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]
+	// 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 m1 = b1->m_invMass, m2 = b2->m_invMass;
-	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
-	m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
-	m_mass.col3.x = -r1.y * i1 - r2.y * i2;
-	m_mass.col1.y = m_mass.col2.x;
-	m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
-	m_mass.col3.y = r1.x * i1 + r2.x * i2;
-	m_mass.col1.z = m_mass.col3.x;
-	m_mass.col2.z = m_mass.col3.y;
-	m_mass.col3.z = i1 + i2;
+	bool fixedRotation = (iA + iB == 0.0f);
 
-	m_motorMass = i1 + i2;
+	m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
+	m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
+	m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
+	m_mass.ex.y = m_mass.ey.x;
+	m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
+	m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
+	m_mass.ex.z = m_mass.ez.x;
+	m_mass.ey.z = m_mass.ez.y;
+	m_mass.ez.z = iA + iB;
+
+	m_motorMass = iA + iB;
 	if (m_motorMass > 0.0f)
 	{
 		m_motorMass = 1.0f / m_motorMass;
 	}
 
-	if (m_enableMotor == false)
+	if (m_enableMotor == false || fixedRotation)
 	{
 		m_motorImpulse = 0.0f;
 	}
 
-	if (m_enableLimit)
+	if (m_enableLimit && fixedRotation == false)
 	{
-		float32 jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		float32 jointAngle = aB - aA - m_referenceAngle;
 		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
 		{
 			m_limitState = e_equalLimits;
@@ -144,66 +156,66 @@ void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
 		m_limitState = e_inactiveLimit;
 	}
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
 		// Scale impulses to support a variable time step.
-		m_impulse *= step.dtRatio;
-		m_motorImpulse *= step.dtRatio;
+		m_impulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
 
 		b2Vec2 P(m_impulse.x, m_impulse.y);
 
-		b1->m_linearVelocity -= m1 * P;
-		b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z);
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
 
-		b2->m_linearVelocity += m2 * P;
-		b2->m_angularVelocity += i2 * (b2Cross(r2, P) + m_motorImpulse + m_impulse.z);
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
 	}
 	else
 	{
 		m_impulse.SetZero();
 		m_motorImpulse = 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 b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	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;
 
-	b2Vec2 v1 = b1->m_linearVelocity;
-	float32 w1 = b1->m_angularVelocity;
-	b2Vec2 v2 = b2->m_linearVelocity;
-	float32 w2 = b2->m_angularVelocity;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	float32 m1 = b1->m_invMass, m2 = b2->m_invMass;
-	float32 i1 = b1->m_invI, i2 = b2->m_invI;
+	bool fixedRotation = (iA + iB == 0.0f);
 
 	// Solve motor constraint.
-	if (m_enableMotor && m_limitState != e_equalLimits)
+	if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
 	{
-		float32 Cdot = w2 - w1 - m_motorSpeed;
-		float32 impulse = m_motorMass * (-Cdot);
+		float32 Cdot = wB - wA - m_motorSpeed;
+		float32 impulse = -m_motorMass * Cdot;
 		float32 oldImpulse = m_motorImpulse;
-		float32 maxImpulse = step.dt * m_maxMotorTorque;
+		float32 maxImpulse = data.step.dt * m_maxMotorTorque;
 		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
 		impulse = m_motorImpulse - oldImpulse;
 
-		w1 -= i1 * impulse;
-		w2 += i2 * impulse;
+		wA -= iA * impulse;
+		wB += iB * impulse;
 	}
 
 	// Solve limit constraint.
-	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
 	{
-		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-
-		// Solve point-to-point constraint
-		b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
-		float32 Cdot2 = w2 - w1;
+		b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+		float32 Cdot2 = wB - wA;
 		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
 
-		b2Vec3 impulse = m_mass.Solve33(-Cdot);
+		b2Vec3 impulse = -m_mass.Solve33(Cdot);
 
 		if (m_limitState == e_equalLimits)
 		{
@@ -214,7 +226,8 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
 			float32 newImpulse = m_impulse.z + impulse.z;
 			if (newImpulse < 0.0f)
 			{
-				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
+				b2Vec2 reduced = m_mass.Solve22(rhs);
 				impulse.x = reduced.x;
 				impulse.y = reduced.y;
 				impulse.z = -m_impulse.z;
@@ -222,13 +235,18 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
 				m_impulse.y += reduced.y;
 				m_impulse.z = 0.0f;
 			}
+			else
+			{
+				m_impulse += impulse;
+			}
 		}
 		else if (m_limitState == e_atUpperLimit)
 		{
 			float32 newImpulse = m_impulse.z + impulse.z;
 			if (newImpulse > 0.0f)
 			{
-				b2Vec2 reduced = m_mass.Solve22(-Cdot1);
+				b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
+				b2Vec2 reduced = m_mass.Solve22(rhs);
 				impulse.x = reduced.x;
 				impulse.y = reduced.y;
 				impulse.z = -m_impulse.z;
@@ -236,57 +254,60 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
 				m_impulse.y += reduced.y;
 				m_impulse.z = 0.0f;
 			}
+			else
+			{
+				m_impulse += impulse;
+			}
 		}
 
 		b2Vec2 P(impulse.x, impulse.y);
 
-		v1 -= m1 * P;
-		w1 -= i1 * (b2Cross(r1, P) + impulse.z);
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + impulse.z);
 
-		v2 += m2 * P;
-		w2 += i2 * (b2Cross(r2, P) + impulse.z);
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + impulse.z);
 	}
 	else
 	{
-		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
-
 		// Solve point-to-point constraint
-		b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+		b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
 		b2Vec2 impulse = m_mass.Solve22(-Cdot);
 
 		m_impulse.x += impulse.x;
 		m_impulse.y += impulse.y;
 
-		v1 -= m1 * impulse;
-		w1 -= i1 * b2Cross(r1, impulse);
+		vA -= mA * impulse;
+		wA -= iA * b2Cross(m_rA, impulse);
 
-		v2 += m2 * impulse;
-		w2 += i2 * b2Cross(r2, impulse);
+		vB += mB * impulse;
+		wB += iB * b2Cross(m_rB, impulse);
 	}
 
-	b1->m_linearVelocity = v1;
-	b1->m_angularVelocity = w1;
-	b2->m_linearVelocity = v2;
-	b2->m_angularVelocity = w2;
+	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 b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	// TODO_ERIN block solve with limit.
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
 
-	B2_NOT_USED(baumgarte);
-
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
+	b2Rot qA(aA), qB(aB);
 
 	float32 angularError = 0.0f;
 	float32 positionError = 0.0f;
 
+	bool fixedRotation = (m_invIA + m_invIB == 0.0f);
+
 	// Solve angular limit constraint.
-	if (m_enableLimit && m_limitState != e_inactiveLimit)
+	if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
 	{
-		float32 angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+		float32 angle = aB - aA - m_referenceAngle;
 		float32 limitImpulse = 0.0f;
 
 		if (m_limitState == e_equalLimits)
@@ -315,79 +336,54 @@ bool b2RevoluteJoint::SolvePositionConstraints(float32 baumgarte)
 			limitImpulse = -m_motorMass * C;
 		}
 
-		b1->m_sweep.a -= b1->m_invI * limitImpulse;
-		b2->m_sweep.a += b2->m_invI * limitImpulse;
-
-		b1->SynchronizeTransform();
-		b2->SynchronizeTransform();
+		aA -= m_invIA * limitImpulse;
+		aB += m_invIB * limitImpulse;
 	}
 
 	// Solve point-to-point constraint.
 	{
-		b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
-		b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+		qA.Set(aA);
+		qB.Set(aB);
+		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
-		b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+		b2Vec2 C = cB + rB - cA - rA;
 		positionError = C.Length();
 
-		float32 invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
-		float32 invI1 = b1->m_invI, invI2 = b2->m_invI;
+		float32 mA = m_invMassA, mB = m_invMassB;
+		float32 iA = m_invIA, iB = m_invIB;
 
-		// Handle large detachment.
-		const float32 k_allowedStretch = 10.0f * b2_linearSlop;
-		if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
-		{
-			// Use a particle solution (no rotation).
-			b2Vec2 u = C; u.Normalize();
-			float32 m = invMass1 + invMass2;
-			if (m > 0.0f)
-			{
-				m = 1.0f / m;
-			}
-			b2Vec2 impulse = m * (-C);
-			const float32 k_beta = 0.5f;
-			b1->m_sweep.c -= k_beta * invMass1 * impulse;
-			b2->m_sweep.c += k_beta * invMass2 * impulse;
+		b2Mat22 K;
+		K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
+		K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
+		K.ey.x = K.ex.y;
+		K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
 
-			C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
-		}
+		b2Vec2 impulse = -K.Solve(C);
 
-		b2Mat22 K1;
-		K1.col1.x = invMass1 + invMass2;	K1.col2.x = 0.0f;
-		K1.col1.y = 0.0f;					K1.col2.y = invMass1 + invMass2;
+		cA -= mA * impulse;
+		aA -= iA * b2Cross(rA, impulse);
 
-		b2Mat22 K2;
-		K2.col1.x =  invI1 * r1.y * r1.y;	K2.col2.x = -invI1 * r1.x * r1.y;
-		K2.col1.y = -invI1 * r1.x * r1.y;	K2.col2.y =  invI1 * r1.x * r1.x;
-
-		b2Mat22 K3;
-		K3.col1.x =  invI2 * r2.y * r2.y;	K3.col2.x = -invI2 * r2.x * r2.y;
-		K3.col1.y = -invI2 * r2.x * r2.y;	K3.col2.y =  invI2 * r2.x * r2.x;
-
-		b2Mat22 K = K1 + K2 + K3;
-		b2Vec2 impulse = K.Solve(-C);
-
-		b1->m_sweep.c -= b1->m_invMass * impulse;
-		b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
-
-		b2->m_sweep.c += b2->m_invMass * impulse;
-		b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
-
-		b1->SynchronizeTransform();
-		b2->SynchronizeTransform();
+		cB += mB * impulse;
+		aB += iB * b2Cross(rB, impulse);
 	}
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
 	
 	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
 }
 
 b2Vec2 b2RevoluteJoint::GetAnchorA() const
 {
-	return m_bodyA->GetWorldPoint(m_localAnchor1);
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
 }
 
 b2Vec2 b2RevoluteJoint::GetAnchorB() const
 {
-	return m_bodyB->GetWorldPoint(m_localAnchor2);
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
 }
 
 b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
@@ -403,16 +399,16 @@ float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
 
 float32 b2RevoluteJoint::GetJointAngle() const
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
-	return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+	return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
 }
 
 float32 b2RevoluteJoint::GetJointSpeed() const
 {
-	b2Body* b1 = m_bodyA;
-	b2Body* b2 = m_bodyB;
-	return b2->m_angularVelocity - b1->m_angularVelocity;
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+	return bB->m_angularVelocity - bA->m_angularVelocity;
 }
 
 bool b2RevoluteJoint::IsMotorEnabled() const
@@ -427,9 +423,9 @@ void b2RevoluteJoint::EnableMotor(bool flag)
 	m_enableMotor = flag;
 }
 
-float32 b2RevoluteJoint::GetMotorTorque() const
+float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
 {
-	return m_motorImpulse;
+	return inv_dt * m_motorImpulse;
 }
 
 void b2RevoluteJoint::SetMotorSpeed(float32 speed)
@@ -453,9 +449,13 @@ bool b2RevoluteJoint::IsLimitEnabled() const
 
 void b2RevoluteJoint::EnableLimit(bool flag)
 {
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_enableLimit = flag;
+	if (flag != m_enableLimit)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_enableLimit = flag;
+		m_impulse.z = 0.0f;
+	}
 }
 
 float32 b2RevoluteJoint::GetLowerLimit() const
@@ -471,8 +471,13 @@ float32 b2RevoluteJoint::GetUpperLimit() const
 void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
 {
 	b2Assert(lower <= upper);
-	m_bodyA->SetAwake(true);
-	m_bodyB->SetAwake(true);
-	m_lowerAngle = lower;
-	m_upperAngle = upper;
+	
+	if (lower != m_lowerAngle || upper != m_upperAngle)
+	{
+		m_bodyA->SetAwake(true);
+		m_bodyB->SetAwake(true);
+		m_impulse.z = 0.0f;
+		m_lowerAngle = lower;
+		m_upperAngle = upper;
+	}
 }

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

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -93,9 +93,6 @@ public:
 	b2Vec2 GetAnchorA() const;
 	b2Vec2 GetAnchorB() const;
 
-	b2Vec2 GetReactionForce(float32 inv_dt) const;
-	float32 GetReactionTorque(float32 inv_dt) const;
-
 	/// Get the current joint angle in radians.
 	float32 GetJointAngle() const;
 
@@ -132,8 +129,17 @@ public:
 	/// Set the maximum motor torque, usually in N-m.
 	void SetMaxMotorTorque(float32 torque);
 
-	/// Get the current motor torque, usually in N-m.
-	float32 GetMotorTorque() const;
+	/// Get the reaction force given the inverse time step.
+	/// Unit is N.
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+
+	/// Get the reaction torque due to the joint limit given the inverse time step.
+	/// Unit is N*m.
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the current motor torque given the inverse time step.
+	/// Unit is N*m.
+	float32 GetMotorTorque(float32 inv_dt) const;
 
 protected:
 	
@@ -142,19 +148,16 @@ protected:
 
 	b2RevoluteJoint(const b2RevoluteJointDef* def);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
-	bool SolvePositionConstraints(float32 baumgarte);
-
-	b2Vec2 m_localAnchor1;	// relative
-	b2Vec2 m_localAnchor2;
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
 	b2Vec3 m_impulse;
 	float32 m_motorImpulse;
 
-	b2Mat33 m_mass;			// effective mass for point-to-point constraint.
-	float32 m_motorMass;	// effective mass for motor/limit angular constraint.
-	
 	bool m_enableMotor;
 	float32 m_maxMotorTorque;
 	float32 m_motorSpeed;
@@ -163,6 +166,20 @@ protected:
 	float32 m_referenceAngle;
 	float32 m_lowerAngle;
 	float32 m_upperAngle;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	b2Mat33 m_mass;			// effective mass for point-to-point constraint.
+	float32 m_motorMass;	// effective mass for motor/limit angular constraint.
 	b2LimitState m_limitState;
 };
 

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.cpp

@@ -0,0 +1,226 @@
+/*
+* Copyright (c) 2007-2011 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/b2RopeJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+
+// Limit:
+// C = norm(pB - pA) - L
+// u = (pB - pA) / norm(pB - pA)
+// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
+// J = [-u -cross(rA, u) u cross(rB, u)]
+// K = J * invM * JT
+//   = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
+
+b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+
+	m_maxLength = def->maxLength;
+
+	m_mass = 0.0f;
+	m_impulse = 0.0f;
+	m_state = e_inactiveLimit;
+	m_length = 0.0f;
+}
+
+void b2RopeJoint::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);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	m_u = cB + m_rB - cA - m_rA;
+
+	m_length = m_u.Length();
+
+	float32 C = m_length - m_maxLength;
+	if (C > 0.0f)
+	{
+		m_state = e_atUpperLimit;
+	}
+	else
+	{
+		m_state = e_inactiveLimit;
+	}
+
+	if (m_length > b2_linearSlop)
+	{
+		m_u *= 1.0f / m_length;
+	}
+	else
+	{
+		m_u.SetZero();
+		m_mass = 0.0f;
+		m_impulse = 0.0f;
+		return;
+	}
+
+	// Compute effective mass.
+	float32 crA = b2Cross(m_rA, m_u);
+	float32 crB = b2Cross(m_rB, m_u);
+	float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
+
+	m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+
+	if (data.step.warmStarting)
+	{
+		// Scale the impulse to support a variable time step.
+		m_impulse *= data.step.dtRatio;
+
+		b2Vec2 P = m_impulse * m_u;
+		vA -= m_invMassA * P;
+		wA -= m_invIA * b2Cross(m_rA, P);
+		vB += m_invMassB * P;
+		wB += m_invIB * b2Cross(m_rB, P);
+	}
+	else
+	{
+		m_impulse = 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 b2RopeJoint::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;
+
+	// Cdot = dot(u, v + cross(w, r))
+	b2Vec2 vpA = vA + b2Cross(wA, m_rA);
+	b2Vec2 vpB = vB + b2Cross(wB, m_rB);
+	float32 C = m_length - m_maxLength;
+	float32 Cdot = b2Dot(m_u, vpB - vpA);
+
+	// Predictive constraint.
+	if (C < 0.0f)
+	{
+		Cdot += data.step.inv_dt * C;
+	}
+
+	float32 impulse = -m_mass * Cdot;
+	float32 oldImpulse = m_impulse;
+	m_impulse = b2Min(0.0f, m_impulse + impulse);
+	impulse = m_impulse - oldImpulse;
+
+	b2Vec2 P = impulse * m_u;
+	vA -= m_invMassA * P;
+	wA -= m_invIA * b2Cross(m_rA, P);
+	vB += m_invMassB * P;
+	wB += m_invIB * b2Cross(m_rB, P);
+
+	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 b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
+
+	b2Rot qA(aA), qB(aB);
+
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 u = cB + rB - cA - rA;
+
+	float32 length = u.Normalize();
+	float32 C = length - m_maxLength;
+
+	C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);
+
+	float32 impulse = -m_mass * C;
+	b2Vec2 P = impulse * u;
+
+	cA -= m_invMassA * P;
+	aA -= m_invIA * b2Cross(rA, P);
+	cB += m_invMassB * P;
+	aB += m_invIB * b2Cross(rB, P);
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
+	return length - m_maxLength < b2_linearSlop;
+}
+
+b2Vec2 b2RopeJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2RopeJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const
+{
+	b2Vec2 F = (inv_dt * m_impulse) * m_u;
+	return F;
+}
+
+float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const
+{
+	B2_NOT_USED(inv_dt);
+	return 0.0f;
+}
+
+float32 b2RopeJoint::GetMaxLength() const
+{
+	return m_maxLength;
+}
+
+b2LimitState b2RopeJoint::GetLimitState() const
+{
+	return m_state;
+}

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2RopeJoint.h

@@ -0,0 +1,104 @@
+/*
+* Copyright (c) 2006-2011 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_ROPE_JOINT_H
+#define B2_ROPE_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Rope joint definition. This requires two body anchor points and
+/// a maximum lengths.
+/// Note: by default the connected objects will not collide.
+/// see collideConnected in b2JointDef.
+struct b2RopeJointDef : public b2JointDef
+{
+	b2RopeJointDef()
+	{
+		type = e_ropeJoint;
+		localAnchorA.Set(-1.0f, 0.0f);
+		localAnchorB.Set(1.0f, 0.0f);
+		maxLength = 0.0f;
+	}
+
+	/// The local anchor point relative to bodyA's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to bodyB's origin.
+	b2Vec2 localAnchorB;
+
+	/// The maximum length of the rope.
+	/// Warning: this must be larger than b2_linearSlop or
+	/// the joint will have no effect.
+	float32 maxLength;
+};
+
+/// A rope joint enforces a maximum distance between two points
+/// on two bodies. It has no other effect.
+/// Warning: if you attempt to change the maximum length during
+/// the simulation you will get some non-physical behavior.
+/// A model that would allow you to dynamically modify the length
+/// would have some sponginess, so I chose not to implement it
+/// that way. See b2DistanceJoint if you want to dynamically
+/// control length.
+class b2RopeJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the maximum length of the rope.
+	float32 GetMaxLength() const;
+
+	b2LimitState GetLimitState() const;
+
+protected:
+
+	friend class b2Joint;
+	b2RopeJoint(const b2RopeJointDef* data);
+
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
+
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	float32 m_maxLength;
+	float32 m_length;
+	float32 m_impulse;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_u;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+	float32 m_mass;
+	b2LimitState m_state;
+};
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -53,14 +53,31 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
 	m_impulse.SetZero();
 }
 
-void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
+void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 {
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
+	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;
 
-	// Compute the effective mass matrix.
-	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
-	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+	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);
+
+	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
 	// J = [-I -r1_skew I r2_skew]
 	//     [ 0       -1 0       1]
@@ -71,128 +88,121 @@ void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
 	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
 	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]
 
-	float32 mA = bA->m_invMass, mB = bB->m_invMass;
-	float32 iA = bA->m_invI, iB = bB->m_invI;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
-	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
-	m_mass.col3.x = -rA.y * iA - rB.y * iB;
-	m_mass.col1.y = m_mass.col2.x;
-	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
-	m_mass.col3.y = rA.x * iA + rB.x * iB;
-	m_mass.col1.z = m_mass.col3.x;
-	m_mass.col2.z = m_mass.col3.y;
-	m_mass.col3.z = iA + iB;
+	m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
+	m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
+	m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
+	m_mass.ex.y = m_mass.ey.x;
+	m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
+	m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
+	m_mass.ex.z = m_mass.ez.x;
+	m_mass.ey.z = m_mass.ez.y;
+	m_mass.ez.z = iA + iB;
 
-	if (step.warmStarting)
+	if (data.step.warmStarting)
 	{
 		// Scale impulses to support a variable time step.
-		m_impulse *= step.dtRatio;
+		m_impulse *= data.step.dtRatio;
 
 		b2Vec2 P(m_impulse.x, m_impulse.y);
 
-		bA->m_linearVelocity -= mA * P;
-		bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z);
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
 
-		bB->m_linearVelocity += mB * P;
-		bB->m_angularVelocity += iB * (b2Cross(rB, P) + m_impulse.z);
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
 	}
 	else
 	{
 		m_impulse.SetZero();
 	}
+
+	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 b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
+void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(step);
+	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;
 
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Vec2 vA = bA->m_linearVelocity;
-	float32 wA = bA->m_angularVelocity;
-	b2Vec2 vB = bB->m_linearVelocity;
-	float32 wB = bB->m_angularVelocity;
-
-	float32 mA = bA->m_invMass, mB = bB->m_invMass;
-	float32 iA = bA->m_invI, iB = bB->m_invI;
-
-	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
-	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
-
-	// Solve point-to-point constraint
-	b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+	b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
 	float32 Cdot2 = wB - wA;
 	b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
 
-	b2Vec3 impulse = m_mass.Solve33(-Cdot);
+	b2Vec3 impulse = -m_mass.Solve33(Cdot);
 	m_impulse += impulse;
 
 	b2Vec2 P(impulse.x, impulse.y);
 
 	vA -= mA * P;
-	wA -= iA * (b2Cross(rA, P) + impulse.z);
+	wA -= iA * (b2Cross(m_rA, P) + impulse.z);
 
 	vB += mB * P;
-	wB += iB * (b2Cross(rB, P) + impulse.z);
+	wB += iB * (b2Cross(m_rB, P) + impulse.z);
 
-	bA->m_linearVelocity = vA;
-	bA->m_angularVelocity = wA;
-	bB->m_linearVelocity = vB;
-	bB->m_angularVelocity = wB;
+	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 b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
+bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
 {
-	B2_NOT_USED(baumgarte);
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
 
-	b2Body* bA = m_bodyA;
-	b2Body* bB = m_bodyB;
+	b2Rot qA(aA), qB(aB);
 
-	float32 mA = bA->m_invMass, mB = bB->m_invMass;
-	float32 iA = bA->m_invI, iB = bB->m_invI;
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
-	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
-	b2Vec2 C1 =  bB->m_sweep.c + rB - bA->m_sweep.c - rA;
-	float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
+	b2Vec2 C1 =  cB + rB - cA - rA;
+	float32 C2 = aB - aA - m_referenceAngle;
 
-	// Handle large detachment.
-	const float32 k_allowedStretch = 10.0f * b2_linearSlop;
 	float32 positionError = C1.Length();
 	float32 angularError = b2Abs(C2);
-	if (positionError > k_allowedStretch)
-	{
-		iA *= 1.0f;
-		iB *= 1.0f;
-	}
 
-	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
-	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
-	m_mass.col3.x = -rA.y * iA - rB.y * iB;
-	m_mass.col1.y = m_mass.col2.x;
-	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
-	m_mass.col3.y = rA.x * iA + rB.x * iB;
-	m_mass.col1.z = m_mass.col3.x;
-	m_mass.col2.z = m_mass.col3.y;
-	m_mass.col3.z = iA + iB;
+	m_mass.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	m_mass.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	m_mass.ez.x = -rA.y * iA - rB.y * iB;
+	m_mass.ex.y = m_mass.ey.x;
+	m_mass.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	m_mass.ez.y = rA.x * iA + rB.x * iB;
+	m_mass.ex.z = m_mass.ez.x;
+	m_mass.ey.z = m_mass.ez.y;
+	m_mass.ez.z = iA + iB;
 
 	b2Vec3 C(C1.x, C1.y, C2);
 
-	b2Vec3 impulse = m_mass.Solve33(-C);
+	b2Vec3 impulse = -m_mass.Solve33(C);
 
 	b2Vec2 P(impulse.x, impulse.y);
 
-	bA->m_sweep.c -= mA * P;
-	bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);
+	cA -= mA * P;
+	aA -= iA * (b2Cross(rA, P) + impulse.z);
 
-	bB->m_sweep.c += mB * P;
-	bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
+	cB += mB * P;
+	aB += iB * (b2Cross(rB, P) + impulse.z);
 
-	bA->SynchronizeTransform();
-	bB->SynchronizeTransform();
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
 
 	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
 }

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WeldJoint.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -65,17 +65,27 @@ protected:
 
 	b2WeldJoint(const b2WeldJointDef* def);
 
-	void InitVelocityConstraints(const b2TimeStep& step);
-	void SolveVelocityConstraints(const b2TimeStep& step);
-
-	bool SolvePositionConstraints(float32 baumgarte);
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
 
+	// Solver shared
 	b2Vec2 m_localAnchorA;
 	b2Vec2 m_localAnchorB;
 	float32 m_referenceAngle;
-
 	b2Vec3 m_impulse;
 
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_rA;
+	b2Vec2 m_rB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
 	b2Mat33 m_mass;
 };
 

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.cpp

@@ -0,0 +1,404 @@
+/*
+* Copyright (c) 2006-2007 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/b2WheelJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
+
+// Linear constraint (point-to-line)
+// d = pB - pA = xB + rB - xA - rA
+// C = dot(ay, d)
+// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
+//      = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
+// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
+
+// Spring linear constraint
+// C = dot(ax, d)
+// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
+// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
+
+// Motor rotational constraint
+// Cdot = wB - wA
+// J = [0 0 -1 0 0 1]
+
+void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
+{
+	bodyA = bA;
+	bodyB = bB;
+	localAnchorA = bodyA->GetLocalPoint(anchor);
+	localAnchorB = bodyB->GetLocalPoint(anchor);
+	localAxisA = bodyA->GetLocalVector(axis);
+}
+
+b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
+: b2Joint(def)
+{
+	m_localAnchorA = def->localAnchorA;
+	m_localAnchorB = def->localAnchorB;
+	m_localXAxisA = def->localAxisA;
+	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
+
+	m_mass = 0.0f;
+	m_impulse = 0.0f;
+	m_motorMass = 0.0;
+	m_motorImpulse = 0.0f;
+	m_springMass = 0.0f;
+	m_springImpulse = 0.0f;
+
+	m_maxMotorTorque = def->maxMotorTorque;
+	m_motorSpeed = def->motorSpeed;
+	m_enableMotor = def->enableMotor;
+
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
+
+	m_bias = 0.0f;
+	m_gamma = 0.0f;
+
+	m_ax.SetZero();
+	m_ay.SetZero();
+}
+
+void b2WheelJoint::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;
+
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
+
+	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 masses.
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = cB + rB - cA - rA;
+
+	// Point to line constraint
+	{
+		m_ay = b2Mul(qA, m_localYAxisA);
+		m_sAy = b2Cross(d + rA, m_ay);
+		m_sBy = b2Cross(rB, m_ay);
+
+		m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
+
+		if (m_mass > 0.0f)
+		{
+			m_mass = 1.0f / m_mass;
+		}
+	}
+
+	// Spring constraint
+	m_springMass = 0.0f;
+	m_bias = 0.0f;
+	m_gamma = 0.0f;
+	if (m_frequencyHz > 0.0f)
+	{
+		m_ax = b2Mul(qA, m_localXAxisA);
+		m_sAx = b2Cross(d + rA, m_ax);
+		m_sBx = b2Cross(rB, m_ax);
+
+		float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
+
+		if (invMass > 0.0f)
+		{
+			m_springMass = 1.0f / invMass;
+
+			float32 C = b2Dot(d, m_ax);
+
+			// Frequency
+			float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+			// Damping coefficient
+			float32 d = 2.0f * m_springMass * m_dampingRatio * omega;
+
+			// Spring stiffness
+			float32 k = m_springMass * omega * omega;
+
+			// magic formulas
+			float32 h = data.step.dt;
+			m_gamma = h * (d + h * k);
+			if (m_gamma > 0.0f)
+			{
+				m_gamma = 1.0f / m_gamma;
+			}
+
+			m_bias = C * h * k * m_gamma;
+
+			m_springMass = invMass + m_gamma;
+			if (m_springMass > 0.0f)
+			{
+				m_springMass = 1.0f / m_springMass;
+			}
+		}
+	}
+	else
+	{
+		m_springImpulse = 0.0f;
+	}
+
+	// Rotational motor
+	if (m_enableMotor)
+	{
+		m_motorMass = iA + iB;
+		if (m_motorMass > 0.0f)
+		{
+			m_motorMass = 1.0f / m_motorMass;
+		}
+	}
+	else
+	{
+		m_motorMass = 0.0f;
+		m_motorImpulse = 0.0f;
+	}
+
+	if (data.step.warmStarting)
+	{
+		// Account for variable time step.
+		m_impulse *= data.step.dtRatio;
+		m_springImpulse *= data.step.dtRatio;
+		m_motorImpulse *= data.step.dtRatio;
+
+		b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
+		float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
+		float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
+
+		vA -= m_invMassA * P;
+		wA -= m_invIA * LA;
+
+		vB += m_invMassB * P;
+		wB += m_invIB * LB;
+	}
+	else
+	{
+		m_impulse = 0.0f;
+		m_springImpulse = 0.0f;
+		m_motorImpulse = 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 b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
+{
+	float32 mA = m_invMassA, mB = m_invMassB;
+	float32 iA = m_invIA, iB = m_invIB;
+
+	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;
+
+	// Solve spring constraint
+	{
+		float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
+		float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
+		m_springImpulse += impulse;
+
+		b2Vec2 P = impulse * m_ax;
+		float32 LA = impulse * m_sAx;
+		float32 LB = impulse * m_sBx;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	// Solve rotational motor constraint
+	{
+		float32 Cdot = wB - wA - m_motorSpeed;
+		float32 impulse = -m_motorMass * Cdot;
+
+		float32 oldImpulse = m_motorImpulse;
+		float32 maxImpulse = data.step.dt * m_maxMotorTorque;
+		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+		impulse = m_motorImpulse - oldImpulse;
+
+		wA -= iA * impulse;
+		wB += iB * impulse;
+	}
+
+	// Solve point to line constraint
+	{
+		float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
+		float32 impulse = -m_mass * Cdot;
+		m_impulse += impulse;
+
+		b2Vec2 P = impulse * m_ay;
+		float32 LA = impulse * m_sAy;
+		float32 LB = impulse * m_sBy;
+
+		vA -= mA * P;
+		wA -= iA * LA;
+
+		vB += mB * P;
+		wB += iB * LB;
+	}
+
+	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 b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
+{
+	b2Vec2 cA = data.positions[m_indexA].c;
+	float32 aA = data.positions[m_indexA].a;
+	b2Vec2 cB = data.positions[m_indexB].c;
+	float32 aB = data.positions[m_indexB].a;
+
+	b2Rot qA(aA), qB(aB);
+
+	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
+	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+	b2Vec2 d = (cB - cA) + rB - rA;
+
+	b2Vec2 ay = b2Mul(qA, m_localYAxisA);
+
+	float32 sAy = b2Cross(d + rA, ay);
+	float32 sBy = b2Cross(rB, ay);
+
+	float32 C = b2Dot(d, ay);
+
+	float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
+
+	float32 impulse;
+	if (k != 0.0f)
+	{
+		impulse = - C / k;
+	}
+	else
+	{
+		impulse = 0.0f;
+	}
+
+	b2Vec2 P = impulse * ay;
+	float32 LA = impulse * sAy;
+	float32 LB = impulse * sBy;
+
+	cA -= m_invMassA * P;
+	aA -= m_invIA * LA;
+	cB += m_invMassB * P;
+	aB += m_invIB * LB;
+
+	data.positions[m_indexA].c = cA;
+	data.positions[m_indexA].a = aA;
+	data.positions[m_indexB].c = cB;
+	data.positions[m_indexB].a = aB;
+
+	return b2Abs(C) <= b2_linearSlop;
+}
+
+b2Vec2 b2WheelJoint::GetAnchorA() const
+{
+	return m_bodyA->GetWorldPoint(m_localAnchorA);
+}
+
+b2Vec2 b2WheelJoint::GetAnchorB() const
+{
+	return m_bodyB->GetWorldPoint(m_localAnchorB);
+}
+
+b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
+{
+	return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
+}
+
+float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+float32 b2WheelJoint::GetJointTranslation() const
+{
+	b2Body* bA = m_bodyA;
+	b2Body* bB = m_bodyB;
+
+	b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
+	b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
+	b2Vec2 d = pB - pA;
+	b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
+
+	float32 translation = b2Dot(d, axis);
+	return translation;
+}
+
+float32 b2WheelJoint::GetJointSpeed() const
+{
+	float32 wA = m_bodyA->m_angularVelocity;
+	float32 wB = m_bodyB->m_angularVelocity;
+	return wB - wA;
+}
+
+bool b2WheelJoint::IsMotorEnabled() const
+{
+	return m_enableMotor;
+}
+
+void b2WheelJoint::EnableMotor(bool flag)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_enableMotor = flag;
+}
+
+void b2WheelJoint::SetMotorSpeed(float32 speed)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_motorSpeed = speed;
+}
+
+void b2WheelJoint::SetMaxMotorTorque(float32 torque)
+{
+	m_bodyA->SetAwake(true);
+	m_bodyB->SetAwake(true);
+	m_maxMotorTorque = torque;
+}
+
+float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
+{
+	return inv_dt * m_motorImpulse;
+}
+
+
+
+
+

src/modules/physics/box2d/Box2D/Dynamics/Joints/b2WheelJoint.h

@@ -0,0 +1,199 @@
+/*
+* Copyright (c) 2006-2011 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_WHEEL_JOINT_H
+#define B2_WHEEL_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Wheel joint definition. This requires defining a line of
+/// motion using an axis and an anchor point. The definition uses local
+/// anchor points and a local axis so that the initial configuration
+/// can violate the constraint slightly. The joint translation is zero
+/// when the local anchor points coincide in world space. Using local
+/// anchors and a local axis helps when saving and loading a game.
+struct b2WheelJointDef : public b2JointDef
+{
+	b2WheelJointDef()
+	{
+		type = e_wheelJoint;
+		localAnchorA.SetZero();
+		localAnchorB.SetZero();
+		localAxisA.Set(1.0f, 0.0f);
+		enableMotor = false;
+		maxMotorTorque = 0.0f;
+		motorSpeed = 0.0f;
+		frequencyHz = 2.0f;
+		dampingRatio = 0.7f;
+	}
+
+	/// Initialize the bodies, anchors, axis, and reference angle using the world
+	/// anchor and world axis.
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
+
+	/// The local anchor point relative to body1's origin.
+	b2Vec2 localAnchorA;
+
+	/// The local anchor point relative to body2's origin.
+	b2Vec2 localAnchorB;
+
+	/// The local translation axis in body1.
+	b2Vec2 localAxisA;
+
+	/// Enable/disable the joint motor.
+	bool enableMotor;
+
+	/// The maximum motor torque, usually in N-m.
+	float32 maxMotorTorque;
+
+	/// The desired motor speed in radians per second.
+	float32 motorSpeed;
+
+	/// Suspension frequency, zero indicates no suspension
+	float32 frequencyHz;
+
+	/// Suspension damping ratio, one indicates critical damping
+	float32 dampingRatio;
+};
+
+/// A wheel joint. This joint provides two degrees of freedom: translation
+/// along an axis fixed in body1 and rotation in the plane. You can use a
+/// joint limit to restrict the range of motion and a joint motor to drive
+/// the rotation or to model rotational friction.
+/// This joint is designed for vehicle suspensions.
+class b2WheelJoint : public b2Joint
+{
+public:
+	b2Vec2 GetAnchorA() const;
+	b2Vec2 GetAnchorB() const;
+
+	b2Vec2 GetReactionForce(float32 inv_dt) const;
+	float32 GetReactionTorque(float32 inv_dt) const;
+
+	/// Get the current joint translation, usually in meters.
+	float32 GetJointTranslation() const;
+
+	/// Get the current joint translation speed, usually in meters per second.
+	float32 GetJointSpeed() const;
+
+	/// Is the joint motor enabled?
+	bool IsMotorEnabled() const;
+
+	/// Enable/disable the joint motor.
+	void EnableMotor(bool flag);
+
+	/// Set the motor speed, usually in radians per second.
+	void SetMotorSpeed(float32 speed);
+
+	/// Get the motor speed, usually in radians per second.
+	float32 GetMotorSpeed() const;
+
+	/// Set/Get the maximum motor force, usually in N-m.
+	void SetMaxMotorTorque(float32 torque);
+	float32 GetMaxMotorTorque() const;
+
+	/// Get the current motor torque given the inverse time step, usually in N-m.
+	float32 GetMotorTorque(float32 inv_dt) const;
+
+	/// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring.
+	void SetSpringFrequencyHz(float32 hz);
+	float32 GetSpringFrequencyHz() const;
+
+	/// Set/Get the spring damping ratio
+	void SetSpringDampingRatio(float32 ratio);
+	float32 GetSpringDampingRatio() const;
+
+protected:
+
+	friend class b2Joint;
+	b2WheelJoint(const b2WheelJointDef* def);
+
+	void InitVelocityConstraints(const b2SolverData& data);
+	void SolveVelocityConstraints(const b2SolverData& data);
+	bool SolvePositionConstraints(const b2SolverData& data);
+
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+
+	// Solver shared
+	b2Vec2 m_localAnchorA;
+	b2Vec2 m_localAnchorB;
+	b2Vec2 m_localXAxisA;
+	b2Vec2 m_localYAxisA;
+
+	float32 m_impulse;
+	float32 m_motorImpulse;
+	float32 m_springImpulse;
+
+	float32 m_maxMotorTorque;
+	float32 m_motorSpeed;
+	bool m_enableMotor;
+
+	// Solver temp
+	int32 m_indexA;
+	int32 m_indexB;
+	b2Vec2 m_localCenterA;
+	b2Vec2 m_localCenterB;
+	float32 m_invMassA;
+	float32 m_invMassB;
+	float32 m_invIA;
+	float32 m_invIB;
+
+	b2Vec2 m_ax, m_ay;
+	float32 m_sAx, m_sBx;
+	float32 m_sAy, m_sBy;
+
+	float32 m_mass;
+	float32 m_motorMass;
+	float32 m_springMass;
+
+	float32 m_bias;
+	float32 m_gamma;
+};
+
+inline float32 b2WheelJoint::GetMotorSpeed() const
+{
+	return m_motorSpeed;
+}
+
+inline float32 b2WheelJoint::GetMaxMotorTorque() const
+{
+	return m_maxMotorTorque;
+}
+
+inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz)
+{
+	m_frequencyHz = hz;
+}
+
+inline float32 b2WheelJoint::GetSpringFrequencyHz() const
+{
+	return m_frequencyHz;
+}
+
+inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio)
+{
+	m_dampingRatio = ratio;
+}
+
+inline float32 b2WheelJoint::GetSpringDampingRatio() const
+{
+	return m_dampingRatio;
+}
+
+#endif

src/modules/physics/box2d/Box2D/Dynamics/b2Body.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2007 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
@@ -28,7 +28,6 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 	b2Assert(bd->linearVelocity.IsValid());
 	b2Assert(b2IsValid(bd->angle));
 	b2Assert(b2IsValid(bd->angularVelocity));
-	b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
 	b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
 	b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
 
@@ -57,12 +56,15 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 
 	m_world = world;
 
-	m_xf.position = bd->position;
-	m_xf.R.Set(bd->angle);
+	m_xf.p = bd->position;
+	m_xf.q.Set(bd->angle);
 
 	m_sweep.localCenter.SetZero();
-	m_sweep.a0 = m_sweep.a = bd->angle;
-	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+	m_sweep.c0 = m_xf.p;
+	m_sweep.c = m_xf.p;
+	m_sweep.a0 = bd->angle;
+	m_sweep.a = bd->angle;
+	m_sweep.alpha0 = 0.0f;
 
 	m_jointList = NULL;
 	m_contactList = NULL;
@@ -74,6 +76,7 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 
 	m_linearDamping = bd->linearDamping;
 	m_angularDamping = bd->angularDamping;
+	m_gravityScale = bd->gravityScale;
 
 	m_force.SetZero();
 	m_torque = 0.0f;
@@ -109,6 +112,12 @@ b2Body::~b2Body()
 
 void b2Body::SetType(b2BodyType type)
 {
+	b2Assert(m_world->IsLocked() == false);
+	if (m_world->IsLocked() == true)
+	{
+		return;
+	}
+
 	if (m_type == type)
 	{
 		return;
@@ -122,6 +131,9 @@ void b2Body::SetType(b2BodyType type)
 	{
 		m_linearVelocity.SetZero();
 		m_angularVelocity = 0.0f;
+		m_sweep.a0 = m_sweep.a;
+		m_sweep.c0 = m_sweep.c;
+		SynchronizeFixtures();
 	}
 
 	SetAwake(true);
@@ -130,9 +142,9 @@ void b2Body::SetType(b2BodyType type)
 	m_torque = 0.0f;
 
 	// Since the body type changed, we need to flag contacts for filtering.
-	for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next)
+	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 	{
-		ce->contact->FlagForFiltering();
+		f->Refilter();
 	}
 }
 
@@ -153,7 +165,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
 	if (m_flags & e_activeFlag)
 	{
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
-		fixture->CreateProxy(broadPhase, m_xf);
+		fixture->CreateProxies(broadPhase, m_xf);
 	}
 
 	fixture->m_next = m_fixtureList;
@@ -235,13 +247,8 @@ void b2Body::DestroyFixture(b2Fixture* fixture)
 
 	if (m_flags & e_activeFlag)
 	{
-		b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy);
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
-		fixture->DestroyProxy(broadPhase);
-	}
-	else
-	{
-		b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy);
+		fixture->DestroyProxies(broadPhase);
 	}
 
 	fixture->Destroy(allocator);
@@ -268,14 +275,16 @@ void b2Body::ResetMassData()
 	// Static and kinematic bodies have zero mass.
 	if (m_type == b2_staticBody || m_type == b2_kinematicBody)
 	{
-		m_sweep.c0 = m_sweep.c = m_xf.position;
+		m_sweep.c0 = m_xf.p;
+		m_sweep.c = m_xf.p;
+		m_sweep.a0 = m_sweep.a;
 		return;
 	}
 
 	b2Assert(m_type == b2_dynamicBody);
 
 	// Accumulate mass over all fixtures.
-	b2Vec2 center = b2Vec2_zero;
+	b2Vec2 localCenter = b2Vec2_zero;
 	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 	{
 		if (f->m_density == 0.0f)
@@ -286,7 +295,7 @@ void b2Body::ResetMassData()
 		b2MassData massData;
 		f->GetMassData(&massData);
 		m_mass += massData.mass;
-		center += massData.mass * massData.center;
+		localCenter += massData.mass * massData.center;
 		m_I += massData.I;
 	}
 
@@ -294,7 +303,7 @@ void b2Body::ResetMassData()
 	if (m_mass > 0.0f)
 	{
 		m_invMass = 1.0f / m_mass;
-		center *= m_invMass;
+		localCenter *= m_invMass;
 	}
 	else
 	{
@@ -306,7 +315,7 @@ void b2Body::ResetMassData()
 	if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
 	{
 		// Center the inertia about the center of mass.
-		m_I -= m_mass * b2Dot(center, center);
+		m_I -= m_mass * b2Dot(localCenter, localCenter);
 		b2Assert(m_I > 0.0f);
 		m_invI = 1.0f / m_I;
 
@@ -319,7 +328,7 @@ void b2Body::ResetMassData()
 
 	// Move center of mass.
 	b2Vec2 oldCenter = m_sweep.c;
-	m_sweep.localCenter = center;
+	m_sweep.localCenter = localCenter;
 	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 
 	// Update center of mass velocity.
@@ -360,7 +369,7 @@ void b2Body::SetMassData(const b2MassData* massData)
 
 	// Move center of mass.
 	b2Vec2 oldCenter = m_sweep.c;
-	m_sweep.localCenter = massData->center;
+	m_sweep.localCenter =  massData->center;
 	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 
 	// Update center of mass velocity.
@@ -398,11 +407,14 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle)
 		return;
 	}
 
-	m_xf.R.Set(angle);
-	m_xf.position = position;
+	m_xf.q.Set(angle);
+	m_xf.p = position;
 
-	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
-	m_sweep.a0 = m_sweep.a = angle;
+	m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+	m_sweep.a = angle;
+
+	m_sweep.c0 = m_sweep.c;
+	m_sweep.a0 = angle;
 
 	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
@@ -416,8 +428,8 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle)
 void b2Body::SynchronizeFixtures()
 {
 	b2Transform xf1;
-	xf1.R.Set(m_sweep.a0);
-	xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
+	xf1.q.Set(m_sweep.a0);
+	xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
 
 	b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
@@ -428,6 +440,8 @@ void b2Body::SynchronizeFixtures()
 
 void b2Body::SetActive(bool flag)
 {
+	b2Assert(m_world->IsLocked() == false);
+
 	if (flag == IsActive())
 	{
 		return;
@@ -441,7 +455,7 @@ void b2Body::SetActive(bool flag)
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 		{
-			f->CreateProxy(broadPhase, m_xf);
+			f->CreateProxies(broadPhase, m_xf);
 		}
 
 		// Contacts are created the next time step.
@@ -454,7 +468,7 @@ void b2Body::SetActive(bool flag)
 		b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
 		for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
 		{
-			f->DestroyProxy(broadPhase);
+			f->DestroyProxies(broadPhase);
 		}
 
 		// Destroy the attached contacts.

src/modules/physics/box2d/Box2D/Dynamics/b2Body.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -40,7 +40,10 @@ enum b2BodyType
 {
 	b2_staticBody = 0,
 	b2_kinematicBody,
-	b2_dynamicBody,
+	b2_dynamicBody
+
+	// TODO_ERIN
+	//b2_bulletBody,
 };
 
 /// A body definition holds all the data needed to construct a rigid body.
@@ -63,7 +66,7 @@ struct b2BodyDef
 		bullet = false;
 		type = b2_staticBody;
 		active = true;
-		inertiaScale = 1.0f;
+		gravityScale = 1.0f;
 	}
 
 	/// The body type: static, kinematic, or dynamic.
@@ -115,8 +118,8 @@ struct b2BodyDef
 	/// Use this to store application specific body data.
 	void* userData;
 
-	/// Experimental: scales the inertia tensor.
-	float32 inertiaScale;
+	/// Scale the gravity applied to this body.
+	float32 gravityScale;
 };
 
 /// A rigid body. These are created via b2World::CreateBody.
@@ -198,6 +201,10 @@ public:
 	/// @param point the world position of the point of application.
 	void ApplyForce(const b2Vec2& force, const b2Vec2& point);
 
+	/// 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);
+
 	/// Apply a torque. This affects the angular velocity
 	/// without affecting the linear velocity of the center of mass.
 	/// This wakes up the body.
@@ -281,6 +288,12 @@ public:
 	/// Set the angular damping of the body.
 	void SetAngularDamping(float32 angularDamping);
 
+	/// Get the gravity scale of the body.
+	float32 GetGravityScale() const;
+
+	/// Set the gravity scale of the body.
+	void SetGravityScale(float32 scale);
+
 	/// Set the type of this body. This may alter the mass and velocity.
 	void SetType(b2BodyType type);
 
@@ -368,17 +381,18 @@ private:
 	friend class b2Island;
 	friend class b2ContactManager;
 	friend class b2ContactSolver;
-	friend class b2TOISolver;
+	friend class b2Contact;
 	
 	friend class b2DistanceJoint;
 	friend class b2GearJoint;
-	friend class b2LineJoint;
+	friend class b2WheelJoint;
 	friend class b2MouseJoint;
 	friend class b2PrismaticJoint;
 	friend class b2PulleyJoint;
 	friend class b2RevoluteJoint;
 	friend class b2WeldJoint;
 	friend class b2FrictionJoint;
+	friend class b2RopeJoint;
 
 	// m_flags
 	enum
@@ -389,7 +403,7 @@ private:
 		e_bulletFlag		= 0x0008,
 		e_fixedRotationFlag	= 0x0010,
 		e_activeFlag		= 0x0020,
-		e_toiFlag			= 0x0040,
+		e_toiFlag			= 0x0040
 	};
 
 	b2Body(const b2BodyDef* bd, b2World* world);
@@ -436,6 +450,7 @@ private:
 
 	float32 m_linearDamping;
 	float32 m_angularDamping;
+	float32 m_gravityScale;
 
 	float32 m_sleepTime;
 
@@ -454,12 +469,12 @@ inline const b2Transform& b2Body::GetTransform() const
 
 inline const b2Vec2& b2Body::GetPosition() const
 {
-	return m_xf.position;
+	return m_xf.p;
 }
 
 inline float32 b2Body::GetAngle() const
 {
-	return m_sweep.a;
+	return m_xf.q.GetAngle();
 }
 
 inline const b2Vec2& b2Body::GetWorldCenter() const
@@ -536,7 +551,7 @@ inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const
 
 inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const
 {
-	return b2Mul(m_xf.R, localVector);
+	return b2Mul(m_xf.q, localVector);
 }
 
 inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
@@ -546,7 +561,7 @@ inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
 
 inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const
 {
-	return b2MulT(m_xf.R, worldVector);
+	return b2MulT(m_xf.q, worldVector);
 }
 
 inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
@@ -579,6 +594,16 @@ inline void b2Body::SetAngularDamping(float32 angularDamping)
 	m_angularDamping = angularDamping;
 }
 
+inline float32 b2Body::GetGravityScale() const
+{
+	return m_gravityScale;
+}
+
+inline void b2Body::SetGravityScale(float32 scale)
+{
+	m_gravityScale = scale;
+}
+
 inline void b2Body::SetBullet(bool flag)
 {
 	if (flag)
@@ -730,6 +755,21 @@ inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
 	m_torque += b2Cross(point - m_sweep.c, force);
 }
 
+inline void b2Body::ApplyForceToCenter(const b2Vec2& force)
+{
+	if (m_type != b2_dynamicBody)
+	{
+		return;
+	}
+
+	if (IsAwake() == false)
+	{
+		SetAwake(true);
+	}
+
+	m_force += force;
+}
+
 inline void b2Body::ApplyTorque(float32 torque)
 {
 	if (m_type != b2_dynamicBody)
@@ -776,17 +816,18 @@ inline void b2Body::ApplyAngularImpulse(float32 impulse)
 
 inline void b2Body::SynchronizeTransform()
 {
-	m_xf.R.Set(m_sweep.a);
-	m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
+	m_xf.q.Set(m_sweep.a);
+	m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
 }
 
-inline void b2Body::Advance(float32 t)
+inline void b2Body::Advance(float32 alpha)
 {
-	// Advance to the new safe time.
-	m_sweep.Advance(t);
+	// Advance to the new safe time. This doesn't sync the broad-phase.
+	m_sweep.Advance(alpha);
 	m_sweep.c = m_sweep.c0;
 	m_sweep.a = m_sweep.a0;
-	SynchronizeTransform();
+	m_xf.q.Set(m_sweep.a);
+	m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
 }
 
 inline b2World* b2Body::GetWorld()

src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -110,10 +110,16 @@ void b2ContactManager::Collide()
 	{
 		b2Fixture* fixtureA = c->GetFixtureA();
 		b2Fixture* fixtureB = c->GetFixtureB();
+		int32 indexA = c->GetChildIndexA();
+		int32 indexB = c->GetChildIndexB();
 		b2Body* bodyA = fixtureA->GetBody();
 		b2Body* bodyB = fixtureB->GetBody();
+		 
+		bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;
+		bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;
 
-		if (bodyA->IsAwake() == false && bodyB->IsAwake() == false)
+		// At least one body must be awake and it must be dynamic or kinematic.
+		if (activeA == false && activeB == false)
 		{
 			c = c->GetNext();
 			continue;
@@ -144,8 +150,8 @@ void b2ContactManager::Collide()
 			c->m_flags &= ~b2Contact::e_filterFlag;
 		}
 
-		int32 proxyIdA = fixtureA->m_proxyId;
-		int32 proxyIdB = fixtureB->m_proxyId;
+		int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
+		int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
 		bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
 
 		// Here we destroy contacts that cease to overlap in the broad-phase.
@@ -170,8 +176,14 @@ void b2ContactManager::FindNewContacts()
 
 void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 {
-	b2Fixture* fixtureA = (b2Fixture*)proxyUserDataA;
-	b2Fixture* fixtureB = (b2Fixture*)proxyUserDataB;
+	b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA;
+	b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB;
+
+	b2Fixture* fixtureA = proxyA->fixture;
+	b2Fixture* fixtureB = proxyB->fixture;
+
+	int32 indexA = proxyA->childIndex;
+	int32 indexB = proxyB->childIndex;
 
 	b2Body* bodyA = fixtureA->GetBody();
 	b2Body* bodyB = fixtureB->GetBody();
@@ -182,6 +194,8 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 		return;
 	}
 
+	// TODO_ERIN use a hash table to remove a potential bottleneck when both
+	// bodies have a lot of contacts.
 	// Does a contact already exist?
 	b2ContactEdge* edge = bodyB->GetContactList();
 	while (edge)
@@ -190,13 +204,16 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 		{
 			b2Fixture* fA = edge->contact->GetFixtureA();
 			b2Fixture* fB = edge->contact->GetFixtureB();
-			if (fA == fixtureA && fB == fixtureB)
+			int32 iA = edge->contact->GetChildIndexA();
+			int32 iB = edge->contact->GetChildIndexB();
+
+			if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
 			{
 				// A contact already exists.
 				return;
 			}
 
-			if (fA == fixtureB && fB == fixtureA)
+			if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
 			{
 				// A contact already exists.
 				return;
@@ -219,11 +236,17 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	}
 
 	// Call the factory.
-	b2Contact* c = b2Contact::Create(fixtureA, fixtureB, m_allocator);
+	b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
+	if (c == NULL)
+	{
+		return;
+	}
 
 	// Contact creation may swap fixtures.
 	fixtureA = c->GetFixtureA();
 	fixtureB = c->GetFixtureB();
+	indexA = c->GetChildIndexA();
+	indexB = c->GetChildIndexB();
 	bodyA = fixtureA->GetBody();
 	bodyB = fixtureB->GetBody();
 
@@ -262,5 +285,9 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
 	}
 	bodyB->m_contactList = &c->m_nodeB;
 
+	// Wake up the bodies
+	bodyA->SetAwake(true);
+	bodyB->SetAwake(true);
+
 	++m_contactCount;
 }

src/modules/physics/box2d/Box2D/Dynamics/b2ContactManager.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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

src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -18,29 +18,26 @@
 
 #include <Box2D/Dynamics/b2Fixture.h>
 #include <Box2D/Dynamics/Contacts/b2Contact.h>
+#include <Box2D/Dynamics/b2World.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/b2BroadPhase.h>
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Common/b2BlockAllocator.h>
 
-
 b2Fixture::b2Fixture()
 {
 	m_userData = NULL;
 	m_body = NULL;
 	m_next = NULL;
-	m_proxyId = b2BroadPhase::e_nullProxy;
+	m_proxies = NULL;
+	m_proxyCount = 0;
 	m_shape = NULL;
 	m_density = 0.0f;
 }
 
-b2Fixture::~b2Fixture()
-{
-	b2Assert(m_shape == NULL);
-	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
-}
-
 void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def)
 {
 	m_userData = def->userData;
@@ -56,13 +53,28 @@ void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2Fixtur
 
 	m_shape = def->shape->Clone(allocator);
 
+	// Reserve proxy space
+	int32 childCount = m_shape->GetChildCount();
+	m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy));
+	for (int32 i = 0; i < childCount; ++i)
+	{
+		m_proxies[i].fixture = NULL;
+		m_proxies[i].proxyId = b2BroadPhase::e_nullProxy;
+	}
+	m_proxyCount = 0;
+
 	m_density = def->density;
 }
 
 void b2Fixture::Destroy(b2BlockAllocator* allocator)
 {
-	// The proxy must be destroyed before calling this.
-	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+	// The proxies must be destroyed before calling this.
+	b2Assert(m_proxyCount == 0);
+
+	// Free the proxy array.
+	int32 childCount = m_shape->GetChildCount();
+	allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy));
+	m_proxies = NULL;
 
 	// Free the child shape.
 	switch (m_shape->m_type)
@@ -75,6 +87,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
 		}
 		break;
 
+	case b2Shape::e_edge:
+		{
+			b2EdgeShape* s = (b2EdgeShape*)m_shape;
+			s->~b2EdgeShape();
+			allocator->Free(s, sizeof(b2EdgeShape));
+		}
+		break;
+
 	case b2Shape::e_polygon:
 		{
 			b2PolygonShape* s = (b2PolygonShape*)m_shape;
@@ -83,6 +103,14 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
 		}
 		break;
 
+	case b2Shape::e_chain:
+		{
+			b2ChainShape* s = (b2ChainShape*)m_shape;
+			s->~b2ChainShape();
+			allocator->Free(s, sizeof(b2ChainShape));
+		}
+		break;
+
 	default:
 		b2Assert(false);
 		break;
@@ -91,50 +119,69 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
 	m_shape = NULL;
 }
 
-void b2Fixture::CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf)
+void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf)
 {
-	b2Assert(m_proxyId == b2BroadPhase::e_nullProxy);
+	b2Assert(m_proxyCount == 0);
 
-	// Create proxy in the broad-phase.
-	m_shape->ComputeAABB(&m_aabb, xf);
-	m_proxyId = broadPhase->CreateProxy(m_aabb, this);
+	// Create proxies in the broad-phase.
+	m_proxyCount = m_shape->GetChildCount();
+
+	for (int32 i = 0; i < m_proxyCount; ++i)
+	{
+		b2FixtureProxy* proxy = m_proxies + i;
+		m_shape->ComputeAABB(&proxy->aabb, xf, i);
+		proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy);
+		proxy->fixture = this;
+		proxy->childIndex = i;
+	}
 }
 
-void b2Fixture::DestroyProxy(b2BroadPhase* broadPhase)
+void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase)
 {
-	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	// Destroy proxies in the broad-phase.
+	for (int32 i = 0; i < m_proxyCount; ++i)
 	{
-		return;
+		b2FixtureProxy* proxy = m_proxies + i;
+		broadPhase->DestroyProxy(proxy->proxyId);
+		proxy->proxyId = b2BroadPhase::e_nullProxy;
 	}
 
-	// Destroy proxy in the broad-phase.
-	broadPhase->DestroyProxy(m_proxyId);
-	m_proxyId = b2BroadPhase::e_nullProxy;
+	m_proxyCount = 0;
 }
 
 void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2)
 {
-	if (m_proxyId == b2BroadPhase::e_nullProxy)
+	if (m_proxyCount == 0)
 	{	
 		return;
 	}
 
-	// Compute an AABB that covers the swept shape (may miss some rotation effect).
-	b2AABB aabb1, aabb2;
-	m_shape->ComputeAABB(&aabb1, transform1);
-	m_shape->ComputeAABB(&aabb2, transform2);
+	for (int32 i = 0; i < m_proxyCount; ++i)
+	{
+		b2FixtureProxy* proxy = m_proxies + i;
+
+		// Compute an AABB that covers the swept shape (may miss some rotation effect).
+		b2AABB aabb1, aabb2;
+		m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex);
+		m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex);
 	
-	m_aabb.Combine(aabb1, aabb2);
+		proxy->aabb.Combine(aabb1, aabb2);
 
-	b2Vec2 displacement = transform2.position - transform1.position;
+		b2Vec2 displacement = transform2.p - transform1.p;
 
-	broadPhase->MoveProxy(m_proxyId, m_aabb, displacement);
+		broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
+	}
 }
 
 void b2Fixture::SetFilterData(const b2Filter& filter)
 {
 	m_filter = filter;
 
+	Refilter();
+}
+
+void b2Fixture::Refilter()
+{
 	if (m_body == NULL)
 	{
 		return;
@@ -154,10 +201,28 @@ void b2Fixture::SetFilterData(const b2Filter& filter)
 
 		edge = edge->next;
 	}
+
+	b2World* world = m_body->GetWorld();
+
+	if (world == NULL)
+	{
+		return;
+	}
+
+	// Touch each proxy so that new pairs may be created
+	b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase;
+	for (int32 i = 0; i < m_proxyCount; ++i)
+	{
+		broadPhase->TouchProxy(m_proxies[i].proxyId);
+	}
 }
 
 void b2Fixture::SetSensor(bool sensor)
 {
-	m_isSensor = sensor;
+	if (sensor != m_isSensor)
+	{
+		m_body->SetAwake(true);
+		m_isSensor = sensor;
+	}
 }
 

src/modules/physics/box2d/Box2D/Dynamics/b2Fixture.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -26,6 +26,7 @@
 class b2BlockAllocator;
 class b2Body;
 class b2BroadPhase;
+class b2Fixture;
 
 /// This holds contact filtering data.
 struct b2Filter
@@ -61,8 +62,6 @@ struct b2FixtureDef
 		isSensor = false;
 	}
 
-	virtual ~b2FixtureDef() {}
-
 	/// The shape, this must be set. The shape will be cloned, so you
 	/// can create the shape on the stack.
 	const b2Shape* shape;
@@ -87,6 +86,14 @@ struct b2FixtureDef
 	b2Filter filter;
 };
 
+/// This proxy is used internally to connect fixtures to the broad-phase.
+struct b2FixtureProxy
+{
+	b2AABB aabb;
+	b2Fixture* fixture;
+	int32 childIndex;
+	int32 proxyId;
+};
 
 /// A fixture is used to attach a shape to a body for collision detection. A fixture
 /// inherits its transform from its parent. Fixtures hold additional non-geometric data
@@ -115,11 +122,15 @@ public:
 
 	/// Set the contact filtering data. This will not update contacts until the next time
 	/// step when either parent body is active and awake.
+	/// This automatically calls Refilter.
 	void SetFilterData(const b2Filter& filter);
 
 	/// Get the contact filtering data.
 	const b2Filter& GetFilterData() const;
 
+	/// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
+	void Refilter();
+
 	/// Get the parent body of this fixture. This is NULL if the fixture is not attached.
 	/// @return the parent body.
 	b2Body* GetBody();
@@ -138,14 +149,13 @@ public:
 	void SetUserData(void* data);
 
 	/// Test a point for containment in this fixture.
-	/// @param xf the shape world transform.
 	/// @param p a point in world coordinates.
 	bool TestPoint(const b2Vec2& p) const;
 
 	/// Cast a ray against this shape.
 	/// @param output the ray-cast results.
 	/// @param input the ray-cast input parameters.
-	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
+	bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const;
 
 	/// Get the mass data for this fixture. The mass data is based on the density and
 	/// the shape. The rotational inertia is about the shape's origin. This operation
@@ -162,19 +172,21 @@ public:
 	/// Get the coefficient of friction.
 	float32 GetFriction() const;
 
-	/// Set the coefficient of friction.
+	/// Set the coefficient of friction. This will _not_ change the friction of
+	/// existing contacts.
 	void SetFriction(float32 friction);
 
 	/// Get the coefficient of restitution.
 	float32 GetRestitution() const;
 
-	/// Set the coefficient of restitution.
+	/// Set the coefficient of restitution. This will _not_ change the restitution of
+	/// existing contacts.
 	void SetRestitution(float32 restitution);
 
 	/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
 	/// If you need a more accurate AABB, compute it using the shape and
 	/// the body transform.
-	const b2AABB& GetAABB() const;
+	const b2AABB& GetAABB(int32 childIndex) const;
 
 protected:
 
@@ -184,7 +196,6 @@ protected:
 	friend class b2ContactManager;
 
 	b2Fixture();
-	~b2Fixture();
 
 	// We need separation create/destroy functions from the constructor/destructor because
 	// the destructor cannot access the allocator (no destructor arguments allowed by C++).
@@ -192,13 +203,11 @@ protected:
 	void Destroy(b2BlockAllocator* allocator);
 
 	// These support body activation/deactivation.
-	void CreateProxy(b2BroadPhase* broadPhase, const b2Transform& xf);
-	void DestroyProxy(b2BroadPhase* broadPhase);
+	void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf);
+	void DestroyProxies(b2BroadPhase* broadPhase);
 
 	void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
 
-	b2AABB m_aabb;
-
 	float32 m_density;
 
 	b2Fixture* m_next;
@@ -209,7 +218,9 @@ protected:
 	float32 m_friction;
 	float32 m_restitution;
 
-	int32 m_proxyId;
+	b2FixtureProxy* m_proxies;
+	int32 m_proxyCount;
+
 	b2Filter m_filter;
 
 	bool m_isSensor;
@@ -308,9 +319,9 @@ inline bool b2Fixture::TestPoint(const b2Vec2& p) const
 	return m_shape->TestPoint(m_body->GetTransform(), p);
 }
 
-inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
+inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const
 {
-	return m_shape->RayCast(output, input, m_body->GetTransform());
+	return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex);
 }
 
 inline void b2Fixture::GetMassData(b2MassData* massData) const
@@ -318,9 +329,10 @@ inline void b2Fixture::GetMassData(b2MassData* massData) const
 	m_shape->ComputeMass(massData, m_density);
 }
 
-inline const b2AABB& b2Fixture::GetAABB() const
+inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const
 {
-	return m_aabb;
+	b2Assert(0 <= childIndex && childIndex < m_proxyCount);
+	return m_proxies[childIndex].aabb;
 }
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/b2Island.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -16,6 +16,7 @@
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
+#include <Box2D/Collision/b2Distance.h>
 #include <Box2D/Dynamics/b2Island.h>
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2Fixture.h>
@@ -24,6 +25,7 @@
 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
 #include <Box2D/Dynamics/Joints/b2Joint.h>
 #include <Box2D/Common/b2StackAllocator.h>
+#include <Box2D/Common/b2Timer.h>
 
 /*
 Position Correction Notes
@@ -178,130 +180,166 @@ b2Island::~b2Island()
 	m_allocator->Free(m_bodies);
 }
 
-void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
+void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
 {
-	// Integrate velocities and apply damping.
+	b2Timer timer;
+
+	float32 h = step.dt;
+
+	// Integrate velocities and apply damping. Initialize the body state.
 	for (int32 i = 0; i < m_bodyCount; ++i)
 	{
 		b2Body* b = m_bodies[i];
 
-		if (b->GetType() != b2_dynamicBody)
-		{
-			continue;
-		}
-
-		// Integrate velocities.
-		b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
-		b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;
-
-		// Apply damping.
-		// ODE: dv/dt + c * v = 0
-		// Solution: v(t) = v0 * exp(-c * t)
-		// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
-		// v2 = exp(-c * dt) * v1
-		// Taylor expansion:
-		// v2 = (1.0f - c * dt) * v1
-		b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
-		b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
-	}
-
-	// Partition contacts so that contacts with static bodies are solved last.
-	int32 i1 = -1;
-	for (int32 i2 = 0; i2 < m_contactCount; ++i2)
-	{
-		b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
-		b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
-		b2Body* bodyA = fixtureA->GetBody();
-		b2Body* bodyB = fixtureB->GetBody();
-		bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
-		if (nonStatic)
-		{
-			++i1;
-			b2Swap(m_contacts[i1], m_contacts[i2]);
-		}
-	}
-
-	// Initialize velocity constraints.
-	b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
-	contactSolver.WarmStart();
-	for (int32 i = 0; i < m_jointCount; ++i)
-	{
-		m_joints[i]->InitVelocityConstraints(step);
-	}
-
-	// Solve velocity constraints.
-	for (int32 i = 0; i < step.velocityIterations; ++i)
-	{
-		for (int32 j = 0; j < m_jointCount; ++j)
-		{
-			m_joints[j]->SolveVelocityConstraints(step);
-		}
-
-		contactSolver.SolveVelocityConstraints();
-	}
-
-	// Post-solve (store impulses for warm starting).
-	contactSolver.StoreImpulses();
-
-	// Integrate positions.
-	for (int32 i = 0; i < m_bodyCount; ++i)
-	{
-		b2Body* b = m_bodies[i];
-
-		if (b->GetType() == b2_staticBody)
-		{
-			continue;
-		}
-
-		// Check for large velocities.
-		b2Vec2 translation = step.dt * b->m_linearVelocity;
-		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
-		{
-			float32 ratio = b2_maxTranslation / translation.Length();
-			b->m_linearVelocity *= ratio;
-		}
-
-		float32 rotation = step.dt * b->m_angularVelocity;
-		if (rotation * rotation > b2_maxRotationSquared)
-		{
-			float32 ratio = b2_maxRotation / b2Abs(rotation);
-			b->m_angularVelocity *= ratio;
-		}
+		b2Vec2 c = b->m_sweep.c;
+		float32 a = b->m_sweep.a;
+		b2Vec2 v = b->m_linearVelocity;
+		float32 w = b->m_angularVelocity;
 
 		// Store positions for continuous collision.
 		b->m_sweep.c0 = b->m_sweep.c;
 		b->m_sweep.a0 = b->m_sweep.a;
 
-		// Integrate
-		b->m_sweep.c += step.dt * b->m_linearVelocity;
-		b->m_sweep.a += step.dt * b->m_angularVelocity;
+		if (b->m_type == b2_dynamicBody)
+		{
+			// Integrate velocities.
+			v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
+			w += h * b->m_invI * b->m_torque;
 
-		// Compute new transform
-		b->SynchronizeTransform();
+			// Apply damping.
+			// ODE: dv/dt + c * v = 0
+			// Solution: v(t) = v0 * exp(-c * t)
+			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
+			// v2 = exp(-c * dt) * v1
+			// Taylor expansion:
+			// v2 = (1.0f - c * dt) * v1
+			v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f);
+			w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f);
+		}
 
-		// Note: shapes are synchronized later.
+		m_positions[i].c = c;
+		m_positions[i].a = a;
+		m_velocities[i].v = v;
+		m_velocities[i].w = w;
 	}
 
-	// Iterate over constraints.
+	timer.Reset();
+
+	// Solver data
+	b2SolverData solverData;
+	solverData.step = step;
+	solverData.positions = m_positions;
+	solverData.velocities = m_velocities;
+
+	// Initialize velocity constraints.
+	b2ContactSolverDef contactSolverDef;
+	contactSolverDef.step = step;
+	contactSolverDef.contacts = m_contacts;
+	contactSolverDef.count = m_contactCount;
+	contactSolverDef.positions = m_positions;
+	contactSolverDef.velocities = m_velocities;
+	contactSolverDef.allocator = m_allocator;
+
+	b2ContactSolver contactSolver(&contactSolverDef);
+	contactSolver.InitializeVelocityConstraints();
+
+	if (step.warmStarting)
+	{
+		contactSolver.WarmStart();
+	}
+	
+	for (int32 i = 0; i < m_jointCount; ++i)
+	{
+		m_joints[i]->InitVelocityConstraints(solverData);
+	}
+
+	profile->solveInit = timer.GetMilliseconds();
+
+	// Solve velocity constraints
+	timer.Reset();
+	for (int32 i = 0; i < step.velocityIterations; ++i)
+	{
+		for (int32 j = 0; j < m_jointCount; ++j)
+		{
+			m_joints[j]->SolveVelocityConstraints(solverData);
+		}
+
+		contactSolver.SolveVelocityConstraints();
+	}
+
+	// Store impulses for warm starting
+	contactSolver.StoreImpulses();
+	profile->solveVelocity = timer.GetMilliseconds();
+
+	// Integrate positions
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Vec2 c = m_positions[i].c;
+		float32 a = m_positions[i].a;
+		b2Vec2 v = m_velocities[i].v;
+		float32 w = m_velocities[i].w;
+
+		// Check for large velocities
+		b2Vec2 translation = h * v;
+		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
+		{
+			float32 ratio = b2_maxTranslation / translation.Length();
+			v *= ratio;
+		}
+
+		float32 rotation = h * w;
+		if (rotation * rotation > b2_maxRotationSquared)
+		{
+			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			w *= ratio;
+		}
+
+		// Integrate
+		c += h * v;
+		a += h * w;
+
+		m_positions[i].c = c;
+		m_positions[i].a = a;
+		m_velocities[i].v = v;
+		m_velocities[i].w = w;
+	}
+
+	// Solve position constraints
+	timer.Reset();
+	bool positionSolved = false;
 	for (int32 i = 0; i < step.positionIterations; ++i)
 	{
-		bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
+		bool contactsOkay = contactSolver.SolvePositionConstraints();
 
 		bool jointsOkay = true;
 		for (int32 i = 0; i < m_jointCount; ++i)
 		{
-			bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
+			bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
 			jointsOkay = jointsOkay && jointOkay;
 		}
 
 		if (contactsOkay && jointsOkay)
 		{
 			// Exit early if the position errors are small.
+			positionSolved = true;
 			break;
 		}
 	}
 
-	Report(contactSolver.m_constraints);
+	// Copy state buffers back to the bodies
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Body* body = m_bodies[i];
+		body->m_sweep.c = m_positions[i].c;
+		body->m_sweep.a = m_positions[i].a;
+		body->m_linearVelocity = m_velocities[i].v;
+		body->m_angularVelocity = m_velocities[i].w;
+		body->SynchronizeTransform();
+	}
+
+	profile->solvePosition = timer.GetMilliseconds();
+
+	Report(contactSolver.m_velocityConstraints);
 
 	if (allowSleep)
 	{
@@ -318,12 +356,6 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl
 				continue;
 			}
 
-			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
-			{
-				b->m_sleepTime = 0.0f;
-				minSleepTime = 0.0f;
-			}
-
 			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
 				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
 				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
@@ -333,12 +365,12 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl
 			}
 			else
 			{
-				b->m_sleepTime += step.dt;
+				b->m_sleepTime += h;
 				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
 			}
 		}
 
-		if (minSleepTime >= b2_timeToSleep)
+		if (minSleepTime >= b2_timeToSleep && positionSolved)
 		{
 			for (int32 i = 0; i < m_bodyCount; ++i)
 			{
@@ -349,7 +381,139 @@ void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSl
 	}
 }
 
-void b2Island::Report(const b2ContactConstraint* constraints)
+void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
+{
+	b2Assert(toiIndexA < m_bodyCount);
+	b2Assert(toiIndexB < m_bodyCount);
+
+	// Initialize the body state.
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Body* b = m_bodies[i];
+		m_positions[i].c = b->m_sweep.c;
+		m_positions[i].a = b->m_sweep.a;
+		m_velocities[i].v = b->m_linearVelocity;
+		m_velocities[i].w = b->m_angularVelocity;
+	}
+
+	b2ContactSolverDef contactSolverDef;
+	contactSolverDef.contacts = m_contacts;
+	contactSolverDef.count = m_contactCount;
+	contactSolverDef.allocator = m_allocator;
+	contactSolverDef.step = subStep;
+	contactSolverDef.positions = m_positions;
+	contactSolverDef.velocities = m_velocities;
+	b2ContactSolver contactSolver(&contactSolverDef);
+
+	// Solve position constraints.
+	for (int32 i = 0; i < subStep.positionIterations; ++i)
+	{
+		bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
+		if (contactsOkay)
+		{
+			break;
+		}
+	}
+
+#if 0
+	// Is the new position really safe?
+	for (int32 i = 0; i < m_contactCount; ++i)
+	{
+		b2Contact* c = m_contacts[i];
+		b2Fixture* fA = c->GetFixtureA();
+		b2Fixture* fB = c->GetFixtureB();
+
+		b2Body* bA = fA->GetBody();
+		b2Body* bB = fB->GetBody();
+
+		int32 indexA = c->GetChildIndexA();
+		int32 indexB = c->GetChildIndexB();
+
+		b2DistanceInput input;
+		input.proxyA.Set(fA->GetShape(), indexA);
+		input.proxyB.Set(fB->GetShape(), indexB);
+		input.transformA = bA->GetTransform();
+		input.transformB = bB->GetTransform();
+		input.useRadii = false;
+
+		b2DistanceOutput output;
+		b2SimplexCache cache;
+		cache.count = 0;
+		b2Distance(&output, &cache, &input);
+
+		if (output.distance == 0 || cache.count == 3)
+		{
+			cache.count += 0;
+		}
+	}
+#endif
+
+	// Leap of faith to new safe state.
+	m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
+	m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
+	m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
+	m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
+
+	// No warm starting is needed for TOI events because warm
+	// starting impulses were applied in the discrete solver.
+	contactSolver.InitializeVelocityConstraints();
+
+	// Solve velocity constraints.
+	for (int32 i = 0; i < subStep.velocityIterations; ++i)
+	{
+		contactSolver.SolveVelocityConstraints();
+	}
+
+	// Don't store the TOI contact forces for warm starting
+	// because they can be quite large.
+
+	float32 h = subStep.dt;
+
+	// Integrate positions
+	for (int32 i = 0; i < m_bodyCount; ++i)
+	{
+		b2Vec2 c = m_positions[i].c;
+		float32 a = m_positions[i].a;
+		b2Vec2 v = m_velocities[i].v;
+		float32 w = m_velocities[i].w;
+
+		// Check for large velocities
+		b2Vec2 translation = h * v;
+		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
+		{
+			float32 ratio = b2_maxTranslation / translation.Length();
+			v *= ratio;
+		}
+
+		float32 rotation = h * w;
+		if (rotation * rotation > b2_maxRotationSquared)
+		{
+			float32 ratio = b2_maxRotation / b2Abs(rotation);
+			w *= ratio;
+		}
+
+		// Integrate
+		c += h * v;
+		a += h * w;
+
+		m_positions[i].c = c;
+		m_positions[i].a = a;
+		m_velocities[i].v = v;
+		m_velocities[i].w = w;
+
+		// Sync bodies
+		b2Body* body = m_bodies[i];
+		body->m_sweep.c = c;
+		body->m_sweep.a = a;
+		body->m_linearVelocity = v;
+		body->m_angularVelocity = w;
+		body->SynchronizeTransform();
+	}
+
+	Report(contactSolver.m_velocityConstraints);
+}
+
+void b2Island::Report(const b2ContactVelocityConstraint* constraints)
 {
 	if (m_listener == NULL)
 	{
@@ -360,13 +524,14 @@ void b2Island::Report(const b2ContactConstraint* constraints)
 	{
 		b2Contact* c = m_contacts[i];
 
-		const b2ContactConstraint* cc = constraints + i;
+		const b2ContactVelocityConstraint* vc = constraints + i;
 		
 		b2ContactImpulse impulse;
-		for (int32 j = 0; j < cc->pointCount; ++j)
+		impulse.count = vc->pointCount;
+		for (int32 j = 0; j < vc->pointCount; ++j)
 		{
-			impulse.normalImpulses[j] = cc->points[j].normalImpulse;
-			impulse.tangentImpulses[j] = cc->points[j].tangentImpulse;
+			impulse.normalImpulses[j] = vc->points[j].normalImpulse;
+			impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
 		}
 
 		m_listener->PostSolve(c, &impulse);

src/modules/physics/box2d/Box2D/Dynamics/b2Island.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -27,21 +27,8 @@ class b2Contact;
 class b2Joint;
 class b2StackAllocator;
 class b2ContactListener;
-struct b2ContactConstraint;
-
-/// This is an internal structure.
-struct b2Position
-{
-	b2Vec2 x;
-	float32 a;
-};
-
-/// This is an internal structure.
-struct b2Velocity
-{
-	b2Vec2 v;
-	float32 w;
-};
+struct b2ContactVelocityConstraint;
+struct b2Profile;
 
 /// This is an internal class.
 class b2Island
@@ -58,13 +45,16 @@ public:
 		m_jointCount = 0;
 	}
 
-	void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
+	void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
+
+	void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB);
 
 	void Add(b2Body* body)
 	{
 		b2Assert(m_bodyCount < m_bodyCapacity);
 		body->m_islandIndex = m_bodyCount;
-		m_bodies[m_bodyCount++] = body;
+		m_bodies[m_bodyCount] = body;
+		++m_bodyCount;
 	}
 
 	void Add(b2Contact* contact)
@@ -79,7 +69,7 @@ public:
 		m_joints[m_jointCount++] = joint;
 	}
 
-	void Report(const b2ContactConstraint* constraints);
+	void Report(const b2ContactVelocityConstraint* constraints);
 
 	b2StackAllocator* m_allocator;
 	b2ContactListener* m_listener;
@@ -98,8 +88,6 @@ public:
 	int32 m_bodyCapacity;
 	int32 m_contactCapacity;
 	int32 m_jointCapacity;
-
-	int32 m_positionIterationCount;
 };
 
 #endif

src/modules/physics/box2d/Box2D/Dynamics/b2TimeStep.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -19,17 +19,52 @@
 #ifndef B2_TIME_STEP_H
 #define B2_TIME_STEP_H
 
-#include <Box2D/Common/b2Settings.h>
+#include <Box2D/Common/b2Math.h>
 
-/// This is an internal structure.
-struct b2TimeStep
+/// Profiling data. Times are in milliseconds.
+struct b2Profile
 {
-	float32 dt;			// time step
-	float32 inv_dt;		// inverse time step (0 if dt == 0).
-	float32 dtRatio;	// dt * inv_dt0
-	int32 velocityIterations;
-	int32 positionIterations;
-	bool warmStarting;
+	float32 step;
+	float32 collide;
+	float32 solve;
+	float32 solveInit;
+	float32 solveVelocity;
+	float32 solvePosition;
+	float32 broadphase;
+	float32 solveTOI;
 };
 
+/// This is an internal structure.
+struct b2TimeStep
+{
+	float32 dt;			// time step
+	float32 inv_dt;		// inverse time step (0 if dt == 0).
+	float32 dtRatio;	// dt * inv_dt0
+	int32 velocityIterations;
+	int32 positionIterations;
+	bool warmStarting;
+};
+
+/// This is an internal structure.
+struct b2Position
+{
+	b2Vec2 c;
+	float32 a;
+};
+
+/// This is an internal structure.
+struct b2Velocity
+{
+	b2Vec2 v;
+	float32 w;
+};
+
+/// Solver Data
+struct b2SolverData
+{
+	b2TimeStep step;
+	b2Position* positions;
+	b2Velocity* velocities;
+};
+
 #endif

src/modules/physics/box2d/Box2D/Dynamics/b2World.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -23,12 +23,15 @@
 #include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
 #include <Box2D/Dynamics/Contacts/b2Contact.h>
 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
-#include <Box2D/Dynamics/Contacts/b2TOISolver.h>
 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Collision/b2BroadPhase.h>
 #include <Box2D/Collision/Shapes/b2CircleShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <Box2D/Collision/Shapes/b2ChainShape.h>
 #include <Box2D/Collision/Shapes/b2PolygonShape.h>
 #include <Box2D/Collision/b2TimeOfImpact.h>
+#include <Box2D/Common/b2Draw.h>
+#include <Box2D/Common/b2Timer.h>
 #include <new>
 
 b2World::b2World(const b2Vec2& gravity, bool doSleep)
@@ -44,6 +47,9 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep)
 
 	m_warmStarting = true;
 	m_continuousPhysics = true;
+	m_subStepping = false;
+
+	m_stepComplete = true;
 
 	m_allowSleep = doSleep;
 	m_gravity = gravity;
@@ -53,10 +59,29 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep)
 	m_inv_dt0 = 0.0f;
 
 	m_contactManager.m_allocator = &m_blockAllocator;
+
+	memset(&m_profile, 0, sizeof(b2Profile));
 }
 
 b2World::~b2World()
 {
+	// Some shapes allocate using b2Alloc.
+	b2Body* b = m_bodyList;
+	while (b)
+	{
+		b2Body* bNext = b->m_next;
+
+		b2Fixture* f = b->m_fixtureList;
+		while (f)
+		{
+			b2Fixture* fNext = f->m_next;
+			f->m_proxyCount = 0;
+			f->Destroy(&m_blockAllocator);
+			f = fNext;
+		}
+
+		b = bNext;
+	}
 }
 
 void b2World::SetDestructionListener(b2DestructionListener* listener)
@@ -74,7 +99,7 @@ void b2World::SetContactListener(b2ContactListener* listener)
 	m_contactManager.m_contactListener = listener;
 }
 
-void b2World::SetDebugDraw(b2DebugDraw* debugDraw)
+void b2World::SetDebugDraw(b2Draw* debugDraw)
 {
 	m_debugDraw = debugDraw;
 }
@@ -125,6 +150,8 @@ void b2World::DestroyBody(b2Body* b)
 		}
 
 		DestroyJoint(je0->joint);
+
+		b->m_jointList = je;
 	}
 	b->m_jointList = NULL;
 
@@ -150,10 +177,13 @@ void b2World::DestroyBody(b2Body* b)
 			m_destructionListener->SayGoodbye(f0);
 		}
 
-		f0->DestroyProxy(&m_contactManager.m_broadPhase);
+		f0->DestroyProxies(&m_contactManager.m_broadPhase);
 		f0->Destroy(&m_blockAllocator);
 		f0->~b2Fixture();
 		m_blockAllocator.Free(f0, sizeof(b2Fixture));
+
+		b->m_fixtureList = f;
+		b->m_fixtureCount -= 1;
 	}
 	b->m_fixtureList = NULL;
 	b->m_fixtureCount = 0;
@@ -337,6 +367,10 @@ void b2World::DestroyJoint(b2Joint* j)
 // Find islands, integrate and solve constraints, solve position constraints
 void b2World::Solve(const b2TimeStep& step)
 {
+	m_profile.solveInit = 0.0f;
+	m_profile.solveVelocity = 0.0f;
+	m_profile.solvePosition = 0.0f;
+
 	// Size the island for the worst case.
 	b2Island island(m_bodyCount,
 					m_contactManager.m_contactCount,
@@ -475,7 +509,11 @@ void b2World::Solve(const b2TimeStep& step)
 			}
 		}
 
-		island.Solve(step, m_gravity, m_allowSleep);
+		b2Profile profile;
+		island.Solve(&profile, step, m_gravity, m_allowSleep);
+		m_profile.solveInit += profile.solveInit;
+		m_profile.solveVelocity += profile.solveVelocity;
+		m_profile.solvePosition += profile.solvePosition;
 
 		// Post solve cleanup.
 		for (int32 i = 0; i < island.m_bodyCount; ++i)
@@ -491,292 +529,357 @@ void b2World::Solve(const b2TimeStep& step)
 
 	m_stackAllocator.Free(stack);
 
-	// Synchronize fixtures, check for out of range bodies.
-	for (b2Body* b = m_bodyList; b; b = b->GetNext())
 	{
-		// If a body was not in an island then it did not move.
-		if ((b->m_flags & b2Body::e_islandFlag) == 0)
+		b2Timer timer;
+		// Synchronize fixtures, check for out of range bodies.
+		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
-			continue;
-		}
-
-		if (b->GetType() == b2_staticBody)
-		{
-			continue;
-		}
-
-		// Update fixtures (for broad-phase).
-		b->SynchronizeFixtures();
-	}
-
-	// Look for new contacts.
-	m_contactManager.FindNewContacts();
-}
-
-// Advance a dynamic body to its first time of contact
-// and adjust the position to ensure clearance.
-void b2World::SolveTOI(b2Body* body)
-{
-	// Find the minimum contact.
-	b2Contact* toiContact = NULL;
-	float32 toi = 1.0f;
-	b2Body* toiOther = NULL;
-	bool found;
-	int32 count;
-	int32 iter = 0;
-
-	bool bullet = body->IsBullet();
-
-	// Iterate until all contacts agree on the minimum TOI. We have
-	// to iterate because the TOI algorithm may skip some intermediate
-	// collisions when objects rotate through each other.
-	do
-	{
-		count = 0;
-		found = false;
-		for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
-		{
-			if (ce->contact == toiContact)
+			// If a body was not in an island then it did not move.
+			if ((b->m_flags & b2Body::e_islandFlag) == 0)
 			{
 				continue;
 			}
 
-			b2Body* other = ce->other;
-			b2BodyType type = other->GetType();
-
-			// Only bullets perform TOI with dynamic bodies.
-			if (bullet == true)
+			if (b->GetType() == b2_staticBody)
 			{
-				// Bullets only perform TOI with bodies that have their TOI resolved.
-				if ((other->m_flags & b2Body::e_toiFlag) == 0)
+				continue;
+			}
+
+			// Update fixtures (for broad-phase).
+			b->SynchronizeFixtures();
+		}
+
+		// Look for new contacts.
+		m_contactManager.FindNewContacts();
+		m_profile.broadphase = timer.GetMilliseconds();
+	}
+}
+
+// Find TOI contacts and solve them.
+void b2World::SolveTOI(const b2TimeStep& step)
+{
+	b2Island island(2 * b2_maxTOIContacts, b2_maxTOIContacts, 0, &m_stackAllocator, m_contactManager.m_contactListener);
+
+	if (m_stepComplete)
+	{
+		for (b2Body* b = m_bodyList; b; b = b->m_next)
+		{
+			b->m_flags &= ~b2Body::e_islandFlag;
+			b->m_sweep.alpha0 = 0.0f;
+		}
+
+		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
+		{
+			// Invalidate TOI
+			c->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
+			c->m_toiCount = 0;
+			c->m_toi = 1.0f;
+		}
+	}
+
+	// Find TOI events and solve them.
+	for (;;)
+	{
+		// Find the first TOI.
+		b2Contact* minContact = NULL;
+		float32 minAlpha = 1.0f;
+
+		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
+		{
+			// Is this contact disabled?
+			if (c->IsEnabled() == false)
+			{
+				continue;
+			}
+
+			// Prevent excessive sub-stepping.
+			if (c->m_toiCount > b2_maxSubSteps)
+			{
+				continue;
+			}
+
+			float32 alpha = 1.0f;
+			if (c->m_flags & b2Contact::e_toiFlag)
+			{
+				// This contact has a valid cached TOI.
+				alpha = c->m_toi;
+			}
+			else
+			{
+				b2Fixture* fA = c->GetFixtureA();
+				b2Fixture* fB = c->GetFixtureB();
+
+				// Is there a sensor?
+				if (fA->IsSensor() || fB->IsSensor())
 				{
 					continue;
 				}
 
-				// No repeated hits on non-static bodies
-				if (type != b2_staticBody && (ce->contact->m_flags & b2Contact::e_bulletHitFlag) != 0)
+				b2Body* bA = fA->GetBody();
+				b2Body* bB = fB->GetBody();
+
+				b2BodyType typeA = bA->m_type;
+				b2BodyType typeB = bB->m_type;
+				b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
+
+				bool activeA = bA->IsAwake() && typeA != b2_staticBody;
+				bool activeB = bB->IsAwake() && typeB != b2_staticBody;
+
+				// Is at least one body active (awake and dynamic or kinematic)?
+				if (activeA == false && activeB == false)
 				{
-						continue;
+					continue;
 				}
+
+				bool collideA = bA->IsBullet() || typeA != b2_dynamicBody;
+				bool collideB = bB->IsBullet() || typeB != b2_dynamicBody;
+
+				// Are these two non-bullet dynamic bodies?
+				if (collideA == false && collideB == false)
+				{
+					continue;
+				}
+
+				// Compute the TOI for this contact.
+				// Put the sweeps onto the same time interval.
+				float32 alpha0 = bA->m_sweep.alpha0;
+
+				if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
+				{
+					alpha0 = bB->m_sweep.alpha0;
+					bA->m_sweep.Advance(alpha0);
+				}
+				else if (bB->m_sweep.alpha0 < bA->m_sweep.alpha0)
+				{
+					alpha0 = bA->m_sweep.alpha0;
+					bB->m_sweep.Advance(alpha0);
+				}
+
+				b2Assert(alpha0 < 1.0f);
+
+				int32 indexA = c->GetChildIndexA();
+				int32 indexB = c->GetChildIndexB();
+
+				// Compute the time of impact in interval [0, minTOI]
+				b2TOIInput input;
+				input.proxyA.Set(fA->GetShape(), indexA);
+				input.proxyB.Set(fB->GetShape(), indexB);
+				input.sweepA = bA->m_sweep;
+				input.sweepB = bB->m_sweep;
+				input.tMax = 1.0f;
+
+				b2TOIOutput output;
+				b2TimeOfImpact(&output, &input);
+
+				// Beta is the fraction of the remaining portion of the .
+				float32 beta = output.t;
+				if (output.state == b2TOIOutput::e_touching)
+				{
+					alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
+				}
+				else
+				{
+					alpha = 1.0f;
+				}
+
+				c->m_toi = alpha;
+				c->m_flags |= b2Contact::e_toiFlag;
 			}
-			else if (type == b2_dynamicBody)
+
+			if (alpha < minAlpha)
 			{
-				continue;
+				// This is the minimum TOI found so far.
+				minContact = c;
+				minAlpha = alpha;
 			}
-
-			// Check for a disabled contact.
-			b2Contact* contact = ce->contact;
-			if (contact->IsEnabled() == false)
-			{
-				continue;
-			}
-
-			// Prevent infinite looping.
-			if (contact->m_toiCount > 10)
-			{
-				continue;
-			}
-
-			b2Fixture* fixtureA = contact->m_fixtureA;
-			b2Fixture* fixtureB = contact->m_fixtureB;
-
-			// Cull sensors.
-			if (fixtureA->IsSensor() || fixtureB->IsSensor())
-			{
-				continue;
-			}
-
-			b2Body* bodyA = fixtureA->m_body;
-			b2Body* bodyB = fixtureB->m_body;
-
-			// Compute the time of impact in interval [0, minTOI]
-			b2TOIInput input;
-			input.proxyA.Set(fixtureA->GetShape());
-			input.proxyB.Set(fixtureB->GetShape());
-			input.sweepA = bodyA->m_sweep;
-			input.sweepB = bodyB->m_sweep;
-			input.tMax = toi;
-
-			b2TOIOutput output;
-			b2TimeOfImpact(&output, &input);
-
-			if (output.state == b2TOIOutput::e_touching && output.t < toi)
-			{
-				toiContact = contact;
-				toi = output.t;
-				toiOther = other;
-				found = true;
-			}
-
-			++count;
 		}
 
-		++iter;
-	} while (found && count > 1 && iter < 50);
-
-	if (toiContact == NULL)
-	{
-		body->Advance(1.0f);
-		return;
-	}
-
-	b2Sweep backup = body->m_sweep;
-	body->Advance(toi);
-	toiContact->Update(m_contactManager.m_contactListener);
-	if (toiContact->IsEnabled() == false)
-	{
-		// Contact disabled. Backup and recurse.
-		body->m_sweep = backup;
-		SolveTOI(body);
-	}
-
-	++toiContact->m_toiCount;
-
-	// Update all the valid contacts on this body and build a contact island.
-	b2Contact* contacts[b2_maxTOIContacts];
-	count = 0;
-	for (b2ContactEdge* ce = body->m_contactList; ce && count < b2_maxTOIContacts; ce = ce->next)
-	{
-		b2Body* other = ce->other;
-		b2BodyType type = other->GetType();
-
-		// Only perform correction with static bodies, so the
-		// body won't get pushed out of the world.
-		if (type == b2_dynamicBody)
+		if (minContact == NULL || 1.0f - 10.0f * b2_epsilon < minAlpha)
 		{
-			continue;
-		}
-
-		// Check for a disabled contact.
-		b2Contact* contact = ce->contact;
-		if (contact->IsEnabled() == false)
-		{
-			continue;
-		}
-
-		b2Fixture* fixtureA = contact->m_fixtureA;
-		b2Fixture* fixtureB = contact->m_fixtureB;
-
-		// Cull sensors.
-		if (fixtureA->IsSensor() || fixtureB->IsSensor())
-		{
-			continue;
-		}
-
-		// The contact likely has some new contact points. The listener
-		// gives the user a chance to disable the contact.
-		if (contact != toiContact)
-		{
-			contact->Update(m_contactManager.m_contactListener);
-		}
-
-		// Did the user disable the contact?
-		if (contact->IsEnabled() == false)
-		{
-			// Skip this contact.
-			continue;
-		}
-
-		if (contact->IsTouching() == false)
-		{
-			continue;
-		}
-
-		contacts[count] = contact;
-		++count;
-	}
-
-	// Reduce the TOI body's overlap with the contact island.
-	b2TOISolver solver(&m_stackAllocator);
-	solver.Initialize(contacts, count, body);
-
-	const float32 k_toiBaumgarte = 0.75f;
-	bool solved = false;
-	for (int32 i = 0; i < 20; ++i)
-	{
-		bool contactsOkay = solver.Solve(k_toiBaumgarte);
-		if (contactsOkay)
-		{
-			solved = true;
+			// No more TOI events. Done!
+			m_stepComplete = true;
 			break;
 		}
-	}
 
-	if (toiOther->GetType() != b2_staticBody)
-	{
-			toiContact->m_flags |= b2Contact::e_bulletHitFlag;
-	}
-}
+		// Advance the bodies to the TOI.
+		b2Fixture* fA = minContact->GetFixtureA();
+		b2Fixture* fB = minContact->GetFixtureB();
+		b2Body* bA = fA->GetBody();
+		b2Body* bB = fB->GetBody();
 
-// Sequentially solve TOIs for each body. We bring each
-// body to the time of contact and perform some position correction.
-// Time is not conserved.
-void b2World::SolveTOI()
-{
-	// Prepare all contacts.
-	for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
-	{
-		// Enable the contact
-		c->m_flags |= b2Contact::e_enabledFlag;
+		b2Sweep backup1 = bA->m_sweep;
+		b2Sweep backup2 = bB->m_sweep;
 
-		// Set the number of TOI events for this contact to zero.
-		c->m_toiCount = 0;
-	}
+		bA->Advance(minAlpha);
+		bB->Advance(minAlpha);
 
-	// Initialize the TOI flag.
-	for (b2Body* body = m_bodyList; body; body = body->m_next)
-	{
-		// Kinematic, and static bodies will not be affected by the TOI event.
-		// If a body was not in an island then it did not move.
-		if ((body->m_flags & b2Body::e_islandFlag) == 0 || body->GetType() == b2_kinematicBody || body->GetType() == b2_staticBody)
-		{
-			body->m_flags |= b2Body::e_toiFlag;
-		}
-		else
-		{
-			body->m_flags &= ~b2Body::e_toiFlag;
-		}
-	}
-
-	// Collide non-bullets.
-	for (b2Body* body = m_bodyList; body; body = body->m_next)
-	{
-		if (body->m_flags & b2Body::e_toiFlag)
+		// The TOI contact likely has some new contact points.
+		minContact->Update(m_contactManager.m_contactListener);
+		minContact->m_flags &= ~b2Contact::e_toiFlag;
+		++minContact->m_toiCount;
+
+		// Is the contact solid?
+		if (minContact->IsEnabled() == false || minContact->IsTouching() == false)
 		{
+			// Restore the sweeps.
+			minContact->SetEnabled(false);
+			bA->m_sweep = backup1;
+			bB->m_sweep = backup2;
+			bA->SynchronizeTransform();
+			bB->SynchronizeTransform();
 			continue;
 		}
 
-		if (body->IsBullet() == true)
+		bA->SetAwake(true);
+		bB->SetAwake(true);
+
+		// Build the island
+		island.Clear();
+		island.Add(bA);
+		island.Add(bB);
+		island.Add(minContact);
+
+		bA->m_flags |= b2Body::e_islandFlag;
+		bB->m_flags |= b2Body::e_islandFlag;
+		minContact->m_flags |= b2Contact::e_islandFlag;
+
+		// Get contacts on bodyA and bodyB.
+		b2Body* bodies[2] = {bA, bB};
+		for (int32 i = 0; i < 2; ++i)
 		{
-			continue;
+			b2Body* body = bodies[i];
+			if (body->m_type == b2_dynamicBody)
+			{
+				for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
+				{
+					if (island.m_bodyCount == island.m_bodyCapacity)
+					{
+						break;
+					}
+
+					if (island.m_contactCount == island.m_contactCapacity)
+					{
+						break;
+					}
+
+					b2Contact* contact = ce->contact;
+
+					// Has this contact already been added to the island?
+					if (contact->m_flags & b2Contact::e_islandFlag)
+					{
+						continue;
+					}
+
+					// Only add static, kinematic, or bullet bodies.
+					b2Body* other = ce->other;
+					if (other->m_type == b2_dynamicBody &&
+						body->IsBullet() == false && other->IsBullet() == false)
+					{
+						continue;
+					}
+
+					// Skip sensors.
+					bool sensorA = contact->m_fixtureA->m_isSensor;
+					bool sensorB = contact->m_fixtureB->m_isSensor;
+					if (sensorA || sensorB)
+					{
+						continue;
+					}
+
+					// Tentatively advance the body to the TOI.
+					b2Sweep backup = other->m_sweep;
+					if ((other->m_flags & b2Body::e_islandFlag) == 0)
+					{
+						other->Advance(minAlpha);
+					}
+
+					// Update the contact points
+					contact->Update(m_contactManager.m_contactListener);
+
+					// Was the contact disabled by the user?
+					if (contact->IsEnabled() == false)
+					{
+						other->m_sweep = backup;
+						other->SynchronizeTransform();
+						continue;
+					}
+
+					// Are there contact points?
+					if (contact->IsTouching() == false)
+					{
+						other->m_sweep = backup;
+						other->SynchronizeTransform();
+						continue;
+					}
+
+					// Add the contact to the island
+					contact->m_flags |= b2Contact::e_islandFlag;
+					island.Add(contact);
+
+					// Has the other body already been added to the island?
+					if (other->m_flags & b2Body::e_islandFlag)
+					{
+						continue;
+					}
+					
+					// Add the other body to the island.
+					other->m_flags |= b2Body::e_islandFlag;
+
+					if (other->m_type != b2_staticBody)
+					{
+						other->SetAwake(true);
+					}
+
+					island.Add(other);
+				}
+			}
 		}
 
-		SolveTOI(body);
+		b2TimeStep subStep;
+		subStep.dt = (1.0f - minAlpha) * step.dt;
+		subStep.inv_dt = 1.0f / subStep.dt;
+		subStep.dtRatio = 1.0f;
+		subStep.positionIterations = 20;
+		subStep.velocityIterations = step.velocityIterations;
+		subStep.warmStarting = false;
+		island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
 
-		body->m_flags |= b2Body::e_toiFlag;
-	}
-
-	// Collide bullets.
-	for (b2Body* body = m_bodyList; body; body = body->m_next)
-	{
-		if (body->m_flags & b2Body::e_toiFlag)
+		// Reset island flags and synchronize broad-phase proxies.
+		for (int32 i = 0; i < island.m_bodyCount; ++i)
 		{
-			continue;
+			b2Body* body = island.m_bodies[i];
+			body->m_flags &= ~b2Body::e_islandFlag;
+
+			if (body->m_type != b2_dynamicBody)
+			{
+				continue;
+			}
+
+			body->SynchronizeFixtures();
+
+			// Invalidate all contact TOIs on this displaced body.
+			for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
+			{
+				ce->contact->m_flags &= ~(b2Contact::e_toiFlag | b2Contact::e_islandFlag);
+			}
 		}
 
-		if (body->IsBullet() == false)
+		// Commit fixture proxy movements to the broad-phase so that new contacts are created.
+		// Also, some contacts can be destroyed.
+		m_contactManager.FindNewContacts();
+
+		if (m_subStepping)
 		{
-			continue;
+			m_stepComplete = false;
+			break;
 		}
-
-		SolveTOI(body);
-
-		body->m_flags |= b2Body::e_toiFlag;
 	}
 }
 
 void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
 {
+	b2Timer stepTimer;
+
 	// If new fixtures were added, we need to find the new contacts.
 	if (m_flags & e_newFixture)
 	{
@@ -802,20 +905,28 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration
 	step.dtRatio = m_inv_dt0 * dt;
 
 	step.warmStarting = m_warmStarting;
-
+	
 	// Update contacts. This is where some contacts are destroyed.
-	m_contactManager.Collide();
+	{
+		b2Timer timer;
+		m_contactManager.Collide();
+		m_profile.collide = timer.GetMilliseconds();
+	}
 
 	// Integrate velocities, solve velocity constraints, and integrate positions.
-	if (step.dt > 0.0f)
+	if (m_stepComplete && step.dt > 0.0f)
 	{
+		b2Timer timer;
 		Solve(step);
+		m_profile.solve = timer.GetMilliseconds();
 	}
 
 	// Handle TOI events.
 	if (m_continuousPhysics && step.dt > 0.0f)
 	{
-		SolveTOI();
+		b2Timer timer;
+		SolveTOI(step);
+		m_profile.solveTOI = timer.GetMilliseconds();
 	}
 
 	if (step.dt > 0.0f)
@@ -829,6 +940,8 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration
 	}
 
 	m_flags &= ~e_locked;
+
+	m_profile.step = stepTimer.GetMilliseconds();
 }
 
 void b2World::ClearForces()
@@ -844,8 +957,8 @@ struct b2WorldQueryWrapper
 {
 	bool QueryCallback(int32 proxyId)
 	{
-		b2Fixture* fixture = (b2Fixture*)broadPhase->GetUserData(proxyId);
-		return callback->ReportFixture(fixture);
+		b2FixtureProxy* proxy = (b2FixtureProxy*)broadPhase->GetUserData(proxyId);
+		return callback->ReportFixture(proxy->fixture);
 	}
 
 	const b2BroadPhase* broadPhase;
@@ -865,9 +978,11 @@ struct b2WorldRayCastWrapper
 	float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
 	{
 		void* userData = broadPhase->GetUserData(proxyId);
-		b2Fixture* fixture = (b2Fixture*)userData;
+		b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
+		b2Fixture* fixture = proxy->fixture;
+		int32 index = proxy->childIndex;
 		b2RayCastOutput output;
-		bool hit = fixture->RayCast(&output, input);
+		bool hit = fixture->RayCast(&output, input, index);
 
 		if (hit)
 		{
@@ -905,12 +1020,38 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 
 			b2Vec2 center = b2Mul(xf, circle->m_p);
 			float32 radius = circle->m_radius;
-			b2Vec2 axis = xf.R.col1;
+			b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
 
 			m_debugDraw->DrawSolidCircle(center, radius, axis, color);
 		}
 		break;
 
+	case b2Shape::e_edge:
+		{
+			b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
+			b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
+			b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
+			m_debugDraw->DrawSegment(v1, v2, color);
+		}
+		break;
+
+	case b2Shape::e_chain:
+		{
+			b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
+			int32 count = chain->GetVertexCount();
+			const b2Vec2* vertices = chain->GetVertices();
+
+			b2Vec2 v1 = b2Mul(xf, vertices[0]);
+			for (int32 i = 1; i < count; ++i)
+			{
+				b2Vec2 v2 = b2Mul(xf, vertices[i]);
+				m_debugDraw->DrawSegment(v1, v2, color);
+				m_debugDraw->DrawCircle(v1, 0.05f, color);
+				v1 = v2;
+			}
+		}
+		break;
+
 	case b2Shape::e_polygon:
 		{
 			b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
@@ -926,6 +1067,9 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 			m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
 		}
 		break;
+            
+    default:
+        break;
 	}
 }
 
@@ -935,8 +1079,8 @@ void b2World::DrawJoint(b2Joint* joint)
 	b2Body* bodyB = joint->GetBodyB();
 	const b2Transform& xf1 = bodyA->GetTransform();
 	const b2Transform& xf2 = bodyB->GetTransform();
-	b2Vec2 x1 = xf1.position;
-	b2Vec2 x2 = xf2.position;
+	b2Vec2 x1 = xf1.p;
+	b2Vec2 x2 = xf2.p;
 	b2Vec2 p1 = joint->GetAnchorA();
 	b2Vec2 p2 = joint->GetAnchorB();
 
@@ -979,7 +1123,7 @@ void b2World::DrawDebugData()
 
 	uint32 flags = m_debugDraw->GetFlags();
 
-	if (flags & b2DebugDraw::e_shapeBit)
+	if (flags & b2Draw::e_shapeBit)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
@@ -1010,7 +1154,7 @@ void b2World::DrawDebugData()
 		}
 	}
 
-	if (flags & b2DebugDraw::e_jointBit)
+	if (flags & b2Draw::e_jointBit)
 	{
 		for (b2Joint* j = m_jointList; j; j = j->GetNext())
 		{
@@ -1018,22 +1162,22 @@ void b2World::DrawDebugData()
 		}
 	}
 
-	if (flags & b2DebugDraw::e_pairBit)
+	if (flags & b2Draw::e_pairBit)
 	{
 		b2Color color(0.3f, 0.9f, 0.9f);
 		for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
 		{
-			b2Fixture* fixtureA = c->GetFixtureA();
-			b2Fixture* fixtureB = c->GetFixtureB();
+			//b2Fixture* fixtureA = c->GetFixtureA();
+			//b2Fixture* fixtureB = c->GetFixtureB();
 
-			b2Vec2 cA = fixtureA->GetAABB().GetCenter();
-			b2Vec2 cB = fixtureB->GetAABB().GetCenter();
+			//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
+			//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
 
-			m_debugDraw->DrawSegment(cA, cB, color);
+			//m_debugDraw->DrawSegment(cA, cB, color);
 		}
 	}
 
-	if (flags & b2DebugDraw::e_aabbBit)
+	if (flags & b2Draw::e_aabbBit)
 	{
 		b2Color color(0.9f, 0.3f, 0.9f);
 		b2BroadPhase* bp = &m_contactManager.m_broadPhase;
@@ -1047,24 +1191,28 @@ void b2World::DrawDebugData()
 
 			for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
 			{
-				b2AABB aabb = bp->GetFatAABB(f->m_proxyId);
-				b2Vec2 vs[4];
-				vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
-				vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
-				vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
-				vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
+				for (int32 i = 0; i < f->m_proxyCount; ++i)
+				{
+					b2FixtureProxy* proxy = f->m_proxies + i;
+					b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
+					b2Vec2 vs[4];
+					vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
+					vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
+					vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
+					vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
 
-				m_debugDraw->DrawPolygon(vs, 4, color);
+					m_debugDraw->DrawPolygon(vs, 4, color);
+				}
 			}
 		}
 	}
 
-	if (flags & b2DebugDraw::e_centerOfMassBit)
+	if (flags & b2Draw::e_centerOfMassBit)
 	{
 		for (b2Body* b = m_bodyList; b; b = b->GetNext())
 		{
 			b2Transform xf = b->GetTransform();
-			xf.position = b->GetWorldCenter();
+			xf.p = b->GetWorldCenter();
 			m_debugDraw->DrawTransform(xf);
 		}
 	}
@@ -1074,3 +1222,18 @@ int32 b2World::GetProxyCount() const
 {
 	return m_contactManager.m_broadPhase.GetProxyCount();
 }
+
+int32 b2World::GetTreeHeight() const
+{
+	return m_contactManager.m_broadPhase.GetTreeHeight();
+}
+
+int32 b2World::GetTreeBalance() const
+{
+	return m_contactManager.m_broadPhase.GetTreeBalance();
+}
+
+float32 b2World::GetTreeQuality() const
+{
+	return m_contactManager.m_broadPhase.GetTreeQuality();
+}

src/modules/physics/box2d/Box2D/Dynamics/b2World.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2011 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
@@ -24,12 +24,14 @@
 #include <Box2D/Common/b2StackAllocator.h>
 #include <Box2D/Dynamics/b2ContactManager.h>
 #include <Box2D/Dynamics/b2WorldCallbacks.h>
+#include <Box2D/Dynamics/b2TimeStep.h>
 
 struct b2AABB;
 struct b2BodyDef;
+struct b2Color;
 struct b2JointDef;
-struct b2TimeStep;
 class b2Body;
+class b2Draw;
 class b2Fixture;
 class b2Joint;
 
@@ -63,7 +65,7 @@ public:
 	/// Register a routine for debug drawing. The debug draw functions are called
 	/// inside with b2World::DrawDebugData method. The debug draw object is owned
 	/// by you and must remain in scope.
-	void SetDebugDraw(b2DebugDraw* debugDraw);
+	void SetDebugDraw(b2Draw* debugDraw);
 
 	/// Create a rigid body given a definition. No reference to the definition
 	/// is retained.
@@ -94,9 +96,12 @@ public:
 				int32 velocityIterations,
 				int32 positionIterations);
 
-	/// Call this after you are done with time steps to clear the forces. You normally
-	/// call this after each call to Step, unless you are performing sub-steps. By default,
-	/// forces will be automatically cleared, so you don't need to call this function.
+	/// Manually clear the force buffer on all bodies. By default, forces are cleared automatically
+	/// after each call to Step. The default behavior is modified by calling SetAutoClearForces.
+	/// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain
+	/// a fixed sized time step under a variable frame-rate.
+	/// When you perform sub-stepping you will disable auto clearing of forces and instead call
+	/// ClearForces after all sub-steps are complete in one pass of your game loop.
 	/// @see SetAutoClearForces
 	void ClearForces();
 
@@ -121,17 +126,21 @@ public:
 	/// the next body in the world list. A NULL body indicates the end of the list.
 	/// @return the head of the world body list.
 	b2Body* GetBodyList();
+	const b2Body* GetBodyList() const;
 
 	/// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
 	/// the next joint in the world list. A NULL joint indicates the end of the list.
 	/// @return the head of the world joint list.
 	b2Joint* GetJointList();
+	const b2Joint* GetJointList() const;
 
 	/// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
 	/// the next contact in the world list. A NULL contact indicates the end of the list.
 	/// @return the head of the world contact list.
-	/// @warning contacts are 
+	/// @warning contacts are created and destroyed in the middle of a time step.
+	/// Use b2ContactListener to avoid missing contacts.
 	b2Contact* GetContactList();
+	const b2Contact* GetContactList() const;
 
 	/// Enable/disable warm starting. For testing.
 	void SetWarmStarting(bool flag) { m_warmStarting = flag; }
@@ -139,6 +148,9 @@ public:
 	/// Enable/disable continuous physics. For testing.
 	void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
 
+	/// Enable/disable single stepped continuous physics. For testing.
+	void SetSubStepping(bool flag) { m_subStepping = flag; }
+
 	/// Get the number of broad-phase proxies.
 	int32 GetProxyCount() const;
 
@@ -151,6 +163,16 @@ public:
 	/// Get the number of contacts (each may have 0 or more contact points).
 	int32 GetContactCount() const;
 
+	/// Get the height of the dynamic tree.
+	int32 GetTreeHeight() const;
+
+	/// Get the balance of the dynamic tree.
+	int32 GetTreeBalance() const;
+
+	/// Get the quality metric of the dynamic tree. The smaller the better.
+	/// The minimum is 1.
+	float32 GetTreeQuality() const;
+
 	/// Change the global gravity vector.
 	void SetGravity(const b2Vec2& gravity);
 	
@@ -166,6 +188,12 @@ public:
 	/// Get the flag that controls automatic clearing of forces after each time step.
 	bool GetAutoClearForces() const;
 
+	/// Get the contact manager for testing.
+	const b2ContactManager& GetContactManager() const;
+
+	/// Get the current profile.
+	const b2Profile& GetProfile() const;
+
 private:
 
 	// m_flags
@@ -173,16 +201,16 @@ private:
 	{
 		e_newFixture	= 0x0001,
 		e_locked		= 0x0002,
-		e_clearForces	= 0x0004,
+		e_clearForces	= 0x0004
 	};
 
 	friend class b2Body;
+	friend class b2Fixture;
 	friend class b2ContactManager;
 	friend class b2Controller;
 
 	void Solve(const b2TimeStep& step);
-	void SolveTOI();
-	void SolveTOI(b2Body* body);
+	void SolveTOI(const b2TimeStep& step);
 
 	void DrawJoint(b2Joint* joint);
 	void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);
@@ -203,20 +231,21 @@ private:
 	b2Vec2 m_gravity;
 	bool m_allowSleep;
 
-	b2Body* m_groundBody;
-
 	b2DestructionListener* m_destructionListener;
-	b2DebugDraw* m_debugDraw;
+	b2Draw* m_debugDraw;
 
 	// This is used to compute the time step ratio to
 	// support a variable time step.
 	float32 m_inv_dt0;
 
-	// This is for debugging the solver.
+	// These are for debugging the solver.
 	bool m_warmStarting;
-
-	// This is for debugging the solver.
 	bool m_continuousPhysics;
+	bool m_subStepping;
+
+	bool m_stepComplete;
+
+	b2Profile m_profile;
 };
 
 inline b2Body* b2World::GetBodyList()
@@ -224,16 +253,31 @@ inline b2Body* b2World::GetBodyList()
 	return m_bodyList;
 }
 
+inline const b2Body* b2World::GetBodyList() const
+{
+	return m_bodyList;
+}
+
 inline b2Joint* b2World::GetJointList()
 {
 	return m_jointList;
 }
 
+inline const b2Joint* b2World::GetJointList() const
+{
+	return m_jointList;
+}
+
 inline b2Contact* b2World::GetContactList()
 {
 	return m_contactManager.m_contactList;
 }
 
+inline const b2Contact* b2World::GetContactList() const
+{
+	return m_contactManager.m_contactList;
+}
+
 inline int32 b2World::GetBodyCount() const
 {
 	return m_bodyCount;
@@ -282,4 +326,14 @@ inline bool b2World::GetAutoClearForces() const
 	return (m_flags & e_clearForces) == e_clearForces;
 }
 
+inline const b2ContactManager& b2World::GetContactManager() const
+{
+	return m_contactManager;
+}
+
+inline const b2Profile& b2World::GetProfile() const
+{
+	return m_profile;
+}
+
 #endif

src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.cpp

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -34,28 +34,3 @@ bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
 	bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
 	return collide;
 }
-
-b2DebugDraw::b2DebugDraw()
-{
-	m_drawFlags = 0;
-}
-
-void b2DebugDraw::SetFlags(uint32 flags)
-{
-	m_drawFlags = flags;
-}
-
-uint32 b2DebugDraw::GetFlags() const
-{
-	return m_drawFlags;
-}
-
-void b2DebugDraw::AppendFlags(uint32 flags)
-{
-	m_drawFlags |= flags;
-}
-
-void b2DebugDraw::ClearFlags(uint32 flags)
-{
-	m_drawFlags &= ~flags;
-}

src/modules/physics/box2d/Box2D/Dynamics/b2WorldCallbacks.h

@@ -1,5 +1,5 @@
 /*
-* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
+* Copyright (c) 2006-2009 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
@@ -27,7 +27,6 @@ class b2Fixture;
 class b2Body;
 class b2Joint;
 class b2Contact;
-struct b2ContactPoint;
 struct b2ContactResult;
 struct b2Manifold;
 
@@ -67,6 +66,7 @@ struct b2ContactImpulse
 {
 	float32 normalImpulses[b2_maxManifoldPoints];
 	float32 tangentImpulses[b2_maxManifoldPoints];
+	int32 count;
 };
 
 /// Implement this class to get contact information. You can use these results for
@@ -152,66 +152,4 @@ public:
 									const b2Vec2& normal, float32 fraction) = 0;
 };
 
-/// Color for debug drawing. Each value has the range [0,1].
-struct b2Color
-{
-	b2Color() {}
-	b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
-	void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }
-	float32 r, g, b;
-};
-
-/// Implement and register this class with a b2World to provide debug drawing of physics
-/// entities in your game.
-class b2DebugDraw
-{
-public:
-	b2DebugDraw();
-
-	virtual ~b2DebugDraw() {}
-
-	enum
-	{
-		e_shapeBit				= 0x0001, ///< draw shapes
-		e_jointBit				= 0x0002, ///< draw joint connections
-		e_aabbBit				= 0x0004, ///< draw axis aligned bounding boxes
-		e_pairBit				= 0x0008, ///< draw broad-phase pairs
-		e_centerOfMassBit		= 0x0010, ///< draw center of mass frame
-	};
-
-	/// Set the drawing flags.
-	void SetFlags(uint32 flags);
-
-	/// Get the drawing flags.
-	uint32 GetFlags() const;
-	
-	/// Append flags to the current flags.
-	void AppendFlags(uint32 flags);
-
-	/// Clear flags from the current flags.
-	void ClearFlags(uint32 flags);
-
-	/// Draw a closed polygon provided in CCW order.
-	virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
-
-	/// Draw a solid closed polygon provided in CCW order.
-	virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
-
-	/// Draw a circle.
-	virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
-	
-	/// Draw a solid circle.
-	virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
-	
-	/// Draw a line segment.
-	virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
-
-	/// Draw a transform. Choose your own length scale.
-	/// @param xf a transform.
-	virtual void DrawTransform(const b2Transform& xf) = 0;
-
-protected:
-	uint32 m_drawFlags;
-};
-
 #endif

src/modules/physics/box2d/Box2D/Rope/b2Rope.cpp

@@ -0,0 +1,259 @@
+/*
+* Copyright (c) 2011 Erin Catto http://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/Rope/b2Rope.h>
+#include <Box2D/Common/b2Draw.h>
+
+b2Rope::b2Rope()
+{
+	m_count = 0;
+	m_ps = NULL;
+	m_p0s = NULL;
+	m_vs = NULL;
+	m_ims = NULL;
+	m_Ls = NULL;
+	m_as = NULL;
+	m_gravity.SetZero();
+	m_k2 = 1.0f;
+	m_k3 = 0.1f;
+}
+
+b2Rope::~b2Rope()
+{
+	b2Free(m_ps);
+	b2Free(m_p0s);
+	b2Free(m_vs);
+	b2Free(m_ims);
+	b2Free(m_Ls);
+	b2Free(m_as);
+}
+
+void b2Rope::Initialize(const b2RopeDef* def)
+{
+	b2Assert(def->count >= 3);
+	m_count = def->count;
+	m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+	m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+	m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
+	m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		m_ps[i] = def->vertices[i];
+		m_p0s[i] = def->vertices[i];
+		m_vs[i].SetZero();
+
+		float32 m = def->masses[i];
+		if (m > 0.0f)
+		{
+			m_ims[i] = 1.0f / m;
+		}
+		else
+		{
+			m_ims[i] = 0.0f;
+		}
+	}
+
+	int32 count2 = m_count - 1;
+	int32 count3 = m_count - 2;
+	m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
+	m_as = (float32*)b2Alloc(count3 * sizeof(float32));
+
+	for (int32 i = 0; i < count2; ++i)
+	{
+		b2Vec2 p1 = m_ps[i];
+		b2Vec2 p2 = m_ps[i+1];
+		m_Ls[i] = b2Distance(p1, p2);
+	}
+
+	for (int32 i = 0; i < count3; ++i)
+	{
+		b2Vec2 p1 = m_ps[i];
+		b2Vec2 p2 = m_ps[i + 1];
+		b2Vec2 p3 = m_ps[i + 2];
+
+		b2Vec2 d1 = p2 - p1;
+		b2Vec2 d2 = p3 - p2;
+
+		float32 a = b2Cross(d1, d2);
+		float32 b = b2Dot(d1, d2);
+
+		m_as[i] = b2Atan2(a, b);
+	}
+
+	m_gravity = def->gravity;
+	m_damping = def->damping;
+	m_k2 = def->k2;
+	m_k3 = def->k3;
+}
+
+void b2Rope::Step(float32 h, int32 iterations)
+{
+	if (h == 0.0)
+	{
+		return;
+	}
+
+	float32 d = expf(- h * m_damping);
+
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		m_p0s[i] = m_ps[i];
+		if (m_ims[i] > 0.0f)
+		{
+			m_vs[i] += h * m_gravity;
+		}
+		m_vs[i] *= d;
+		m_ps[i] += h * m_vs[i];
+
+	}
+
+	for (int32 i = 0; i < iterations; ++i)
+	{
+		SolveC2();
+		SolveC3();
+		SolveC2();
+	}
+
+	float32 inv_h = 1.0f / h;
+	for (int32 i = 0; i < m_count; ++i)
+	{
+		m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]);
+	}
+}
+
+void b2Rope::SolveC2()
+{
+	int32 count2 = m_count - 1;
+
+	for (int32 i = 0; i < count2; ++i)
+	{
+		b2Vec2 p1 = m_ps[i];
+		b2Vec2 p2 = m_ps[i + 1];
+
+		b2Vec2 d = p2 - p1;
+		float32 L = d.Normalize();
+
+		float32 im1 = m_ims[i];
+		float32 im2 = m_ims[i + 1];
+
+		if (im1 + im2 == 0.0f)
+		{
+			continue;
+		}
+
+		float32 s1 = im1 / (im1 + im2);
+		float32 s2 = im2 / (im1 + im2);
+
+		p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
+		p2 += m_k2 * s2 * (m_Ls[i] - L) * d;
+
+		m_ps[i] = p1;
+		m_ps[i + 1] = p2;
+	}
+}
+
+void b2Rope::SetAngle(float32 angle)
+{
+	int32 count3 = m_count - 2;
+	for (int32 i = 0; i < count3; ++i)
+	{
+		m_as[i] = angle;
+	}
+}
+
+void b2Rope::SolveC3()
+{
+	int32 count3 = m_count - 2;
+
+	for (int32 i = 0; i < count3; ++i)
+	{
+		b2Vec2 p1 = m_ps[i];
+		b2Vec2 p2 = m_ps[i + 1];
+		b2Vec2 p3 = m_ps[i + 2];
+
+		float32 m1 = m_ims[i];
+		float32 m2 = m_ims[i + 1];
+		float32 m3 = m_ims[i + 2];
+
+		b2Vec2 d1 = p2 - p1;
+		b2Vec2 d2 = p3 - p2;
+
+		float32 L1sqr = d1.LengthSquared();
+		float32 L2sqr = d2.LengthSquared();
+
+		if (L1sqr * L2sqr == 0.0f)
+		{
+			continue;
+		}
+
+		float32 a = b2Cross(d1, d2);
+		float32 b = b2Dot(d1, d2);
+
+		float32 angle = b2Atan2(a, b);
+
+		b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
+		b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
+
+		b2Vec2 J1 = -Jd1;
+		b2Vec2 J2 = Jd1 - Jd2;
+		b2Vec2 J3 = Jd2;
+
+		float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3);
+		if (mass == 0.0f)
+		{
+			continue;
+		}
+
+		mass = 1.0f / mass;
+
+		float32 C = angle - m_as[i];
+
+		while (C > b2_pi)
+		{
+			angle -= 2 * b2_pi;
+			C = angle - m_as[i];
+		}
+
+		while (C < -b2_pi)
+		{
+			angle += 2.0f * b2_pi;
+			C = angle - m_as[i];
+		}
+
+		float32 impulse = - m_k3 * mass * C;
+
+		p1 += (m1 * impulse) * J1;
+		p2 += (m2 * impulse) * J2;
+		p3 += (m3 * impulse) * J3;
+
+		m_ps[i] = p1;
+		m_ps[i + 1] = p2;
+		m_ps[i + 2] = p3;
+	}
+}
+
+void b2Rope::Draw(b2Draw* draw) const
+{
+	b2Color c(0.4f, 0.5f, 0.7f);
+
+	for (int32 i = 0; i < m_count - 1; ++i)
+	{
+		draw->DrawSegment(m_ps[i], m_ps[i+1], c);
+	}
+}

src/modules/physics/box2d/Box2D/Rope/b2Rope.h

@@ -0,0 +1,115 @@
+/*
+* Copyright (c) 2011 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_ROPE_H
+#define B2_ROPE_H
+
+#include <Box2D/Common/b2Math.h>
+
+class b2Draw;
+
+/// 
+struct b2RopeDef
+{
+	b2RopeDef()
+	{
+		vertices = NULL;
+		count = 0;
+		masses = NULL;
+		gravity.SetZero();
+		damping = 0.1f;
+		k2 = 0.9f;
+		k3 = 0.1f;
+	}
+
+	///
+	b2Vec2* vertices;
+
+	///
+	int32 count;
+
+	///
+	float32* masses;
+
+	///
+	b2Vec2 gravity;
+
+	///
+	float32 damping;
+
+	/// Stretching stiffness
+	float32 k2;
+
+	/// Bending stiffness. Values above 0.5 can make the simulation blow up.
+	float32 k3;
+};
+
+/// 
+class b2Rope
+{
+public:
+	b2Rope();
+	~b2Rope();
+
+	///
+	void Initialize(const b2RopeDef* def);
+
+	///
+	void Step(float32 timeStep, int32 iterations);
+
+	///
+	int32 GetVertexCount() const
+	{
+		return m_count;
+	}
+
+	///
+	const b2Vec2* GetVertices() const
+	{
+		return m_ps;
+	}
+
+	///
+	void Draw(b2Draw* draw) const;
+
+	///
+	void SetAngle(float32 angle);
+
+private:
+
+	void SolveC2();
+	void SolveC3();
+
+	int32 m_count;
+	b2Vec2* m_ps;
+	b2Vec2* m_p0s;
+	b2Vec2* m_vs;
+
+	float32* m_ims;
+
+	float32* m_Ls;
+	float32* m_as;
+
+	b2Vec2 m_gravity;
+	float32 m_damping;
+
+	float32 m_k2;
+	float32 m_k3;
+};
+
+#endif

src/modules/physics/box2d/FrictionJoint.cpp

@@ -0,0 +1,72 @@
+/**
+* Copyright (c) 2006-2011 LOVE Development Team
+* 
+* 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 "FrictionJoint.h"
+
+#include <common/math.h>
+
+// Module
+#include "Body.h"
+#include "World.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	FrictionJoint::FrictionJoint(Body * body1, Body * body2, float x, float y)
+		: Joint(body1, body2), joint(NULL)
+	{	
+		b2FrictionJointDef def;
+		def.Initialize(body1->body, body2->body, world->scaleDown(b2Vec2(x,y)));
+		joint = (b2FrictionJoint*)createJoint(&def);
+	}
+
+	FrictionJoint::~FrictionJoint()
+	{
+		destroyJoint(joint);
+		joint = 0;
+	}
+	
+	void FrictionJoint::setMaxForce(float force)
+	{
+		joint->SetMaxForce(force);
+	}
+
+	float FrictionJoint::getMaxForce() const
+	{
+		return joint->GetMaxForce();
+	}
+	
+	void FrictionJoint::setMaxTorque(float torque)
+	{
+		joint->SetMaxTorque(torque);
+	}
+	
+	float FrictionJoint::getMaxTorque() const
+	{
+		return joint->GetMaxTorque();
+	}
+
+
+} // box2d
+} // physics
+} // love

src/modules/physics/box2d/FrictionJoint.h

@@ -0,0 +1,78 @@
+/**
+* Copyright (c) 2006-2011 LOVE Development Team
+* 
+* 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 LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H
+#define LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H
+
+// Module
+#include "Joint.h"
+
+namespace love
+{
+namespace physics
+{
+namespace box2d
+{
+	/**
+	* A FrictionJoint applies friction to a body.
+	**/
+	class FrictionJoint : public Joint
+	{
+	private:
+
+		// The Box2D friction joint object.
+		b2FrictionJoint * joint;
+
+	public:
+
+		/**
+		* Creates a new FrictionJoint connecting body1 and body2.
+		**/
+		FrictionJoint(Body * body1, Body * body2, float x, float y);
+		
+		virtual ~FrictionJoint();
+
+		/**
+		* Sets the maximum friction force in Newtons.
+		**/
+		void setMaxForce(float force);
+
+		/**
+		* Gets the maximum friction force in Newtons.
+		**/
+		float getMaxForce() const;
+
+		/**
+		* Sets the maximum friction torque in Newton-meters.
+		**/
+		void setMaxTorque(float torque);
+
+		/**
+		* Gets the maximum friction torque in Newton-meters.
+		**/
+		float getMaxTorque() const;
+
+	};
+
+} // box2d
+} // physics
+} // love
+
+#endif // LOVE_PHYSICS_BOX2D_FRICTION_JOINT_H

src/modules/physics/box2d/Joint.cpp

@@ -72,6 +72,14 @@ namespace box2d
 			return JOINT_MOUSE;
 		case e_gearJoint: 
 			return JOINT_GEAR;
+		case e_frictionJoint:
+			return JOINT_FRICTION;
+		case e_weldJoint:
+			return JOINT_WELD;
+		case e_wheelJoint:
+			return JOINT_WHEEL;
+		case e_ropeJoint:
+			return JOINT_ROPE;
 		default:
 			return JOINT_INVALID;
 		}
@@ -116,6 +124,11 @@ namespace box2d
 	{
 		return joint->IsActive();
 	}
+	
+	bool Joint::getCollideConnected() const
+	{
+		return joint->GetCollideConnected();
+	}
 
 } // box2d
 } // physics

src/modules/physics/box2d/Joint.h

@@ -100,6 +100,8 @@ namespace box2d
 		float getReactionTorque(float dt);
 		
 		bool isActive() const;
+		
+		bool getCollideConnected() const;
 
 	protected: