Add state orientation interpolation

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>

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

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>

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>