Add test thruster

src/math/Ray.cs

@@ -0,0 +1,128 @@
+using Godot;
+
+namespace Quadratic.Carto.MathExt;
+
+public struct Ray
+{
+    public Vector3 origin;
+    public Vector3 direction;
+
+    /// <summary>
+    /// Creates a new ray, using origin and <b>normalized</b> direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, normalized.</param>
+    public Ray(Vector3 origin, Vector3 direction)
+    {
+        this.origin = origin;
+        this.direction = direction;
+    }
+
+    /// <summary>
+    /// Creates a new ray, using start and end points.
+    /// </summary>
+    /// <param name="start">The start point vector.</param>
+    /// <param name="end">The end point vector.</param>
+    public static Ray FromStartAndEnd(Vector3 start, Vector3 end)
+    {
+        return new Ray(start, (end - start).Normalized());
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and non-normalized direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, non-normalized.</param>
+    public static Ray FromOriginAndDirection(Vector3 origin, Vector3 direction)
+    {
+        return new Ray(origin, direction.Normalized());
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and normalized direction.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="direction">The direction vector, normalized.</param>
+    public static Ray FromOriginAndNormalizedDirection(Vector3 origin, Vector3 direction)
+    {
+        return new Ray(origin, direction);
+    }
+
+    /// <summary>
+    /// Creates a new ray, using origin and rotation quaternion.
+    /// </summary>
+    /// <param name="origin">The origin vector.</param>
+    /// <param name="rotation">The rotation quaternion.</param>
+    public static Ray FromOriginAndRotation(Vector3 origin, Quaternion rotation)
+    {
+        return new Ray(origin, rotation * Vector3.Forward);
+    }
+
+    /// <summary>
+    /// Get the point at the given distance along the ray.
+    /// </summary>
+    /// <param name="distance">The distance along the ray.</param>
+    public readonly Vector3 GetPoint(float distance)
+    {
+        return this.origin + this.direction * distance;
+    }
+
+    /// <summary>
+    /// Returns the point along the ray that is closest to the given point.
+    /// </summary>
+    /// <param name="point"></param>
+    /// <returns></returns>
+    public readonly float MinDistancePoint(Vector3 point)
+    {
+        /*
+             The minimum distance between a ray and a point:
+
+                                  d: direction
+             ------+--------------<----* O: origin
+                   |                    \
+                   * P                   * P'
+
+             If the line between origin and target point (OP) is on the same
+             side as d, the distance is the magnitude of the vertical line
+             between P and Od. Else (OP'), the distance is simply the distance 
+             between O and P.
+         */
+
+        var op = point - this.origin; // vector OP
+        var opd = op.Dot(this.direction); // OP dot d is the projection of OP onto d
+        if (opd < 0) // OP' case
+        {
+            return op.Length();
+        }
+        else // OP case
+        {
+            var od = this.direction * opd; // vector Od
+            return (op - od).Length();
+        }
+    }
+
+    public readonly Vector3 ClosestPoint(Vector3 point)
+    {
+        var pointDist = MinDistancePoint(point);
+        return GetPoint(pointDist);
+    }
+
+    public readonly Ray Transform(Transform3D transform)
+    {
+        return new Ray(
+            transform.Origin + transform.Basis * this.origin,
+            (transform.Basis * this.direction).Normalized()
+        );
+    }
+
+    public readonly Ray InverseTransform(Transform3D transform)
+    {
+        var inv = transform.Inverse();
+        return Transform(inv);
+    }
+
+    public override readonly string ToString()
+    {
+        return $"Ray({this.origin}, {this.direction})";
+    }
+}

src/math/VectorsExt.Conversion.cs

@@ -0,0 +1,48 @@
+namespace Quadratic.Carto.MathExt;
+
+
+public static partial class VectorsExt
+{
+    public static Godot.Vector3 AsGodot(this System.Numerics.Vector3 v)
+    {
+        return new Godot.Vector3(v.X, v.Y, v.Z);
+    }
+
+    public static System.Numerics.Vector3 AsSystem(this Godot.Vector3 v)
+    {
+        return new System.Numerics.Vector3(v.X, v.Y, v.Z);
+    }
+
+    public static Godot.Vector2 AsGodot(this System.Numerics.Vector2 v)
+    {
+        return new Godot.Vector2(v.X, v.Y);
+    }
+
+    public static System.Numerics.Vector2 AsSystem(this Godot.Vector2 v)
+    {
+        return new System.Numerics.Vector2(v.X, v.Y);
+    }
+
+    public static Godot.Quaternion AsGodot(this System.Numerics.Quaternion q)
+    {
+        return new Godot.Quaternion(q.X, q.Y, q.Z, q.W);
+    }
+
+    public static System.Numerics.Quaternion AsSystem(this Godot.Quaternion q)
+    {
+        return new System.Numerics.Quaternion(q.X, q.Y, q.Z, q.W);
+    }
+
+    public static Godot.Vector4 AsGodot(this System.Numerics.Vector4 v)
+    {
+        return new Godot.Vector4(v.X, v.Y, v.Z, v.W);
+    }
+
+    public static System.Numerics.Vector4 AsSystem(this Godot.Vector4 v)
+    {
+        return new System.Numerics.Vector4(v.X, v.Y, v.Z, v.W);
+    }
+}
+
+
+

src/math/VectorsExt.Math.cs

@@ -0,0 +1,99 @@
+using Godot;
+
+namespace Quadratic.Carto.MathExt;
+
+public static partial class VectorsExt
+{
+    public static Quaternion FromToRotation(Vector3 from, Vector3 to)
+    {
+        var fromDotTo = from.Dot(to);
+        if (Mathf.IsEqualApprox(fromDotTo, 1))
+        {
+            return Quaternion.Identity;
+        }
+        else if (Mathf.IsEqualApprox(fromDotTo, -1))
+        {
+            return new Quaternion(Vector3.Right, Mathf.Pi);
+        }
+        var axis = from.Cross(to).Normalized();
+        var angle = Mathf.Acos(fromDotTo);
+        return new Quaternion(axis, angle);
+    }
+
+    /// <summary>
+    /// Calculates the closest distance between two lines, defined by origin and direction. This
+    /// method assumes normalized direction vectors.
+    /// </summary>
+    /// <param name="aOrigin"></param>
+    /// <param name="aDirection"></param>
+    /// <param name="bOrigin"></param>
+    /// <param name="bDirection"></param>
+    public static float ClosestDistanceBetweenLines(
+        Vector3 aOrigin,
+        Vector3 aDirection,
+        Vector3 bOrigin,
+        Vector3 bDirection)
+    {
+        // Rule out the case where the lines are parallel.
+        float aDotB = aDirection.Dot(bDirection);
+        if (Mathf.IsEqualApprox(aDotB, 1))
+        {
+            // Lines are parallel
+            var aToB = bOrigin - aOrigin;
+            var aPerp = aToB - aDirection * aToB.Dot(aDirection);
+            return aPerp.Length();
+        }
+        else
+        {
+            // The vector that's perpendicular to both lines.
+            // Since both lines are normalized, this vector is also normalized.
+            var normalAB = aDirection.Cross(bDirection);
+            var aToB = bOrigin - aOrigin;
+            return aToB.Dot(normalAB);
+        }
+    }
+
+    /// <summary>
+    /// Calculates the closest distance and points between two lines, defined by origin and
+    /// direction. This method assumes normalized direction vectors.
+    /// </summary>
+    /// <param name="aOrigin"></param>
+    /// <param name="aDirection"></param>
+    /// <param name="bOrigin"></param>
+    /// <param name="bDirection"></param>
+    /// <param name="aClosest"></param>
+    /// <param name="bClosest"></param>
+    public static float ClosestDistanceBetweenLines(
+        Vector3 aOrigin,
+        Vector3 aDirection,
+        Vector3 bOrigin,
+        Vector3 bDirection,
+        out Vector3 aClosest,
+        out Vector3 bClosest)
+    {
+        // Rule out the case where the lines are parallel.
+        float aDotB = aDirection.Dot(bDirection);
+        if (Mathf.IsEqualApprox(aDotB, 1))
+        {
+            // Lines are parallel
+            var aToB = bOrigin - aOrigin;
+            var aPerp = aToB - aDirection * aToB.Dot(aDirection);
+            aClosest = aOrigin;
+            bClosest = bOrigin + aPerp;
+            return aPerp.Length();
+        }
+        else
+        {
+            // https://en.wikipedia.org/wiki/Skew_lines#Nearest_Points
+            // The vector that's perpendicular to both lines.
+            // Since both lines are normalized, this vector is also normalized.
+            var n = aDirection.Cross(bDirection);
+            var n1 = aDirection.Cross(n);
+            var n2 = bDirection.Cross(n);
+            var aToB = bOrigin - aOrigin;
+            aClosest = aOrigin + aToB.Dot(n2) / aDirection.Dot(n2) * aDirection;
+            bClosest = bOrigin + aToB.Dot(n1) / bDirection.Dot(n1) * bDirection;
+            return aToB.Dot(n);
+        }
+    }
+}

src/testing/TestThruster.cs

@@ -0,0 +1,113 @@
+using Godot;
+using ImGuiNET;
+using Quadratic.Carto.MathExt;
+
+namespace Quadratic.Carto.Testing;
+
+public partial class TestThruster : RigidBody3D
+{
+    /// <summary>
+    /// Throttle level between 0 and 1
+    /// </summary>
+    public float Throttle
+    {
+        get => _throttle; set
+        {
+            _throttle = Mathf.Clamp(value, 0.0f, 1.0f);
+        }
+    }
+    float _throttle = 0.0f;
+
+    Vector3 torque = new Vector3();
+
+    bool stabilize = false;
+    bool stabilizeDebounce = false;
+
+    public override void _Process(double delta)
+    {
+        if (Input.IsKeyPressed(Key.Shift))
+        {
+            Throttle += 0.1f * (float)delta;
+        }
+        else if (Input.IsKeyPressed(Key.Ctrl))
+        {
+            Throttle -= 0.1f * (float)delta;
+        }
+
+        torque = Vector3.Zero;
+        if (Input.IsKeyPressed(Key.W))
+        {
+            torque += new Vector3(-1.0f, 0.0f, 0.0f); // Forward (X-)
+        }
+        if (Input.IsKeyPressed(Key.S))
+        {
+            torque += new Vector3(1.0f, 0.0f, 0.0f); // Backward (X+)
+        }
+        if (Input.IsKeyPressed(Key.A))
+        {
+            torque += new Vector3(0.0f, 0.0f, 1.0f); // Left (Z+)
+        }
+        if (Input.IsKeyPressed(Key.D))
+        {
+            torque += new Vector3(0.0f, 0.0f, -1.0f); // Right (Z-)
+        }
+        if (Input.IsKeyPressed(Key.Q))
+        {
+            torque += new Vector3(0.0f, 1.0f, 0.0f); // Up (Y+)
+        }
+        if (Input.IsKeyPressed(Key.E))
+        {
+            torque += new Vector3(0.0f, -1.0f, 0.0f); // Down (Y-)
+        }
+        torque *= 0.01f;
+
+        if (Input.IsKeyPressed(Key.T) && !stabilizeDebounce)
+        {
+            stabilizeDebounce = true;
+            stabilize = !stabilize;
+        }
+        else if (!Input.IsKeyPressed(Key.T) && stabilizeDebounce)
+        {
+            stabilizeDebounce = false;
+        }
+
+        // Debug window
+        ImGui.Begin("Debug");
+        ImGui.Text("Status");
+        ImGui.LabelText("Position", this.GlobalPosition.ToString());
+        var vel = this.LinearVelocity.AsSystem();
+        ImGui.SliderFloat3("Velocity", ref vel, -100.0f, 100.0f);
+        var angVel = this.AngularVelocity.AsSystem();
+        ImGui.SliderFloat3("Angular Velocity", ref angVel, -100.0f, 100.0f);
+
+        ImGui.Text("Controls");
+        ImGui.SliderFloat("Throttle", ref _throttle, 0.0f, 1.0f);
+        ImGui.Checkbox("Stabilize", ref stabilize);
+        var torqueVec = torque.AsSystem();
+        ImGui.SliderFloat3("Torque", ref torqueVec, -1.0f, 1.0f);
+        ImGui.End();
+    }
+
+    public override void _PhysicsProcess(double delta)
+    {
+        var thrust = Throttle * 100.0f;
+        var thrustVec = this.Transform * new Vector3(0.0f, thrust, 0f);
+        this.ApplyCentralForce(thrustVec);
+
+        Vector3 torque;
+        if (!this.torque.IsZeroApprox())
+        {
+            torque = this.torque;
+        }
+        else if (stabilize)
+        {
+            torque = -AngularVelocity * 0.1f;
+        }
+        else
+        {
+            torque = Vector3.Zero;
+        }
+        ApplyTorque(torque);
+    }
+
+}