Skip to content
Snippets Groups Projects
Commit 33caa804 authored by acpaquette's avatar acpaquette Committed by GitHub
Browse files

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: default avatarJesse Mapel <jmapel@usgs.gov>
parent f2879b36
Branches
Tags
No related merge requests found
...@@ -32,12 +32,16 @@ set(ALE_BUILD_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include/") ...@@ -32,12 +32,16 @@ set(ALE_BUILD_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/include/")
set(ALE_INSTALL_INCLUDE_DIR "include/ale") set(ALE_INSTALL_INCLUDE_DIR "include/ale")
add_library(ale SHARED add_library(ale SHARED
${CMAKE_CURRENT_SOURCE_DIR}/src/ale.cpp ${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/Rotation.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/Orientations.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/States.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/States.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/Isd.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/Isd.cpp
${CMAKE_CURRENT_SOURCE_DIR}/src/Util.cpp) ${CMAKE_CURRENT_SOURCE_DIR}/src/Util.cpp)
set(ALE_HEADERS "${ALE_BUILD_INCLUDE_DIR}/ale.h" 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}/Rotation.h"
"${ALE_BUILD_INCLUDE_DIR}/Orientations.h"
"${ALE_BUILD_INCLUDE_DIR}/States.h" "${ALE_BUILD_INCLUDE_DIR}/States.h"
"${ALE_BUILD_INCLUDE_DIR}/Isd.h" "${ALE_BUILD_INCLUDE_DIR}/Isd.h"
"${ALE_BUILD_INCLUDE_DIR}/Util.h") "${ALE_BUILD_INCLUDE_DIR}/Util.h")
......
...@@ -17,3 +17,23 @@ if (NOT TARGET gtest) ...@@ -17,3 +17,23 @@ if (NOT TARGET gtest)
add_library(gtest ${GOOGLETEST_SOURCES}) add_library(gtest ${GOOGLETEST_SOURCES})
endif() 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()
#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
#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
...@@ -4,6 +4,9 @@ ...@@ -4,6 +4,9 @@
#include <memory> #include <memory>
#include <vector> #include <vector>
#include "States.h"
#include "Util.h"
namespace ale { namespace ale {
enum RotationInterpolation { enum RotationInterpolation {
...@@ -79,7 +82,7 @@ namespace ale { ...@@ -79,7 +82,7 @@ namespace ale {
* *
* @return The state rotation matrix in row-major order. * @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. * The rotation as Euler angles.
* *
...@@ -99,12 +102,14 @@ namespace ale { ...@@ -99,12 +102,14 @@ namespace ale {
/** /**
* Rotate a vector * 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. * @param av The angular velocity to use when rotating state vectors. Defaults to 0.
* *
* @return The rotated vector. * @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. * Get the inverse rotation.
*/ */
......
...@@ -6,27 +6,9 @@ ...@@ -6,27 +6,9 @@
#include <gsl/gsl_interp.h> #include <gsl/gsl_interp.h>
#include <ale.h> #include <ale.h>
namespace ale { #include "Util.h"
/** 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) {}; namespace ale {
Vec3d() : x(0.0), y(0.0), z(0.0) {};
};
/** A state vector with position and velocity*/ /** A state vector with position and velocity*/
struct State { struct State {
...@@ -130,4 +112,3 @@ class States { ...@@ -130,4 +112,3 @@ class States {
} }
#endif #endif
...@@ -10,6 +10,26 @@ ...@@ -10,6 +10,26 @@
namespace ale { namespace ale {
using json = nlohmann::json; 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); double getMinHeight(nlohmann::json isd);
std::string getSensorModelName(json isd); std::string getSensorModelName(json isd);
std::string getImageId(json isd); std::string getImageId(json isd);
......
...@@ -6,6 +6,8 @@ ...@@ -6,6 +6,8 @@
#include <gsl/gsl_interp.h> #include <gsl/gsl_interp.h>
#include "InterpUtils.h"
#include <nlohmann/json.hpp> #include <nlohmann/json.hpp>
using json = nlohmann::json; using json = nlohmann::json;
...@@ -27,9 +29,6 @@ namespace ale { ...@@ -27,9 +29,6 @@ namespace ale {
to interpolate over it. They were migrated, with minor modifications, from to interpolate over it. They were migrated, with minor modifications, from
Isis::NumericalApproximation **/ 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. **/ /** 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); const std::vector<double>& x, const std::vector<double>& y);
......
#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;
}
}
#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;
}
}
#include "Rotation.h" #include "Rotation.h"
#include <algorithm>
#include <exception> #include <exception>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include "InterpUtils.h"
namespace ale { namespace ale {
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
// Helper Functions // 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. // Helper function to convert an axis number into a unit Eigen vector down that axis.
Eigen::Vector3d axis(int axisIndex) { Eigen::Vector3d axis(int axisIndex) {
...@@ -43,16 +39,11 @@ namespace ale { ...@@ -43,16 +39,11 @@ namespace ale {
* as the AV from the destination to the source. This matches how NAIF * as the AV from the destination to the source. This matches how NAIF
* defines AV. * defines AV.
*/ */
Eigen::Quaterniond::Matrix3 avSkewMatrix( Eigen::Quaterniond::Matrix3 avSkewMatrix(const ale::Vec3d& av) {
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 avMat; Eigen::Quaterniond::Matrix3 avMat;
avMat << 0.0, av[2], -av[1], avMat << 0.0, av.z, -av.y,
-av[2], 0.0, av[0], -av.z, 0.0, av.x,
av[1], -av[0], 0.0; av.y, -av.x, 0.0;
return avMat; return avMat;
} }
...@@ -162,7 +153,7 @@ namespace ale { ...@@ -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 rotMat = m_impl->quat.toRotationMatrix();
Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av); Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
Eigen::Quaterniond::Matrix3 dtMat = rotMat * avMat; Eigen::Quaterniond::Matrix3 dtMat = rotMat * avMat;
...@@ -204,29 +195,30 @@ namespace ale { ...@@ -204,29 +195,30 @@ namespace ale {
} }
std::vector<double> Rotation::operator()( ale::Vec3d Rotation::operator()(const ale::Vec3d &vector) const {
const std::vector<double>& vector, Eigen::Vector3d eigenVector(vector.x, vector.y, vector.z);
const std::vector<double>& av
) const {
if (vector.size() == 3) {
Eigen::Map<Eigen::Vector3d> eigenVector((double *)vector.data());
Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector); Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector);
return std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size()); std::vector<double> tempVec = std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size());
return Vec3d(tempVec);
} }
else if (vector.size() == 6) {
Eigen::Map<Eigen::Vector3d> positionVector((double *)vector.data()); ale::State Rotation::operator()(
Eigen::Map<Eigen::Vector3d> velocityVector((double *)vector.data() + 3); const ale::State& state,
const ale::Vec3d& av
) const {
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 rotMat = m_impl->quat.toRotationMatrix();
Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av); Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat; Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat;
Eigen::Vector3d rotatedPosition = rotMat * positionVector; Eigen::Vector3d rotatedPosition = rotMat * positionVector;
Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector; Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector;
return {rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)}; return State({rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
} rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)});
else {
throw std::invalid_argument("Vector to rotate is the wrong size.");
}
} }
......
...@@ -28,21 +28,6 @@ namespace ale { ...@@ -28,21 +28,6 @@ namespace ale {
to interpolate over it. They were migrated, with minor modifications, from to interpolate over it. They were migrated, with minor modifications, from
Isis::NumericalApproximation **/ 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. **/ /** 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,
......
...@@ -12,6 +12,7 @@ target_link_libraries(runAleTests ...@@ -12,6 +12,7 @@ target_link_libraries(runAleTests
GSL::gslcblas GSL::gslcblas
Eigen3::Eigen Eigen3::Eigen
gtest gtest
gmock
Threads::Threads Threads::Threads
nlohmann_json::nlohmann_json nlohmann_json::nlohmann_json
) )
......
#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]);
}
}
...@@ -217,152 +217,126 @@ TEST(RotationTest, ToAxisAngle) { ...@@ -217,152 +217,126 @@ TEST(RotationTest, ToAxisAngle) {
TEST(RotationTest, RotateVector) { TEST(RotationTest, RotateVector) {
Rotation rotation(0.5, 0.5, 0.5, 0.5); Rotation rotation(0.5, 0.5, 0.5, 0.5);
vector<double> unitX = {1.0, 0.0, 0.0}; Vec3d unitX(1.0, 0.0, 0.0);
vector<double> unitY = {0.0, 1.0, 0.0}; Vec3d unitY(0.0, 1.0, 0.0);
vector<double> unitZ = {0.0, 0.0, 1.0}; Vec3d unitZ(0.0, 0.0, 1.0);
vector<double> rotatedX = rotation(unitX); Vec3d rotatedX = rotation(unitX);
vector<double> rotatedY = rotation(unitY); Vec3d rotatedY = rotation(unitY);
vector<double> rotatedZ = rotation(unitZ); Vec3d rotatedZ = rotation(unitZ);
ASSERT_EQ(rotatedX.size(), 3); EXPECT_NEAR(rotatedX.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[0], 0.0, 1e-10); EXPECT_NEAR(rotatedX.y, 1.0, 1e-10);
EXPECT_NEAR(rotatedX[1], 1.0, 1e-10); EXPECT_NEAR(rotatedX.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[2], 0.0, 1e-10); EXPECT_NEAR(rotatedY.x, 0.0, 1e-10);
ASSERT_EQ(rotatedY.size(), 3); EXPECT_NEAR(rotatedY.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[0], 0.0, 1e-10); EXPECT_NEAR(rotatedY.z, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[1], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.x, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[2], 1.0, 1e-10); EXPECT_NEAR(rotatedZ.y, 0.0, 1e-10);
ASSERT_EQ(rotatedZ.size(), 3); EXPECT_NEAR(rotatedZ.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10);
EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10);
} }
TEST(RotationTest, RotateState) { TEST(RotationTest, RotateState) {
Rotation rotation(0.5, 0.5, 0.5, 0.5); 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}; Vec3d 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}; State 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}; State 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}; State 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}; State 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}; State 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}; State unitVZ({0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
vector<double> rotatedX = rotation(unitX, av); State rotatedX = rotation(unitX, av);
vector<double> rotatedY = rotation(unitY, av); State rotatedY = rotation(unitY, av);
vector<double> rotatedZ = rotation(unitZ, av); State rotatedZ = rotation(unitZ, av);
vector<double> rotatedVX = rotation(unitVX, av); State rotatedVX = rotation(unitVX, av);
vector<double> rotatedVY = rotation(unitVY, av); State rotatedVY = rotation(unitVY, av);
vector<double> rotatedVZ = rotation(unitVZ, av); State rotatedVZ = rotation(unitVZ, av);
ASSERT_EQ(rotatedX.size(), 6); EXPECT_NEAR(rotatedX.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[0], 0.0, 1e-10); EXPECT_NEAR(rotatedX.position.y, 1.0, 1e-10);
EXPECT_NEAR(rotatedX[1], 1.0, 1e-10); EXPECT_NEAR(rotatedX.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[2], 0.0, 1e-10); EXPECT_NEAR(rotatedX.velocity.x, 2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedX[3], 2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedX.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[4], 0.0, 1e-10); EXPECT_NEAR(rotatedX.velocity.z, -2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedX[5], -2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedY.position.x, 0.0, 1e-10);
ASSERT_EQ(rotatedY.size(), 6); EXPECT_NEAR(rotatedY.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[0], 0.0, 1e-10); EXPECT_NEAR(rotatedY.position.z, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[1], 0.0, 1e-10); EXPECT_NEAR(rotatedY.velocity.x, -2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedY[2], 1.0, 1e-10); EXPECT_NEAR(rotatedY.velocity.y, 2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedY[3], -2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedY.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[4], 2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedZ.position.x, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[5], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.position.y, 0.0, 1e-10);
ASSERT_EQ(rotatedZ.size(), 6); EXPECT_NEAR(rotatedZ.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.y, -2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.z, 2.0 / 3.0 * M_PI, 1e-10);
EXPECT_NEAR(rotatedZ[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[4], -2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedVX.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[5], 2.0 / 3.0 * M_PI, 1e-10); EXPECT_NEAR(rotatedVX.position.z, 0.0, 1e-10);
ASSERT_EQ(rotatedVX.size(), 6); EXPECT_NEAR(rotatedVX.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[0], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.velocity.y, 1.0, 1e-10);
EXPECT_NEAR(rotatedVX[1], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[2], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[4], 1.0, 1e-10); EXPECT_NEAR(rotatedVY.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[5], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.velocity.x, 0.0, 1e-10);
ASSERT_EQ(rotatedVY.size(), 6); EXPECT_NEAR(rotatedVY.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[0], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.velocity.z, 1.0, 1e-10);
EXPECT_NEAR(rotatedVY[1], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[2], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[4], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.velocity.x, 1.0, 1e-10);
EXPECT_NEAR(rotatedVY[5], 1.0, 1e-10); EXPECT_NEAR(rotatedVZ.velocity.y, 0.0, 1e-10);
ASSERT_EQ(rotatedVZ.size(), 6); EXPECT_NEAR(rotatedVZ.velocity.z, 0.0, 1e-10);
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, RotateStateNoAv) { TEST(RotationTest, RotateStateNoAv) {
Rotation rotation(0.5, 0.5, 0.5, 0.5); 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}; State 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}; State 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}; State 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}; State 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}; State 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}; State unitVZ({0.0, 0.0, 0.0, 0.0, 0.0, 1.0});
vector<double> rotatedX = rotation(unitX); State rotatedX = rotation(unitX);
vector<double> rotatedY = rotation(unitY); State rotatedY = rotation(unitY);
vector<double> rotatedZ = rotation(unitZ); State rotatedZ = rotation(unitZ);
vector<double> rotatedVX = rotation(unitVX); State rotatedVX = rotation(unitVX);
vector<double> rotatedVY = rotation(unitVY); State rotatedVY = rotation(unitVY);
vector<double> rotatedVZ = rotation(unitVZ); State rotatedVZ = rotation(unitVZ);
ASSERT_EQ(rotatedX.size(), 6); EXPECT_NEAR(rotatedX.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[0], 0.0, 1e-10); EXPECT_NEAR(rotatedX.position.y, 1.0, 1e-10);
EXPECT_NEAR(rotatedX[1], 1.0, 1e-10); EXPECT_NEAR(rotatedX.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[2], 0.0, 1e-10); EXPECT_NEAR(rotatedX.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[3], 0.0, 1e-10); EXPECT_NEAR(rotatedX.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[4], 0.0, 1e-10); EXPECT_NEAR(rotatedX.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedX[5], 0.0, 1e-10); EXPECT_NEAR(rotatedY.position.x, 0.0, 1e-10);
ASSERT_EQ(rotatedY.size(), 6); EXPECT_NEAR(rotatedY.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[0], 0.0, 1e-10); EXPECT_NEAR(rotatedY.position.z, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[1], 0.0, 1e-10); EXPECT_NEAR(rotatedY.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[2], 1.0, 1e-10); EXPECT_NEAR(rotatedY.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[3], 0.0, 1e-10); EXPECT_NEAR(rotatedY.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedY[4], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.position.x, 1.0, 1e-10);
EXPECT_NEAR(rotatedY[5], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.position.y, 0.0, 1e-10);
ASSERT_EQ(rotatedZ.size(), 6); EXPECT_NEAR(rotatedZ.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[0], 1.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[1], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[2], 0.0, 1e-10); EXPECT_NEAR(rotatedZ.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[4], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedZ[5], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.position.z, 0.0, 1e-10);
ASSERT_EQ(rotatedVX.size(), 6); EXPECT_NEAR(rotatedVX.velocity.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[0], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.velocity.y, 1.0, 1e-10);
EXPECT_NEAR(rotatedVX[1], 0.0, 1e-10); EXPECT_NEAR(rotatedVX.velocity.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[2], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[4], 1.0, 1e-10); EXPECT_NEAR(rotatedVY.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVX[5], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.velocity.x, 0.0, 1e-10);
ASSERT_EQ(rotatedVY.size(), 6); EXPECT_NEAR(rotatedVY.velocity.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[0], 0.0, 1e-10); EXPECT_NEAR(rotatedVY.velocity.z, 1.0, 1e-10);
EXPECT_NEAR(rotatedVY[1], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.x, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[2], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.y, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[3], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.position.z, 0.0, 1e-10);
EXPECT_NEAR(rotatedVY[4], 0.0, 1e-10); EXPECT_NEAR(rotatedVZ.velocity.x, 1.0, 1e-10);
EXPECT_NEAR(rotatedVY[5], 1.0, 1e-10); EXPECT_NEAR(rotatedVZ.velocity.y, 0.0, 1e-10);
ASSERT_EQ(rotatedVZ.size(), 6); EXPECT_NEAR(rotatedVZ.velocity.z, 0.0, 1e-10);
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);
} }
TEST(RotationTest, Inverse) { TEST(RotationTest, Inverse) {
......
#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));
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment