diff --git a/.travis.yml b/.travis.yml
index 8b4bddddc164277290e79d9ea8cf5a680a20e27a..edc87c644d1e1bb99f43064c90a7fdb10ec2ba4a 100644
--- a/.travis.yml
+++ b/.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:
diff --git a/include/ale/Vectors.h b/include/ale/Vectors.h
index dabde1ef28f6aa34e9080d77640b59279151009d..93e3baa8460d71d3174481f9a39e547127253642 100644
--- a/include/ale/Vectors.h
+++ b/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);
diff --git a/src/InterpUtils.cpp b/src/InterpUtils.cpp
index 741a70997c3344ecd7cc42d48498323336afec46..2b9d5dc13e76a1bad40d53cc593d8dce9337c96f 100644
--- a/src/InterpUtils.cpp
+++ b/src/InterpUtils.cpp
@@ -34,8 +34,8 @@ namespace ale {
}
int interpolationIndex(const std::vector<double> ×, 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()) {
diff --git a/src/Orientations.cpp b/src/Orientations.cpp
index 799f73a62662590b4bc337e78dc6995b3f184abe..ef872f2f119fd9419766317254b9f85717f7ff88 100644
--- a/src/Orientations.cpp
+++ b/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 {
- 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);
+ 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]);
+ 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 {
- 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));
+ 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]);
+ 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);
}
diff --git a/tests/ctests/OrientationsTests.cpp b/tests/ctests/OrientationsTests.cpp
index 31d5e40fc2f394ae6f98add0d07b9c1ef846f3a7..285efac258c17d3bf32eb2988981b2f4a69133f3 100644
--- a/tests/ctests/OrientationsTests.cpp
+++ b/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);
+}
diff --git a/tests/ctests/TestInterpUtils.cpp b/tests/ctests/TestInterpUtils.cpp
index 457a70c75a8f648c8531ed3ffcdb5a96c089f87f..4ac6046582279e71b6390af8cd2a37473d231375 100644
--- a/tests/ctests/TestInterpUtils.cpp
+++ b/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) {