Added time dep only rotation interpolation

include/ale/Orientations.h

@@ -41,6 +41,14 @@ namespace ale {
     std::vector<int> getTimeDependentFrames() const;
     Rotation getConstantRotation() const;
 
+    /**
+     * Get the time dependent component of the interpolated rotation at a specific time.
+     */
+    Rotation interpolateTimeDep(
+      double time,
+      RotationInterpolation interpType=SLERP
+    ) const;
+
     /**
      * Get the interpolated rotation at a specific time.
      */

src/Orientations.cpp

@@ -51,27 +51,34 @@ namespace ale {
     return m_constRotation;
   }
 
-  Rotation Orientations::interpolate(
+  Rotation Orientations::interpolateTimeDep(
     double time,
     RotationInterpolation interpType
   ) const {
-    Rotation interpRotation;
+    Rotation timeDepRotation;
     if (m_times.size() > 1) {
       int interpIndex = interpolationIndex(m_times, time);
       double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
-      interpRotation = m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+      timeDepRotation = m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
     }
     else if (m_avs.empty()) {
-      interpRotation = m_constRotation * m_rotations.front();
+      timeDepRotation = m_rotations.front();
     }
     else {
       double t = time - m_times.front();
       std::vector<double> axis = {m_avs.front().x, m_avs.front().y, m_avs.front().z};
       double angle = t * m_avs.front().norm();
       Rotation newRotation(axis, angle);
-      interpRotation = m_constRotation * newRotation * m_rotations.front();
+      timeDepRotation = newRotation * m_rotations.front();
+    }
+    return timeDepRotation;
   }
-    return interpRotation;
+
+  Rotation Orientations::interpolate(
+    double time,
+    RotationInterpolation interpType
+  ) const {
+    return m_constRotation * interpolateTimeDep(time, interpType);
   }
 
 

tests/ctests/OrientationsTests.cpp

@@ -93,6 +93,19 @@ TEST_F(OrientationTest, ConstructorAccessors) {
   }
 }
 
+TEST_F(ConstOrientationTest, InterpolateTimeDep) {
+  Rotation interpRotation = constOrientations.interpolateTimeDep(0.25);
+  Rotation expectedRotation = orientations.interpolate(0.25);
+  vector<double> quat = interpRotation.toQuaternion();
+  vector<double> expectedQuat = expectedRotation.toQuaternion();
+  ASSERT_EQ(quat.size(), 4);
+  ASSERT_EQ(expectedQuat.size(), 4);
+  EXPECT_NEAR(quat[0], expectedQuat[0], 1e-10);
+  EXPECT_NEAR(quat[1], expectedQuat[1], 1e-10);
+  EXPECT_NEAR(quat[2], expectedQuat[2], 1e-10);
+  EXPECT_NEAR(quat[3], expectedQuat[3], 1e-10);
+}
+
 TEST_F(OrientationTest, Interpolate) {
   Rotation interpRotation = orientations.interpolate(0.25);
   vector<double> quat = interpRotation.toQuaternion();