diff --git a/CMakeLists.txt b/CMakeLists.txt
index 77c48a3f34bb659ebf97d085bf9063bd164a5164..9d2de3f11461c9f1aa1d8306ffa313bca278706b 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 82035b29bcb5a9afeb8aa00eb101b7dad11f251c..8fbcd5ce08c5fd99120fd8c6bfb49f17167fd1ad 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 0000000000000000000000000000000000000000..ecd533fd44d8cbe6ee4bc430626436f2af42ad9d
--- /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> ×, 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 0000000000000000000000000000000000000000..902e8a658a9ac723d479150347257a7d73165e50
--- /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> ×,
+ 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 b1d67073bd1ac997cb1358d0ddfbdacd5bf4a007..c3ffbf1b36179897afc0887f72a8d2ff67242808 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 c87cc7f059caa4dea35c0a1a244d9379c096afb3..2f908d2efb8519d1addc376d1bca6174cec82c03 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 c7cd3d0093ebaa617196fad956a6332952e1b6f3..1023183b2a62c7930b005a75ba33bb53e76ef744 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 64c5ab41b4bbe336da2144d1dfea7605b7023e3b..b233e8b66a2f11454deb18d42965b034fde2f340 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> ×, 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 0000000000000000000000000000000000000000..0a1b6655479d354a495a086747797fb4b4517a9d
--- /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> ×, 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 0000000000000000000000000000000000000000..c99e536ef332a178e226ea23464cb95fa6257f23
--- /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> ×,
+ 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 ef646ea0979b25f7b9e43d8e9099a3cd9ed5d087..bda92fc62c6a4bdcc792d790adc4ed4d7ace8dc5 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 3bcdc31c2f9989562a6509f10b4580233124bab4..571d8b2f875093d165149a4ebe14aa9d84c81a8b 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> ×, 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 44817829d5483e7f62fb38480d78e1e59067b1d0..85a7ab1cfd2450e6482a48b160d4d69f2c84efeb 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 0000000000000000000000000000000000000000..03f0d93f94c5bbfd3dadd9236834df92081091b1
--- /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 8b813409e684ae901981a3a11d3389b1f36b97fe..37636fdd620a881da3797c6fec1da35f9212fa4c 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 0000000000000000000000000000000000000000..6dc2e996d19be95c9c595c40245cd958ee8d1f9f
--- /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));
+}