Fix joint validation (#954)

Joint creation may wake a body. This was triggering a validation error
while the joint wasn't fully built. I moved the validation closer to the
API.

Added motion locking sample. This shows that motion locking is somewhat
of a kludge because constraints in the locked direction can be softened.

Author: erincatto

include/box2d/types.h

@@ -226,7 +226,8 @@ typedef struct b2BodyDef
 	/// Use this to store application specific body data.
 	void* userData;
 
-	/// Motions locks to restrict linear and angular movement
+	/// Motions locks to restrict linear and angular movement.
+	/// Caution: may lead to softer constraints along the locked direction
 	b2MotionLocks motionLocks;
 
 	/// Set this flag to false if this body should never fall asleep.

samples/sample.cpp

@@ -327,6 +327,7 @@ void Sample::MouseDown( b2Vec2 p, int button, int mod )
 		if ( B2_IS_NON_NULL( queryContext.bodyId ) )
 		{
 			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_kinematicBody;
 			m_groundBodyId = b2CreateBody( m_worldId, &bodyDef );
 
 			b2MouseJointDef jointDef = b2DefaultMouseJointDef();
@@ -336,7 +337,7 @@ void Sample::MouseDown( b2Vec2 p, int button, int mod )
 			jointDef.base.localFrameB.p = b2Body_GetLocalPoint( queryContext.bodyId, p );
 			jointDef.hertz = 7.5f;
 			jointDef.dampingRatio = 0.7f;
-			jointDef.maxForce = 1000.0f * b2Body_GetMass( queryContext.bodyId ) * b2Length(b2World_GetGravity(m_worldId));
+			jointDef.maxForce = 100.0f * b2Body_GetMass( queryContext.bodyId ) * b2Length(b2World_GetGravity(m_worldId));
 			m_mouseJointId = b2CreateMouseJoint( m_worldId, &jointDef );
 
 			b2Body_SetAwake( queryContext.bodyId, true );

samples/sample_bodies.cpp

@@ -874,3 +874,113 @@ class Kinematic : public Sample
 };
 
 static int sampleKinematic = RegisterSample( "Bodies", "Kinematic", Kinematic::Create );
+
+// Motion locking can be a bit squishy
+class MixedLocks : public Sample
+{
+public:
+	explicit MixedLocks( SampleContext* context )
+		: Sample( context )
+	{
+		if ( m_context->restart == false )
+		{
+			m_context->camera.m_center = { 0.0f, 2.5f };
+			m_context->camera.m_zoom = 3.5f;
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			b2BodyId groundId = b2CreateBody( m_worldId, &bodyDef );
+
+			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			b2Segment segment = { { -40.0, 0.0f }, { 40.0f, 0.0f } };
+			b2CreateSegmentShape( groundId, &shapeDef, &segment );
+		}
+
+		b2Polygon box = b2MakeSquare( 0.5f );
+		b2ShapeDef shapeDef = b2DefaultShapeDef();
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.position = { 2.0f, 1.0f };
+			bodyDef.name = "static";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { 1.0f, 1.0f };
+			bodyDef.name = "free";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { 1.0f, 3.0f };
+			bodyDef.name = "free";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { -1.0f, 1.0f };
+			bodyDef.motionLocks.angularZ = true;
+			bodyDef.name = "angular z";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { -2.0f, 2.0f };
+			bodyDef.motionLocks.linearX = true;
+			bodyDef.name = "linear x";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { -1.0f, 2.5f };
+			bodyDef.motionLocks.linearY = true;
+			bodyDef.motionLocks.angularZ = true;
+			bodyDef.name = "lin y ang z";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+
+		{
+			b2BodyDef bodyDef = b2DefaultBodyDef();
+			bodyDef.type = b2_dynamicBody;
+			bodyDef.position = { 0.0f, 1.0f };
+			bodyDef.motionLocks.linearX = true;
+			bodyDef.motionLocks.linearY = true;
+			bodyDef.motionLocks.angularZ = true;
+			bodyDef.name = "full";
+
+			b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+			b2CreatePolygonShape( bodyId, &shapeDef, &box );
+		}
+	}
+
+	static Sample* Create( SampleContext* context )
+	{
+		return new MixedLocks( context );
+	}
+};
+
+static int sampleMixedLocks = RegisterSample( "Bodies", "Mixed Locks", MixedLocks::Create );

src/body.c

@@ -341,6 +341,7 @@ bool b2WakeBody( b2World* world, b2Body* body )
 	if ( body->setIndex >= b2_firstSleepingSet )
 	{
 		b2WakeSolverSet( world, body->setIndex );
+		b2ValidateSolverSets( world );
 		return true;
 	}
 

src/joint.c

@@ -331,7 +331,7 @@ static b2JointPair b2CreateJoint( b2World* world, const b2JointDef* def, b2Joint
 		jointSim = b2JointSimArray_Add( &set->jointSims );
 		memset( jointSim, 0, sizeof( b2JointSim ) );
 
-		// These must be set to accomodate the merge below
+		// These must be set to accommodate the merge below
 		jointSim->jointId = jointId;
 		jointSim->bodyIdA = bodyIdA;
 		jointSim->bodyIdB = bodyIdB;

src/solver_set.c

@@ -150,8 +150,6 @@ void b2WakeSolverSet( b2World* world, int setIndex )
 
 	// destroy the sleeping set
 	b2DestroySolverSet( world, setIndex );
-
-	b2ValidateSolverSets( world );
 }
 
 void b2TrySleepIsland( b2World* world, int islandId )