From 4de63f9887ef2deaeed805611b225fe3dbac1050 Mon Sep 17 00:00:00 2001
From: Sylvain Guillet <sylvain.guillet@live.com>
Date: Mon, 30 Sep 2024 22:13:12 +0200
Subject: [PATCH] Add state orientation interpolation

---
 .../IO.Astrodynamics.CLI.csproj               |  2 +-
 .../StateOrientationTests.cs                  | 42 +++++++++++++++++++
 .../IO.Astrodynamics/IO.Astrodynamics.nuspec  |  2 +-
 .../OrbitalParameters/StateOrientation.cs     | 26 +++++++++++-
 4 files changed, 69 insertions(+), 3 deletions(-)

diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj b/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj
index 13b3b659..4db0a73e 100644
--- a/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj
+++ b/IO.Astrodynamics.Net/IO.Astrodynamics.CLI/IO.Astrodynamics.CLI.csproj
@@ -9,7 +9,7 @@
         <FileVersion>0.0.1</FileVersion>
         <PackAsTool>true</PackAsTool>
         <ToolCommandName>astro</ToolCommandName>
-        <Version>0.5.0-preview-8</Version>
+        <Version>0.5.0-preview-9</Version>
         <Title>Astrodynamics command line interface</Title>
         <Authors>Sylvain Guillet</Authors>
         <Description>This CLI allows end user to exploit IO.Astrodynamics framework </Description>
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/StateOrientationTests.cs b/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/StateOrientationTests.cs
index ab340144..88b12c9c 100644
--- a/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/StateOrientationTests.cs
+++ b/IO.Astrodynamics.Net/IO.Astrodynamics.Tests/OrbitalParameters/StateOrientationTests.cs
@@ -101,5 +101,47 @@ public void AtDate()
             Assert.Equal(TimeSystem.Time.J2000TDB.AddSeconds(10), res.Epoch);
             Assert.Equal(Frames.Frame.ECLIPTIC_J2000, res.ReferenceFrame);
         }
+        
+        [Fact]
+        public void Interpolate_SameReferenceFrame_ReturnsInterpolatedState()
+        {
+            // Arrange
+            var initialOrientation = new StateOrientation(new Quaternion(1, 0, 0, 0), new Vector3(10, 20, 30), TimeSystem.Time.J2000TDB, Frames.Frame.ICRF);
+            var finalOrientation = new StateOrientation(new Quaternion(0, 1, 0, 0), new Vector3(20, 30, 40), TimeSystem.Time.J2000TDB.AddSeconds(10), Frames.Frame.ICRF);
+            var date = TimeSystem.Time.J2000TDB.AddSeconds(5);
+
+            // Act
+            var result = initialOrientation.Interpolate(finalOrientation, date);
+
+            // Assert
+            Assert.Equal(new Quaternion(0.7071067811865476, 0.7071067811865476, 0, 0), result.Rotation,TestHelpers.QuaternionComparer);
+            Assert.Equal(new Vector3(15, 25, 35), result.AngularVelocity);
+            Assert.Equal(date, result.Epoch);
+            Assert.Equal(Frames.Frame.ICRF, result.ReferenceFrame);
+        }
+
+        [Fact]
+        public void Interpolate_DifferentReferenceFrames_ThrowsArgumentException()
+        {
+            // Arrange
+            var initialOrientation = new StateOrientation(new Quaternion(1, 0, 0, 0), new Vector3(0, 0, 0), TimeSystem.Time.J2000TDB, Frames.Frame.ICRF);
+            var finalOrientation = new StateOrientation(new Quaternion(0, 1, 0, 0), new Vector3(0, 0, 0), TimeSystem.Time.J2000TDB.AddSeconds(10), Frames.Frame.ECLIPTIC_J2000);
+            var date = TimeSystem.Time.J2000TDB.AddSeconds(5);
+
+            // Act & Assert
+            Assert.Throws<ArgumentException>(() => initialOrientation.Interpolate(finalOrientation, date));
+        }
+
+        [Fact]
+        public void Interpolate_DateOutOfRange_ThrowsArgumentException()
+        {
+            // Arrange
+            var initialOrientation = new StateOrientation(new Quaternion(1, 0, 0, 0), new Vector3(0, 0, 0), TimeSystem.Time.J2000TDB, Frames.Frame.ICRF);
+            var finalOrientation = new StateOrientation(new Quaternion(0, 1, 0, 0), new Vector3(0, 0, 0), TimeSystem.Time.J2000TDB.AddSeconds(10), Frames.Frame.ICRF);
+            var date = TimeSystem.Time.J2000TDB.AddSeconds(15);
+
+            // Act & Assert
+            Assert.Throws<ArgumentException>(() => initialOrientation.Interpolate(finalOrientation, date));
+        }
     }
 }
\ No newline at end of file
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec b/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec
index 56194289..6060c403 100644
--- a/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec
+++ b/IO.Astrodynamics.Net/IO.Astrodynamics/IO.Astrodynamics.nuspec
@@ -4,7 +4,7 @@
         <id>IO.Astrodynamics</id>
         <authors>Sylvain Guillet</authors>
         <copyright>Sylvain Guillet</copyright>
-        <version>6.0.0-preview-8</version>
+        <version>6.0.0-preview-9</version>
         <title>Astrodynamics framework</title>
         <icon>images\dragonfly-dark-trans.png</icon>
         <readme>docs\README.md</readme>
diff --git a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/StateOrientation.cs b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/StateOrientation.cs
index abb3d95c..1aeaa66b 100644
--- a/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/StateOrientation.cs
+++ b/IO.Astrodynamics.Net/IO.Astrodynamics/OrbitalParameters/StateOrientation.cs
@@ -48,7 +48,31 @@ public StateOrientation AtDate(in Time date)
         {
             var deltaT = (date - Epoch).TotalSeconds;
             return new StateOrientation(
-                 (new Quaternion(AngularVelocity.Normalize(), AngularVelocity.Magnitude() * deltaT).Normalize()*Rotation.Normalize()).Normalize(), AngularVelocity, date, ReferenceFrame);
+                (new Quaternion(AngularVelocity.Normalize(), AngularVelocity.Magnitude() * deltaT).Normalize() * Rotation.Normalize()).Normalize(), AngularVelocity, date,
+                ReferenceFrame);
+        }
+
+        /// <summary>
+        /// Interpolates between two state orientations.
+        /// </summary>
+        /// <param name="other"></param>
+        /// <param name="date"></param>
+        /// <returns></returns>
+        /// <exception cref="ArgumentException"></exception>
+        public StateOrientation Interpolate(StateOrientation other, Time date)
+        {
+            if (ReferenceFrame != other.ReferenceFrame)
+            {
+                throw new ArgumentException("Cannot interpolate between two different reference frames.");
+            }
+            if(date < Epoch || date > other.Epoch)
+            {
+                throw new ArgumentException("Date is out of range.");
+            }
+            var ratio = (date - Epoch).TotalSeconds / (other.Epoch - Epoch).TotalSeconds;
+
+            return new StateOrientation(Rotation.SLERP(other.Rotation, ratio), AngularVelocity.LinearInterpolation(other.AngularVelocity, ratio),
+                Epoch + (other.Epoch - Epoch) * ratio, ReferenceFrame);
         }
 
         /// <summary>