Commit 28a4c5af authored by Jesse Mapel's avatar Jesse Mapel
Browse files

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
parent 11b9ff81

.travis.yml

+4 −0
Original line number Diff line number Diff line
@@ -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

+5 −0
Original line number Diff line number Diff line
@@ -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

+2 −2
Original line number Diff line number Diff line
@@ -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

+29 −11
Original line number Diff line number Diff line
@@ -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

+64 −9
Original line number Diff line number Diff line
@@ -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);

}