Fixed no av seg fault

src/Orientations.cpp

@@ -84,7 +84,10 @@ namespace ale {
 
   Vec3d Orientations::interpolateAV(double time) const {
     Vec3d interpAv;
-    if (m_times.size() > 1) {
+    if (m_avs.empty()) {
+      throw std::invalid_argument("Cannot interpolate angular velocities for an orientation without them.");
+    }
+    else if (m_avs.size() > 1) {
       int interpIndex = interpolationIndex(m_times, time);
       double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
       interpAv = Vec3d(linearInterpolate(m_avs[interpIndex], m_avs[interpIndex + 1], t));
@@ -144,11 +147,13 @@ namespace ale {
       Rotation inverseConst = m_constRotation.inverse();
       Rotation rhsRot = rhs.interpolate(time);
       mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot);
+      if (!getAngularVelocities().empty() && !rhs.getAngularVelocities().empty()) {
         Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
         Vec3d rhsAv = rhs.interpolateAV(time);
         combinedAv += rhsAv;
         mergedAvs.push_back(combinedAv);
       }
+    }
 
     m_times = mergedTimes;
     m_rotations = mergedRotations;

tests/ctests/OrientationsTests.cpp

@@ -30,6 +30,16 @@ class OrientationTest : public ::testing::Test {
     Orientations orientations;
 };
 
+class NoAVOrientationTest : public OrientationTest{
+  protected:
+    void SetUp() override {
+      OrientationTest::SetUp();
+      noAvOrientations = Orientations(rotations, times);
+    }
+
+    Orientations noAvOrientations;
+};
+
 class ConstOrientationTest : public OrientationTest{
   protected:
     void SetUp() override {
@@ -151,6 +161,10 @@ TEST_F(OrientationTest, InterpolateAv) {
   EXPECT_NEAR(interpAv.z, M_PI / (3.0 * sqrt(3.0)), 1e-10);
 }
 
+TEST_F(NoAVOrientationTest, InterpolateAv) {
+  EXPECT_ANY_THROW(noAvOrientations.interpolateAV(0.25));
+}
+
 TEST_F(OrientationTest, RotateAt) {
   Vec3d rotatedX = orientations.rotateVectorAt(0.0, Vec3d(1.0, 0.0, 0.0));
   EXPECT_NEAR(rotatedX.x, 0.0, 1e-10);
@@ -266,6 +280,35 @@ TEST_F(ConstOrientationTest, OrientationMultiplication) {
   EXPECT_EQ(constQuats[1], 1);
   EXPECT_EQ(constQuats[2], 0);
   EXPECT_EQ(constQuats[3], 0);
+
+  const vector<Vec3d> &originalAVs = orientations.getAngularVelocities();
+  const vector<Vec3d> &avs = multOrientation.getAngularVelocities();
+  ASSERT_EQ(originalAVs.size(), avs.size());
+  for (size_t i = 0; i < avs.size(); i++) {
+    // We are chaining the same rotation with itself so the angular velocities
+    // should just double
+    EXPECT_EQ(2 * originalAVs[i].x, avs[i].x);
+    EXPECT_EQ(2 * originalAVs[i].y, avs[i].y);
+    EXPECT_EQ(2 * originalAVs[i].z, avs[i].z);
+  }
+}
+
+TEST_F(NoAVOrientationTest, MultiplicationNoAV) {
+  Orientations multOrientation = noAvOrientations * orientations;
+  Orientations multAVOrientation = orientations * orientations;
+  vector<Rotation> outputRotations = multOrientation.getRotations();
+  vector<Rotation> outputAVRotations = multAVOrientation.getRotations();
+  ASSERT_EQ(outputRotations.size(), outputAVRotations.size());
+  for (size_t i = 0; i < outputRotations.size(); i++) {
+    vector<double> quats = outputRotations[i].toQuaternion();
+    vector<double> aVQuats = outputAVRotations[i].toQuaternion();
+    EXPECT_EQ(aVQuats[0], quats[0]);
+    EXPECT_EQ(aVQuats[1], quats[1]);
+    EXPECT_EQ(aVQuats[2], quats[2]);
+    EXPECT_EQ(aVQuats[3], quats[3]);
+  }
+
+  EXPECT_TRUE(multOrientation.getAngularVelocities().empty());
 }
 
 TEST_F(ConstOrientationTest, OrientationInverse) {