From 27550f6de370a31af1dfac6f8615ee5411c6ba6f Mon Sep 17 00:00:00 2001 From: notgiven688 <37874600+notgiven688@users.noreply.github.com> Date: Wed, 14 Aug 2024 07:52:21 +0200 Subject: [PATCH] Add kinematic body demo (#165) --- src/Jitter2/Dynamics/RigidBody.cs | 4 +- src/JitterDemo/Demos/Demo22.cs | 131 ++++++++++++++++++++++++++ src/JitterDemo/Demos/Player/Player.cs | 29 ++---- src/JitterDemo/Playground.cs | 3 +- 4 files changed, 146 insertions(+), 21 deletions(-) create mode 100644 src/JitterDemo/Demos/Demo22.cs diff --git a/src/Jitter2/Dynamics/RigidBody.cs b/src/Jitter2/Dynamics/RigidBody.cs index 08742c38..8ba16449 100644 --- a/src/Jitter2/Dynamics/RigidBody.cs +++ b/src/Jitter2/Dynamics/RigidBody.cs @@ -430,10 +430,12 @@ public void AddForce(in JVector force) } /// - /// 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 . + /// 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 . /// /// The force to be applied. /// The position where the force will be applied. + [ReferenceFrame(ReferenceFrame.World)] public void AddForce(in JVector force, in JVector position) { ref RigidBodyData data = ref Data; diff --git a/src/JitterDemo/Demos/Demo22.cs b/src/JitterDemo/Demos/Demo22.cs new file mode 100644 index 00000000..d3d152b1 --- /dev/null +++ b/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(); + } +} \ No newline at end of file diff --git a/src/JitterDemo/Demos/Player/Player.cs b/src/JitterDemo/Demos/Player/Player.cs index b90c2de6..8de31c77 100644 --- a/src/JitterDemo/Demos/Player/Player.cs +++ b/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(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(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(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; - } } } \ No newline at end of file diff --git a/src/JitterDemo/Playground.cs b/src/JitterDemo/Playground.cs index 7a00185f..8102fa2c 100644 --- a/src/JitterDemo/Playground.cs +++ b/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;