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

@@ -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

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()

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

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

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
 #

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;
+  }
 }

src/Vectors.cpp

+#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;
+  }
+}

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)

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);
+}

tests/ctests/VectorTests.cpp

+#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);
+}