Update to Box2D 2.2.1 (stop updating, Erin!)

--HG--
branch : box2d-update

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

@@ -64,6 +64,4 @@ For discussion please visit http://box2d.org/forum
 #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.h

@@ -79,21 +79,6 @@ public:
 	/// @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;
 

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

@@ -63,6 +63,10 @@ inline b2EdgeShape::b2EdgeShape()
 {
 	m_type = e_edge;
 	m_radius = b2_polygonRadius;
+	m_vertex0.x = 0.0f;
+	m_vertex0.y = 0.0f;
+	m_vertex3.x = 0.0f;
+	m_vertex3.y = 0.0f;
 	m_hasVertex0 = false;
 	m_hasVertex3 = false;
 }

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

@@ -50,16 +50,16 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
 	case b2Shape::e_chain:
 		{
 			const b2ChainShape* chain = (b2ChainShape*)shape;
-			b2Assert(0 <= index && index < chain->GetVertexCount());
+			b2Assert(0 <= index && index < chain->m_count);
 
-			m_buffer[0] = chain->GetVertex(index);
-			if (index + 1 < chain->GetVertexCount())
+			m_buffer[0] = chain->m_vertices[index];
+			if (index + 1 < chain->m_count)
 			{
-				m_buffer[1] = chain->GetVertex(index + 1);
+				m_buffer[1] = chain->m_vertices[index + 1];
 			}
 			else
 			{
-				m_buffer[1] = chain->GetVertex(0);
+				m_buffer[1] = chain->m_vertices[0];
 			}
 
 			m_vertices = m_buffer;

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

@@ -51,3 +51,44 @@ b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
 	x.y = det * (a11 * b.y - a21 * b.x);
 	return x;
 }
+
+///
+void b2Mat33::GetInverse22(b2Mat33* M) const
+{
+	float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+	float32 det = a * d - b * c;
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	M->ex.x =  det * d;	M->ey.x = -det * b; M->ex.z = 0.0f;
+	M->ex.y = -det * c;	M->ey.y =  det * a; M->ey.z = 0.0f;
+	M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
+}
+
+/// Returns the zero matrix if singular.
+void b2Mat33::GetSymInverse33(b2Mat33* M) const
+{
+	float32 det = b2Dot(ex, b2Cross(ey, ez));
+	if (det != 0.0f)
+	{
+		det = 1.0f / det;
+	}
+
+	float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
+	float32 a22 = ey.y, a23 = ez.y;
+	float32 a33 = ez.z;
+
+	M->ex.x = det * (a22 * a33 - a23 * a23);
+	M->ex.y = det * (a13 * a23 - a12 * a33);
+	M->ex.z = det * (a12 * a23 - a13 * a22);
+
+	M->ey.x = M->ex.y;
+	M->ey.y = det * (a11 * a33 - a13 * a13);
+	M->ey.z = det * (a13 * a12 - a11 * a23);
+
+	M->ez.x = M->ex.z;
+	M->ez.y = M->ey.z;
+	M->ez.z = det * (a11 * a22 - a12 * a12);
+}

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

@@ -295,6 +295,14 @@ struct b2Mat33
 	/// 2-by-2 matrix equation.
 	b2Vec2 Solve22(const b2Vec2& b) const;
 
+	/// Get the inverse of this matrix as a 2-by-2.
+	/// Returns the zero matrix if singular.
+	void GetInverse22(b2Mat33* M) const;
+
+	/// Get the symmetric inverse of this matrix as a 3-by-3.
+	/// Returns the zero matrix if singular.
+	void GetSymInverse33(b2Mat33* M) const;
+
 	b2Vec3 ex, ey, ez;
 };
 
@@ -533,6 +541,12 @@ inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
 	return v.x * A.ex + v.y * A.ey + v.z * A.ez;
 }
 
+/// Multiply a matrix times a vector.
+inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
+{
+	return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
+}
+
 /// Multiply two rotations: q * r
 inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
 {

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

@@ -18,8 +18,10 @@
 
 #include <Box2D/Common/b2Settings.h>
 #include <cstdlib>
+#include <cstdio>
+#include <cstdarg>
 
-b2Version b2_version = {2, 2, 0};
+b2Version b2_version = {2, 2, 1};
 
 // Memory allocators. Modify these to use your own allocator.
 void* b2Alloc(int32 size)
@@ -31,3 +33,12 @@ void b2Free(void* mem)
 {
 	free(mem);
 }
+
+// You can modify this to use your logging facility.
+void b2Log(const char* string, ...)
+{
+	va_list args;
+	va_start(args, string);
+	vprintf(string, args);
+	va_end(args);
+}

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

@@ -132,6 +132,9 @@ void* b2Alloc(int32 size);
 /// If you implement b2Alloc, you should also implement this function.
 void b2Free(void* mem);
 
+/// Logging function.
+void b2Log(const char* string, ...);
+
 /// Version numbering scheme.
 /// See http://en.wikipedia.org/wiki/Software_versioning
 struct b2Version

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

@@ -18,11 +18,11 @@
 
 #include <Box2D/Common/b2Timer.h>
 
-#if defined(WIN32)
+#if defined(_WIN32)
 
 float64 b2Timer::s_invFrequency = 0.0f;
 
-#include <Windows.h>
+#include <windows.h>
 
 b2Timer::b2Timer()
 {

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

@@ -35,7 +35,7 @@ public:
 
 private:
 
-#if defined(WIN32)
+#if defined(_WIN32)
 	float64 m_start;
 	static float64 s_invFrequency;
 #elif defined(__linux__) || defined (__APPLE__)

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

@@ -123,8 +123,13 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
 		m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
 		m_bias = C * h * k * m_gamma;
 
-		m_mass = invMass + m_gamma;
-		m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
+		invMass += m_gamma;
+		m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
+	}
+	else
+	{
+		m_gamma = 0.0f;
+		m_bias = 0.0f;
 	}
 
 	if (data.step.warmStarting)
@@ -236,3 +241,20 @@ float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
 	B2_NOT_USED(inv_dt);
 	return 0.0f;
 }
+
+void b2DistanceJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2DistanceJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.length = %.15lef;\n", m_length);
+	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
+	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -44,16 +44,17 @@ struct b2DistanceJointDef : public b2JointDef
 	void Initialize(b2Body* bodyA, b2Body* bodyB,
 					const b2Vec2& anchorA, const b2Vec2& anchorB);
 
-	/// The local anchor point relative to body1's origin.
+	/// The local anchor point relative to bodyA's origin.
 	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
+	/// The local anchor point relative to bodyB's origin.
 	b2Vec2 localAnchorB;
 
 	/// The natural length between the anchor points.
 	float32 length;
 
-	/// The mass-spring-damper frequency in Hertz.
+	/// The mass-spring-damper frequency in Hertz. A value of 0
+	/// disables softness.
 	float32 frequencyHz;
 
 	/// The damping ratio. 0 = no damping, 1 = critical damping.
@@ -78,19 +79,28 @@ public:
 	/// Unit is N*m. This is always zero for a distance joint.
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
 	/// Set/get the natural length.
 	/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
 	void SetLength(float32 length);
 	float32 GetLength() const;
 
-	// Set/get frequency in Hz.
+	/// Set/get frequency in Hz.
 	void SetFrequency(float32 hz);
 	float32 GetFrequency() const;
 
-	// Set/get damping ratio.
+	/// Set/get damping ratio.
 	void SetDampingRatio(float32 ratio);
 	float32 GetDampingRatio() const;
 
+	/// Dump joint to dmLog
+	void Dump();
+
 protected:
 
 	friend class b2Joint;

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

@@ -233,3 +233,19 @@ float32 b2FrictionJoint::GetMaxTorque() const
 {
 	return m_maxTorque;
 }
+
+void b2FrictionJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2FrictionJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.maxForce = %.15lef;\n", m_maxForce);
+	b2Log("  jd.maxTorque = %.15lef;\n", m_maxTorque);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -61,6 +61,12 @@ public:
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
 	/// Set the maximum friction force in N.
 	void SetMaxForce(float32 force);
 
@@ -73,6 +79,9 @@ public:
 	/// Get the maximum friction torque in N*m.
 	float32 GetMaxTorque() const;
 
+	/// Dump joint to dmLog
+	void Dump();
+
 protected:
 
 	friend class b2Joint;

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

@@ -44,16 +44,21 @@
 b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 : b2Joint(def)
 {
-	m_typeA = def->joint1->GetType();
-	m_typeB = def->joint2->GetType();
+	m_joint1 = def->joint1;
+	m_joint2 = def->joint2;
+
+	m_typeA = m_joint1->GetType();
+	m_typeB = m_joint2->GetType();
 
 	b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
 	b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
 
 	float32 coordinateA, coordinateB;
 
-	m_bodyC = def->joint1->GetBodyA();
-	m_bodyA = def->joint1->GetBodyB();
+	// TODO_ERIN there might be some problem with the joint edges in b2Joint.
+
+	m_bodyC = m_joint1->GetBodyA();
+	m_bodyA = m_joint1->GetBodyB();
 
 	// Get geometry of joint1
 	b2Transform xfA = m_bodyA->m_xf;
@@ -76,7 +81,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 		b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
 		m_localAnchorC = prismatic->m_localAnchorA;
 		m_localAnchorA = prismatic->m_localAnchorB;
-		m_referenceAngleA = prismatic->m_refAngle;
+		m_referenceAngleA = prismatic->m_referenceAngle;
 		m_localAxisC = prismatic->m_localXAxisA;
 
 		b2Vec2 pC = m_localAnchorC;
@@ -84,8 +89,8 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 		coordinateA = b2Dot(pA - pC, m_localAxisC);
 	}
 
-	m_bodyD = def->joint2->GetBodyA();
-	m_bodyB = def->joint2->GetBodyB();
+	m_bodyD = m_joint2->GetBodyA();
+	m_bodyB = m_joint2->GetBodyB();
 
 	// Get geometry of joint2
 	b2Transform xfB = m_bodyB->m_xf;
@@ -108,7 +113,7 @@ b2GearJoint::b2GearJoint(const b2GearJointDef* def)
 		b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
 		m_localAnchorD = prismatic->m_localAnchorA;
 		m_localAnchorB = prismatic->m_localAnchorB;
-		m_referenceAngleB = prismatic->m_refAngle;
+		m_referenceAngleB = prismatic->m_referenceAngle;
 		m_localAxisD = prismatic->m_localXAxisA;
 
 		b2Vec2 pD = m_localAnchorD;
@@ -315,9 +320,9 @@ bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
 	if (m_typeB == e_revoluteJoint)
 	{
 		JvBD.SetZero();
-		JwB = 1.0f;
-		JwD = 1.0f;
-		mass += m_iB + m_iD;
+		JwB = m_ratio;
+		JwD = m_ratio;
+		mass += m_ratio * m_ratio * (m_iB + m_iD);
 
 		coordinateB = aB - aD - m_referenceAngleB;
 	}
@@ -398,3 +403,21 @@ float32 b2GearJoint::GetRatio() const
 {
 	return m_ratio;
 }
+
+void b2GearJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	int32 index1 = m_joint1->m_index;
+	int32 index2 = m_joint2->m_index;
+
+	b2Log("  b2GearJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.joint1 = joints[%d];\n", index1);
+	b2Log("  jd.joint2 = joints[%d];\n", index2);
+	b2Log("  jd.ratio = %.15lef;\n", m_ratio);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -62,10 +62,19 @@ public:
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// Get the first joint.
+	b2Joint* GetJoint1() { return m_joint1; }
+
+	/// Get the second joint.
+	b2Joint* GetJoint2() { return m_joint2; }
+
 	/// Set/Get the gear ratio.
 	void SetRatio(float32 ratio);
 	float32 GetRatio() const;
 
+	/// Dump joint to dmLog
+	void Dump();
+
 protected:
 
 	friend class b2Joint;
@@ -75,6 +84,9 @@ protected:
 	void SolveVelocityConstraints(const b2SolverData& data);
 	bool SolvePositionConstraints(const b2SolverData& data);
 
+	b2Joint* m_joint1;
+	b2Joint* m_joint2;
+
 	b2JointType m_typeA;
 	b2JointType m_typeB;
 

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

@@ -177,6 +177,7 @@ b2Joint::b2Joint(const b2JointDef* def)
 	m_next = NULL;
 	m_bodyA = def->bodyA;
 	m_bodyB = def->bodyB;
+	m_index = 0;
 	m_collideConnected = def->collideConnected;
 	m_islandFlag = false;
 	m_userData = def->userData;

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

@@ -118,10 +118,10 @@ public:
 	/// Get the anchor point on bodyB in world coordinates.
 	virtual b2Vec2 GetAnchorB() const = 0;
 
-	/// Get the reaction force on body2 at the joint anchor in Newtons.
+	/// Get the reaction force on bodyB at the joint anchor in Newtons.
 	virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
 
-	/// Get the reaction torque on body2 in N*m.
+	/// Get the reaction torque on bodyB in N*m.
 	virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
 
 	/// Get the next joint the world joint list.
@@ -142,10 +142,14 @@ public:
 	/// the flag is only checked when fixture AABBs begin to overlap.
 	bool GetCollideConnected() const;
 
+	/// Dump this joint to the log file.
+	virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }
+
 protected:
 	friend class b2World;
 	friend class b2Body;
 	friend class b2Island;
+	friend class b2GearJoint;
 
 	static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
 	static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
@@ -167,6 +171,8 @@ protected:
 	b2Body* m_bodyA;
 	b2Body* m_bodyB;
 
+	int32 m_index;
+
 	bool m_islandFlag;
 	bool m_collideConnected;
 

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

@@ -89,6 +89,9 @@ public:
 	void SetDampingRatio(float32 ratio);
 	float32 GetDampingRatio() const;
 
+	/// The mouse joint does not support dumping.
+	void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }
+
 protected:
 	friend class b2Joint;
 

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

@@ -103,8 +103,9 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
 	m_localAnchorA = def->localAnchorA;
 	m_localAnchorB = def->localAnchorB;
 	m_localXAxisA = def->localAxisA;
+	m_localXAxisA.Normalize();
 	m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
-	m_refAngle = def->referenceAngle;
+	m_referenceAngle = def->referenceAngle;
 
 	m_impulse.SetZero();
 	m_motorMass = 0.0;
@@ -348,6 +349,17 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
 
 		vB += mB * P;
 		wB += iB * LB;
+
+		b2Vec2 Cdot10 = Cdot1;
+
+		Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
+		Cdot1.y = wB - wA;
+
+		if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f)
+		{
+			b2Vec2 test = b2Mul22(m_K, df);
+			Cdot1.x += 0.0f;
+		}
 	}
 
 	data.velocities[m_indexA].v = vA;
@@ -384,7 +396,7 @@ bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
 	b2Vec3 impulse;
 	b2Vec2 C1;
 	C1.x = b2Dot(perp, d);
-	C1.y = aB - aA - m_refAngle;
+	C1.y = aB - aA - m_referenceAngle;
 
 	float32 linearError = b2Abs(C1.x);
 	float32 angularError = b2Abs(C1.y);
@@ -419,43 +431,45 @@ bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
 
 	if (active)
 	{
-		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 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+		float32 k12 = iA * s1 + iB * s2;
+		float32 k13 = iA * s1 * a1 + iB * s2 * 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 k23 = iA * a1 + iB * a2;
+		float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
 
-		m_K.ex.Set(k11, k12, k13);
-		m_K.ey.Set(k12, k22, k23);
-		m_K.ez.Set(k13, k23, k33);
+		b2Mat33 K;
+		K.ex.Set(k11, k12, k13);
+		K.ey.Set(k12, k22, k23);
+		K.ez.Set(k13, k23, k33);
 
 		b2Vec3 C;
 		C.x = C1.x;
 		C.y = C1.y;
 		C.z = C2;
 
-		impulse = m_K.Solve33(-C);
+		impulse = K.Solve33(-C);
 	}
 	else
 	{
-		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
-		float32 k12 = iA * m_s1 + iB * m_s2;
+		float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
+		float32 k12 = iA * s1 + iB * s2;
 		float32 k22 = iA + iB;
 		if (k22 == 0.0f)
 		{
 			k22 = 1.0f;
 		}
 
-		m_K.ex.Set(k11, k12, 0.0f);
-		m_K.ey.Set(k12, k22, 0.0f);
+		b2Mat22 K;
+		K.ex.Set(k11, k12);
+		K.ey.Set(k12, k22);
 
-		b2Vec2 impulse1 = m_K.Solve22(-C1);
+		b2Vec2 impulse1 = K.Solve(-C1);
 		impulse.x = impulse1.x;
 		impulse.y = impulse1.y;
 		impulse.z = 0.0f;
@@ -599,3 +613,25 @@ float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const
 {
 	return inv_dt * m_motorImpulse;
 }
+
+void b2PrismaticJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2PrismaticJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
+	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
+	b2Log("  jd.enableLimit = bool(%d);\n", m_enableLimit);
+	b2Log("  jd.lowerTranslation = %.15lef;\n", m_lowerTranslation);
+	b2Log("  jd.upperTranslation = %.15lef;\n", m_upperTranslation);
+	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
+	b2Log("  jd.maxMotorForce = %.15lef;\n", m_maxMotorForce);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -45,16 +45,16 @@ struct b2PrismaticJointDef : public b2JointDef
 	}
 
 	/// Initialize the bodies, anchors, axis, and reference angle using the world
-	/// anchor and world axis.
+	/// anchor and unit world axis.
 	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
 
-	/// The local anchor point relative to body1's origin.
+	/// The local anchor point relative to bodyA's origin.
 	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
+	/// The local anchor point relative to bodyB's origin.
 	b2Vec2 localAnchorB;
 
-	/// The local translation axis in body1.
+	/// The local translation unit axis in bodyA.
 	b2Vec2 localAxisA;
 
 	/// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
@@ -92,6 +92,18 @@ public:
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
+	/// The local joint axis relative to bodyA.
+	const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
+
+	/// Get the reference angle.
+	float32 GetReferenceAngle() const { return m_referenceAngle; }
+
 	/// Get the current joint translation, usually in meters.
 	float32 GetJointTranslation() const;
 
@@ -127,10 +139,14 @@ public:
 
 	/// Set the maximum motor force, usually in N.
 	void SetMaxMotorForce(float32 force);
+	float32 GetMaxMotorForce() const { return m_maxMotorForce; }
 
 	/// Get the current motor force given the inverse time step, usually in N.
 	float32 GetMotorForce(float32 inv_dt) const;
 
+	/// Dump to b2Log
+	void Dump();
+
 protected:
 	friend class b2Joint;
 	friend class b2GearJoint;
@@ -145,7 +161,7 @@ protected:
 	b2Vec2 m_localAnchorB;
 	b2Vec2 m_localXAxisA;
 	b2Vec2 m_localYAxisA;
-	float32 m_refAngle;
+	float32 m_referenceAngle;
 	b2Vec3 m_impulse;
 	float32 m_motorImpulse;
 	float32 m_lowerTranslation;

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

@@ -59,6 +59,9 @@ b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
 	m_localAnchorA = def->localAnchorA;
 	m_localAnchorB = def->localAnchorB;
 
+	m_lengthA = def->lengthA;
+	m_lengthB = def->lengthB;
+
 	b2Assert(def->ratio != 0.0f);
 	m_ratio = def->ratio;
 
@@ -308,3 +311,22 @@ float32 b2PulleyJoint::GetRatio() const
 {
 	return m_ratio;
 }
+
+void b2PulleyJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2PulleyJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);
+	b2Log("  jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.lengthA = %.15lef;\n", m_lengthA);
+	b2Log("  jd.lengthB = %.15lef;\n", m_lengthB);
+	b2Log("  jd.ratio = %.15lef;\n", m_ratio);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -100,6 +100,9 @@ public:
 	/// Get the pulley ratio.
 	float32 GetRatio() const;
 
+	/// Dump joint to dmLog
+	void Dump();
+
 protected:
 
 	friend class b2Joint;
@@ -111,6 +114,8 @@ protected:
 
 	b2Vec2 m_groundAnchorA;
 	b2Vec2 m_groundAnchorB;
+	float32 m_lengthA;
+	float32 m_lengthB;
 	
 	// Solver shared
 	b2Vec2 m_localAnchorA;

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

@@ -481,3 +481,24 @@ void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
 		m_upperAngle = upper;
 	}
 }
+
+void b2RevoluteJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2RevoluteJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
+	b2Log("  jd.enableLimit = bool(%d);\n", m_enableLimit);
+	b2Log("  jd.lowerAngle = %.15lef;\n", m_lowerAngle);
+	b2Log("  jd.upperAngle = %.15lef;\n", m_upperAngle);
+	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
+	b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -52,13 +52,13 @@ struct b2RevoluteJointDef : public b2JointDef
 	/// anchor point.
 	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
 
-	/// The local anchor point relative to body1's origin.
+	/// The local anchor point relative to bodyA's origin.
 	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
+	/// The local anchor point relative to bodyB's origin.
 	b2Vec2 localAnchorB;
 
-	/// The body2 angle minus body1 angle in the reference state (radians).
+	/// The bodyB angle minus bodyA angle in the reference state (radians).
 	float32 referenceAngle;
 
 	/// A flag to enable joint limits.
@@ -93,6 +93,15 @@ public:
 	b2Vec2 GetAnchorA() const;
 	b2Vec2 GetAnchorB() const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
+	/// Get the reference angle.
+	float32 GetReferenceAngle() const { return m_referenceAngle; }
+
 	/// Get the current joint angle in radians.
 	float32 GetJointAngle() const;
 
@@ -128,6 +137,7 @@ public:
 
 	/// Set the maximum motor torque, usually in N-m.
 	void SetMaxMotorTorque(float32 torque);
+	float32 GetMaxMotorTorque() const { return m_maxMotorTorque; }
 
 	/// Get the reaction force given the inverse time step.
 	/// Unit is N.
@@ -141,6 +151,9 @@ public:
 	/// Unit is N*m.
 	float32 GetMotorTorque(float32 inv_dt) const;
 
+	/// Dump to b2Log.
+	void Dump();
+
 protected:
 	
 	friend class b2Joint;

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

@@ -224,3 +224,18 @@ b2LimitState b2RopeJoint::GetLimitState() const
 {
 	return m_state;
 }
+
+void b2RopeJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2RopeJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.maxLength = %.15lef;\n", m_maxLength);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -64,11 +64,21 @@ public:
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
-	/// Get the maximum length of the rope.
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
+	/// Set/Get the maximum length of the rope.
+	void SetMaxLength(float32 length) { m_maxLength = length; }
 	float32 GetMaxLength() const;
 
 	b2LimitState GetLimitState() const;
 
+	/// Dump joint to dmLog
+	void Dump();
+
 protected:
 
 	friend class b2Joint;

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

@@ -49,6 +49,8 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
 	m_localAnchorA = def->localAnchorA;
 	m_localAnchorB = def->localAnchorB;
 	m_referenceAngle = def->referenceAngle;
+	m_frequencyHz = def->frequencyHz;
+	m_dampingRatio = def->dampingRatio;
 
 	m_impulse.SetZero();
 }
@@ -91,15 +93,50 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
 	float32 mA = m_invMassA, mB = m_invMassB;
 	float32 iA = m_invIA, iB = m_invIB;
 
-	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;
+	b2Mat33 K;
+	K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
+	K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
+	K.ez.x = -m_rA.y * iA - m_rB.y * iB;
+	K.ex.y = K.ey.x;
+	K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
+	K.ez.y = m_rA.x * iA + m_rB.x * iB;
+	K.ex.z = K.ez.x;
+	K.ey.z = K.ez.y;
+	K.ez.z = iA + iB;
+
+	if (m_frequencyHz > 0.0f)
+	{
+		K.GetInverse22(&m_mass);
+
+		float32 invM = iA + iB;
+		float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
+
+		float32 C = aB - aA - m_referenceAngle;
+
+		// Frequency
+		float32 omega = 2.0f * b2_pi * m_frequencyHz;
+
+		// Damping coefficient
+		float32 d = 2.0f * m * m_dampingRatio * omega;
+
+		// Spring stiffness
+		float32 k = m * omega * omega;
+
+		// magic formulas
+		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 * h * k * m_gamma;
+
+		invM += m_gamma;
+		m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
+	}
+	else
+	{
+		K.GetSymInverse33(&m_mass);
+		m_gamma = 0.0f;
+		m_bias = 0.0f;
+	}
 
 	if (data.step.warmStarting)
 	{
@@ -135,20 +172,47 @@ void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
 	float32 mA = m_invMassA, mB = m_invMassB;
 	float32 iA = m_invIA, iB = m_invIB;
 
-	b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
-	float32 Cdot2 = wB - wA;
-	b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
+	if (m_frequencyHz > 0.0f)
+	{
+		float32 Cdot2 = wB - wA;
+
+		float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
+		m_impulse.z += impulse2;
+
+		wA -= iA * impulse2;
+		wB += iB * impulse2;
 
-	b2Vec3 impulse = -m_mass.Solve33(Cdot);
-	m_impulse += impulse;
+		b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
 
-	b2Vec2 P(impulse.x, impulse.y);
+		b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
+		m_impulse.x += impulse1.x;
+		m_impulse.y += impulse1.y;
 
-	vA -= mA * P;
-	wA -= iA * (b2Cross(m_rA, P) + impulse.z);
+		b2Vec2 P = impulse1;
 
-	vB += mB * P;
-	wB += iB * (b2Cross(m_rB, P) + impulse.z);
+		vA -= mA * P;
+		wA -= iA * b2Cross(m_rA, P);
+
+		vB += mB * P;
+		wB += iB * b2Cross(m_rB, P);
+	}
+	else
+	{
+		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 = -b2Mul(m_mass, Cdot);
+		m_impulse += impulse;
+
+		b2Vec2 P(impulse.x, impulse.y);
+
+		vA -= mA * P;
+		wA -= iA * (b2Cross(m_rA, P) + impulse.z);
+
+		vB += mB * P;
+		wB += iB * (b2Cross(m_rB, P) + impulse.z);
+	}
 
 	data.velocities[m_indexA].v = vA;
 	data.velocities[m_indexA].w = wA;
@@ -171,33 +235,53 @@ bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
 	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
 	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
 
-	b2Vec2 C1 =  cB + rB - cA - rA;
-	float32 C2 = aB - aA - m_referenceAngle;
+	float32 positionError, angularError;
 
-	float32 positionError = C1.Length();
-	float32 angularError = b2Abs(C2);
+	b2Mat33 K;
+	K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+	K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+	K.ez.x = -rA.y * iA - rB.y * iB;
+	K.ex.y = K.ey.x;
+	K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+	K.ez.y = rA.x * iA + rB.x * iB;
+	K.ex.z = K.ez.x;
+	K.ey.z = K.ez.y;
+	K.ez.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;
+	if (m_frequencyHz > 0.0f)
+	{
+		b2Vec2 C1 =  cB + rB - cA - rA;
 
-	b2Vec3 C(C1.x, C1.y, C2);
+		positionError = C1.Length();
+		angularError = 0.0f;
 
-	b2Vec3 impulse = -m_mass.Solve33(C);
+		b2Vec2 P = -K.Solve22(C1);
 
-	b2Vec2 P(impulse.x, impulse.y);
+		cA -= mA * P;
+		aA -= iA * b2Cross(rA, P);
 
-	cA -= mA * P;
-	aA -= iA * (b2Cross(rA, P) + impulse.z);
+		cB += mB * P;
+		aB += iB * b2Cross(rB, P);
+	}
+	else
+	{
+		b2Vec2 C1 =  cB + rB - cA - rA;
+		float32 C2 = aB - aA - m_referenceAngle;
+
+		positionError = C1.Length();
+		angularError = b2Abs(C2);
 
-	cB += mB * P;
-	aB += iB * (b2Cross(rB, P) + impulse.z);
+		b2Vec3 C(C1.x, C1.y, C2);
+	
+		b2Vec3 impulse = -K.Solve33(C);
+		b2Vec2 P(impulse.x, impulse.y);
+
+		cA -= mA * P;
+		aA -= iA * (b2Cross(rA, P) + impulse.z);
+
+		cB += mB * P;
+		aB += iB * (b2Cross(rB, P) + impulse.z);
+	}
 
 	data.positions[m_indexA].c = cA;
 	data.positions[m_indexA].a = aA;
@@ -227,3 +311,20 @@ float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
 {
 	return inv_dt * m_impulse.z;
 }
+
+void b2WeldJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2WeldJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.referenceAngle = %.15lef;\n", m_referenceAngle);
+	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
+	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -32,20 +32,29 @@ struct b2WeldJointDef : public b2JointDef
 		localAnchorA.Set(0.0f, 0.0f);
 		localAnchorB.Set(0.0f, 0.0f);
 		referenceAngle = 0.0f;
+		frequencyHz = 0.0f;
+		dampingRatio = 0.0f;
 	}
 
 	/// Initialize the bodies, anchors, and reference angle using a world
 	/// anchor point.
-	void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
+	void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
 
-	/// The local anchor point relative to body1's origin.
+	/// The local anchor point relative to bodyA's origin.
 	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
+	/// The local anchor point relative to bodyB's origin.
 	b2Vec2 localAnchorB;
 
-	/// The body2 angle minus body1 angle in the reference state (radians).
+	/// The bodyB angle minus bodyA angle in the reference state (radians).
 	float32 referenceAngle;
+	
+	/// The mass-spring-damper frequency in Hertz. Rotation only.
+	/// Disable softness with a value of 0.
+	float32 frequencyHz;
+
+	/// The damping ratio. 0 = no damping, 1 = critical damping.
+	float32 dampingRatio;
 };
 
 /// A weld joint essentially glues two bodies together. A weld joint may
@@ -59,6 +68,26 @@ public:
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
+	/// Get the reference angle.
+	float32 GetReferenceAngle() const { return m_referenceAngle; }
+
+	/// Set/get frequency in Hz.
+	void SetFrequency(float32 hz) { m_frequencyHz = hz; }
+	float32 GetFrequency() const { return m_frequencyHz; }
+
+	/// Set/get damping ratio.
+	void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; }
+	float32 GetDampingRatio() const { return m_dampingRatio; }
+
+	/// Dump to b2Log
+	void Dump();
+
 protected:
 
 	friend class b2Joint;
@@ -69,10 +98,15 @@ protected:
 	void SolveVelocityConstraints(const b2SolverData& data);
 	bool SolvePositionConstraints(const b2SolverData& data);
 
+	float32 m_frequencyHz;
+	float32 m_dampingRatio;
+	float32 m_bias;
+
 	// Solver shared
 	b2Vec2 m_localAnchorA;
 	b2Vec2 m_localAnchorB;
 	float32 m_referenceAngle;
+	float32 m_gamma;
 	b2Vec3 m_impulse;
 
 	// Solver temp

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

@@ -398,7 +398,22 @@ float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
 	return inv_dt * m_motorImpulse;
 }
 
-
-
-
-
+void b2WheelJoint::Dump()
+{
+	int32 indexA = m_bodyA->m_islandIndex;
+	int32 indexB = m_bodyB->m_islandIndex;
+
+	b2Log("  b2WheelJointDef jd;\n");
+	b2Log("  jd.bodyA = bodies[%d];\n", indexA);
+	b2Log("  jd.bodyB = bodies[%d];\n", indexB);
+	b2Log("  jd.collideConnected = bool(%d);\n", m_collideConnected);
+	b2Log("  jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
+	b2Log("  jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
+	b2Log("  jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
+	b2Log("  jd.enableMotor = bool(%d);\n", m_enableMotor);
+	b2Log("  jd.motorSpeed = %.15lef;\n", m_motorSpeed);
+	b2Log("  jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
+	b2Log("  jd.frequencyHz = %.15lef;\n", m_frequencyHz);
+	b2Log("  jd.dampingRatio = %.15lef;\n", m_dampingRatio);
+	b2Log("  joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
+}

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

@@ -46,13 +46,13 @@ struct b2WheelJointDef : public b2JointDef
 	/// 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.
+	/// The local anchor point relative to bodyA's origin.
 	b2Vec2 localAnchorA;
 
-	/// The local anchor point relative to body2's origin.
+	/// The local anchor point relative to bodyB's origin.
 	b2Vec2 localAnchorB;
 
-	/// The local translation axis in body1.
+	/// The local translation axis in bodyA.
 	b2Vec2 localAxisA;
 
 	/// Enable/disable the joint motor.
@@ -72,19 +72,30 @@ struct b2WheelJointDef : public b2JointDef
 };
 
 /// 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
+/// along an axis fixed in bodyA 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:
+	void GetDefinition(b2WheelJointDef* def) const;
+
 	b2Vec2 GetAnchorA() const;
 	b2Vec2 GetAnchorB() const;
 
 	b2Vec2 GetReactionForce(float32 inv_dt) const;
 	float32 GetReactionTorque(float32 inv_dt) const;
 
+	/// The local anchor point relative to bodyA's origin.
+	const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
+
+	/// The local anchor point relative to bodyB's origin.
+	const b2Vec2& GetLocalAnchorB() const  { return m_localAnchorB; }
+
+	/// The local joint axis relative to bodyA.
+	const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
+
 	/// Get the current joint translation, usually in meters.
 	float32 GetJointTranslation() const;
 
@@ -118,6 +129,9 @@ public:
 	void SetSpringDampingRatio(float32 ratio);
 	float32 GetSpringDampingRatio() const;
 
+	/// Dump to b2Log
+	void Dump();
+
 protected:
 
 	friend class b2Joint;

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

@@ -481,4 +481,34 @@ void b2Body::SetActive(bool flag)
 		}
 		m_contactList = NULL;
 	}
+}
+
+void b2Body::Dump()
+{
+	int32 bodyIndex = m_islandIndex;
+
+	b2Log("{\n");
+	b2Log("  b2BodyDef bd;\n");
+	b2Log("  bd.type = b2BodyType(%d);\n", m_type);
+	b2Log("  bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
+	b2Log("  bd.angle = %.15lef;\n", m_sweep.a);
+	b2Log("  bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
+	b2Log("  bd.angularVelocity = %.15lef;\n", m_angularVelocity);
+	b2Log("  bd.linearDamping = %.15lef;\n", m_linearDamping);
+	b2Log("  bd.angularDamping = %.15lef;\n", m_angularDamping);
+	b2Log("  bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
+	b2Log("  bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
+	b2Log("  bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
+	b2Log("  bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
+	b2Log("  bd.active = bool(%d);\n", m_flags & e_activeFlag);
+	b2Log("  bd.gravityScale = %.15lef;\n", m_gravityScale);
+	b2Log("  bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
+	b2Log("\n");
+	for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
+	{
+		b2Log("  {\n");
+		f->Dump(bodyIndex);
+		b2Log("  }\n");
+	}
+	b2Log("}\n");
 }

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

@@ -375,6 +375,9 @@ public:
 	b2World* GetWorld();
 	const b2World* GetWorld() const;
 
+	/// Dump this body to a log file
+	void Dump();
+
 private:
 
 	friend class b2World;
@@ -474,7 +477,7 @@ inline const b2Vec2& b2Body::GetPosition() const
 
 inline float32 b2Body::GetAngle() const
 {
-	return m_xf.q.GetAngle();
+	return m_sweep.a;
 }
 
 inline const b2Vec2& b2Body::GetWorldCenter() const

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

@@ -115,16 +115,6 @@ void b2ContactManager::Collide()
 		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;
-
-		// At least one body must be awake and it must be dynamic or kinematic.
-		if (activeA == false && activeB == false)
-		{
-			c = c->GetNext();
-			continue;
-		}
-
 		// Is this contact flagged for filtering?
 		if (c->m_flags & b2Contact::e_filterFlag)
 		{
@@ -150,6 +140,16 @@ void b2ContactManager::Collide()
 			c->m_flags &= ~b2Contact::e_filterFlag;
 		}
 
+		bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;
+		bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;
+
+		// At least one body must be awake and it must be dynamic or kinematic.
+		if (activeA == false && activeB == false)
+		{
+			c = c->GetNext();
+			continue;
+		}
+
 		int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
 		int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
 		bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);

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

@@ -226,3 +226,78 @@ void b2Fixture::SetSensor(bool sensor)
 	}
 }
 
+void b2Fixture::Dump(int32 bodyIndex)
+{
+	b2Log("    b2FixtureDef fd;\n");
+	b2Log("    fd.friction = %.15lef;\n", m_friction);
+	b2Log("    fd.restitution = %.15lef;\n", m_restitution);
+	b2Log("    fd.density = %.15lef;\n", m_density);
+	b2Log("    fd.isSensor = bool(%d);\n", m_isSensor);
+	b2Log("    fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);
+	b2Log("    fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);
+	b2Log("    fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);
+
+	switch (m_shape->m_type)
+	{
+	case b2Shape::e_circle:
+		{
+			b2CircleShape* s = (b2CircleShape*)m_shape;
+			b2Log("    b2CircleShape shape;\n");
+			b2Log("    shape.m_radius = %.15lef;\n", s->m_radius);
+			b2Log("    shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y);
+		}
+		break;
+
+	case b2Shape::e_edge:
+		{
+			b2EdgeShape* s = (b2EdgeShape*)m_shape;
+			b2Log("    b2EdgeShape shape;\n");
+			b2Log("    shape.m_radius = %.15lef;\n", s->m_radius);
+			b2Log("    shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y);
+			b2Log("    shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y);
+			b2Log("    shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y);
+			b2Log("    shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y);
+			b2Log("    shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0);
+			b2Log("    shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3);
+		}
+		break;
+
+	case b2Shape::e_polygon:
+		{
+			b2PolygonShape* s = (b2PolygonShape*)m_shape;
+			b2Log("    b2PolygonShape shape;\n");
+			b2Log("    b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
+			for (int32 i = 0; i < s->m_vertexCount; ++i)
+			{
+				b2Log("    vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
+			}
+			b2Log("    shape.Set(vs, %d);\n", s->m_vertexCount);
+		}
+		break;
+
+	case b2Shape::e_chain:
+		{
+			b2ChainShape* s = (b2ChainShape*)m_shape;
+			b2Log("    b2ChainShape shape;\n");
+			b2Log("    b2Vec2 vs[%d];\n", s->m_count);
+			for (int32 i = 0; i < s->m_count; ++i)
+			{
+				b2Log("    vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
+			}
+			b2Log("    shape.CreateChain(vs, %d);\n", s->m_count);
+			b2Log("    shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y);
+			b2Log("    shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y);
+			b2Log("    shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex);
+			b2Log("    shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex);
+		}
+		break;
+
+	default:
+		return;
+	}
+
+	b2Log("\n");
+	b2Log("    fd.shape = &shape;\n");
+	b2Log("\n");
+	b2Log("    bodies[%d]->CreateFixture(&fd);\n", bodyIndex);
+}

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

@@ -31,6 +31,13 @@ class b2Fixture;
 /// This holds contact filtering data.
 struct b2Filter
 {
+	b2Filter()
+	{
+		categoryBits = 0x0001;
+		maskBits = 0xFFFF;
+		groupIndex = 0;
+	}
+
 	/// The collision category bits. Normally you would just set one bit.
 	uint16 categoryBits;
 
@@ -56,9 +63,6 @@ struct b2FixtureDef
 		friction = 0.2f;
 		restitution = 0.0f;
 		density = 0.0f;
-		filter.categoryBits = 0x0001;
-		filter.maskBits = 0xFFFF;
-		filter.groupIndex = 0;
 		isSensor = false;
 	}
 
@@ -188,6 +192,9 @@ public:
 	/// the body transform.
 	const b2AABB& GetAABB(int32 childIndex) const;
 
+	/// Dump this fixture to the log file.
+	void Dump(int32 bodyIndex);
+
 protected:
 
 	friend class b2Body;

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

@@ -21,18 +21,18 @@
 
 #include <Box2D/Common/b2Math.h>
 
-/// Profiling data. Times are in milliseconds.
-struct b2Profile
-{
-	float32 step;
-	float32 collide;
-	float32 solve;
-	float32 solveInit;
-	float32 solveVelocity;
-	float32 solvePosition;
-	float32 broadphase;
-	float32 solveTOI;
-};
+/// Profiling data. Times are in milliseconds.
+struct b2Profile
+{
+	float32 step;
+	float32 collide;
+	float32 solve;
+	float32 solveInit;
+	float32 solveVelocity;
+	float32 solvePosition;
+	float32 broadphase;
+	float32 solveTOI;
+};
 
 /// This is an internal structure.
 struct b2TimeStep

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

@@ -34,7 +34,7 @@
 #include <Box2D/Common/b2Timer.h>
 #include <new>
 
-b2World::b2World(const b2Vec2& gravity, bool doSleep)
+b2World::b2World(const b2Vec2& gravity)
 {
 	m_destructionListener = NULL;
 	m_debugDraw = NULL;
@@ -51,7 +51,7 @@ b2World::b2World(const b2Vec2& gravity, bool doSleep)
 
 	m_stepComplete = true;
 
-	m_allowSleep = doSleep;
+	m_allowSleep = true;
 	m_gravity = gravity;
 
 	m_flags = e_clearForces;
@@ -364,6 +364,24 @@ void b2World::DestroyJoint(b2Joint* j)
 	}
 }
 
+//
+void b2World::SetAllowSleeping(bool flag)
+{
+	if (flag == m_allowSleep)
+	{
+		return;
+	}
+
+	m_allowSleep = flag;
+	if (m_allowSleep == false)
+	{
+		for (b2Body* b = m_bodyList; b; b = b->m_next)
+		{
+			b->SetAwake(true);
+		}
+	}
+}
+
 // Find islands, integrate and solve constraints, solve position constraints
 void b2World::Solve(const b2TimeStep& step)
 {
@@ -1038,8 +1056,8 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
 	case b2Shape::e_chain:
 		{
 			b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
-			int32 count = chain->GetVertexCount();
-			const b2Vec2* vertices = chain->GetVertices();
+			int32 count = chain->m_count;
+			const b2Vec2* vertices = chain->m_vertices;
 
 			b2Vec2 v1 = b2Mul(xf, vertices[0]);
 			for (int32 i = 1; i < count; ++i)
@@ -1237,3 +1255,62 @@ float32 b2World::GetTreeQuality() const
 {
 	return m_contactManager.m_broadPhase.GetTreeQuality();
 }
+
+void b2World::Dump()
+{
+	if ((m_flags & e_locked) == e_locked)
+	{
+		return;
+	}
+
+	b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);
+	b2Log("m_world->SetGravity(g);\n");
+
+	b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
+	b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
+	int32 i = 0;
+	for (b2Body* b = m_bodyList; b; b = b->m_next)
+	{
+		b->m_islandIndex = i;
+		b->Dump();
+		++i;
+	}
+
+	i = 0;
+	for (b2Joint* j = m_jointList; j; j = j->m_next)
+	{
+		j->m_index = i;
+		++i;
+	}
+
+	// First pass on joints, skip gear joints.
+	for (b2Joint* j = m_jointList; j; j = j->m_next)
+	{
+		if (j->m_type == e_gearJoint)
+		{
+			continue;
+		}
+
+		b2Log("{\n");
+		j->Dump();
+		b2Log("}\n");
+	}
+
+	// Second pass on joints, only gear joints.
+	for (b2Joint* j = m_jointList; j; j = j->m_next)
+	{
+		if (j->m_type != e_gearJoint)
+		{
+			continue;
+		}
+
+		b2Log("{\n");
+		j->Dump();
+		b2Log("}\n");
+	}
+
+	b2Log("b2Free(joints);\n");
+	b2Log("b2Free(bodies);\n");
+	b2Log("joints = NULL;\n");
+	b2Log("bodies = NULL;\n");
+}

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

@@ -43,8 +43,7 @@ class b2World
 public:
 	/// Construct a world object.
 	/// @param gravity the world gravity vector.
-	/// @param doSleep improve performance by not simulating inactive bodies.
-	b2World(const b2Vec2& gravity, bool doSleep);
+	b2World(const b2Vec2& gravity);
 
 	/// Destruct the world. All physics entities are destroyed and all heap memory is released.
 	~b2World();
@@ -142,14 +141,21 @@ public:
 	b2Contact* GetContactList();
 	const b2Contact* GetContactList() const;
 
+	/// Enable/disable sleep.
+	void SetAllowSleeping(bool flag);
+	bool GetAllowSleeping() const { return m_allowSleep; }
+
 	/// Enable/disable warm starting. For testing.
 	void SetWarmStarting(bool flag) { m_warmStarting = flag; }
+	bool GetWarmStarting() const { return m_warmStarting; }
 
 	/// Enable/disable continuous physics. For testing.
 	void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
+	bool GetContinuousPhysics() const { return m_continuousPhysics; }
 
 	/// Enable/disable single stepped continuous physics. For testing.
 	void SetSubStepping(bool flag) { m_subStepping = flag; }
+	bool GetSubStepping() const { return m_subStepping; }
 
 	/// Get the number of broad-phase proxies.
 	int32 GetProxyCount() const;
@@ -194,6 +200,10 @@ public:
 	/// Get the current profile.
 	const b2Profile& GetProfile() const;
 
+	/// Dump the world into the log file.
+	/// @warning this should be called outside of a time step.
+	void Dump();
+
 private:
 
 	// m_flags