[upstream] Draw forces (erincatto/box2d#988)

Added b2Body_ClearForces.
Joint and force scale for debug draw.

Author: ikpil

src/Box2D.NET.Samples/Draw.cs

@@ -67,9 +67,9 @@ public Draw()
         m_debugDraw.drawContacts = false;
         m_debugDraw.drawGraphColors = false;
         m_debugDraw.drawContactNormals = false;
-        m_debugDraw.drawContactImpulses = false;
+        m_debugDraw.drawContactForces = false;
         m_debugDraw.drawContactFeatures = false;
-        m_debugDraw.drawFrictionImpulses = false;
+        m_debugDraw.drawFrictionForces = false;
         m_debugDraw.drawIslands = false;
 
         m_debugDraw.context = this;

src/Box2D.NET.Samples/SampleApp.cs

@@ -789,21 +789,26 @@ private void UpdateUI()
 
                     ImGui.Separator();
 
-                    ImGui.Checkbox("Shapes", ref _context.settings.drawShapes);
-                    ImGui.Checkbox("Joints", ref _context.settings.drawJoints);
-                    ImGui.Checkbox("Joint Extras", ref _context.settings.drawJointExtras);
-                    ImGui.Checkbox("Bounds", ref _context.settings.drawBounds);
-                    ImGui.Checkbox("Contact Points", ref _context.settings.drawContactPoints);
-                    ImGui.Checkbox("Contact Normals", ref _context.settings.drawContactNormals);
-                    ImGui.Checkbox("Contact Impulses", ref _context.settings.drawContactImpulses);
-                    ImGui.Checkbox("Contact Features", ref _context.settings.drawContactFeatures);
-                    ImGui.Checkbox("Friction Impulses", ref _context.settings.drawFrictionImpulses);
-                    ImGui.Checkbox("Mass", ref _context.settings.drawMass);
-                    ImGui.Checkbox("Body Names", ref _context.settings.drawBodyNames);
-                    ImGui.Checkbox("Graph Colors", ref _context.settings.drawGraphColors);
-                    ImGui.Checkbox("Islands", ref _context.settings.drawIslands);
-                    ImGui.Checkbox("Counters", ref _context.settings.drawCounters);
-                    ImGui.Checkbox("Profile", ref _context.settings.drawProfile);
+                    ImGui.Checkbox( "Shapes", ref _context.settings.drawShapes );
+                    ImGui.Checkbox( "Joints", ref _context.settings.drawJoints );
+                    ImGui.Checkbox( "Joint Extras", ref _context.settings.drawJointExtras );
+                    ImGui.Checkbox( "Bounds", ref _context.settings.drawBounds );
+                    ImGui.Checkbox( "Contact Points", ref _context.settings.drawContactPoints );
+                    ImGui.Checkbox( "Contact Normals", ref _context.settings.drawContactNormals );
+                    ImGui.Checkbox( "Contact Features", ref _context.settings.drawContactFeatures );
+                    ImGui.Checkbox( "Contact Forces", ref _context.settings.drawContactForces );
+                    ImGui.Checkbox( "Friction Forces", ref _context.settings.drawFrictionForces );
+                    ImGui.Checkbox( "Mass", ref _context.settings.drawMass );
+                    ImGui.Checkbox( "Body Names", ref _context.settings.drawBodyNames );
+                    ImGui.Checkbox( "Graph Colors", ref _context.settings.drawGraphColors );
+                    ImGui.Checkbox( "Islands", ref _context.settings.drawIslands );
+                    ImGui.Checkbox( "Counters", ref _context.settings.drawCounters );
+                    ImGui.Checkbox( "Profile", ref _context.settings.drawProfile );
+
+                    ImGui.PushItemWidth( 80.0f );
+                    ImGui.InputFloat( "Joint Scale", ref _context.settings.jointScale );
+                    ImGui.InputFloat( "Force Scale", ref _context.settings.forceScale );
+                    ImGui.PopItemWidth();
 
                     Vector2 button_sz = new Vector2(-1, 0);
                     if (ImGui.Button("Pause (P)", button_sz))

src/Box2D.NET.Samples/Samples/Issues/UnstableJoints.cs

@@ -0,0 +1,102 @@
+// SPDX-FileCopyrightText: 2025 Erin Catto
+// SPDX-FileCopyrightText: 2025 Ikpil Choi(ikpil@naver.com)
+// SPDX-License-Identifier: MIT
+
+using static Box2D.NET.B2Types;
+using static Box2D.NET.B2Bodies;
+using static Box2D.NET.B2Shapes;
+using static Box2D.NET.B2Joints;
+
+namespace Box2D.NET.Samples.Samples.Issues;
+
+public class UnstableJoints : Sample
+{
+    private static readonly int SamplePrismaticJointCrash = SampleFactory.Shared.RegisterSample("Issues", "Unstable Joints", Create);
+
+    private static Sample Create(SampleContext context)
+    {
+        return new UnstableJoints(context);
+    }
+
+    public UnstableJoints(SampleContext context) : base(context)
+    {
+        if (m_context.settings.restart == false)
+        {
+            m_context.camera.m_center = new B2Vec2(0.0f, 1.75f);
+            m_context.camera.m_zoom = 32.0f;
+        }
+
+        {
+            B2BodyDef bodyDef = b2DefaultBodyDef();
+            B2BodyId groundId = b2CreateBody(m_worldId, ref bodyDef);
+
+            B2ShapeDef shapeDef = b2DefaultShapeDef();
+            B2Segment segment = new B2Segment(new B2Vec2(-100.0f, 0.0f), new B2Vec2(100.0f, 0.0f));
+            b2CreateSegmentShape(groundId, ref shapeDef, ref segment);
+        }
+
+        B2BodyId centerId;
+        {
+            B2BodyDef bd = b2DefaultBodyDef();
+            bd.type = B2BodyType.b2_dynamicBody;
+            bd.position = new B2Vec2(0, 3);
+            centerId = b2CreateBody(m_worldId, ref bd);
+
+            B2ShapeDef sd = b2DefaultShapeDef();
+
+            B2Circle circle;
+            circle.center = new B2Vec2(0, 0);
+
+            // Note: this will crash due to divergence (inf/nan) with a radius of 0.1
+            //circle.radius = 0.1f;
+            circle.radius = 0.5f;
+
+            b2CreateCircleShape(centerId, ref sd, ref circle);
+        }
+
+        B2PrismaticJointDef jd = b2DefaultPrismaticJointDef();
+        jd.enableSpring = true;
+        jd.hertz = 10.0f;
+        jd.dampingRatio = 2.0f;
+
+        {
+            B2BodyDef bd = b2DefaultBodyDef();
+            bd.type = B2BodyType.b2_dynamicBody;
+            bd.position = new B2Vec2(-3.5f, 3);
+
+            B2BodyId leftId = b2CreateBody(m_worldId, ref bd);
+
+            B2ShapeDef sd = b2DefaultShapeDef();
+
+            B2Circle circle;
+            circle.center = new B2Vec2(0, 0);
+            circle.radius = 2.0f;
+            b2CreateCircleShape(leftId, ref sd, ref circle);
+
+            jd.@base.bodyIdA = centerId;
+            jd.@base.bodyIdB = leftId;
+            jd.targetTranslation = -3.0f;
+            b2CreatePrismaticJoint(m_worldId, ref jd);
+        }
+
+        {
+            B2BodyDef bd = b2DefaultBodyDef();
+            bd.type = B2BodyType.b2_dynamicBody;
+            bd.position = new B2Vec2(3.5f, 3);
+            B2BodyId rightId = b2CreateBody(m_worldId, ref bd);
+
+            B2ShapeDef sd = b2DefaultShapeDef();
+
+            B2Circle circle;
+            circle.center = new B2Vec2(0, 0);
+            circle.radius = 2.0f;
+
+            b2CreateCircleShape(rightId, ref sd, ref circle);
+
+            jd.@base.bodyIdA = centerId;
+            jd.@base.bodyIdB = rightId;
+            jd.targetTranslation = 3.0f;
+            b2CreatePrismaticJoint(m_worldId, ref jd);
+        }
+    }
+}

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

@@ -486,6 +486,8 @@ public virtual void Draw(Settings settings)
         }
 
         m_context.draw.m_debugDraw.drawingBounds = m_camera.GetViewBounds();
+        m_context.draw.m_debugDraw.jointScale = settings.jointScale;
+        m_context.draw.m_debugDraw.forceScale = settings.forceScale;
         m_context.draw.m_debugDraw.drawShapes = settings.drawShapes;
         m_context.draw.m_debugDraw.drawJoints = settings.drawJoints;
         m_context.draw.m_debugDraw.drawJointExtras = settings.drawJointExtras;
@@ -495,9 +497,9 @@ public virtual void Draw(Settings settings)
         m_context.draw.m_debugDraw.drawContacts = settings.drawContactPoints;
         m_context.draw.m_debugDraw.drawGraphColors = settings.drawGraphColors;
         m_context.draw.m_debugDraw.drawContactNormals = settings.drawContactNormals;
-        m_context.draw.m_debugDraw.drawContactImpulses = settings.drawContactImpulses;
+        m_context.draw.m_debugDraw.drawContactForces = settings.drawContactForces;
         m_context.draw.m_debugDraw.drawContactFeatures = settings.drawContactFeatures;
-        m_context.draw.m_debugDraw.drawFrictionImpulses = settings.drawFrictionImpulses;
+        m_context.draw.m_debugDraw.drawFrictionForces = settings.drawFrictionForces;
         m_context.draw.m_debugDraw.drawIslands = settings.drawIslands;
 
         b2World_Draw(m_worldId, m_context.draw.m_debugDraw);

src/Box2D.NET.Samples/Settings.cs

@@ -14,36 +14,40 @@ public class Settings
     public const int MAX_TOKENS = 32;
     public const string fileName = "settings.ini";
 
-    public int sampleIndex = 0;
     public int windowWidth = 1920;
     public int windowHeight = 1080;
 
     public float uiScale = 1.0f;
     public float hertz = 60.0f;
+    public float jointScale = 1.0f;
+    public float forceScale = 1.0f;
     public int subStepCount = 4;
     public int workerCount = 1;
-
-    public bool drawShapes = true;
-    public bool drawJoints = true;
+    
+    public bool restart = false;
+    public bool pause = false;
+    public bool singleStep = false;
     public bool drawJointExtras = false;
     public bool drawBounds = false;
     public bool drawMass = false;
     public bool drawBodyNames = false;
     public bool drawContactPoints = false;
     public bool drawContactNormals = false;
-    public bool drawContactImpulses = false;
     public bool drawContactFeatures = false;
-    public bool drawFrictionImpulses = false;
+    public bool drawContactForces = false;
+    public bool drawFrictionForces = false;
     public bool drawIslands = false;
     public bool drawGraphColors = false;
     public bool drawCounters = false;
     public bool drawProfile = false;
     public bool enableWarmStarting = true;
     public bool enableContinuous = true;
     public bool enableSleep = true;
-    public bool pause = false;
-    public bool singleStep = false;
-    public bool restart = false;
+    
+    // These are persisted
+    public int sampleIndex = 0;
+    public bool drawShapes = true;
+    public bool drawJoints = true;
 
     public void Save()
     {
@@ -80,9 +84,9 @@ public void CopyFrom(Settings other)
         drawBodyNames = other.drawBodyNames;
         drawContactPoints = other.drawContactPoints;
         drawContactNormals = other.drawContactNormals;
-        drawContactImpulses = other.drawContactImpulses;
+        drawContactForces = other.drawContactForces;
         drawContactFeatures = other.drawContactFeatures;
-        drawFrictionImpulses = other.drawFrictionImpulses;
+        drawFrictionForces = other.drawFrictionForces;
         drawIslands = other.drawIslands;
         drawGraphColors = other.drawGraphColors;
         drawCounters = other.drawCounters;

src/Box2D.NET/B2Bodies.cs

@@ -24,6 +24,9 @@ namespace Box2D.NET
 {
     public static class B2Bodies
     {
+        // Length of body debug name
+        public const int B2_NAME_LENGTH = 32;
+
         // Identity body state, notice the deltaRotation is {1, 0}
         public static readonly B2BodyState b2_identityBodyState = new B2BodyState()
         {
@@ -921,6 +924,7 @@ public static void b2Body_SetTargetTransform(B2BodyId bodyId, B2Transform target
             state.angularVelocity = angularVelocity;
         }
 
+        /// Get the linear velocity of a local point attached to a body. Usually in meters per second.
         public static B2Vec2 b2Body_GetLocalPointVelocity(B2BodyId bodyId, B2Vec2 localPoint)
         {
             B2World world = b2GetWorld(bodyId.world0);
@@ -939,6 +943,7 @@ public static B2Vec2 b2Body_GetLocalPointVelocity(B2BodyId bodyId, B2Vec2 localP
             return v;
         }
 
+        /// Get the linear velocity of a world point attached to a body. Usually in meters per second.
         public static B2Vec2 b2Body_GetWorldPointVelocity(B2BodyId bodyId, B2Vec2 worldPoint)
         {
             B2World world = b2GetWorld(bodyId.world0);
@@ -957,6 +962,13 @@ public static B2Vec2 b2Body_GetWorldPointVelocity(B2BodyId bodyId, B2Vec2 worldP
             return v;
         }
 
+        /// Apply a force at a world point. If the force is not applied at the center of mass,
+        /// it will generate a torque and affect the angular velocity. This optionally wakes up the body.
+        /// The force is ignored if the body is not awake.
+        /// @param bodyId The body id
+        /// @param force The world force vector, usually in newtons (N)
+        /// @param point The world position of the point of application
+        /// @param wake Option to wake up the body
         public static void b2Body_ApplyForce(B2BodyId bodyId, B2Vec2 force, B2Vec2 point, bool wake)
         {
             B2World world = b2GetWorld(bodyId.world0);
@@ -980,6 +992,11 @@ public static void b2Body_ApplyForce(B2BodyId bodyId, B2Vec2 force, B2Vec2 point
             }
         }
 
+        /// Apply a force to the center of mass. This optionally wakes up the body.
+        /// The force is ignored if the body is not awake.
+        /// @param bodyId The body id
+        /// @param force the world force vector, usually in newtons (N).
+        /// @param wake also wake up the body
         public static void b2Body_ApplyForceToCenter(B2BodyId bodyId, B2Vec2 force, bool wake)
         {
             B2World world = b2GetWorld(bodyId.world0);
@@ -1002,6 +1019,11 @@ public static void b2Body_ApplyForceToCenter(B2BodyId bodyId, B2Vec2 force, bool
             }
         }
 
+        /// Apply a torque. This affects the angular velocity without affecting the linear velocity.
+        /// This optionally wakes the body. The torque is ignored if the body is not awake.
+        /// @param bodyId The body id
+        /// @param torque about the z-axis (out of the screen), usually in N*m.
+        /// @param wake also wake up the body
         public static void b2Body_ApplyTorque(B2BodyId bodyId, float torque, bool wake)
         {
             B2World world = b2GetWorld(bodyId.world0);
@@ -1024,6 +1046,20 @@ public static void b2Body_ApplyTorque(B2BodyId bodyId, float torque, bool wake)
             }
         }
 
+        /// Clear the force and torque on this body. Forces and torques are automatically cleared after each world
+        /// step. So this only needs to be called if the application wants to remove the effect of previous
+        /// calls to apply forces and torques before the world step is called.
+        /// @param bodyId The body id
+        public static void b2Body_ClearForces(B2BodyId bodyId)
+        {
+            B2World world = b2GetWorld(bodyId.world0);
+            B2Body body = b2GetBodyFullId(world, bodyId);
+            B2BodySim bodySim = b2GetBodySim(world, body);
+            bodySim.force = b2Vec2_zero;
+            bodySim.torque = 0.0f;
+        }
+
+
         /// Apply an impulse at a point. This immediately modifies the velocity.
         /// It also modifies the angular velocity if the point of application
         /// is not at the center of mass. This optionally wakes the body.
@@ -1365,7 +1401,7 @@ public static void b2Body_SetName(B2BodyId bodyId, string name)
             }
             else
             {
-                body.name = "";
+                body.name = string.Empty;
             }
         }
 

src/Box2D.NET/B2ConstraintGraphs.cs

@@ -365,5 +365,13 @@ public static void b2RemoveJointFromGraph(B2World world, int bodyIdA, int bodyId
                 movedJoint.localIndex = localIndex;
             }
         }
+
+        public static readonly B2HexColor[] b2_graphColors = new B2HexColor[]
+        {
+            B2HexColor.b2_colorRed, B2HexColor.b2_colorOrange, B2HexColor.b2_colorYellow, B2HexColor.b2_colorGreen, B2HexColor.b2_colorCyan, B2HexColor.b2_colorBlue,
+            B2HexColor.b2_colorViolet, B2HexColor.b2_colorPink, B2HexColor.b2_colorChocolate, B2HexColor.b2_colorGoldenRod, B2HexColor.b2_colorCoral, B2HexColor.b2_colorRosyBrown,
+            B2HexColor.b2_colorAqua, B2HexColor.b2_colorPeru, B2HexColor.b2_colorLime, B2HexColor.b2_colorGold, B2HexColor.b2_colorPlum, B2HexColor.b2_colorSnow,
+            B2HexColor.b2_colorTeal, B2HexColor.b2_colorKhaki, B2HexColor.b2_colorSalmon, B2HexColor.b2_colorPeachPuff, B2HexColor.b2_colorHoneyDew, B2HexColor.b2_colorBlack,
+        };
     }
 }

src/Box2D.NET/B2DebugDraw.cs

@@ -19,8 +19,14 @@ public class B2DebugDraw
         public DrawPointFcn DrawPointFcn;
         public DrawStringFcn DrawStringFcn;
 
-        /// Bounds to use if restricting drawing to a rectangular region
+        /// World bounds to use for debug draw
         public B2AABB drawingBounds;
+        
+        /// Scale to use when drawing forces
+        public float forceScale;
+
+        /// Global scaling for joint drawing
+        public float jointScale;
 
         /// Option to draw shapes
         public bool drawShapes;
@@ -46,17 +52,17 @@ public class B2DebugDraw
         /// Option to visualize the graph coloring used for contacts and joints
         public bool drawGraphColors;
 
+        /// Option to draw contact feature ids
+        public bool drawContactFeatures;
+
         /// Option to draw contact normals
         public bool drawContactNormals;
 
-        /// Option to draw contact normal impulses
-        public bool drawContactImpulses;
-
-        /// Option to draw contact feature ids
-        public bool drawContactFeatures;
+        /// Option to draw contact normal forces
+        public bool drawContactForces;
 
-        /// Option to draw contact friction impulses
-        public bool drawFrictionImpulses;
+        /// Option to draw contact friction forces
+        public bool drawFrictionForces;
 
         /// Option to draw islands as bounding boxes
         public bool drawIslands;