From 0ab9466ff5ac2f1529a355d4b993906e3d4bf749 Mon Sep 17 00:00:00 2001
From: Tornado Tech <54727692+Tornado-Technology@users.noreply.github.com>
Date: Thu, 1 Aug 2024 17:34:36 +1000
Subject: [PATCH] Added inverse kinematic

---
 Hypercube.Math/Vectors/Vector2.cs             |  6 ++
 .../Animation/Procedural/IKFabric2D.cs        | 96 +++++++++++++++++++
 .../Animation/Procedural/Segment2D.cs         | 45 +++++++++
 3 files changed, 147 insertions(+)
 create mode 100644 Hypercube.Shared/Animation/Procedural/IKFabric2D.cs
 create mode 100644 Hypercube.Shared/Animation/Procedural/Segment2D.cs

diff --git a/Hypercube.Math/Vectors/Vector2.cs b/Hypercube.Math/Vectors/Vector2.cs
index f3bfa70..a9c3441 100644
--- a/Hypercube.Math/Vectors/Vector2.cs
+++ b/Hypercube.Math/Vectors/Vector2.cs
@@ -40,6 +40,12 @@ public Vector2 Normalized
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
         get => this / Length;
     }
+
+    public float Angle
+    {
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        get => MathF.Atan2(Y, X);
+    }
     
     public Vector2(float x, float y)
     {
diff --git a/Hypercube.Shared/Animation/Procedural/IKFabric2D.cs b/Hypercube.Shared/Animation/Procedural/IKFabric2D.cs
new file mode 100644
index 0000000..6b16899
--- /dev/null
+++ b/Hypercube.Shared/Animation/Procedural/IKFabric2D.cs
@@ -0,0 +1,96 @@
+using Hypercube.Math.Vectors;
+
+namespace Hypercube.Shared.Animation.Procedural;
+
+public sealed class IKFabric2D
+{
+    public Vector2 Position { get; private set; }
+    public Vector2 Target { get; private set; }
+    public bool Fixed { get; private set; }
+    public float MaxReach { get; private set; }
+    
+    private readonly List<Segment2D> _segments;
+    
+    public IKFabric2D(Vector2 position, int segments, float angle, float segmentLength, bool @fixed = true)
+    {
+        Position = position;
+        Fixed = @fixed;
+        
+        _segments = new List<Segment2D>
+        {
+            new(position, angle, segmentLength)
+        };
+
+        MaxReach += segmentLength;
+        
+        for (var i = 1; i < segments; i++)
+        {
+            AddSegment(angle, segmentLength);
+        }
+    }
+
+    public bool CanReach(Vector2 target)
+    {
+        return (Position - target).LengthSquared <= MaxReach * MaxReach;
+    }
+
+    public void AddSegment(float angle, float length)
+    {
+        var previous = _segments[^1];
+
+        MaxReach += length;
+        var segment = new Segment2D(Vector2.Zero, angle, length);
+        
+        _segments.Add(segment);
+        
+        segment.Follow(previous.Position);
+    }
+
+    public void Update()
+    {
+        for (var i = 0; i < _segments.Count; i++)
+        {
+            var segment = _segments[i];
+            segment.Update();
+            
+            if (i == 0) {
+                segment.Follow(Target);
+                continue;
+            }
+
+            var previous = _segments[i - 1];
+            segment.Follow(previous.Position);
+        }
+
+        var last = _segments.Count - 1;
+        var lastSegment = _segments[last];
+
+        if (Fixed)
+            lastSegment.SetPosition(Position);    
+        
+        lastSegment.Update();
+        
+        for (var i = last - 1; i >= 0; i--) {
+            var segment = _segments[i];
+            var nextSegment = _segments[i + 1];
+
+            segment.SetPosition(nextSegment.TargetPosition);
+            segment.Update();
+        }
+    }
+
+    public void SetPosition(Vector3 position)
+    {
+        Position = position;
+    }
+
+    public void SetTarget(Vector3 target)
+    {
+        Target = target;
+    }
+
+    public void SetFixed(bool value)
+    {
+        Fixed = value;
+    }
+}
\ No newline at end of file
diff --git a/Hypercube.Shared/Animation/Procedural/Segment2D.cs b/Hypercube.Shared/Animation/Procedural/Segment2D.cs
new file mode 100644
index 0000000..59a00fe
--- /dev/null
+++ b/Hypercube.Shared/Animation/Procedural/Segment2D.cs
@@ -0,0 +1,45 @@
+using Hypercube.Math.Vectors;
+
+namespace Hypercube.Shared.Animation.Procedural;
+
+public sealed class Segment2D
+{
+    public float Angle { get; private set; }
+    public float Length { get; private set; }
+
+    public Vector2 Position { get; private set; }
+    public Vector2 TargetPosition { get; private set; }
+    
+    public Segment2D(Vector2 position, float angle, float length)
+    {
+        Position = position;
+        Angle = angle;
+        Length = length;
+
+        Update();
+    }
+
+    public void Follow(Vector2 target)
+    {
+        var direction = target - Position;
+
+        Angle = direction.Angle;
+        
+        // set magnitude
+        direction = direction.Normalized * Length;
+        direction *= -1;
+
+        Position = target + direction;
+    }
+
+    public void Update()
+    {
+        var delta = new Vector2(MathF.Cos(Angle), MathF.Sin(Angle)) * Length;
+        TargetPosition = Position + delta;
+    }
+
+    public void SetPosition(Vector2 position)
+    {
+        Position = position;
+    }
+}
\ No newline at end of file