Commit 633f4dfb authored by Jesse Mapel's avatar Jesse Mapel
Browse files

Added time dep only rotation interpolation

parent 473aeebc

include/ale/Orientations.h

+8 −0
Original line number Original line Diff line number Diff line
@@ -41,6 +41,14 @@ namespace ale { 
    std::vector<int> getTimeDependentFrames() const;

    std::vector<int> getTimeDependentFrames() const;

    Rotation getConstantRotation() const;

    Rotation getConstantRotation() const;





    /**

     * Get the time dependent component of the interpolated rotation at a specific time.

     */

    Rotation interpolateTimeDep(

      double time,

      RotationInterpolation interpType=SLERP

    ) const;



    /**

    /**

     * Get the interpolated rotation at a specific time.

     * Get the interpolated rotation at a specific time.

     */

     */

src/Orientations.cpp

+13 −6
Original line number Original line Diff line number Diff line
@@ -51,27 +51,34 @@ namespace ale { 
    return m_constRotation;

    return m_constRotation;

  }

  }





  Rotation Orientations::interpolate(

  Rotation Orientations::interpolateTimeDep(

    double time,

    double time,

    RotationInterpolation interpType

    RotationInterpolation interpType

  ) const {

  ) const {

    Rotation interpRotation;

    Rotation timeDepRotation;

    if (m_times.size() > 1) {

    if (m_times.size() > 1) {

      int interpIndex = interpolationIndex(m_times, time);

      int interpIndex = interpolationIndex(m_times, time);

      double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);

      double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);

      interpRotation = m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);

      timeDepRotation = m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);

    }

    }

    else if (m_avs.empty()) {

    else if (m_avs.empty()) {

      interpRotation = m_constRotation * m_rotations.front();

      timeDepRotation = m_rotations.front();

    }

    }

    else {

    else {

      double t = time - m_times.front();

      double t = time - m_times.front();

      std::vector<double> axis = {m_avs.front().x, m_avs.front().y, m_avs.front().z};

      std::vector<double> axis = {m_avs.front().x, m_avs.front().y, m_avs.front().z};

      double angle = t * m_avs.front().norm();

      double angle = t * m_avs.front().norm();

      Rotation newRotation(axis, angle);

      Rotation newRotation(axis, angle);

      interpRotation = m_constRotation * newRotation * m_rotations.front();

      timeDepRotation = newRotation * m_rotations.front();

    }

    return timeDepRotation;

  }

  }

    return interpRotation;



  Rotation Orientations::interpolate(

    double time,

    RotationInterpolation interpType

  ) const {

    return m_constRotation * interpolateTimeDep(time, interpType);

  }

  }

tests/ctests/OrientationsTests.cpp

+13 −0
Original line number Original line Diff line number Diff line
@@ -93,6 +93,19 @@ TEST_F(OrientationTest, ConstructorAccessors) { 
  }

  }

}

}





TEST_F(ConstOrientationTest, InterpolateTimeDep) {

  Rotation interpRotation = constOrientations.interpolateTimeDep(0.25);

  Rotation expectedRotation = orientations.interpolate(0.25);

  vector<double> quat = interpRotation.toQuaternion();

  vector<double> expectedQuat = expectedRotation.toQuaternion();

  ASSERT_EQ(quat.size(), 4);

  ASSERT_EQ(expectedQuat.size(), 4);

  EXPECT_NEAR(quat[0], expectedQuat[0], 1e-10);

  EXPECT_NEAR(quat[1], expectedQuat[1], 1e-10);

  EXPECT_NEAR(quat[2], expectedQuat[2], 1e-10);

  EXPECT_NEAR(quat[3], expectedQuat[3], 1e-10);

}



TEST_F(OrientationTest, Interpolate) {

TEST_F(OrientationTest, Interpolate) {

  Rotation interpRotation = orientations.interpolate(0.25);

  Rotation interpRotation = orientations.interpolate(0.25);

  vector<double> quat = interpRotation.toQuaternion();

  vector<double> quat = interpRotation.toQuaternion();