From 57c6481e5ef9d872bf90aebf1f7ce704c64c411d Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Tue, 16 Jun 2020 13:24:11 -0700
Subject: [PATCH] Added Orientations operations (#360)
* Ticked version #
* Added Orientations::inverse
* Added Orientation inverse test
* Added Orientation multiplication operaotrs
* Added Vector operators
* Now using new operator
* Improved docs
* Small fix for exported target
---
CMakeLists.txt | 5 +-
cmake/config.cmake.in | 1 +
include/ale/Orientations.h | 24 +++++++
include/ale/Vectors.h | 29 +++++++++
setup.py | 2 +-
src/Orientations.cpp | 59 +++++++++++++++++
src/Vectors.cpp | 19 ++++++
tests/ctests/CMakeLists.txt | 1 +
tests/ctests/OrientationsTests.cpp | 100 ++++++++++++++++++++++++++---
tests/ctests/VectorTests.cpp | 55 ++++++++++++++++
10 files changed, 283 insertions(+), 12 deletions(-)
create mode 100644 src/Vectors.cpp
create mode 100644 tests/ctests/VectorTests.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a5c4adf..9c5ab7e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,7 +7,7 @@
# Specify the required version of CMake.
# cmake 3.10 required for ctest/gtest integration
cmake_minimum_required(VERSION 3.10)
-project(ale VERSION 0.8.0 DESCRIPTION "Abstraction Library for Ephemerides ")
+project(ale VERSION 0.8.1 DESCRIPTION "Abstraction Library for Ephemerides ")
# include what we need
include(GNUInstallDirs)
@@ -53,7 +53,8 @@ set(ALE_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/src/InterpUtils.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/Orientations.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/States.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/Isd.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/src/Util.cpp)
+ ${CMAKE_CURRENT_SOURCE_DIR}/src/Util.cpp
+ ${CMAKE_CURRENT_SOURCE_DIR}/src/Vectors.cpp)
set(ALE_HEADER_FILES ${ALE_BUILD_INCLUDE_DIR}/InterpUtils.h
${ALE_BUILD_INCLUDE_DIR}/Rotation.h
${ALE_BUILD_INCLUDE_DIR}/Orientations.h
diff --git a/cmake/config.cmake.in b/cmake/config.cmake.in
index c223d95..82500f6 100644
--- a/cmake/config.cmake.in
+++ b/cmake/config.cmake.in
@@ -3,5 +3,6 @@ set(${CMAKE_FIND_PACKAGE_NAME}_CONFIG ${CMAKE_CURRENT_LIST_FILE})
find_package_handle_standard_args(@PROJECT_NAME@ CONFIG_MODE)
if(NOT TARGET @PROJECT_NAME@::ale)
+ find_package(nlohmann_json REQUIRED)
include("${CMAKE_CURRENT_LIST_DIR}/aleTargets.cmake")
endif()
diff --git a/include/ale/Orientations.h b/include/ale/Orientations.h
index 3a253d1..ece9eef 100644
--- a/include/ale/Orientations.h
+++ b/include/ale/Orientations.h
@@ -71,15 +71,35 @@ namespace ale {
bool invert=false
) const;
+ /**
+ * Add an additional constant multiplication.
+ * This is equivalent to left multiplication by a constant rotation
+ */
+ Orientations &addConstantRotation(const Rotation &addedConst);
+
/**
* Multiply this orientation by another orientation
*/
Orientations &operator*=(const Orientations &rhs);
+
/**
* Multiply this orientation by a constant rotation
*/
Orientations &operator*=(const Rotation &rhs);
+ /**
+ * Invert the orientations.
+ * Note that inverting a set of orientations twice does not result in
+ * the original orientations. the constant rotation is applied after the
+ * time dependent rotation. This means in the inverse, the constant
+ * rotation is applied first and then the time dependent rotation.
+ * So, the inverted set of orientations is entirely time dependent.
+ * Then, the original constant rotations cannot be recovered when inverting
+ * again. The set of orientations will still operate the same way, but its
+ * internal state will not be the same.
+ */
+ Orientations inverse() const;
+
private:
std::vector<Rotation> m_rotations;
std::vector<ale::Vec3d> m_avs;
@@ -88,6 +108,10 @@ namespace ale {
std::vector<int> m_constFrames;
Rotation m_constRotation;
};
+
+ Orientations operator*(Orientations lhs, const Rotation &rhs);
+ Orientations operator*(const Rotation &lhs, Orientations rhs);
+ Orientations operator*(Orientations lhs, const Orientations &rhs);
}
#endif
diff --git a/include/ale/Vectors.h b/include/ale/Vectors.h
index 3992073..dabde1e 100644
--- a/include/ale/Vectors.h
+++ b/include/ale/Vectors.h
@@ -2,6 +2,7 @@
#define ALE_VECTORS_H
#include <stdexcept>
+#include <vector>
namespace ale {
/** A 3D cartesian vector */
@@ -22,7 +23,35 @@ namespace ale {
Vec3d(double x, double y, double z) : x(x), y(y), z(z) {};
Vec3d() : x(0.0), y(0.0), z(0.0) {};
+ Vec3d &operator*=(double scalar) {
+ x *= scalar;
+ y *= scalar;
+ z *= scalar;
+ return *this;
+ };
+
+ Vec3d &operator+=(Vec3d addend) {
+ x += addend.x;
+ y += addend.y;
+ z += addend.z;
+ return *this;
+ };
+
+ Vec3d &operator-=(Vec3d addend) {
+ x -= addend.x;
+ y -= addend.y;
+ z -= addend.z;
+ return *this;
+ };
};
+
+ Vec3d operator*(double scalar, Vec3d vec);
+
+ Vec3d operator*(Vec3d vec, double scalar);
+
+ Vec3d operator+(Vec3d leftVec, const Vec3d &rightVec);
+
+ Vec3d operator-(Vec3d leftVec, const Vec3d &rightVec);
}
#endif
diff --git a/setup.py b/setup.py
index 3b1a2b7..bb71daa 100644
--- a/setup.py
+++ b/setup.py
@@ -4,7 +4,7 @@ import sys
from setuptools import setup, find_packages
NAME = "Ale"
-VERSION = "0.8.0"
+VERSION = "0.8.1"
# To install the library, run the following
#
diff --git a/src/Orientations.cpp b/src/Orientations.cpp
index 004b2c0..799f73a 100644
--- a/src/Orientations.cpp
+++ b/src/Orientations.cpp
@@ -102,6 +102,12 @@ namespace ale {
}
+ Orientations &Orientations::addConstantRotation(const Rotation &addedConst) {
+ m_constRotation = addedConst * m_constRotation;
+ return *this;
+ }
+
+
Orientations &Orientations::operator*=(const Orientations &rhs) {
std::vector<double> mergedTimes = orderedVecMerge(m_times, rhs.m_times);
std::vector<Rotation> mergedRotations;
@@ -144,4 +150,57 @@ namespace ale {
return *this;
}
+
+
+ Orientations Orientations::inverse() const {
+ std::vector<Rotation> newRotations;
+ // The time dependent rotation is applied and the constant rotation is applied second,
+ // so we have to subsume the constant rotations into the time dependent rotations
+ // in the inverse.
+ Rotation constInverseRotation = m_constRotation.inverse();
+ for (size_t i = 0; i < m_rotations.size(); i++) {
+ newRotations.push_back(m_rotations[i].inverse() * constInverseRotation);
+ }
+
+ std::vector<Vec3d> rotatedAvs;
+ for (size_t i = 0; i < m_avs.size(); i++) {
+ Vec3d rotatedAv = -1.0 * (m_constRotation * m_rotations[i])(m_avs[i]);
+ rotatedAvs.push_back(rotatedAv);
+ }
+
+ // Because the constant rotation was subsumed by the time dependet rotations, everything
+ // is a time dependent rotation in the inverse.
+ std::vector<int> newTimeDepFrames;
+ std::vector<int>::const_reverse_iterator timeDepIt = m_timeDepFrames.crbegin();
+ for (; timeDepIt != m_timeDepFrames.crend(); timeDepIt++) {
+ newTimeDepFrames.push_back(*timeDepIt);
+ }
+ std::vector<int>::const_reverse_iterator constIt = m_constFrames.crbegin();
+ // Skip the last frame in the constant list because it's the first frame
+ // in the time dependent list
+ if (constIt != m_constFrames.crend()) {
+ constIt++;
+ }
+ for (; constIt != m_constFrames.rend(); constIt++) {
+ newTimeDepFrames.push_back(*constIt);
+ }
+
+ return Orientations(newRotations, m_times, rotatedAvs, Rotation(1, 0, 0, 0),
+ std::vector<int>(), newTimeDepFrames);
+ }
+
+
+ Orientations operator*(Orientations lhs, const Rotation &rhs) {
+ return lhs *= rhs;
+ }
+
+
+ Orientations operator*(const Rotation &lhs, Orientations rhs) {
+ return rhs.addConstantRotation(lhs);
+ }
+
+
+ Orientations operator*(Orientations lhs, const Orientations &rhs) {
+ return lhs *= rhs;
+ }
}
diff --git a/src/Vectors.cpp b/src/Vectors.cpp
new file mode 100644
index 0000000..396ffee
--- /dev/null
+++ b/src/Vectors.cpp
@@ -0,0 +1,19 @@
+#include "ale/Vectors.h"
+
+namespace ale {
+ Vec3d operator*(double scalar, Vec3d vec) {
+ return vec *= scalar;
+ }
+
+ Vec3d operator*(Vec3d vec, double scalar) {
+ return vec *= scalar;
+ }
+
+ Vec3d operator+(Vec3d leftVec, const Vec3d &rightVec) {
+ return leftVec += rightVec;
+ }
+
+ Vec3d operator-(Vec3d leftVec, const Vec3d &rightVec) {
+ return leftVec -= rightVec;
+ }
+}
diff --git a/tests/ctests/CMakeLists.txt b/tests/ctests/CMakeLists.txt
index cf4ab52..bb61043 100644
--- a/tests/ctests/CMakeLists.txt
+++ b/tests/ctests/CMakeLists.txt
@@ -6,6 +6,7 @@ set (ALE_TEST_SOURCE ${CMAKE_SOURCE_DIR}/tests/ctests/IsdTests.cpp
${CMAKE_SOURCE_DIR}/tests/ctests/RotationTests.cpp
${CMAKE_SOURCE_DIR}/tests/ctests/StatesTests.cpp
${CMAKE_SOURCE_DIR}/tests/ctests/TestInterpUtils.cpp
+ ${CMAKE_SOURCE_DIR}/tests/ctests/VectorTests.cpp
${CMAKE_SOURCE_DIR}/tests/ctests/TestMain.cpp)
if(ALE_BUILD_LOAD)
diff --git a/tests/ctests/OrientationsTests.cpp b/tests/ctests/OrientationsTests.cpp
index 0b9d511..31d5e40 100644
--- a/tests/ctests/OrientationsTests.cpp
+++ b/tests/ctests/OrientationsTests.cpp
@@ -34,7 +34,9 @@ class ConstOrientationTest : public OrientationTest{
void SetUp() override {
OrientationTest::SetUp();
constRotation = Rotation(0, 1, 0, 0);
- constOrientations = Orientations(rotations, times, avs, constRotation);
+ vector<int> constFrames = {-74021, -74020, -74000};
+ vector<int> timeDepFrames = {-74000, -74900, 1};
+ constOrientations = Orientations(rotations, times, avs, constRotation, constFrames, timeDepFrames);
}
Rotation constRotation;
@@ -108,21 +110,47 @@ TEST_F(OrientationTest, RotateAt) {
}
TEST_F(OrientationTest, RotationMultiplication) {
- Rotation rhs( 0.5, 0.5, 0.5, 0.5);
- orientations *= rhs;
- vector<Rotation> outputRotations = orientations.getRotations();
+ vector<Rotation> originalRotations = orientations.getRotations();
+ vector<double> originalConstQuats = orientations.getConstantRotation().toQuaternion();
+ Rotation constRot( 0.5, 0.5, 0.5, 0.5);
vector<vector<double>> expectedQuats = {
{-0.5, 0.5, 0.5, 0.5},
{-1.0, 0.0, 0.0, 0.0},
{ 0.5, 0.5, 0.5, 0.5}
};
- for (size_t i = 0; i < outputRotations.size(); i++) {
- vector<double> quats = outputRotations[i].toQuaternion();
+
+ Orientations rightMultiplied = orientations * constRot;
+ vector<Rotation> outputRightRotations = rightMultiplied.getRotations();
+ ASSERT_EQ(expectedQuats.size(), outputRightRotations.size());
+ for (size_t i = 0; i < outputRightRotations.size(); i++) {
+ vector<double> quats = outputRightRotations[i].toQuaternion();
EXPECT_EQ(expectedQuats[i][0], quats[0]);
EXPECT_EQ(expectedQuats[i][1], quats[1]);
EXPECT_EQ(expectedQuats[i][2], quats[2]);
EXPECT_EQ(expectedQuats[i][3], quats[3]);
}
+ vector<double> outputRightConstQuats = rightMultiplied.getConstantRotation().toQuaternion();
+ EXPECT_EQ(originalConstQuats[0], outputRightConstQuats[0]);
+ EXPECT_EQ(originalConstQuats[1], outputRightConstQuats[1]);
+ EXPECT_EQ(originalConstQuats[2], outputRightConstQuats[2]);
+ EXPECT_EQ(originalConstQuats[3], outputRightConstQuats[3]);
+
+ Orientations leftMultiplied = constRot * orientations;
+ vector<Rotation> outputLeftRotations = leftMultiplied.getRotations();
+ ASSERT_EQ(originalRotations.size(), outputLeftRotations.size());
+ for (size_t i = 0; i < outputLeftRotations.size(); i++) {
+ vector<double> originalQuats = originalRotations[i].toQuaternion();
+ vector<double> quats = outputLeftRotations[i].toQuaternion();
+ EXPECT_EQ(originalQuats[0], quats[0]);
+ EXPECT_EQ(originalQuats[1], quats[1]);
+ EXPECT_EQ(originalQuats[2], quats[2]);
+ EXPECT_EQ(originalQuats[3], quats[3]);
+ }
+ vector<double> outputLeftConstQuats = leftMultiplied.getConstantRotation().toQuaternion();
+ EXPECT_EQ(0.5, outputLeftConstQuats[0]);
+ EXPECT_EQ(0.5, outputLeftConstQuats[1]);
+ EXPECT_EQ(0.5, outputLeftConstQuats[2]);
+ EXPECT_EQ(0.5, outputLeftConstQuats[3]);
}
TEST_F(OrientationTest, OrientationMultiplication) {
@@ -162,8 +190,8 @@ TEST_F(ConstOrientationTest, RotateAt) {
}
TEST_F(ConstOrientationTest, OrientationMultiplication) {
- constOrientations *= orientations;
- vector<Rotation> outputRotations = constOrientations.getRotations();
+ Orientations multOrientation = constOrientations * orientations;
+ vector<Rotation> outputRotations = multOrientation.getRotations();
vector<vector<double>> expectedQuats = {
{-0.5, 0.5, 0.5, 0.5},
{-0.5,-0.5,-0.5,-0.5},
@@ -176,9 +204,63 @@ TEST_F(ConstOrientationTest, OrientationMultiplication) {
EXPECT_EQ(expectedQuats[i][2], quats[2]);
EXPECT_EQ(expectedQuats[i][3], quats[3]);
}
- vector<double> constQuats = constOrientations.getConstantRotation().toQuaternion();
+ vector<double> constQuats = multOrientation.getConstantRotation().toQuaternion();
EXPECT_EQ(constQuats[0], 0);
EXPECT_EQ(constQuats[1], 1);
EXPECT_EQ(constQuats[2], 0);
EXPECT_EQ(constQuats[3], 0);
}
+
+TEST_F(ConstOrientationTest, OrientationInverse) {
+ Orientations inverseOrientation = constOrientations.inverse();
+
+ vector<int> constFrames = inverseOrientation.getConstantFrames();
+ ASSERT_EQ(constFrames.size(), 0);
+
+ vector<int> timeDepFrames = inverseOrientation.getTimeDependentFrames();
+ ASSERT_EQ(timeDepFrames.size(), 5);
+ EXPECT_EQ(timeDepFrames[0], 1);
+ EXPECT_EQ(timeDepFrames[1], -74900);
+ EXPECT_EQ(timeDepFrames[2], -74000);
+ EXPECT_EQ(timeDepFrames[3], -74020);
+ EXPECT_EQ(timeDepFrames[4], -74021);
+
+ vector<double> newTimes = inverseOrientation.getTimes();
+ ASSERT_EQ(newTimes.size(), 3);
+ EXPECT_EQ(newTimes[0], 0.0);
+ EXPECT_EQ(newTimes[1], 2.0);
+ EXPECT_EQ(newTimes[2], 4.0);
+
+ vector<Rotation> newRotations = inverseOrientation.getRotations();
+ vector<vector<double>> expectedQuats = {
+ {-0.5, -0.5, 0.5, -0.5},
+ {-0.5, 0.5, 0.5, -0.5},
+ {0.0, -1.0, 0.0, 0.0}
+ };
+ ASSERT_EQ(newRotations.size(), expectedQuats.size());
+ for (size_t i = 0; i < newRotations.size(); i++) {
+ vector<double> newQuats = newRotations[i].toQuaternion();
+ ASSERT_EQ(newQuats.size(), 4) << "Rotation " << i;
+ EXPECT_EQ(newQuats[0], expectedQuats[i][0]) << "Rotation " << i;
+ EXPECT_EQ(newQuats[1], expectedQuats[i][1]) << "Rotation " << i;
+ EXPECT_EQ(newQuats[2], expectedQuats[i][2]) << "Rotation " << i;
+ EXPECT_EQ(newQuats[3], expectedQuats[i][3]) << "Rotation " << i;
+ }
+
+ vector<Vec3d> newAvs = inverseOrientation.getAngularVelocities();
+ 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)
+ };
+ ASSERT_EQ(newAvs.size(), expectedAvs.size());
+ EXPECT_EQ(newAvs[0].x, expectedAvs[0].x);
+ EXPECT_EQ(newAvs[0].y, expectedAvs[0].y);
+ EXPECT_EQ(newAvs[0].z, expectedAvs[0].z);
+ EXPECT_EQ(newAvs[1].x, expectedAvs[1].x);
+ EXPECT_EQ(newAvs[1].y, expectedAvs[1].y);
+ EXPECT_EQ(newAvs[1].z, expectedAvs[1].z);
+ EXPECT_EQ(newAvs[2].x, expectedAvs[2].x);
+ EXPECT_EQ(newAvs[2].y, expectedAvs[2].y);
+ EXPECT_EQ(newAvs[2].z, expectedAvs[2].z);
+}
diff --git a/tests/ctests/VectorTests.cpp b/tests/ctests/VectorTests.cpp
new file mode 100644
index 0000000..ca6aa4d
--- /dev/null
+++ b/tests/ctests/VectorTests.cpp
@@ -0,0 +1,55 @@
+#include "gmock/gmock.h"
+
+#include "ale/Vectors.h"
+
+using namespace std;
+using namespace ale;
+
+TEST(Vector, multiplication) {
+ Vec3d testVec(1.0, 2.0, 3.0);
+
+ Vec3d scaledVec = 2.0 * testVec;
+ EXPECT_EQ(scaledVec.x, 2.0);
+ EXPECT_EQ(scaledVec.y, 4.0);
+ EXPECT_EQ(scaledVec.z, 6.0);
+
+ Vec3d rightMultiplied = testVec * 2.0;
+ EXPECT_EQ(rightMultiplied.x, 2.0);
+ EXPECT_EQ(rightMultiplied.y, 4.0);
+ EXPECT_EQ(rightMultiplied.z, 6.0);
+
+ testVec *= -1;
+ EXPECT_EQ(testVec.x, -1.0);
+ EXPECT_EQ(testVec.y, -2.0);
+ EXPECT_EQ(testVec.z, -3.0);
+}
+
+TEST(Vector, addition) {
+ Vec3d vec1(1.0, 2.0, 3.0);
+ Vec3d vec2(5.0, 7.0, 9.0);
+
+ Vec3d sumVec = vec1 + vec2;
+ EXPECT_EQ(sumVec.x, 6.0);
+ EXPECT_EQ(sumVec.y, 9.0);
+ EXPECT_EQ(sumVec.z, 12.0);
+
+ vec1 += vec2;
+ EXPECT_EQ(vec1.x, 6.0);
+ EXPECT_EQ(vec1.y, 9.0);
+ EXPECT_EQ(vec1.z, 12.0);
+}
+
+TEST(Vector, subtraction) {
+ Vec3d vec1(1.0, 2.0, 3.0);
+ Vec3d vec2(5.0, 7.0, 9.0);
+
+ Vec3d diffVec = vec1 - vec2;
+ EXPECT_EQ(diffVec.x, -4.0);
+ EXPECT_EQ(diffVec.y, -5.0);
+ EXPECT_EQ(diffVec.z, -6.0);
+
+ vec1 -= vec2;
+ EXPECT_EQ(vec1.x, -4.0);
+ EXPECT_EQ(vec1.y, -5.0);
+ EXPECT_EQ(vec1.z, -6.0);
+}
--
GitLab