Orientations now only need 1 rotation (#363)

* Orientations now only need 1 rotation

* Testing fix for mac travis error

* Only fix on Mac

* Added Orientations constructor error test

.travis.yml

@@ -43,6 +43,10 @@ install:
   - conda env create -n ale python=3.7.3
   - conda env update -f environment.yml -n ale
   - source activate ale
+  - |
+    if [ "$TRAVIS_OS_NAME" == "osx" ]; then
+      install_name_tool -change @rpath/libiomp5.dylib @loader_path/libiomp5.dylib ${CONDA_PREFIX}/lib/libmkl_intel_thread.dylib;
+    fi
   - conda install pytest
 
 script:

include/ale/Vectors.h

@@ -3,6 +3,7 @@
 
 #include <stdexcept>
 #include <vector>
+#include <math.h>
 
 namespace ale {
   /** A 3D cartesian vector */
@@ -43,6 +44,10 @@ namespace ale {
       z -= addend.z;
       return *this;
     };
+
+    double norm() const {
+      return sqrt(x*x + y*y + z*z);
+    }
   };
 
   Vec3d operator*(double scalar, Vec3d vec);

src/InterpUtils.cpp

@@ -34,8 +34,8 @@ namespace ale {
   }
 
   int interpolationIndex(const std::vector<double> &times, double interpTime) {
-    if (times.size() < 2){
-      throw std::invalid_argument("There must be at least two times.");
+    if (times.empty()){
+      throw std::invalid_argument("There must be at least one time.");
     }
     auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime);
     if (nextTimeIt == times.end()) {

src/Orientations.cpp

@@ -13,8 +13,8 @@ namespace ale {
     const std::vector<int> time_dependent_frames
   ) :
     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.");
+    if (m_rotations.size() < 1 || m_times.size() < 1) {
+      throw std::invalid_argument("There must be at least one rotation and time.");
     }
     if (m_rotations.size() != m_times.size()) {
       throw std::invalid_argument("The number of rotations and times must be the same.");
@@ -55,16 +55,36 @@ namespace ale {
     double time,
     RotationInterpolation interpType
   ) const {
+    Rotation interpRotation;
+    if (m_times.size() > 1) {
       int interpIndex = interpolationIndex(m_times, time);
       double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
-    return m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+      interpRotation = m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+    }
+    else if (m_avs.empty()) {
+      interpRotation = m_constRotation * 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();
+    }
+    return interpRotation;
   }
 
 
   Vec3d Orientations::interpolateAV(double time) const {
+    Vec3d interpAv;
+    if (m_times.size() > 1) {
       int interpIndex = interpolationIndex(m_times, time);
       double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
-    Vec3d interpAv = Vec3d(linearInterpolate(m_avs[interpIndex], m_avs[interpIndex + 1], t));
+      interpAv = Vec3d(linearInterpolate(m_avs[interpIndex], m_avs[interpIndex + 1], t));
+    }
+    else {
+      interpAv = m_avs.front();
+    }
     return interpAv;
   }
 
@@ -119,9 +139,7 @@ namespace ale {
       mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot);
       Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
       Vec3d rhsAv = rhs.interpolateAV(time);
-      combinedAv.x += rhsAv.x;
-      combinedAv.y += rhsAv.y;
-      combinedAv.z += rhsAv.z;
+      combinedAv += rhsAv;
       mergedAvs.push_back(combinedAv);
     }
 

tests/ctests/OrientationsTests.cpp

@@ -17,9 +17,10 @@ class OrientationTest : public ::testing::Test {
       times.push_back(0);
       times.push_back(2);
       times.push_back(4);
-      avs.push_back(Vec3d(2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI));
-      avs.push_back(Vec3d(2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI));
-      avs.push_back(Vec3d(2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI));
+      double avConstant = M_PI / (3.0 * sqrt(3.0));
+      avs.push_back(Vec3d(avConstant, avConstant, avConstant));
+      avs.push_back(Vec3d(avConstant, avConstant, avConstant));
+      avs.push_back(Vec3d(avConstant, avConstant, avConstant));
       orientations = Orientations(rotations, times, avs);
     }
 
@@ -43,6 +44,31 @@ class ConstOrientationTest : public OrientationTest{
     Orientations constOrientations;
 };
 
+class SingleOrientationTest : public ::testing::Test{
+  protected:
+    void SetUp() override {
+      rotations.push_back(Rotation( 0.5, 0.5, 0.5, 0.5));
+      times.push_back(0);
+      double avConstant = M_PI / (3.0 * sqrt(3.0));
+      avs.push_back(Vec3d(avConstant, avConstant, avConstant));
+      orientations = Orientations(rotations, times, avs);
+    }
+
+    vector<Rotation> rotations;
+    vector<double> times;
+    vector<Vec3d> avs;
+    Orientations orientations;
+};
+
+TEST(Orientations, BadConstructors) {
+  Rotation simpleRotation(1.0, 0.0, 0.0, 0.0);
+  EXPECT_THROW(Orientations({}, {}), invalid_argument);
+  EXPECT_THROW(Orientations({}, {0.0, 2.0, 4.0}), invalid_argument);
+  EXPECT_THROW(Orientations({simpleRotation, simpleRotation}, {}), invalid_argument);
+  EXPECT_THROW(Orientations({simpleRotation, simpleRotation}, {0.0, 2.0, 4.0}), invalid_argument);
+  EXPECT_THROW(Orientations({simpleRotation, simpleRotation}, {0.0, 2.0}, {Vec3d(1.0, 2.0, 3.0)}), invalid_argument);
+}
+
 TEST_F(OrientationTest, ConstructorAccessors) {
   vector<Rotation> outputRotations = orientations.getRotations();
   vector<double> outputTimes = orientations.getTimes();
@@ -77,6 +103,24 @@ TEST_F(OrientationTest, Interpolate) {
   EXPECT_NEAR(quat[3], sin(M_PI * 3.0/8.0) * 1/sqrt(3.0), 1e-10);
 }
 
+TEST_F(OrientationTest, Extrapolate) {
+  Rotation afterRotation = orientations.interpolate(6);
+  vector<double> afterQuat = afterRotation.toQuaternion();
+  ASSERT_EQ(afterQuat.size(), 4);
+  EXPECT_NEAR(afterQuat[0], -0.5, 1e-10);
+  EXPECT_NEAR(afterQuat[1], -0.5, 1e-10);
+  EXPECT_NEAR(afterQuat[2], -0.5, 1e-10);
+  EXPECT_NEAR(afterQuat[3], -0.5, 1e-10);
+
+  Rotation beforeRotation = orientations.interpolate(-2);
+  vector<double> beforeQuat = beforeRotation.toQuaternion();
+  ASSERT_EQ(beforeQuat.size(), 4);
+  EXPECT_NEAR(beforeQuat[0], 1.0, 1e-10);
+  EXPECT_NEAR(beforeQuat[1], 0.0, 1e-10);
+  EXPECT_NEAR(beforeQuat[2], 0.0, 1e-10);
+  EXPECT_NEAR(beforeQuat[3], 0.0, 1e-10);
+}
+
 TEST_F(OrientationTest, InterpolateAtRotation) {
   Rotation interpRotation = orientations.interpolate(0.0);
   vector<double> quat = interpRotation.toQuaternion();
@@ -89,9 +133,9 @@ TEST_F(OrientationTest, InterpolateAtRotation) {
 
 TEST_F(OrientationTest, InterpolateAv) {
   Vec3d interpAv = orientations.interpolateAV(0.25);
-  EXPECT_NEAR(interpAv.x, 2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(interpAv.y, 2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(interpAv.z, 2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(interpAv.x, M_PI / (3.0 * sqrt(3.0)), 1e-10);
+  EXPECT_NEAR(interpAv.y, M_PI / (3.0 * sqrt(3.0)), 1e-10);
+  EXPECT_NEAR(interpAv.z, M_PI / (3.0 * sqrt(3.0)), 1e-10);
 }
 
 TEST_F(OrientationTest, RotateAt) {
@@ -248,10 +292,11 @@ TEST_F(ConstOrientationTest, OrientationInverse) {
   }
 
   vector<Vec3d> newAvs = inverseOrientation.getAngularVelocities();
+  double avConstant = M_PI / (3.0 * sqrt(3.0));
   vector<Vec3d> expectedAvs = {
-    Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI),
-    Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI),
-    Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI)
+    Vec3d(-avConstant, avConstant, avConstant),
+    Vec3d(-avConstant, avConstant, avConstant),
+    Vec3d(-avConstant, avConstant, avConstant)
   };
   ASSERT_EQ(newAvs.size(), expectedAvs.size());
   EXPECT_EQ(newAvs[0].x, expectedAvs[0].x);
@@ -264,3 +309,13 @@ TEST_F(ConstOrientationTest, OrientationInverse) {
   EXPECT_EQ(newAvs[2].y, expectedAvs[2].y);
   EXPECT_EQ(newAvs[2].z, expectedAvs[2].z);
 }
+
+TEST_F(SingleOrientationTest, extrapolate) {
+    Rotation interpRotation = orientations.interpolate(2);
+    vector<double> quat = interpRotation.toQuaternion();
+    ASSERT_EQ(quat.size(), 4);
+    EXPECT_NEAR(quat[0], -0.5, 1e-10);
+    EXPECT_NEAR(quat[1], 0.5, 1e-10);
+    EXPECT_NEAR(quat[2], 0.5, 1e-10);
+    EXPECT_NEAR(quat[3], 0.5, 1e-10);
+}

tests/ctests/TestInterpUtils.cpp

@@ -245,6 +245,9 @@ TEST(InterpUtilsTest, interpolationIndex) {
   EXPECT_EQ(interpolationIndex({1, 3, 5, 6}, 4), 1);
   EXPECT_EQ(interpolationIndex({1, 3, 5, 6}, 0), 0);
   EXPECT_EQ(interpolationIndex({1, 3, 5, 6}, 8), 2);
+  EXPECT_EQ(interpolationIndex({1}, 8), 0);
+  EXPECT_EQ(interpolationIndex({1}, -2), 0);
+  ASSERT_THROW(interpolationIndex({}, 4), std::invalid_argument);
 }
 
 TEST(InterpUtilsTest, orderedVecMerge) {