From 33caa804e99f090f9adb5d076a316cbad5760620 Mon Sep 17 00:00:00 2001
From: acpaquette <acpaquette@usgs.gov>
Date: Thu, 2 Apr 2020 17:25:56 -0700
Subject: [PATCH] Orientation  (#343)

* Added first pass orientations class

* Added stub Orientations tests

* More Orientation tests and fixed interpUtils

* Fixed new enum name

* minimal docs

* Added vec merge

* Added Orientation multiplications

* Moved Vec3d struct into utils

* Removed interpolationIndex from ale.cpp/h as it's now located in interpUtils

* Updated Orientation/Rotation to use State and Vec3d structs

* Updated associated tests

* Updated things based on PR feedback

Co-authored-by: Jesse Mapel <jmapel@usgs.gov>
---
 CMakeLists.txt                    |   4 +
 cmake/gtest.cmake                 |  22 ++-
 include/InterpUtils.h             |  45 ++++++
 include/Orientations.h            |  81 ++++++++++
 include/Rotation.h                |  11 +-
 include/States.h                  |  81 ++++------
 include/Util.h                    |  20 +++
 include/ale.h                     |   5 +-
 src/InterpUtils.cpp               |  61 ++++++++
 src/Orientations.cpp              | 128 +++++++++++++++
 src/Rotation.cpp                  |  68 ++++----
 src/ale.cpp                       |  31 +---
 tests/ctests/CMakeLists.txt       |   1 +
 tests/ctests/OrientationsTest.cpp | 132 ++++++++++++++++
 tests/ctests/RotationTest.cpp     | 250 +++++++++++++-----------------
 tests/ctests/TestInterpUtils.cpp  |  47 ++++++
 16 files changed, 731 insertions(+), 256 deletions(-)
 create mode 100644 include/InterpUtils.h
 create mode 100644 include/Orientations.h
 create mode 100644 src/InterpUtils.cpp
 create mode 100644 src/Orientations.cpp
 create mode 100644 tests/ctests/OrientationsTest.cpp
 create mode 100644 tests/ctests/TestInterpUtils.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77c48a3..9d2de3f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,12 +32,16 @@ set(ALE_BUILD_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include/")
 set(ALE_INSTALL_INCLUDE_DIR "include/ale")
 add_library(ale SHARED
             ${CMAKE_CURRENT_SOURCE_DIR}/src/ale.cpp
+            ${CMAKE_CURRENT_SOURCE_DIR}/src/InterpUtils.cpp
             ${CMAKE_CURRENT_SOURCE_DIR}/src/Rotation.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)
 set(ALE_HEADERS "${ALE_BUILD_INCLUDE_DIR}/ale.h"
+                "${ALE_BUILD_INCLUDE_DIR}/InterpUtils.h"
                 "${ALE_BUILD_INCLUDE_DIR}/Rotation.h"
+                "${ALE_BUILD_INCLUDE_DIR}/Orientations.h"
                 "${ALE_BUILD_INCLUDE_DIR}/States.h"
                 "${ALE_BUILD_INCLUDE_DIR}/Isd.h"
                 "${ALE_BUILD_INCLUDE_DIR}/Util.h")
diff --git a/cmake/gtest.cmake b/cmake/gtest.cmake
index 82035b2..8fbcd5c 100644
--- a/cmake/gtest.cmake
+++ b/cmake/gtest.cmake
@@ -16,4 +16,24 @@ if (NOT TARGET gtest)
   endforeach()
 
   add_library(gtest ${GOOGLETEST_SOURCES})
-endif()
\ No newline at end of file
+endif()
+
+if (NOT TARGET gmock)
+  set(GOOGLEMOCK_ROOT googletest/googlemock CACHE STRING "Google Mock source root")
+
+  include_directories(SYSTEM
+      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}
+      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/include
+      )
+
+  set(GOOGLEMOCK_SOURCES
+      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/src/gmock-all.cc
+      ${PROJECT_SOURCE_DIR}/${GOOGLEMOCK_ROOT}/src/gmock_main.cc
+      )
+
+  foreach(_source ${GOOGLEMOCK_SOURCES})
+      set_source_files_properties(${_source} PROPERTIES GENERATED 1)
+  endforeach()
+
+  add_library(gmock ${GOOGLEMOCK_SOURCES})
+endif()
diff --git a/include/InterpUtils.h b/include/InterpUtils.h
new file mode 100644
index 0000000..ecd533f
--- /dev/null
+++ b/include/InterpUtils.h
@@ -0,0 +1,45 @@
+#ifndef ALE_INCLUDE_INTERP_UTILS_H
+#define ALE_INCLUDE_INTERP_UTILS_H
+
+#include <vector>
+
+#include "Util.h"
+
+namespace ale {
+  /**
+   * Linearly interpolate between two values.
+   * @param x The first value.
+   * @param y The second value.
+   * @param t The distance to interpolate. 0 is x and 1 is y.
+   */
+  double linearInterpolate(double x, double y, double t);
+
+  /**
+   * Linearly interpolate between two vectors.
+   * @param x The first vectors.
+   * @param y The second vectors.
+   * @param t The distance to interpolate. 0 is x and 1 is y.
+   */
+  std::vector<double> linearInterpolate(const std::vector<double> &x, const std::vector<double> &y, double t);
+
+  ale::Vec3d linearInterpolate(const ale::Vec3d &x, const ale::Vec3d &y, double t);
+
+  /**
+   * Compute the index of the first time to use when interpolating at a given time.
+   * @param times The ordered vector of times to search. Must have at least 2 times.
+   * @param interpTime The time to search for the interpolation index of.
+   * @return int The index of the time that comes before interpTime. If there is
+   *             no time that comes before interpTime, then returns 0. If all
+   *             times come before interpTime, then returns the second to last
+   *             index.
+   */
+  int interpolationIndex(const std::vector<double> &times, double interpTime);
+
+  /**
+   * Merge, sort, and remove duplicates from two vectors
+   */
+   std::vector<double> orderedVecMerge(const std::vector<double> &x, const std::vector<double> &y);
+
+}
+
+#endif
diff --git a/include/Orientations.h b/include/Orientations.h
new file mode 100644
index 0000000..902e8a6
--- /dev/null
+++ b/include/Orientations.h
@@ -0,0 +1,81 @@
+#ifndef ALE_ORIENTATIONS_H
+#define ALE_ORIENTATIONS_H
+
+#include <vector>
+
+#include "Rotation.h"
+
+namespace ale {
+  class Orientations {
+  public:
+    /**
+     * Construct a default empty orientation object
+     */
+    Orientations() {};
+    /**
+     * Construct an orientation object give a set of rotations
+     * and optionally angular velocities at specific times.
+     */
+    Orientations(
+      const std::vector<Rotation> &rotations,
+      const std::vector<double> &times,
+      const std::vector<Vec3d> &avs = std::vector<ale::Vec3d>()
+    );
+    /**
+     * Orientations destructor
+     */
+    ~Orientations() {};
+
+    /**
+     * Const accessor methods
+     */
+    std::vector<Rotation> rotations() const;
+    std::vector<ale::Vec3d> angularVelocities() const;
+    std::vector<double> times() const;
+
+    /**
+     * Get the interpolated rotation at a specific time.
+     */
+    Rotation interpolate(
+      double time,
+      RotationInterpolation interpType=SLERP
+    ) const;
+    /**
+     * Get the interpolated angular velocity at a specific time
+     */
+    ale::Vec3d interpolateAV(double time) const;
+
+    ale::Vec3d rotateVectorAt(
+      double time,
+      const ale::Vec3d &vector,
+      RotationInterpolation interpType=SLERP,
+      bool invert=false
+    ) const;
+
+    /**
+     * Rotate a position or state vector at a specific time
+     */
+    ale::State rotateStateAt(
+      double time,
+      const ale::State &state,
+      RotationInterpolation interpType=SLERP,
+      bool invert=false
+    ) const;
+
+    /**
+     * Multiply this orientation by another orientation
+     */
+    Orientations &operator*=(const Orientations &rhs);
+    /**
+     * Multiply this orientation by a constant rotation
+     */
+    Orientations &operator*=(const Rotation &rhs);
+
+  private:
+    std::vector<Rotation> m_rotations;
+    std::vector<ale::Vec3d> m_avs;
+    std::vector<double> m_times;
+  };
+}
+
+#endif
diff --git a/include/Rotation.h b/include/Rotation.h
index b1d6707..c3ffbf1 100644
--- a/include/Rotation.h
+++ b/include/Rotation.h
@@ -4,6 +4,9 @@
 #include <memory>
 #include <vector>
 
+#include "States.h"
+#include "Util.h"
+
 namespace ale {
 
   enum RotationInterpolation {
@@ -79,7 +82,7 @@ namespace ale {
        *
        * @return The state rotation matrix in row-major order.
        */
-      std::vector<double> toStateRotationMatrix(const std::vector<double> &av) const;
+      std::vector<double> toStateRotationMatrix(const ale::Vec3d &av) const;
       /**
        * The rotation as Euler angles.
        *
@@ -99,12 +102,14 @@ namespace ale {
       /**
        * Rotate a vector
        *
-       * @param vector The vector to rotate. Cab be a 3 element position or 6 element state.
+       * @param vector The vector to rotate. Can be a 3 element position or 6 element state.
        * @param av The angular velocity to use when rotating state vectors. Defaults to 0.
        *
        * @return The rotated vector.
        */
-      std::vector<double> operator()(const std::vector<double>& vector, const std::vector<double>& av = {0.0, 0.0, 0.0}) const;
+      ale::Vec3d operator()(const ale::Vec3d &av) const;
+
+      ale::State operator()(const ale::State &state, const ale::Vec3d& av = Vec3d(0.0, 0.0, 0.0)) const;
       /**
        * Get the inverse rotation.
        */
diff --git a/include/States.h b/include/States.h
index c87cc7f..2f908d2 100644
--- a/include/States.h
+++ b/include/States.h
@@ -4,29 +4,11 @@
 #include<vector>
 #include <stdexcept>
 #include <gsl/gsl_interp.h>
-#include <ale.h> 
+#include <ale.h>
 
-namespace ale {
-
-/** A 3D cartesian vector */
-struct Vec3d {
-  double x;
-  double y;
-  double z;
-
-  // Accepts an {x,y,z} vector
-  Vec3d(const std::vector<double>& vec) {
-    if (vec.size() != 3) {
-      throw std::invalid_argument("Input vector must have 3 entries.");
-    }
-    x = vec[0];
-    y = vec[1];
-    z = vec[2];
-  }; 
+#include "Util.h"
 
-  Vec3d(double x, double y, double z) : x(x), y(y), z(z) {};
-  Vec3d() : x(0.0), y(0.0), z(0.0) {};
-};
+namespace ale {
 
 /** A state vector with position and velocity*/
 struct State {
@@ -42,7 +24,7 @@ struct State {
     velocity = {vec[3], vec[4], vec[5]};
   };
 
-  State(ale::Vec3d position, ale::Vec3d velocity) : position(position), velocity(velocity) {}; 
+  State(ale::Vec3d position, ale::Vec3d velocity) : position(position), velocity(velocity) {};
   State() {};
 };
 
@@ -50,19 +32,19 @@ class States {
   public:
     // Constructors
     States();
-    States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions, 
+    States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
            int refFrame=1);
 
-    States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions, 
+    States(const std::vector<double>& ephemTimes, const std::vector<ale::Vec3d>& positions,
            const std::vector<ale::Vec3d>& velocities, int refFrame=1);
 
-    States(const std::vector<double>& ephemTimes, const std::vector<ale::State>& states, 
-           int refFrame=1); 
+    States(const std::vector<double>& ephemTimes, const std::vector<ale::State>& states,
+           int refFrame=1);
 
     ~States();
-    
+
     // Getters
-    std::vector<ale::State> getStates() const; //! Returns state vectors (6-element positions&velocities) 
+    std::vector<ale::State> getStates() const; //! Returns state vectors (6-element positions&velocities)
     std::vector<ale::Vec3d> getPositions() const; //! Returns the current positions
     std::vector<ale::Vec3d> getVelocities() const; //! Returns the current velocities
     std::vector<double> getTimes() const; //! Returns the current times
@@ -71,14 +53,14 @@ class States {
 
   /**
    * Returns a single state by interpolating state.
-   * If the Cache has been minimized, a cubic hermite is used to interpolate the 
-   * position and velocity over the reduced cache. 
+   * If the Cache has been minimized, a cubic hermite is used to interpolate the
+   * position and velocity over the reduced cache.
    * If not, a standard gsl interpolation will be done.
-   * 
+   *
    * @param time Time to get a value at
    * @param interp Interpolation type to use. Will be ignored if cache is minimized.
-   * 
-   * @return ale::State 
+   *
+   * @return ale::State
    */
     ale::State getState(double time, ale::interpolation interp=LINEAR) const;
 
@@ -89,45 +71,44 @@ class States {
     ale::Vec3d getVelocity(double time, ale::interpolation interp=LINEAR) const;
 
     /** Returns the first ephemeris time **/
-    double getStartTime(); 
+    double getStartTime();
 
     /** Returns the last ephemeris time **/
-    double getStopTime(); 
-    
+    double getStopTime();
+
   /**
    * Perform a cache reduction. After running this, getStates(), getPositions(),
    * and getVelocities() will return vectors of reduced size, and getState(),
    * getPosition(), and getVelocity() will
-   * returns values interpolated over the reduced cache using a cubic hermite spline 
-   *  
-   * Adapted from Isis::SpicePosition::reduceCache().  
-   * 
+   * returns values interpolated over the reduced cache using a cubic hermite spline
+   *
+   * Adapted from Isis::SpicePosition::reduceCache().
+   *
    * @param tolerance Maximum error between hermite approximation and original value.
    */
     States minimizeCache(double tolerance=0.01);
 
   private:
-  
+
   /**
    * Calculates the points (indicies) which need to be kept for the hermite spline to
-   * interpolate between to mantain a maximum error of tolerance. 
-   *  
-   * Adapted from Isis::SpicePosition::HermiteIndices. 
-   *  
+   * interpolate between to mantain a maximum error of tolerance.
+   *
+   * Adapted from Isis::SpicePosition::HermiteIndices.
+   *
    * @param tolerance Maximum error between hermite approximation and original value.
    * @param indexList The list of indicies that need to be kept.
    * @param baseTime Scaled base time for fit
    * @param timeScale Time scale for fit.
-   * 
-   * @return std::vector<int> 
+   *
+   * @return std::vector<int>
    */
-    std::vector<int> hermiteIndices(double tolerance, std::vector <int> indexList, 
+    std::vector<int> hermiteIndices(double tolerance, std::vector <int> indexList,
                                     double baseTime, double timeScale);
     std::vector<ale::State> m_states; //! Represent at states internally to keep pos, vel together
     std::vector<double> m_ephemTimes; //! Time in seconds
-    int m_refFrame;  //! Naif IDs for reference frames 
+    int m_refFrame;  //! Naif IDs for reference frames
   };
 }
 
 #endif
-
diff --git a/include/Util.h b/include/Util.h
index c7cd3d0..1023183 100644
--- a/include/Util.h
+++ b/include/Util.h
@@ -10,6 +10,26 @@
 namespace ale {
   using json = nlohmann::json;
 
+  /** A 3D cartesian vector */
+  struct Vec3d {
+    double x;
+    double y;
+    double z;
+
+    // Accepts an {x,y,z} vector
+    Vec3d(const std::vector<double>& vec) {
+      if (vec.size() != 3) {
+        throw std::invalid_argument("Input vector must have 3 entries.");
+      }
+      x = vec[0];
+      y = vec[1];
+      z = vec[2];
+    };
+
+    Vec3d(double x, double y, double z) : x(x), y(y), z(z) {};
+    Vec3d() : x(0.0), y(0.0), z(0.0) {};
+  };
+
   double getMinHeight(nlohmann::json isd);
   std::string getSensorModelName(json isd);
   std::string getImageId(json isd);
diff --git a/include/ale.h b/include/ale.h
index 64c5ab4..b233e8b 100644
--- a/include/ale.h
+++ b/include/ale.h
@@ -6,6 +6,8 @@
 
 #include <gsl/gsl_interp.h>
 
+#include "InterpUtils.h"
+
 #include <nlohmann/json.hpp>
 using json = nlohmann::json;
 
@@ -27,9 +29,6 @@ namespace ale {
   to interpolate over it. They were migrated, with minor modifications, from
   Isis::NumericalApproximation **/
 
-  /** Determines the lower index for the interpolation interval. */
-  int interpolationIndex(const std::vector<double> &times, double interpTime);
-
   /** Evaluates a cubic hermite at time, interpTime, between the appropriate two points in x. **/
   double evaluateCubicHermite(const double interpTime, const std::vector<double>& derivs,
                               const std::vector<double>& x, const std::vector<double>& y);
diff --git a/src/InterpUtils.cpp b/src/InterpUtils.cpp
new file mode 100644
index 0000000..0a1b665
--- /dev/null
+++ b/src/InterpUtils.cpp
@@ -0,0 +1,61 @@
+#include "InterpUtils.h"
+
+#include <exception>
+
+#include <algorithm>
+#include <unordered_set>
+
+namespace ale {
+  double linearInterpolate(double x, double y, double t) {
+    return x + t * (y - x);
+  }
+
+  std::vector<double> linearInterpolate(const std::vector<double> &x, const std::vector<double> &y, double t) {
+    if (x.size() != y.size()) {
+      throw std::invalid_argument("X and Y vectors must be the same size.");
+    }
+    std::vector<double> interpVec(x.size());
+    for (size_t i = 0; i < interpVec.size(); i++) {
+      interpVec[i] = linearInterpolate(x[i], y[i], t);
+    }
+    return interpVec;
+  }
+
+  ale::Vec3d linearInterpolate(const ale::Vec3d &x, const ale::Vec3d &y, double t) {
+    ale::Vec3d interpVec;
+
+    interpVec.x = linearInterpolate(x.x, y.x, t);
+    interpVec.y = linearInterpolate(x.y, y.y, t);
+    interpVec.z = linearInterpolate(x.z, y.z, t);
+
+    return interpVec;
+  }
+
+  int interpolationIndex(const std::vector<double> &times, double interpTime) {
+    if (times.size() < 2){
+      throw std::invalid_argument("There must be at least two times.");
+    }
+    auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime);
+    if (nextTimeIt == times.end()) {
+      --nextTimeIt;
+    }
+    if (nextTimeIt != times.begin()) {
+      --nextTimeIt;
+    }
+    return std::distance(times.begin(), nextTimeIt);
+  }
+
+  std::vector<double> orderedVecMerge(const std::vector<double> &x, const std::vector<double> &y) {
+    std::unordered_set<double> mergedSet;
+    for (double val: x) {
+      mergedSet.insert(val);
+    }
+    for (double val: y) {
+      mergedSet.insert(val);
+    }
+    std::vector<double> merged;
+    merged.assign(mergedSet.begin(), mergedSet.end());
+    std::sort(merged.begin(), merged.end());
+    return merged;
+  }
+}
diff --git a/src/Orientations.cpp b/src/Orientations.cpp
new file mode 100644
index 0000000..c99e536
--- /dev/null
+++ b/src/Orientations.cpp
@@ -0,0 +1,128 @@
+#include "Orientations.h"
+
+#include "InterpUtils.h"
+
+namespace ale {
+
+  Orientations::Orientations(
+    const std::vector<Rotation> &rotations,
+    const std::vector<double> &times,
+    const std::vector<ale::Vec3d> &avs
+  ) :
+    m_rotations(rotations), m_avs(avs), m_times(times) {
+    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() != m_times.size()) {
+      throw std::invalid_argument("The number of rotations and times must be the same.");
+    }
+    if ( !m_avs.empty() && (m_avs.size() != m_times.size()) ) {
+      throw std::invalid_argument("The number of angular velocities and times must be the same.");
+    }
+  }
+
+
+  std::vector<Rotation> Orientations::rotations() const {
+    return m_rotations;
+  }
+
+
+  std::vector<ale::Vec3d> Orientations::angularVelocities() const {
+    return m_avs;
+  }
+
+
+  std::vector<double> Orientations::times() const {
+    return m_times;
+  }
+
+
+  Rotation Orientations::interpolate(
+    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_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+  }
+
+
+  ale::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]);
+    ale::Vec3d interpAv = ale::Vec3d(linearInterpolate(m_avs[interpIndex], m_avs[interpIndex + 1], t));
+    return interpAv;
+  }
+
+  ale::Vec3d Orientations::rotateVectorAt(
+    double time,
+    const ale::Vec3d &vector,
+    RotationInterpolation interpType,
+    bool invert
+  ) const {
+    Rotation interpRot = interpolate(time, interpType);
+    return interpRot(vector);
+  }
+
+
+  ale::State Orientations::rotateStateAt(
+    double time,
+    const ale::State &state,
+    RotationInterpolation interpType,
+    bool invert
+  ) const {
+    Rotation interpRot = interpolate(time, interpType);
+    ale::Vec3d av(0.0, 0.0, 0.0);
+    if (!m_avs.empty()) {
+      av = interpolateAV(time);
+    }
+    if (invert) {
+      ale::Vec3d negAv = interpRot(av);
+      av = {-negAv.x, -negAv.y, -negAv.z};
+      interpRot = interpRot.inverse();
+    }
+    return interpRot(state, av);
+  }
+
+
+  Orientations &Orientations::operator*=(const Orientations &rhs) {
+    std::vector<double> mergedTimes = orderedVecMerge(m_times, rhs.m_times);
+    std::vector<Rotation> mergedRotations;
+    std::vector<ale::Vec3d> mergedAvs;
+    for (double time: mergedTimes) {
+      Rotation rhsRot = rhs.interpolate(time);
+      mergedRotations.push_back(interpolate(time)*rhsRot);
+      ale::Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
+      ale::Vec3d rhsAv = rhs.interpolateAV(time);
+      combinedAv.x += rhsAv.x;
+      combinedAv.y += rhsAv.y;
+      combinedAv.z += rhsAv.z;
+      mergedAvs.push_back(combinedAv);
+    }
+
+    m_times = mergedTimes;
+    m_rotations = mergedRotations;
+    m_avs = mergedAvs;
+
+    return *this;
+  }
+
+
+  Orientations &Orientations::operator*=(const Rotation &rhs) {
+    std::vector<Rotation> updatedRotations;
+    for (size_t i = 0; i < m_rotations.size(); i++) {
+      updatedRotations.push_back(m_rotations[i]*rhs);
+    }
+
+    Rotation inverse = rhs.inverse();
+    std::vector<Vec3d> updatedAvs;
+    for (size_t i = 0; i < m_avs.size(); i++) {
+      updatedAvs.push_back(inverse(m_avs[i]));
+    }
+
+    m_rotations = updatedRotations;
+    m_avs = updatedAvs;
+
+    return *this;
+  }
+}
diff --git a/src/Rotation.cpp b/src/Rotation.cpp
index ef646ea..bda92fc 100644
--- a/src/Rotation.cpp
+++ b/src/Rotation.cpp
@@ -1,21 +1,17 @@
 #include "Rotation.h"
 
-#include <algorithm>
 #include <exception>
 
 #include <Eigen/Geometry>
 
+#include "InterpUtils.h"
+
 namespace ale {
 
 ///////////////////////////////////////////////////////////////////////////////
 // Helper Functions
 ///////////////////////////////////////////////////////////////////////////////
 
-  // Linearly interpolate between two values
-  double linearInterpolate(double x, double y, double t) {
-    return x + t * (y - x);
-  }
-
 
   // Helper function to convert an axis number into a unit Eigen vector down that axis.
   Eigen::Vector3d axis(int axisIndex) {
@@ -43,16 +39,11 @@ namespace ale {
    * as the AV from the destination to the source. This matches how NAIF
    * defines AV.
    */
-  Eigen::Quaterniond::Matrix3 avSkewMatrix(
-        const std::vector<double>& av
-  ) {
-    if (av.size() != 3) {
-      throw std::invalid_argument("Angular velocity vector to rotate is the wrong size.");
-    }
+  Eigen::Quaterniond::Matrix3 avSkewMatrix(const ale::Vec3d& av) {
     Eigen::Quaterniond::Matrix3 avMat;
-    avMat <<  0.0,    av[2], -av[1],
-             -av[2],  0.0,    av[0],
-              av[1], -av[0],  0.0;
+    avMat <<  0.0,    av.z, -av.y,
+             -av.z,  0.0,    av.x,
+              av.y, -av.x,  0.0;
     return avMat;
   }
 
@@ -162,7 +153,7 @@ namespace ale {
   }
 
 
-  std::vector<double> Rotation::toStateRotationMatrix(const std::vector<double> &av) const {
+  std::vector<double> Rotation::toStateRotationMatrix(const ale::Vec3d &av) const {
     Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
     Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
     Eigen::Quaterniond::Matrix3 dtMat = rotMat * avMat;
@@ -204,29 +195,30 @@ namespace ale {
   }
 
 
-  std::vector<double> Rotation::operator()(
-        const std::vector<double>& vector,
-        const std::vector<double>& av
+  ale::Vec3d Rotation::operator()(const ale::Vec3d &vector) const {
+    Eigen::Vector3d eigenVector(vector.x, vector.y, vector.z);
+    Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector);
+    std::vector<double> tempVec = std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size());
+    return Vec3d(tempVec);
+  }
+
+  ale::State Rotation::operator()(
+        const ale::State& state,
+        const ale::Vec3d& av
   ) const {
-    if (vector.size() == 3) {
-      Eigen::Map<Eigen::Vector3d> eigenVector((double *)vector.data());
-      Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector);
-      return std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size());
-    }
-    else if (vector.size() == 6) {
-      Eigen::Map<Eigen::Vector3d> positionVector((double *)vector.data());
-      Eigen::Map<Eigen::Vector3d> velocityVector((double *)vector.data() + 3);
-      Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
-      Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
-      Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat;
-      Eigen::Vector3d rotatedPosition = rotMat * positionVector;
-      Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector;
-      return {rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
-              rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)};
-    }
-    else {
-      throw std::invalid_argument("Vector to rotate is the wrong size.");
-    }
+    ale::Vec3d position = state.position;
+    ale::Vec3d velocity = state.velocity;
+
+    Eigen::Vector3d positionVector(position.x, position.y, position.z);
+    Eigen::Vector3d velocityVector(velocity.x, velocity.y, velocity.z);
+    Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
+    Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
+    Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat;
+    Eigen::Vector3d rotatedPosition = rotMat * positionVector;
+    Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector;
+
+    return State({rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
+                  rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)});
   }
 
 
diff --git a/src/ale.cpp b/src/ale.cpp
index 3bcdc31..571d8b2 100644
--- a/src/ale.cpp
+++ b/src/ale.cpp
@@ -22,30 +22,15 @@ using namespace std;
 namespace ale {
 
 
-  // Temporarily moved over from States.cpp. Will be moved into interpUtils in the future. 
+  // Temporarily moved over from States.cpp. Will be moved into interpUtils in the future.
 
-  /** The following helper functions are used to calculate the reduced states cache and cubic hermite 
-  to interpolate over it. They were migrated, with minor modifications, from 
+  /** The following helper functions are used to calculate the reduced states cache and cubic hermite
+  to interpolate over it. They were migrated, with minor modifications, from
   Isis::NumericalApproximation **/
 
-  /** Determines the lower index for the interpolation interval. */
-  int interpolationIndex(const std::vector<double> &times, double interpTime) {
-    if (times.size() < 2){
-      throw std::invalid_argument("There must be at least two times.");
-    }
-    auto nextTimeIt = std::upper_bound(times.begin(), times.end(), interpTime);
-    if (nextTimeIt == times.end()) {
-      --nextTimeIt;
-    }
-    if (nextTimeIt != times.begin()) {
-      --nextTimeIt;
-    }
-    return std::distance(times.begin(), nextTimeIt);
-  }
-
 
   /** Evaluates a cubic hermite at time, interpTime, between the appropriate two points in x. **/
-  double evaluateCubicHermite(const double interpTime, const std::vector<double>& derivs, 
+  double evaluateCubicHermite(const double interpTime, const std::vector<double>& derivs,
                               const std::vector<double>& x, const std::vector<double>& y) {
     if( (derivs.size() != x.size()) || (derivs.size() != y.size()) ) {
        throw std::invalid_argument("EvaluateCubicHermite - The size of the first derivative vector does not match the number of (x,y) data points.");
@@ -72,7 +57,7 @@ namespace ale {
   }
 
   /** Evaluate velocities using a Cubic Hermite Spline at a time a, within some interval in x, **/
- double evaluateCubicHermiteFirstDeriv(const double interpTime, const std::vector<double>& deriv, 
+ double evaluateCubicHermiteFirstDeriv(const double interpTime, const std::vector<double>& deriv,
                                        const std::vector<double>& times, const std::vector<double>& y) {
     if(deriv.size() != times.size()) {
        throw std::invalid_argument("EvaluateCubicHermiteFirstDeriv - The size of the first derivative vector does not match the number of (x,y) data points.");
@@ -467,7 +452,7 @@ namespace ale {
      PyTuple_SetItem(pArgs, 2, pStringFormatter);
 
      // Call the function with the arguments.
-     PyObject* pResult = PyObject_CallObject(pFunc, pArgs); 
+     PyObject* pResult = PyObject_CallObject(pFunc, pArgs);
 
      if(!pResult) {
         throw invalid_argument("No Valid instrument found for label.");
@@ -483,10 +468,10 @@ namespace ale {
      char *temp_str = PyBytes_AS_STRING(temp_bytes); // Borrowed pointer
      cResult = temp_str; // copy into std::string
 
-     Py_DECREF(pResultStr); 
+     Py_DECREF(pResultStr);
      Py_DECREF(pStringFileName);
      Py_DECREF(pStringProps);
-     Py_DECREF(pStringFormatter); 
+     Py_DECREF(pStringFormatter);
 
      return cResult;
  }
diff --git a/tests/ctests/CMakeLists.txt b/tests/ctests/CMakeLists.txt
index 4481782..85a7ab1 100644
--- a/tests/ctests/CMakeLists.txt
+++ b/tests/ctests/CMakeLists.txt
@@ -12,6 +12,7 @@ target_link_libraries(runAleTests
                       GSL::gslcblas
                       Eigen3::Eigen
                       gtest
+                      gmock
                       Threads::Threads
                       nlohmann_json::nlohmann_json
                       )
diff --git a/tests/ctests/OrientationsTest.cpp b/tests/ctests/OrientationsTest.cpp
new file mode 100644
index 0000000..03f0d93
--- /dev/null
+++ b/tests/ctests/OrientationsTest.cpp
@@ -0,0 +1,132 @@
+#include "gtest/gtest.h"
+
+#include "Orientations.h"
+
+#include <cmath>
+#include <exception>
+
+using namespace std;
+using namespace ale;
+
+class OrientationTest : public ::testing::Test {
+  protected:
+    void SetUp() override {
+      rotations.push_back(Rotation( 0.5, 0.5, 0.5, 0.5));
+      rotations.push_back(Rotation(-0.5, 0.5, 0.5, 0.5));
+      rotations.push_back(Rotation( 1.0, 0.0, 0.0, 0.0));
+      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));
+      orientations = Orientations(rotations, times, avs);
+    }
+
+    vector<Rotation> rotations;
+    vector<double> times;
+    vector<Vec3d> avs;
+    Orientations orientations;
+};
+
+TEST_F(OrientationTest, ConstructorAccessors) {
+  vector<Rotation> outputRotations = orientations.rotations();
+  vector<double> outputTimes = orientations.times();
+  vector<Vec3d> outputAvs = orientations.angularVelocities();
+  ASSERT_EQ(outputRotations.size(), rotations.size());
+  for (size_t i = 0; i < outputRotations.size(); i++) {
+    vector<double> quats = rotations[i].toQuaternion();
+    vector<double> outputQuats = outputRotations[i].toQuaternion();
+    EXPECT_EQ(outputQuats[0], quats[0]);
+    EXPECT_EQ(outputQuats[1], quats[1]);
+    EXPECT_EQ(outputQuats[2], quats[2]);
+    EXPECT_EQ(outputQuats[3], quats[3]);
+  }
+  ASSERT_EQ(outputTimes.size(), times.size());
+  for (size_t i = 0; i < outputTimes.size(); i++) {
+    EXPECT_EQ(outputTimes[0], times[0]);
+  }
+  for (size_t i = 0; i < outputAvs.size(); i++) {
+    EXPECT_EQ(outputAvs[i].x, avs[i].x);
+    EXPECT_EQ(outputAvs[i].y, avs[i].y);
+    EXPECT_EQ(outputAvs[i].z, avs[i].z);
+  }
+}
+
+TEST_F(OrientationTest, Interpolate) {
+  Rotation interpRotation = orientations.interpolate(0.25);
+  vector<double> quat = interpRotation.toQuaternion();
+  ASSERT_EQ(quat.size(), 4);
+  EXPECT_NEAR(quat[0], cos(M_PI * 3.0/8.0), 1e-10);
+  EXPECT_NEAR(quat[1], sin(M_PI * 3.0/8.0) * 1/sqrt(3.0), 1e-10);
+  EXPECT_NEAR(quat[2], 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, InterpolateAtRotation) {
+  Rotation interpRotation = orientations.interpolate(0.0);
+  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);
+}
+
+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);
+}
+
+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);
+  EXPECT_NEAR(rotatedX.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedX.z, 0.0, 1e-10);
+  Vec3d rotatedY = orientations.rotateVectorAt(0.0, Vec3d(0.0, 1.0, 0.0));
+  EXPECT_NEAR(rotatedY.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.z, 1.0, 1e-10);
+  Vec3d rotatedZ = orientations.rotateVectorAt(0.0, Vec3d(0.0, 0.0, 1.0));
+  EXPECT_NEAR(rotatedZ.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.z, 0.0, 1e-10);
+}
+
+TEST_F(OrientationTest, RotationMultiplication) {
+  Rotation rhs( 0.5, 0.5, 0.5, 0.5);
+  orientations *= rhs;
+  vector<Rotation> outputRotations = orientations.rotations();
+  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();
+    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]);
+  }
+}
+
+TEST_F(OrientationTest, OrientationMultiplication) {
+  Orientations duplicateOrientations(orientations);
+  orientations *= duplicateOrientations;
+  vector<Rotation> outputRotations = orientations.rotations();
+  vector<vector<double>> expectedQuats = {
+    {-0.5, 0.5, 0.5, 0.5},
+    {-0.5,-0.5,-0.5,-0.5},
+    { 1.0, 0.0, 0.0, 0.0}
+  };
+  for (size_t i = 0; i < outputRotations.size(); i++) {
+    vector<double> quats = outputRotations[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]);
+  }
+}
diff --git a/tests/ctests/RotationTest.cpp b/tests/ctests/RotationTest.cpp
index 8b81340..37636fd 100644
--- a/tests/ctests/RotationTest.cpp
+++ b/tests/ctests/RotationTest.cpp
@@ -217,152 +217,126 @@ TEST(RotationTest, ToAxisAngle) {
 
 TEST(RotationTest, RotateVector) {
   Rotation rotation(0.5, 0.5, 0.5, 0.5);
-  vector<double> unitX = {1.0, 0.0, 0.0};
-  vector<double> unitY = {0.0, 1.0, 0.0};
-  vector<double> unitZ = {0.0, 0.0, 1.0};
-  vector<double> rotatedX = rotation(unitX);
-  vector<double> rotatedY = rotation(unitY);
-  vector<double> rotatedZ = rotation(unitZ);
-  ASSERT_EQ(rotatedX.size(), 3);
-  EXPECT_NEAR(rotatedX[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[1], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedX[2], 0.0, 1e-10);
-  ASSERT_EQ(rotatedY.size(), 3);
-  EXPECT_NEAR(rotatedY[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[2], 1.0, 1e-10);
-  ASSERT_EQ(rotatedZ.size(), 3);
-  EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10);
+  Vec3d unitX(1.0, 0.0, 0.0);
+  Vec3d unitY(0.0, 1.0, 0.0);
+  Vec3d unitZ(0.0, 0.0, 1.0);
+  Vec3d rotatedX = rotation(unitX);
+  Vec3d rotatedY = rotation(unitY);
+  Vec3d rotatedZ = rotation(unitZ);
+  EXPECT_NEAR(rotatedX.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedX.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.z, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.z, 0.0, 1e-10);
 }
 
 TEST(RotationTest, RotateState) {
   Rotation rotation(0.5, 0.5, 0.5, 0.5);
-  std::vector<double> av = {2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI};
-  vector<double> unitX =  {1.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-  vector<double> unitY =  {0.0, 1.0, 0.0, 0.0, 0.0, 0.0};
-  vector<double> unitZ =  {0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
-  vector<double> unitVX = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0};
-  vector<double> unitVY = {0.0, 0.0, 0.0, 0.0, 1.0, 0.0};
-  vector<double> unitVZ = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
-  vector<double> rotatedX = rotation(unitX, av);
-  vector<double> rotatedY = rotation(unitY, av);
-  vector<double> rotatedZ = rotation(unitZ, av);
-  vector<double> rotatedVX = rotation(unitVX, av);
-  vector<double> rotatedVY = rotation(unitVY, av);
-  vector<double> rotatedVZ = rotation(unitVZ, av);
-  ASSERT_EQ(rotatedX.size(), 6);
-  EXPECT_NEAR(rotatedX[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[1], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedX[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[3], 2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(rotatedX[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[5], -2.0 / 3.0 * M_PI, 1e-10);
-  ASSERT_EQ(rotatedY.size(), 6);
-  EXPECT_NEAR(rotatedY[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[2], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedY[3], -2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(rotatedY[4], 2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(rotatedY[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedZ.size(), 6);
-  EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[4], -2.0 / 3.0 * M_PI, 1e-10);
-  EXPECT_NEAR(rotatedZ[5], 2.0 / 3.0 * M_PI, 1e-10);
-  ASSERT_EQ(rotatedVX.size(), 6);
-  EXPECT_NEAR(rotatedVX[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[4], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedVY.size(), 6);
-  EXPECT_NEAR(rotatedVY[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[5], 1.0, 1e-10);
-  ASSERT_EQ(rotatedVZ.size(), 6);
-  EXPECT_NEAR(rotatedVZ[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[3], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[5], 0.0, 1e-10);
+  Vec3d av(2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI, 2.0 / 3.0 * M_PI);
+  State unitX({1.0, 0.0, 0.0, 0.0, 0.0, 0.0});
+  State unitY({0.0, 1.0, 0.0, 0.0, 0.0, 0.0});
+  State unitZ({0.0, 0.0, 1.0, 0.0, 0.0, 0.0});
+  State unitVX({0.0, 0.0, 0.0, 1.0, 0.0, 0.0});
+  State unitVY({0.0, 0.0, 0.0, 0.0, 1.0, 0.0});
+  State unitVZ({0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
+  State rotatedX = rotation(unitX, av);
+  State rotatedY = rotation(unitY, av);
+  State rotatedZ = rotation(unitZ, av);
+  State rotatedVX = rotation(unitVX, av);
+  State rotatedVY = rotation(unitVY, av);
+  State rotatedVZ = rotation(unitVZ, av);
+  EXPECT_NEAR(rotatedX.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.position.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedX.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.x, 2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.z, -2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedY.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.position.z, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.x, -2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.y, 2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.y, -2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.z, 2.0 / 3.0 * M_PI, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.z, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.z, 0.0, 1e-10);
 }
 
 TEST(RotationTest, RotateStateNoAv) {
   Rotation rotation(0.5, 0.5, 0.5, 0.5);
-  vector<double> unitX =  {1.0, 0.0, 0.0, 0.0, 0.0, 0.0};
-  vector<double> unitY =  {0.0, 1.0, 0.0, 0.0, 0.0, 0.0};
-  vector<double> unitZ =  {0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
-  vector<double> unitVX = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0};
-  vector<double> unitVY = {0.0, 0.0, 0.0, 0.0, 1.0, 0.0};
-  vector<double> unitVZ = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
-  vector<double> rotatedX = rotation(unitX);
-  vector<double> rotatedY = rotation(unitY);
-  vector<double> rotatedZ = rotation(unitZ);
-  vector<double> rotatedVX = rotation(unitVX);
-  vector<double> rotatedVY = rotation(unitVY);
-  vector<double> rotatedVZ = rotation(unitVZ);
-  ASSERT_EQ(rotatedX.size(), 6);
-  EXPECT_NEAR(rotatedX[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[1], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedX[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedX[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedY.size(), 6);
-  EXPECT_NEAR(rotatedY[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[2], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedY[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedY[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedZ.size(), 6);
-  EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedZ[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedVX.size(), 6);
-  EXPECT_NEAR(rotatedVX[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[4], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedVX[5], 0.0, 1e-10);
-  ASSERT_EQ(rotatedVY.size(), 6);
-  EXPECT_NEAR(rotatedVY[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[3], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVY[5], 1.0, 1e-10);
-  ASSERT_EQ(rotatedVZ.size(), 6);
-  EXPECT_NEAR(rotatedVZ[0], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[1], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[2], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[3], 1.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[4], 0.0, 1e-10);
-  EXPECT_NEAR(rotatedVZ[5], 0.0, 1e-10);
-}
-
-TEST(RotationTest, RotateWrongSizeAV) {
-  Rotation rotation;
-  ASSERT_NO_THROW(rotation({1.0, 1.0, 1.0}));
-  ASSERT_NO_THROW(rotation({1.0, 1.0, 1.0}, {1.0, 1.0}));
-}
-
-TEST(RotationTest, RotateWrongSizeVector) {
-  Rotation rotation;
-  ASSERT_THROW(rotation({1.0, 1.0, 1.0, 1.0}), std::invalid_argument);
+  State unitX({1.0, 0.0, 0.0, 0.0, 0.0, 0.0});
+  State unitY({0.0, 1.0, 0.0, 0.0, 0.0, 0.0});
+  State unitZ({0.0, 0.0, 1.0, 0.0, 0.0, 0.0});
+  State unitVX({0.0, 0.0, 0.0, 1.0, 0.0, 0.0});
+  State unitVY({0.0, 0.0, 0.0, 0.0, 1.0, 0.0});
+  State unitVZ({0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
+  State rotatedX = rotation(unitX);
+  State rotatedY = rotation(unitY);
+  State rotatedZ = rotation(unitZ);
+  State rotatedVX = rotation(unitVX);
+  State rotatedVY = rotation(unitVY);
+  State rotatedVZ = rotation(unitVZ);
+  EXPECT_NEAR(rotatedX.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.position.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedX.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedX.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.position.z, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedY.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedZ.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.y, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVX.velocity.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVY.velocity.z, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.x, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.position.z, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.x, 1.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.y, 0.0, 1e-10);
+  EXPECT_NEAR(rotatedVZ.velocity.z, 0.0, 1e-10);
 }
 
 TEST(RotationTest, Inverse) {
diff --git a/tests/ctests/TestInterpUtils.cpp b/tests/ctests/TestInterpUtils.cpp
new file mode 100644
index 0000000..6dc2e99
--- /dev/null
+++ b/tests/ctests/TestInterpUtils.cpp
@@ -0,0 +1,47 @@
+#include "gmock/gmock.h"
+
+#include "InterpUtils.h"
+
+#include <cmath>
+#include <exception>
+
+using namespace std;
+using namespace ale;
+
+TEST(InterpUtilsTest, LinearInterpolate) {
+  EXPECT_EQ(linearInterpolate(1, 3, 0.5), 2);
+  EXPECT_EQ(linearInterpolate(1, 1, 0.5), 1);
+}
+
+TEST(InterpUtilsTest, LinearExtrapolate) {
+  EXPECT_EQ(linearInterpolate(1, 3, 1.5), 4);
+  EXPECT_EQ(linearInterpolate(1, 3, -0.5), 0);
+}
+
+TEST(InterpUtilsTest, NDLinearInterpolate) {
+  std::vector<double> interpVec = linearInterpolate({1, 4}, {3, 2}, 0.5);
+  ASSERT_EQ(interpVec.size(), 2);
+  EXPECT_EQ(interpVec[0], 2);
+  EXPECT_EQ(interpVec[1], 3);
+}
+
+TEST(InterpUtilsTest, InterpEmptyVector) {
+  ASSERT_THROW(linearInterpolate({}, {1, 2}, 0.3), std::invalid_argument);
+}
+
+TEST(InterpUtilsTest, InterpDifferentSizeVector) {
+  ASSERT_THROW(linearInterpolate({2, 3, 4}, {1, 2}, 0.3), std::invalid_argument);
+}
+
+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);
+}
+
+TEST(InterpUtilsTest, orderedVecMerge) {
+  vector<double> vec1 = {0, 2, 4, 3, 5};
+  vector<double> vec2 = {-10, 4, 5, 6, 0};
+  vector<double> merged = orderedVecMerge(vec1, vec2);
+  ASSERT_THAT(orderedVecMerge(vec1, vec2), testing::ElementsAre(-10, 0, 2, 3, 4, 5, 6));
+}
-- 
GitLab