Commit 9344dd42 authored by Jesse Mapel's avatar Jesse Mapel Committed by Jesse Mapel
Browse files

Fixed no av seg fault

parent 62455a7f

src/Orientations.cpp

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

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