[upstream] Tunable joint stiffness (box2d #934)

Joint stiffness is now tunable per joint. This allows specific joints to
have increased stiffness, which may be important for some games.
Removed `b2World_SetJointTuning`
Added `b2Joint_SetConstraintTuning` and `b2Joint_GetConstraintTuning`.
Added `b2StoreWorldId` and `b2LoadWorldId`.
Added `b2Joint_GetReferenceAngle`, `b2Joint_SetReferenceAngle`,
`b2Joint_SetLocalAxisA`, and `b2Joint_GetLocalAxisA`.
Removed `b2WeldJoint_GetReferenceAngle` and
`b2WeldJoint_SetReferenceAngle`.
Added `b2SpringDamper` math utility function used by the character
sample.
Fixed bug in `b2SolvePlanes`.

Author: ikpil

src/Box2D.NET.Samples/SampleApp.cs

@@ -417,7 +417,7 @@ public byte[] AllocFcn(uint size, int alignment)
         long sizeAligned = ((size - 1) | (uint)(alignment - 1)) + 1;
         B2_ASSERT((sizeAligned & (alignment - 1)) == 0);
 
-// #if defined( _WIN64 ) || defined( _WIN32 )
+// #if defined( _MSC_VER ) || defined( __MINGW32__ ) || defined( __MINGW64__ )
 //         void* ptr = _aligned_malloc( sizeAligned, alignment );
 // #else
 //         void* ptr = aligned_alloc(alignment, sizeAligned);
@@ -429,7 +429,7 @@ public byte[] AllocFcn(uint size, int alignment)
 
     private void FreeFcn(byte[] mem)
     {
-// #if defined( _WIN64 ) || defined( _WIN32 )
+// #if defined( _MSC_VER ) || defined( __MINGW32__ ) || defined( __MINGW64__ )
 //         _aligned_free( mem );
 // #else
 //         free(mem);
@@ -737,8 +737,8 @@ private void UpdateUI()
                 if (ImGui.BeginTabItem("Controls"))
                 {
                     ImGui.PushItemWidth(100.0f);
-                    ImGui.SliderInt("Sub-steps", ref _context.settings.subStepCount, 1, 50);
-                    ImGui.SliderFloat("Hertz", ref _context.settings.hertz, 5.0f, 120.0f, "%.0f hz");
+                    ImGui.SliderInt("Sub-steps", ref _context.settings.subStepCount, 1, 32);
+                    ImGui.SliderFloat("Hertz", ref _context.settings.hertz, 5.0f, 240.0f, "%.0f hz");
 
                     if (ImGui.SliderInt("Workers", ref _context.settings.workerCount, 1, maxWorkers))
                     {

src/Box2D.NET.Samples/Samples/Characters/Mover.cs

@@ -397,13 +397,8 @@ void SolveMove(float timeStep, float throttle)
         {
             float pogoCurrentLength = castResult.fraction * rayLength;
 
-            float zeta = m_pogoDampingRatio;
-            float hertz = m_pogoHertz;
-            float omega = 2.0f * B2_PI * hertz;
-            float omegaH = omega * timeStep;
-
-            m_pogoVelocity = (m_pogoVelocity - omega * omegaH * (pogoCurrentLength - pogoRestLength)) /
-                             (1.0f + 2.0f * zeta * omegaH + omegaH * omegaH);
+            float offset = pogoCurrentLength - pogoRestLength;
+            m_pogoVelocity = b2SpringDamper(m_pogoHertz, m_pogoDampingRatio, offset, m_pogoVelocity, timeStep);
 
             B2Vec2 delta = castResult.fraction * translation;
             m_draw.DrawSegment(origin, origin + delta, B2HexColor.b2_colorGray);
@@ -445,15 +440,13 @@ void SolveMove(float timeStep, float throttle)
             mover.radius = m_capsule.radius;
 
             b2World_CollideMover(m_worldId, ref mover, collideFilter, PlaneResultFcn, this);
-            B2PlaneSolverResult solverResult = b2SolvePlanes(target, m_planes, m_planeCount);
-
-            m_totalIterations += solverResult.iterationCount;
+            B2PlaneSolverResult result = b2SolvePlanes(target - m_transform.p, m_planes, m_planeCount);
 
-            B2Vec2 moverTranslation = solverResult.position - m_transform.p;
+            m_totalIterations += result.iterationCount;
 
-            float fraction = b2World_CastMover(m_worldId, ref mover, moverTranslation, castFilter);
+            float fraction = b2World_CastMover(m_worldId, ref mover, result.translation, castFilter);
 
-            B2Vec2 delta = fraction * moverTranslation;
+            B2Vec2 delta = fraction * result.translation;
             m_transform.p += delta;
 
             if (b2LengthSquared(delta) < tolerance * tolerance)

src/Box2D.NET.Samples/Samples/Determinisms/FallingHinges.cs

@@ -38,7 +38,8 @@ public FallingHinges(SampleContext context) : base(context)
             m_camera.m_center = new B2Vec2(0.0f, 7.5f);
             m_camera.m_zoom = 10.0f;
         }
-        m_data = CreateFallingHinges( m_worldId );
+
+        m_data = CreateFallingHinges(m_worldId);
         m_done = false;
     }
 
@@ -47,9 +48,9 @@ public override void Step()
     {
         base.Step();
 
-        if (m_done == false)
+        if (m_context.settings.pause == false && m_done == false)
         {
-            m_done = UpdateFallingHinges( m_worldId, ref m_data );
+            m_done = UpdateFallingHinges(m_worldId, ref m_data);
         }
     }
 
@@ -60,7 +61,6 @@ public override void Draw(Settings settings)
         if (m_done)
         {
             DrawTextLine($"sleep step = {m_data.sleepStep}, hash = 0x{m_data.hash:X8}");
-            
         }
     }
 }

src/Box2D.NET.Samples/Samples/Doohickey.cs

@@ -13,17 +13,16 @@ namespace Box2D.NET.Samples.Samples;
 
 public class Doohickey
 {
-    B2BodyId m_wheelId1;
-    B2BodyId m_wheelId2;
-    B2BodyId m_barId1;
-    B2BodyId m_barId2;
+    private B2BodyId m_wheelId1;
+    private B2BodyId m_wheelId2;
+    private B2BodyId m_barId1;
+    private B2BodyId m_barId2;
 
-    B2JointId m_axleId1;
-    B2JointId m_axleId2;
-    B2JointId m_sliderId;
-
-    bool m_isSpawned;
+    private B2JointId m_axleId1;
+    private B2JointId m_axleId2;
+    private B2JointId m_sliderId;
 
+    private bool m_isSpawned;
 
     public Doohickey()
     {
@@ -37,6 +36,8 @@ public void Spawn(B2WorldId worldId, B2Vec2 position, float scale)
         bodyDef.type = B2BodyType.b2_dynamicBody;
 
         B2ShapeDef shapeDef = b2DefaultShapeDef();
+        shapeDef.material.rollingResistance = 0.1f;
+        
         B2Circle circle = new B2Circle(new B2Vec2(0.0f, 0.0f), 1.0f * scale);
         B2Capsule capsule = new B2Capsule(new B2Vec2(-3.5f * scale, 0.0f), new B2Vec2(3.5f * scale, 0.0f), 0.15f * scale);
 

src/Box2D.NET.Samples/Samples/Joints/BallAndChain.cs

@@ -51,7 +51,8 @@ public BallAndChain(SampleContext context) : base(context)
 
             B2ShapeDef shapeDef = b2DefaultShapeDef();
             shapeDef.density = 20.0f;
-
+            shapeDef.filter.categoryBits = 0x1;
+            shapeDef.filter.maskBits = 0x2;
             B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef();
 
             int jointIndex = 0;
@@ -70,8 +71,10 @@ public BallAndChain(SampleContext context) : base(context)
                 jointDef.bodyIdB = bodyId;
                 jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot);
                 jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot);
-                // jointDef.enableMotor = true;
+                jointDef.enableMotor = true;
                 jointDef.maxMotorTorque = m_frictionTorque;
+                jointDef.enableSpring = i > 0;
+                jointDef.hertz = 4.0f;
                 m_jointIds[jointIndex++] = b2CreateRevoluteJoint(m_worldId, ref jointDef);
 
                 prevBodyId = bodyId;
@@ -83,8 +86,10 @@ public BallAndChain(SampleContext context) : base(context)
                 B2BodyDef bodyDef = b2DefaultBodyDef();
                 bodyDef.type = B2BodyType.b2_dynamicBody;
                 bodyDef.position = new B2Vec2((1.0f + 2.0f * m_count) * hx + circle.radius - hx, m_count * hx);
-
                 B2BodyId bodyId = b2CreateBody(m_worldId, ref bodyDef);
+                
+                shapeDef.filter.categoryBits = 0x2;
+                shapeDef.filter.maskBits = 0x1;
                 b2CreateCircleShape(bodyId, ref shapeDef, ref circle);
 
                 B2Vec2 pivot = new B2Vec2((2.0f * m_count) * hx, m_count * hx);
@@ -94,6 +99,8 @@ public BallAndChain(SampleContext context) : base(context)
                 jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot);
                 jointDef.enableMotor = true;
                 jointDef.maxMotorTorque = m_frictionTorque;
+                jointDef.enableSpring = true;
+                jointDef.hertz = 4.0f;
                 m_jointIds[jointIndex++] = b2CreateRevoluteJoint(m_worldId, ref jointDef);
                 B2_ASSERT(jointIndex == m_count + 1);
             }

src/Box2D.NET.Samples/Samples/Joints/Bridge.cs

@@ -26,7 +26,10 @@ public class Bridge : Sample
     private B2BodyId[] m_bodyIds = new B2BodyId[m_count];
     private B2JointId[] m_jointIds = new B2JointId[m_count + 1];
     private float m_frictionTorque;
-    private float m_gravityScale;
+    private float m_constraintHertz;
+    private float m_constraintDampingRatio;
+    private float m_springHertz;
+    private float m_springDampingRatio;
 
     private static Sample Create(SampleContext context)
     {
@@ -48,15 +51,25 @@ public Bridge(SampleContext context) : base(context)
         }
 
         {
-            B2Polygon box = b2MakeBox(0.5f, 0.125f);
+            m_constraintHertz = 60.0f;
+            m_constraintDampingRatio = 0.0f;
+            m_springHertz = 2.0f;
+            m_springDampingRatio = 0.7f;
+            m_frictionTorque = 200.0f;
+
+            B2Polygon box = b2MakeBox( 0.5f, 0.125f );
 
             B2ShapeDef shapeDef = b2DefaultShapeDef();
             shapeDef.density = 20.0f;
 
             B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef();
+            jointDef.enableMotor = true;
+            jointDef.maxMotorTorque = m_frictionTorque;
+            jointDef.enableSpring = true;
+            jointDef.hertz = m_springHertz;
+            jointDef.dampingRatio = m_springDampingRatio;
+
             int jointIndex = 0;
-            m_frictionTorque = 200.0f;
-            m_gravityScale = 1.0f;
 
             float xbase = -80.0f;
 
@@ -76,8 +89,6 @@ public Bridge(SampleContext context) : base(context)
                 jointDef.bodyIdB = m_bodyIds[i];
                 jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot);
                 jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot);
-                jointDef.enableMotor = true;
-                jointDef.maxMotorTorque = m_frictionTorque;
                 m_jointIds[jointIndex++] = b2CreateRevoluteJoint(m_worldId, ref jointDef);
 
                 prevBodyId = m_bodyIds[i];
@@ -89,8 +100,6 @@ public Bridge(SampleContext context) : base(context)
                 jointDef.bodyIdB = groundId;
                 jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot);
                 jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot);
-                jointDef.enableMotor = true;
-                jointDef.maxMotorTorque = m_frictionTorque;
                 m_jointIds[jointIndex++] = b2CreateRevoluteJoint(m_worldId, ref jointDef);
 
                 B2_ASSERT(jointIndex == m_count + 1);
@@ -133,15 +142,14 @@ public override void UpdateGui()
     {
         base.UpdateGui();
 
-        float height = 80.0f;
+        float height = 180.0f;
         ImGui.SetNextWindowPos(new Vector2(10.0f, m_camera.m_height - height - 50.0f), ImGuiCond.Once);
-        ImGui.SetNextWindowSize(new Vector2(240.0f, height));
+        ImGui.SetNextWindowSize(new Vector2(320.0f, height));
 
         ImGui.Begin("Bridge", ImGuiWindowFlags.NoResize);
 
-        // Slider takes half the window
-        ImGui.PushItemWidth(ImGui.GetWindowWidth() * 0.5f);
-        bool updateFriction = ImGui.SliderFloat("Joint Friction", ref m_frictionTorque, 0.0f, 1000.0f, "%2.f");
+        ImGui.PushItemWidth(ImGui.GetWindowWidth() * 0.6f);
+        bool updateFriction = ImGui.SliderFloat("Joint Friction", ref m_frictionTorque, 0.0f, 10000.0f, "%2.f");
         if (updateFriction)
         {
             for (int i = 0; i <= m_count; ++i)
@@ -150,14 +158,40 @@ public override void UpdateGui()
             }
         }
 
-        if (ImGui.SliderFloat("Gravity scale", ref m_gravityScale, -1.0f, 1.0f, "%.1f"))
+        if ( ImGui.SliderFloat( "Spring hertz", ref m_springHertz, 0.0f, 30.0f, "%.0f" ) )
         {
-            for (int i = 0; i < m_count; ++i)
+            for ( int i = 0; i <= m_count; ++i )
             {
-                b2Body_SetGravityScale(m_bodyIds[i], m_gravityScale);
+                b2RevoluteJoint_SetSpringHertz( m_jointIds[i], m_springHertz );
             }
         }
 
+        if ( ImGui.SliderFloat( "Spring damping", ref m_springDampingRatio, 0.0f, 2.0f, "%.1f" ) )
+        {
+            for ( int i = 0; i <= m_count; ++i )
+            {
+                b2RevoluteJoint_SetSpringDampingRatio( m_jointIds[i], m_springDampingRatio );
+            }
+        }
+
+        if ( ImGui.SliderFloat( "Constraint hertz", ref m_constraintHertz, 15.0f, 240.0f, "%.0f" ) )
+        {
+            for ( int i = 0; i <= m_count; ++i )
+            {
+                b2Joint_SetConstraintTuning( m_jointIds[i], m_constraintHertz, m_constraintDampingRatio );
+            }
+        }
+
+        if ( ImGui.SliderFloat( "Constraint damping", ref m_constraintDampingRatio, 0.0f, 10.0f, "%.1f" ) )
+        {
+            for ( int i = 0; i <= m_count; ++i )
+            {
+                b2Joint_SetConstraintTuning( m_jointIds[i], m_constraintHertz, m_constraintDampingRatio );
+            }
+        }
+
+        ImGui.PopItemWidth();
+
         ImGui.End();
     }
 }

src/Box2D.NET.Samples/Samples/Joints/Door.cs

@@ -24,6 +24,8 @@ public class Door : Sample
     private B2JointId m_jointId;
     private float m_impulse;
     private float m_translationError;
+    private float m_jointHertz;
+    private float m_jointDampingRatio;
     private bool m_enableLimit;
 
     private static Sample Create(SampleContext context)
@@ -47,6 +49,10 @@ public Door(SampleContext context) : base(context)
         }
 
         m_enableLimit = true;
+        m_impulse = 50000.0f;
+        m_translationError = 0.0f;
+        m_jointHertz = 240.0f;
+        m_jointDampingRatio = 1.0f;
 
         {
             B2BodyDef bodyDef = b2DefaultBodyDef();
@@ -62,12 +68,11 @@ public Door(SampleContext context) : base(context)
             B2Polygon box = b2MakeBox(0.1f, 1.5f);
             b2CreatePolygonShape(m_doorId, ref shapeDef, ref box);
 
-            B2Vec2 pivot = new B2Vec2(0.0f, 0.0f);
             B2RevoluteJointDef jointDef = b2DefaultRevoluteJointDef();
             jointDef.bodyIdA = groundId;
             jointDef.bodyIdB = m_doorId;
-            jointDef.localAnchorA = b2Body_GetLocalPoint(jointDef.bodyIdA, pivot);
-            jointDef.localAnchorB = b2Body_GetLocalPoint(jointDef.bodyIdB, pivot);
+            jointDef.localAnchorA = new B2Vec2(0.0f, 0.0f);
+            jointDef.localAnchorB = new B2Vec2(0.0f, -1.5f);
             jointDef.targetAngle = 0.0f;
             jointDef.enableSpring = true;
             jointDef.hertz = 1.0f;
@@ -81,10 +86,8 @@ public Door(SampleContext context) : base(context)
             jointDef.enableLimit = m_enableLimit;
 
             m_jointId = b2CreateRevoluteJoint(m_worldId, ref jointDef);
+            b2Joint_SetConstraintTuning(m_jointId, m_jointHertz, m_jointDampingRatio);
         }
-
-        m_impulse = 50000.0f;
-        m_translationError = 0.0f;
     }
 
     public override void UpdateGui()
@@ -109,6 +112,16 @@ public override void UpdateGui()
             b2RevoluteJoint_EnableLimit(m_jointId, m_enableLimit);
         }
 
+        if ( ImGui.SliderFloat( "hertz", ref m_jointHertz, 15.0f, 480.0f, "%.0f" ) )
+        {
+            b2Joint_SetConstraintTuning( m_jointId, m_jointHertz, m_jointDampingRatio );
+        }
+
+        if ( ImGui.SliderFloat( "damping", ref m_jointDampingRatio, 0.0f, 10.0f, "%.1f" ) )
+        {
+            b2Joint_SetConstraintTuning( m_jointId, m_jointHertz, m_jointDampingRatio );
+        }
+        
         ImGui.End();
     }
 

src/Box2D.NET.Samples/Samples/Joints/GearLift.cs

@@ -79,7 +79,7 @@ public GearLift(SampleContext context) : base(context)
         float linkCount = 40;
         float doorHalfHeight = 1.5f;
 
-        B2Vec2 gearPosition1 = new B2Vec2(-4.25f, 10.25f);
+        B2Vec2 gearPosition1 = new B2Vec2(-4.25f, 9.75f);
         B2Vec2 gearPosition2 = gearPosition1 + new B2Vec2(2.0f, 1.0f);
         B2Vec2 linkAttachPosition = gearPosition2 + new B2Vec2(gearRadius + 2.0f * toothHalfWidth + toothRadius, 0.0f);
         B2Vec2 doorPosition = linkAttachPosition - new B2Vec2(0.0f, 2.0f * linkCount * linkHalfLength + doorHalfHeight);