Add test thruster

project.godot

@@ -19,6 +19,12 @@ config/icon="res://icon.svg"
 
 ImGuiRoot="*res://addons/imgui-godot/data/ImGuiRoot.tscn"
 
+[display]
+
+window/size/viewport_width=1920
+window/size/viewport_height=1080
+window/stretch/mode="viewport"
+
 [dotnet]
 
 project/assembly_name="Quadratic.Carto"

res/scene/test.tscn

@@ -1,6 +1,60 @@
-[gd_scene load_steps=2 format=3 uid="uid://tht1tf5iq6lw"]
+[gd_scene load_steps=11 format=3 uid="uid://tht1tf5iq6lw"]
 
-[ext_resource type="Script" path="res://src/Hello.cs" id="1_ex5yf"]
+[ext_resource type="Script" path="res://src/testing/TestThruster.cs" id="1_y7wni"]
 
-[node name="Node3D" type="Node3D"]
-script = ExtResource("1_ex5yf")
+[sub_resource type="BoxShape3D" id="BoxShape3D_xxi7g"]
+size = Vector3(16.9697, 0.0310059, 15.6934)
+
+[sub_resource type="PlaneMesh" id="PlaneMesh_xcndr"]
+size = Vector2(20, 20)
+
+[sub_resource type="BoxShape3D" id="BoxShape3D_8al46"]
+
+[sub_resource type="BoxMesh" id="BoxMesh_jgj3c"]
+
+[sub_resource type="PhysicalSkyMaterial" id="PhysicalSkyMaterial_uxbcq"]
+
+[sub_resource type="Sky" id="Sky_iakm3"]
+sky_material = SubResource("PhysicalSkyMaterial_uxbcq")
+
+[sub_resource type="Environment" id="Environment_0rcmt"]
+background_mode = 2
+sky = SubResource("Sky_iakm3")
+
+[sub_resource type="CameraAttributesPhysical" id="CameraAttributesPhysical_irbnp"]
+
+[sub_resource type="Compositor" id="Compositor_tt8nt"]
+
+[node name="root" type="Node3D"]
+
+[node name="ground" type="StaticBody3D" parent="."]
+disable_mode = 1
+input_ray_pickable = false
+
+[node name="CollisionShape3D" type="CollisionShape3D" parent="ground"]
+shape = SubResource("BoxShape3D_xxi7g")
+
+[node name="MeshInstance3D" type="MeshInstance3D" parent="ground"]
+mesh = SubResource("PlaneMesh_xcndr")
+
+[node name="test-rocket" type="RigidBody3D" parent="."]
+transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.815224, 0)
+script = ExtResource("1_y7wni")
+
+[node name="CollisionShape3D" type="CollisionShape3D" parent="test-rocket"]
+shape = SubResource("BoxShape3D_8al46")
+
+[node name="MeshInstance3D" type="MeshInstance3D" parent="test-rocket"]
+mesh = SubResource("BoxMesh_jgj3c")
+
+[node name="Camera3D" type="Camera3D" parent="test-rocket"]
+transform = Transform3D(1, 0, 0, 0, 0.91772, 0.397228, 0, -0.397228, 0.91772, 0, 4.51745, 14.147)
+current = true
+
+[node name="DirectionalLight3D" type="DirectionalLight3D" parent="."]
+transform = Transform3D(0.767277, -0.356166, -0.533322, 0.641316, 0.42612, 0.638072, 0, -0.831605, 0.555367, -9.52374, 5.38917, 8.97674)
+
+[node name="WorldEnvironment" type="WorldEnvironment" parent="."]
+environment = SubResource("Environment_0rcmt")
+camera_attributes = SubResource("CameraAttributesPhysical_irbnp")
+compositor = SubResource("Compositor_tt8nt")

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);
+    }
+
+}