Skip to content
Snippets Groups Projects
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
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
.travis.yml
+ 4
− 0
View file @ 28a4c5af
...@@ -43,6 +43,10 @@ install: ...@@ -43,6 +43,10 @@ install:
- conda env create -n ale python=3.7.3 - conda env create -n ale python=3.7.3
- conda env update -f environment.yml -n ale - conda env update -f environment.yml -n ale
- source activate 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 - conda install pytest
script: script:
......
include/ale/Vectors.h
+ 5
− 0
View file @ 28a4c5af
...@@ -3,6 +3,7 @@ ...@@ -3,6 +3,7 @@
#include <stdexcept> #include <stdexcept>
#include <vector> #include <vector>
#include <math.h>
namespace ale { namespace ale {
/** A 3D cartesian vector */ /** A 3D cartesian vector */
...@@ -43,6 +44,10 @@ namespace ale { ...@@ -43,6 +44,10 @@ namespace ale {
z -= addend.z; z -= addend.z;
return *this; return *this;
}; };
double norm() const {
return sqrt(x*x + y*y + z*z);
}
}; };
Vec3d operator*(double scalar, Vec3d vec); Vec3d operator*(double scalar, Vec3d vec);
......
src/InterpUtils.cpp
+ 2
− 2
View file @ 28a4c5af
...@@ -34,8 +34,8 @@ namespace ale { ...@@ -34,8 +34,8 @@ namespace ale {
} }
int interpolationIndex(const std::vector<double> &times, double interpTime) { int interpolationIndex(const std::vector<double> &times, double interpTime) {
if (times.size() < 2){ if (times.empty()){
throw std::invalid_argument("There must be at least two times."); throw std::invalid_argument("There must be at least one time.");
} }
auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime); auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime);
if (nextTimeIt == times.end()) { if (nextTimeIt == times.end()) {
......
src/Orientations.cpp
+ 29
− 11
View file @ 28a4c5af
...@@ -13,8 +13,8 @@ namespace ale { ...@@ -13,8 +13,8 @@ namespace ale {
const std::vector<int> time_dependent_frames 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) { 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) { if (m_rotations.size() < 1 || m_times.size() < 1) {
throw std::invalid_argument("There must be at least two rotations and times."); throw std::invalid_argument("There must be at least one rotation and time.");
} }
if (m_rotations.size() != m_times.size()) { if (m_rotations.size() != m_times.size()) {
throw std::invalid_argument("The number of rotations and times must be the same."); throw std::invalid_argument("The number of rotations and times must be the same.");
...@@ -55,16 +55,36 @@ namespace ale { ...@@ -55,16 +55,36 @@ namespace ale {
double time, double time,
RotationInterpolation interpType RotationInterpolation interpType
) const { ) const {
Rotation interpRotation;
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]);
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 Orientations::interpolateAV(double time) const {
Vec3d interpAv;
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]);
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; return interpAv;
} }
...@@ -119,9 +139,7 @@ namespace ale { ...@@ -119,9 +139,7 @@ namespace ale {
mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot); mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot);
Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time)); Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
Vec3d rhsAv = rhs.interpolateAV(time); Vec3d rhsAv = rhs.interpolateAV(time);
combinedAv.x += rhsAv.x; combinedAv += rhsAv;
combinedAv.y += rhsAv.y;
combinedAv.z += rhsAv.z;
mergedAvs.push_back(combinedAv); mergedAvs.push_back(combinedAv);
} }
......
tests/ctests/OrientationsTests.cpp
+ 64
− 9
View file @ 28a4c5af
...@@ -17,9 +17,10 @@ class OrientationTest : public ::testing::Test { ...@@ -17,9 +17,10 @@ class OrientationTest : public ::testing::Test {
times.push_back(0); times.push_back(0);
times.push_back(2); times.push_back(2);
times.push_back(4); 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)); double avConstant = M_PI / (3.0 * sqrt(3.0));
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(avConstant, avConstant, avConstant));
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(avConstant, avConstant, avConstant));
avs.push_back(Vec3d(avConstant, avConstant, avConstant));
orientations = Orientations(rotations, times, avs); orientations = Orientations(rotations, times, avs);
} }
...@@ -43,6 +44,31 @@ class ConstOrientationTest : public OrientationTest{ ...@@ -43,6 +44,31 @@ class ConstOrientationTest : public OrientationTest{
Orientations constOrientations; 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) { TEST_F(OrientationTest, ConstructorAccessors) {
vector<Rotation> outputRotations = orientations.getRotations(); vector<Rotation> outputRotations = orientations.getRotations();
vector<double> outputTimes = orientations.getTimes(); vector<double> outputTimes = orientations.getTimes();
...@@ -77,6 +103,24 @@ TEST_F(OrientationTest, Interpolate) { ...@@ -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); 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) { TEST_F(OrientationTest, InterpolateAtRotation) {
Rotation interpRotation = orientations.interpolate(0.0); Rotation interpRotation = orientations.interpolate(0.0);
vector<double> quat = interpRotation.toQuaternion(); vector<double> quat = interpRotation.toQuaternion();
...@@ -89,9 +133,9 @@ TEST_F(OrientationTest, InterpolateAtRotation) { ...@@ -89,9 +133,9 @@ TEST_F(OrientationTest, InterpolateAtRotation) {
TEST_F(OrientationTest, InterpolateAv) { TEST_F(OrientationTest, InterpolateAv) {
Vec3d interpAv = orientations.interpolateAV(0.25); Vec3d interpAv = orientations.interpolateAV(0.25);
EXPECT_NEAR(interpAv.x, 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, 2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(interpAv.y, M_PI / (3.0 * sqrt(3.0)), 1e-10);
EXPECT_NEAR(interpAv.z, 2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(interpAv.z, M_PI / (3.0 * sqrt(3.0)), 1e-10);
} }
TEST_F(OrientationTest, RotateAt) { TEST_F(OrientationTest, RotateAt) {
...@@ -248,10 +292,11 @@ TEST_F(ConstOrientationTest, OrientationInverse) { ...@@ -248,10 +292,11 @@ TEST_F(ConstOrientationTest, OrientationInverse) {
} }
vector<Vec3d> newAvs = inverseOrientation.getAngularVelocities(); vector<Vec3d> newAvs = inverseOrientation.getAngularVelocities();
double avConstant = M_PI / (3.0 * sqrt(3.0));
vector<Vec3d> expectedAvs = { vector<Vec3d> expectedAvs = {
Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI), Vec3d(-avConstant, avConstant, avConstant),
Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI), Vec3d(-avConstant, avConstant, avConstant),
Vec3d(-2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI) Vec3d(-avConstant, avConstant, avConstant)
}; };
ASSERT_EQ(newAvs.size(), expectedAvs.size()); ASSERT_EQ(newAvs.size(), expectedAvs.size());
EXPECT_EQ(newAvs[0].x, expectedAvs[0].x); EXPECT_EQ(newAvs[0].x, expectedAvs[0].x);
...@@ -264,3 +309,13 @@ TEST_F(ConstOrientationTest, OrientationInverse) { ...@@ -264,3 +309,13 @@ TEST_F(ConstOrientationTest, OrientationInverse) {
EXPECT_EQ(newAvs[2].y, expectedAvs[2].y); EXPECT_EQ(newAvs[2].y, expectedAvs[2].y);
EXPECT_EQ(newAvs[2].z, expectedAvs[2].z); 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);
}
tests/ctests/TestInterpUtils.cpp
+ 3
− 0
View file @ 28a4c5af
...@@ -245,6 +245,9 @@ TEST(InterpUtilsTest, interpolationIndex) { ...@@ -245,6 +245,9 @@ TEST(InterpUtilsTest, interpolationIndex) {
EXPECT_EQ(interpolationIndex({1, 3, 5, 6}, 4), 1); 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}, 0), 0);
EXPECT_EQ(interpolationIndex({1, 3, 5, 6}, 8), 2); 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) { TEST(InterpUtilsTest, orderedVecMerge) {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment