[upstream] Fix joint validation (box2d #954)

src/Box2D.NET.Samples/Samples/Bodies/MixedLocks.cs

@@ -0,0 +1,120 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-FileCopyrightText: 2025 Ikpil Choi([email protected])
+// SPDX-License-Identifier: MIT
+
+using static Box2D.NET.B2Geometries;
+using static Box2D.NET.B2Types;
+using static Box2D.NET.B2Bodies;
+using static Box2D.NET.B2Shapes;
+
+namespace Box2D.NET.Samples.Samples.Bodies;
+
+// Motion locking can be a bit squishy
+public class MixedLocks : Sample
+{
+    private static readonly int SampleMixedLocks = SampleFactory.Shared.RegisterSample("Bodies", "Mixed Locks", Create);
+
+    private static Sample Create(SampleContext context)
+    {
+        return new MixedLocks(context);
+    }
+
+    public MixedLocks(SampleContext context) : base(context)
+    {
+        if (m_context.settings.restart == false)
+        {
+            m_context.camera.m_center = new B2Vec2(0.0f, 2.5f);
+            m_context.camera.m_zoom = 3.5f;
+        }
+
+        {
+            B2BodyDef bodyDef = b2DefaultBodyDef();
+            B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef);
+
+            B2ShapeDef shapeDef = b2DefaultShapeDef();
+            B2Segment segment = new B2Segment(new B2Vec2(-40.0f, 0.0f), new B2Vec2(40.0f, 0.0f));
+            b2CreateSegmentShape(groundId, ref shapeDef, ref segment);
+        }
+
+        {
+            B2Polygon box = b2MakeSquare(0.5f);
+            B2ShapeDef shapeDef = b2DefaultShapeDef();
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.position = new B2Vec2(2.0f, 1.0f);
+                bodyDef.name = "static";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(1.0f, 1.0f);
+                bodyDef.name = "free";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(1.0f, 3.0f);
+                bodyDef.name = "free";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(-1.0f, 1.0f);
+                bodyDef.motionLocks.angularZ = true;
+                bodyDef.name = "angular z";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(-2.0f, 2.0f);
+                bodyDef.motionLocks.linearX = true;
+                bodyDef.name = "linear x";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(-1.0f, 2.5f);
+                bodyDef.motionLocks.linearY = true;
+                bodyDef.motionLocks.angularZ = true;
+                bodyDef.name = "lin y ang z";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+
+            {
+                B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_dynamicBody;
+                bodyDef.position = new B2Vec2(0.0f, 1.0f);
+                bodyDef.motionLocks.linearX = true;
+                bodyDef.motionLocks.linearY = true;
+                bodyDef.motionLocks.angularZ = true;
+                bodyDef.name = "full";
+
+                B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                b2CreatePolygonShape(bodyId, ref shapeDef, ref box);
+            }
+        }
+    }
+}

src/Box2D.NET.Samples/Samples/Sample.cs

@@ -297,6 +297,7 @@ public virtual void MouseDown(B2Vec2 p, MouseButton button, KeyModifiers mod)
             if (B2_IS_NON_NULL(queryContext.bodyId))
             {
                 B2BodyDef bodyDef = b2DefaultBodyDef();
+                bodyDef.type = B2BodyType.b2_kinematicBody;
                 m_groundBodyId = b2CreateBody(m_worldId, ref bodyDef);
 
                 B2MouseJointDef jointDef = b2DefaultMouseJointDef();
@@ -306,7 +307,7 @@ public virtual void MouseDown(B2Vec2 p, MouseButton button, KeyModifiers mod)
                 [email protected] = 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, ref jointDef);
 
                 b2Body_SetAwake(queryContext.bodyId, true);

src/Box2D.NET/B2Bodies.cs

@@ -353,6 +353,7 @@ public static bool b2WakeBody(B2World world, B2Body body)
             if (body.setIndex >= (int)B2SetType.b2_firstSleepingSet)
             {
                 b2WakeSolverSet(world, body.setIndex);
+                b2ValidateSolverSets(world);
                 return true;
             }
 

src/Box2D.NET/B2BodyDef.cs

@@ -53,7 +53,8 @@ public struct B2BodyDef
         /// Use this to store application specific body data.
         public object 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
         public B2MotionLocks motionLocks;
 
         /// Set this flag to false if this body should never fall asleep.

src/Box2D.NET/B2Joints.cs

@@ -342,7 +342,7 @@ public static B2JointPair b2CreateJoint(B2World world, ref B2JointDef def, B2Joi
                 //memset( jointSim, 0, sizeof( b2JointSim ) );
                 jointSim.Clear();
 
-                // 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/Box2D.NET/B2SolverSets.cs

@@ -167,8 +167,6 @@ public static void b2WakeSolverSet(B2World world, int setIndex)
 
             // destroy the sleeping set
             b2DestroySolverSet(world, setIndex);
-
-            b2ValidateSolverSets(world);
         }
 
         public static void b2TrySleepIsland(B2World world, int islandId)