diff --git a/src/Orientations.cpp b/src/Orientations.cpp index 0e5c3d069de56db6a5fc8eb5ea44fee27dcf2c90..503179c47c93420bab446a81f2e39d5b6874386b 100644 --- a/src/Orientations.cpp +++ b/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,10 +147,12 @@ namespace ale { Rotation inverseConst = m_constRotation.inverse(); Rotation rhsRot = rhs.interpolate(time); mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot); - Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time)); - Vec3d rhsAv = rhs.interpolateAV(time); - combinedAv += rhsAv; - mergedAvs.push_back(combinedAv); + 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; diff --git a/tests/ctests/OrientationsTests.cpp b/tests/ctests/OrientationsTests.cpp index 40df1fd98ee44a286c1a6d39e86293ecaf999cd8..71ade67570b37163ae3caa950c039bf559460835 100644 --- a/tests/ctests/OrientationsTests.cpp +++ b/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) {