Add kinematic body demo (#165)

src/Jitter2/Dynamics/RigidBody.cs

@@ -430,10 +430,12 @@ public void AddForce(in JVector force)
     }
 
     /// <summary>
-    /// Applies a force to the rigid body, altering its velocity. This force is applied for a single frame only and is reset to zero with the subsequent call to <see cref="World.Step(float, bool)"/>.
+    /// Applies a force to the rigid body, altering its velocity. This force is applied for a single frame only and is
+    /// reset to zero with the subsequent call to <see cref="World.Step(float, bool)"/>.
     /// </summary>
     /// <param name="force">The force to be applied.</param>
     /// <param name="position">The position where the force will be applied.</param>
+    [ReferenceFrame(ReferenceFrame.World)]
     public void AddForce(in JVector force, in JVector position)
     {
         ref RigidBodyData data = ref Data;

src/JitterDemo/Demos/Demo22.cs

@@ -0,0 +1,131 @@
+using System;
+using Jitter2;
+using Jitter2.Collision.Shapes;
+using Jitter2.Dynamics;
+using Jitter2.Dynamics.Constraints;
+using Jitter2.LinearMath;
+using JitterDemo.Renderer;
+using JitterDemo.Renderer.OpenGL;
+
+namespace JitterDemo;
+
+public class Demo22 : IDemo
+{
+    private static class Curve
+    {
+        public static JVector Path(float time)
+        {
+            return new JVector(8.5f * MathF.Sin((float)time * 0.5f),
+                14 + MathF.Cos(time * 0.5f) * 5.0f, 0);
+        }
+
+        public static JVector Derivative(float time)
+        {
+            return new JVector(4.25f * MathF.Cos((float)time * 0.5f),
+                -2.5f * MathF.Sin(time * 0.5f), 0);
+        }
+    }
+
+    public string Name => "Kinematic bodies";
+
+    private Playground pg = null!;
+    private World world = null!;
+    private Player player = null!;
+
+    private RigidBody platform = null!;
+
+    public void Build()
+    {
+        pg = (Playground)RenderWindow.Instance;
+        world = pg.World;
+
+        pg.ResetScene(true);
+
+        var leftPlank = world.CreateRigidBody();
+        leftPlank.AddShape(new BoxShape(20,0.1f,6));
+        leftPlank.IsStatic = true;
+
+        var rightPlank = world.CreateRigidBody();
+        rightPlank.AddShape(new BoxShape(20,0.1f,6));
+        rightPlank.IsStatic = true;
+
+        leftPlank.Position = new JVector(-21, 13, 0);
+        rightPlank.Position = new JVector(21, 16, 0);
+
+        // (*) The mass is specified as the inverse mass, so an inverse mass of 0.01 corresponds to a mass of 100.
+        // Lower the inverse mass (i.e., increase the mass) to make the body behave more kinematically,
+        // akin to an unstoppable object. However, be cautious when setting the inverse mass to zero (infinite mass),
+        // as it can cause the solver to fail if a collision occurs between an unstoppable object (e.g., a platform)
+        // and an immovable object (e.g., a static object).
+
+        platform = world.CreateRigidBody();
+        platform.AddShape(new BoxShape(4,0.1f,6));
+        platform.AddShape(new SphereShape(0.2f)); // guide to the eye
+        platform.SetMassInertia(JMatrix.Zero, 0.01f, true); // (*)
+        platform.AffectedByGravity = false;
+        platform.Position = new JVector(0, 12, 0);
+
+        player = new Player(world, new JVector(-20, 15, 0));
+        player.Body.Orientation = JQuaternion.CreateRotationY(-MathF.PI / 2.0f);
+    }
+
+    public void Draw()
+    {
+        // What is happening in the following *two* lines of code?
+        //
+        // We want to platform to follow a path p(t). We can not set the position
+        // directly, as it will just teleport the body in tiny steps, which
+        // does not produce the kinematic physics we are after.
+        //
+        // We introduce k(t) and manually set the velocity of the platform to be
+        //
+        // v(t) = alpha(k(t) - x(t))
+        // <=> x'(t) = alpha(k(t) - x(t))
+        // <=> 1/alpha x'(t) + x(t) = k(t)
+        //
+        // where alpha is a constant.
+        //
+        // If we now set k(t) to be
+        //  ______________________________
+        // | k(t) = 1/alpha p'(t) + p(t) |
+        // -------------------------------
+        // , we arrive at
+        //
+        // x(t) = C*exp(-alpha*t) + p(t)
+        //
+        // where the first term describes the offset of the target path p(t)
+        // and the position of the platform x(t). This term vanishes with larger t.
+
+        JVector k = Curve.Derivative((float)pg.Time) + Curve.Path((float)pg.Time);
+        platform.Velocity = k - platform.Position;
+
+        // Draw the curve
+
+        const int stepMax = 100;
+        const float maxTime = 4.0f * MathF.PI;
+
+        for (int step = 0; step < stepMax; step++)
+        {
+            float ta = maxTime / stepMax * step;
+            float tb = maxTime / stepMax * (step + 1);
+
+            pg.DebugRenderer.PushLine(DebugRenderer.Color.Green,
+                Conversion.FromJitter(Curve.Path(ta)),
+                Conversion.FromJitter(Curve.Path(tb)));
+        }
+
+        // Player handling with keyboard
+
+        Keyboard kb = Keyboard.Instance;
+
+        if (kb.IsKeyDown(Keyboard.Key.Left)) player.SetAngularInput(-1.0f);
+        else if (kb.IsKeyDown(Keyboard.Key.Right)) player.SetAngularInput(1.0f);
+        else player.SetAngularInput(0.0f);
+
+        if (kb.IsKeyDown(Keyboard.Key.Up)) player.SetLinearInput(-JVector.UnitZ);
+        else if (kb.IsKeyDown(Keyboard.Key.Down)) player.SetLinearInput(JVector.UnitZ);
+        else player.SetLinearInput(JVector.Zero);
+
+        if (kb.IsKeyDown(Keyboard.Key.LeftControl)) player.Jump();
+    }
+}

src/JitterDemo/Demos/Player/Player.cs

@@ -12,7 +12,6 @@ namespace JitterDemo;
 public class Player
 {
     public RigidBody Body { get; }
-    public LinearMotor FrictionMotor { get; }
     public AngularMotor AngularMovement { get; }
 
     private readonly float capsuleHalfHeight;
@@ -48,11 +47,8 @@ public Player(World world, JVector position)
         var ur = world.CreateConstraint<HingeAngle>(Body, world.NullBody);
         ur.Initialize(JVector.UnitY, AngularLimit.Full);
 
-        // Add a "motor" to the body. The motor target velocity is zero.
-        // This acts like friction and stops the player.
-        FrictionMotor = world.CreateConstraint<LinearMotor>(Body, world.NullBody);
-        FrictionMotor.Initialize(JVector.UnitZ, JVector.UnitX);
-        FrictionMotor.MaximumForce = 10;
+        // Add some friction
+        Body.Friction = 0.8f;
 
         // An angular motor for turning.
         AngularMovement = world.CreateConstraint<AngularMotor>(Body, world.NullBody);
@@ -203,9 +199,8 @@ public void Jump()
 
     public void SetLinearInput(JVector deltaMove)
     {
-        if (!CanJump(out _, out _))
+        if (!CanJump(out var floor, out JVector hitpoint))
         {
-            FrictionMotor.IsEnabled = false;
             return;
         }
 
@@ -222,19 +217,15 @@ public void SetLinearInput(JVector deltaMove)
         {
             if (bodyVelLen < 5f)
             {
-                Body.AddForce(JVector.Transform(deltaMove, Body.Orientation) * 10);
+                var force = JVector.Transform(deltaMove, Body.Orientation) * 10.0f;
+
+                Body.AddForce(force);
+
+                // follow Newton's law (for once) and add a force
+                // with equal magnitude in the opposite direction.
+                floor!.AddForce(-force, Body.Position + hitpoint);
             }
         }
 
-        if (bodyVelLen > 0.01f)
-        {
-            FrictionMotor.LocalAxis1 = JVector.TransposedTransform(bodyVel * (1.0f / bodyVelLen), Body.Orientation);
-            FrictionMotor.TargetVelocity = 0;
-            FrictionMotor.IsEnabled = true;
-        }
-        else
-        {
-            FrictionMotor.IsEnabled = false;
-        }
     }
 }

src/JitterDemo/Playground.cs

@@ -51,7 +51,8 @@ public partial class Playground : RenderWindow
         // new Demo18(),  // point test
         // new Demo19(),  // ray cast test
         new Demo20(),
-        new Demo21()
+        new Demo21(),
+        new Demo22()
     };
 
     private IDemo? currentDemo;