Fixed constant rotations in Orientations (#350)

include/ale/Isd.h

@@ -62,8 +62,6 @@ namespace ale {
 
     PositionInterpolation interpMethod;
 
-    Rotation const_rotation;
-
     States inst_pos;
     States sun_pos;
 

include/ale/Orientations.h

@@ -21,7 +21,6 @@ namespace ale {
       const std::vector<Rotation> &rotations,
       const std::vector<double> &times,
       const std::vector<Vec3d> &avs = std::vector<ale::Vec3d>(),
-      const int refFrame = 1,
       const Rotation &constRot = Rotation(1, 0, 0, 0),
       const std::vector<int> const_frames = std::vector<int>(),
       const std::vector<int> time_dependent_frames = std::vector<int>()
@@ -40,7 +39,6 @@ namespace ale {
     std::vector<double> getTimes() const;
     std::vector<int> getConstantFrames() const;
     std::vector<int> getTimeDependentFrames() const;
-    int getReferenceFrame() const;
     Rotation getConstantRotation() const;
 
     /**
@@ -89,7 +87,6 @@ namespace ale {
     std::vector<int> m_timeDepFrames;
     std::vector<int> m_constFrames;
     Rotation m_constRotation;
-    int m_refFrame;
   };
 }
 

src/Orientations.cpp

@@ -8,12 +8,11 @@ namespace ale {
     const std::vector<Rotation> &rotations,
     const std::vector<double> &times,
     const std::vector<Vec3d> &avs,
-    const int refFrame,
     const Rotation &const_rot,
     const std::vector<int> const_frames,
     const std::vector<int> time_dependent_frames
   ) :
-    m_rotations(rotations), m_avs(avs), m_times(times), m_refFrame(refFrame), m_timeDepFrames(time_dependent_frames), m_constFrames(const_frames), m_constRotation(const_rot) {
+    m_rotations(rotations), m_avs(avs), m_times(times), m_timeDepFrames(time_dependent_frames), m_constFrames(const_frames), m_constRotation(const_rot) {
     if (m_rotations.size() < 2 || m_times.size() < 2) {
       throw std::invalid_argument("There must be at least two rotations and times.");
     }
@@ -48,10 +47,6 @@ namespace ale {
     return m_constFrames;
   }
 
-  int Orientations::getReferenceFrame() const {
-    return m_refFrame;
-  }
-
   Rotation Orientations::getConstantRotation() const {
     return m_constRotation;
   }
@@ -62,7 +57,7 @@ namespace ale {
   ) const {
     int interpIndex = interpolationIndex(m_times, time);
     double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
-    return m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+    return m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
   }
 
 
@@ -80,6 +75,9 @@ namespace ale {
     bool invert
   ) const {
     Rotation interpRot = interpolate(time, interpType);
+    if (invert) {
+      interpRot = interpRot.inverse();
+    }
     return interpRot(vector);
   }
 
@@ -109,8 +107,10 @@ namespace ale {
     std::vector<Rotation> mergedRotations;
     std::vector<Vec3d> mergedAvs;
     for (double time: mergedTimes) {
+      // interpolate includes the constant rotation, so invert it to undo that
+      Rotation inverseConst = m_constRotation.inverse();
       Rotation rhsRot = rhs.interpolate(time);
-      mergedRotations.push_back(interpolate(time)*rhsRot);
+      mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot);
       Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
       Vec3d rhsAv = rhs.interpolateAV(time);
       combinedAv.x += rhsAv.x;
@@ -133,10 +133,10 @@ namespace ale {
       updatedRotations.push_back(m_rotations[i]*rhs);
     }
 
-    Rotation inverse = rhs.inverse();
+    Rotation inverseRhs = rhs.inverse();
     std::vector<Vec3d> updatedAvs;
     for (size_t i = 0; i < m_avs.size(); i++) {
-      updatedAvs.push_back(inverse(m_avs[i]));
+      updatedAvs.push_back(inverseRhs(m_avs[i]));
     }
 
     m_rotations = updatedRotations;

src/Util.cpp

@@ -534,7 +534,6 @@ Orientations getInstrumentPointing(json isd) {
     std::vector<Rotation> rotations = getJsonQuatArray(pointing.at("quaternions"));
     std::vector<double> times = getJsonArray<double>(pointing.at("ephemeris_times"));
     std::vector<Vec3d> velocities = getJsonVec3dArray(pointing.at("angular_velocities"));
-    int refFrame = pointing.at("reference_frame").get<int>();
 
     std::vector<int> constFrames;
     if (pointing.find("constant_frames") != pointing.end()){
@@ -553,7 +552,7 @@ Orientations getInstrumentPointing(json isd) {
 
     Rotation constRot(rotArray);
 
-    Orientations orientation(rotations, times, velocities, refFrame, constRot, constFrames, timeDepFrames);
+    Orientations orientation(rotations, times, velocities, constRot, constFrames, timeDepFrames);
 
     return orientation;
 
@@ -569,8 +568,6 @@ Orientations getBodyRotation(json isd) {
     std::vector<double> times = getJsonArray<double>(bodrot.at("ephemeris_times"));
     std::vector<Vec3d> velocities = getJsonVec3dArray(bodrot.at("angular_velocities"));
 
-    int refFrame = bodrot.at("reference_frame").get<int>();
-
     std::vector<int> constFrames;
     if (bodrot.find("constant_frames") != bodrot.end()){
       constFrames  = getJsonArray<int>(bodrot.at("constant_frames"));
@@ -588,7 +585,7 @@ Orientations getBodyRotation(json isd) {
 
     Rotation constRot(rotArray);
 
-    Orientations orientation(rotations, times, velocities, refFrame, constRot, constFrames, timeDepFrames);
+    Orientations orientation(rotations, times, velocities, constRot, constFrames, timeDepFrames);
     return orientation;
 
   } catch (...) {

tests/ctests/IsdTests.cpp

@@ -268,7 +268,6 @@ TEST(Isd, GetInstrumentPointing) {
   std::vector<int> timeDepFrames = instPointing.getTimeDependentFrames();
 
   ASSERT_EQ(rotations.size(), 2);
-  ASSERT_EQ(instPointing.getReferenceFrame(), 2);
   ASSERT_EQ(instPointing.getTimes()[0], 300);
   ASSERT_EQ(instPointing.getTimes()[1], 600);
 
@@ -339,7 +338,6 @@ TEST(Isd, GetBodyRotation) {
   std::vector<int> timeDepFrames = bodyRot.getTimeDependentFrames();
 
   ASSERT_EQ(rotations.size(), 2);
-  ASSERT_EQ(bodyRot.getReferenceFrame(), 2);
   ASSERT_EQ(bodyRot.getTimes()[0], 300);
   ASSERT_EQ(bodyRot.getTimes()[1], 600);
 

tests/ctests/OrientationsTests.cpp

@@ -29,6 +29,18 @@ class OrientationTest : public ::testing::Test {
     Orientations orientations;
 };
 
+class ConstOrientationTest : public OrientationTest{
+  protected:
+    void SetUp() override {
+      OrientationTest::SetUp();
+      constRotation = Rotation(0, 1, 0, 0);
+      constOrientations = Orientations(rotations, times, avs, constRotation);
+    }
+
+    Rotation constRotation;
+    Orientations constOrientations;
+};
+
 TEST_F(OrientationTest, ConstructorAccessors) {
   vector<Rotation> outputRotations = orientations.getRotations();
   vector<double> outputTimes = orientations.getTimes();
@@ -130,3 +142,43 @@ TEST_F(OrientationTest, OrientationMultiplication) {
     EXPECT_EQ(expectedQuats[i][3], quats[3]);
   }
 }
+
+TEST_F(ConstOrientationTest, RotateAt) {
+  Vec3d rotatedX = constRotation(orientations.rotateVectorAt(0.0, Vec3d(1.0, 0.0, 0.0)));
+  Vec3d constRotatedX = constOrientations.rotateVectorAt(0.0, Vec3d(1.0, 0.0, 0.0));
+  EXPECT_NEAR(rotatedX.x, constRotatedX.x, 1e-10);
+  EXPECT_NEAR(rotatedX.y, constRotatedX.y, 1e-10);
+  EXPECT_NEAR(rotatedX.z, constRotatedX.z, 1e-10);
+  Vec3d rotatedY = constRotation(orientations.rotateVectorAt(0.0, Vec3d(0.0, 1.0, 0.0)));
+  Vec3d constRotatedY = constOrientations.rotateVectorAt(0.0, Vec3d(0.0, 1.0, 0.0));
+  EXPECT_NEAR(rotatedY.x, constRotatedY.x, 1e-10);
+  EXPECT_NEAR(rotatedY.y, constRotatedY.y, 1e-10);
+  EXPECT_NEAR(rotatedY.z, constRotatedY.z, 1e-10);
+  Vec3d rotatedZ = constRotation(orientations.rotateVectorAt(0.0, Vec3d(0.0, 0.0, 1.0)));
+  Vec3d constRotatedZ = constOrientations.rotateVectorAt(0.0, Vec3d(0.0, 0.0, 1.0));
+  EXPECT_NEAR(rotatedZ.x, constRotatedZ.x, 1e-10);
+  EXPECT_NEAR(rotatedZ.y, constRotatedZ.y, 1e-10);
+  EXPECT_NEAR(rotatedZ.z, constRotatedZ.z, 1e-10);
+}
+
+TEST_F(ConstOrientationTest, OrientationMultiplication) {
+  constOrientations *= orientations;
+  vector<Rotation> outputRotations = constOrientations.getRotations();
+  vector<vector<double>> expectedQuats = {
+    {-0.5, 0.5, 0.5, 0.5},
+    {-0.5,-0.5,-0.5,-0.5},
+    { 1.0, 0.0, 0.0, 0.0}
+  };
+  for (size_t i = 0; i < outputRotations.size(); i++) {
+    vector<double> quats = outputRotations[i].toQuaternion();
+    EXPECT_EQ(expectedQuats[i][0], quats[0]);
+    EXPECT_EQ(expectedQuats[i][1], quats[1]);
+    EXPECT_EQ(expectedQuats[i][2], quats[2]);
+    EXPECT_EQ(expectedQuats[i][3], quats[3]);
+  }
+  vector<double> constQuats = constOrientations.getConstantRotation().toQuaternion();
+  EXPECT_EQ(constQuats[0], 0);
+  EXPECT_EQ(constQuats[1], 1);
+  EXPECT_EQ(constQuats[2], 0);
+  EXPECT_EQ(constQuats[3], 0);
+}