From 1a14f2595ad83dae3d6a715506bf2f735168416c Mon Sep 17 00:00:00 2001
From: cosmonaut <evan@moonside.games>
Date: Wed, 26 Aug 2020 14:56:32 -0700
Subject: [PATCH] implement 3d spline

---
 Curve/CubicBezierCurve3D.cs     |  2 +-
 Curve/Curve.csproj              |  1 +
 Curve/ICurve3D.cs               | 10 ++++++
 Curve/QuadraticBezierCurve3D.cs |  2 +-
 Curve/SplineCurve3D.cs          | 60 +++++++++++++++++++++++++++++++++
 5 files changed, 73 insertions(+), 2 deletions(-)
 create mode 100644 Curve/ICurve3D.cs
 create mode 100644 Curve/SplineCurve3D.cs

diff --git a/Curve/CubicBezierCurve3D.cs b/Curve/CubicBezierCurve3D.cs
index 8ebf5c0..a6c6180 100644
--- a/Curve/CubicBezierCurve3D.cs
+++ b/Curve/CubicBezierCurve3D.cs
@@ -6,7 +6,7 @@ namespace MoonTools.Curve
     /// <summary>
     /// A 3-dimensional Bezier curve defined by 4 points.
     /// </summary>
-    public struct CubicBezierCurve3D : IEquatable<CubicBezierCurve3D>
+    public struct CubicBezierCurve3D : IEquatable<CubicBezierCurve3D>, ICurve3D
     {
         /// <summary>
         /// The start point.
diff --git a/Curve/Curve.csproj b/Curve/Curve.csproj
index ad7b52c..2ec8b03 100644
--- a/Curve/Curve.csproj
+++ b/Curve/Curve.csproj
@@ -19,6 +19,7 @@
       <PrivateAssets>all</PrivateAssets>
       <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
     </PackageReference>
+    <PackageReference Include="System.Collections.Immutable" Version="1.7.1"/>
     <PackageReference Include="System.Numerics.Vectors" Version="4.5.0"/>
     <PackageReference Include="Microsoft.Bcl.HashCode" Version="1.1.0"/>
   </ItemGroup>
diff --git a/Curve/ICurve3D.cs b/Curve/ICurve3D.cs
new file mode 100644
index 0000000..cf43e2e
--- /dev/null
+++ b/Curve/ICurve3D.cs
@@ -0,0 +1,10 @@
+using System.Numerics;
+
+namespace MoonTools.Curve
+{
+    public interface ICurve3D
+    {
+        Vector3 Point(float t, float startTime, float endTime);
+        Vector3 Velocity(float t, float startTime, float endTime);
+    }
+}
diff --git a/Curve/QuadraticBezierCurve3D.cs b/Curve/QuadraticBezierCurve3D.cs
index 14da833..ca16092 100644
--- a/Curve/QuadraticBezierCurve3D.cs
+++ b/Curve/QuadraticBezierCurve3D.cs
@@ -6,7 +6,7 @@ namespace MoonTools.Curve
     /// <summary>
     /// A 3-dimensional Bezier curve defined by 3 points.
     /// </summary>
-    public struct QuadraticBezierCurve3D : IEquatable<QuadraticBezierCurve3D>
+    public struct QuadraticBezierCurve3D : IEquatable<QuadraticBezierCurve3D>, ICurve3D
     {
         /// <summary>
         /// The start point.
diff --git a/Curve/SplineCurve3D.cs b/Curve/SplineCurve3D.cs
new file mode 100644
index 0000000..38b94db
--- /dev/null
+++ b/Curve/SplineCurve3D.cs
@@ -0,0 +1,60 @@
+using System.Collections.Immutable;
+using System.Numerics;
+
+namespace MoonTools.Curve
+{
+    /// <summary>
+    /// A concatenation of 3D curves with time values.
+    /// </summary>
+    public struct SplineCurve3D
+    {
+        public ImmutableArray<ICurve3D> Curves { get; }
+        public ImmutableArray<float> Times { get; }
+        public float TotalTime { get; }
+        public bool Loop { get; }
+
+        public SplineCurve3D(ImmutableArray<ICurve3D> curves, ImmutableArray<float> times, bool loop = false)
+        {
+            TotalTime = 0;
+
+            for (int i = 0; i < times.Length; i++)
+            {
+                TotalTime += times[i];
+            }
+
+            Curves = curves;
+            Times = times;
+            Loop = loop;
+        }
+
+        public Vector3 Point(float t)
+        {
+            if (!Loop && t >= TotalTime)
+            {
+                var lastIndex = Curves.Length - 1;
+                return Curves[lastIndex].Point(Times[lastIndex], 0, Times[lastIndex]);
+            }
+
+            t %= TotalTime;
+
+            var index = 0;
+            var startTime = 0f;
+            var incrementalTime = 0f;
+
+            for (int i = 0; i < Times.Length; i++)
+            {
+                incrementalTime += Times[i];
+
+                if (t < incrementalTime)
+                {
+                    break;
+                }
+
+                index++;
+                startTime = Times[i];
+            }
+
+            return Curves[index].Point(t - startTime, 0, Times[index]);
+        }
+    }
+}