diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9d2de3f11461c9f1aa1d8306ffa313bca278706b..81498d6729e30db324bc6e5652ac9c7840d7a12a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,7 +15,6 @@ include(GNUInstallDirs)
set(CMAKE_CXX_STANDARD 11)
# Third Party Dependencies
-find_package(GSL REQUIRED)
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
find_package(nlohmann_json REQUIRED)
@@ -56,8 +55,6 @@ target_include_directories(ale
target_link_libraries(ale
PRIVATE
- GSL::gsl
- GSL::gslcblas
Eigen3::Eigen
Python::Python
nlohmann_json::nlohmann_json)
diff --git a/environment.yml b/environment.yml
index c86ab5accda1df0eb9ad3d28512ba38474118df8..c7af6ecc5d662936f0150ffed371cdb8b7c506ed 100644
--- a/environment.yml
+++ b/environment.yml
@@ -8,7 +8,6 @@ dependencies:
- cmake>=3.10
- pytest
- eigen
- - gsl
- jupyter
- nlohmann_json
- numpy
diff --git a/include/States.h b/include/States.h
index 2f908d2efb8519d1addc376d1bca6174cec82c03..b204c8f854862eac1c5c7236e323e0702c5c42cd 100644
--- a/include/States.h
+++ b/include/States.h
@@ -3,7 +3,6 @@
#include<vector>
#include <stdexcept>
-#include <gsl/gsl_interp.h>
#include <ale.h>
#include "Util.h"
@@ -55,7 +54,7 @@ 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 not, a standard gsl interpolation will be done.
+ * If not, a standard lagrange 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.
diff --git a/include/ale.h b/include/ale.h
index b233e8b66a2f11454deb18d42965b034fde2f340..a81a777bc2aa421e4b3551550a1e28bb9ecfdbc9 100644
--- a/include/ale.h
+++ b/include/ale.h
@@ -4,12 +4,9 @@
#include <string>
#include <vector>
-#include <gsl/gsl_interp.h>
-
#include "InterpUtils.h"
#include <nlohmann/json.hpp>
-using json = nlohmann::json;
namespace ale {
@@ -17,13 +14,15 @@ namespace ale {
enum interpolation {
/// Interpolate using linear interpolation
LINEAR = 0,
- /// Interpolate using spline interpolation
+ /// Interpolate using a cubic spline
SPLINE = 1,
+ /// Interpolate using Lagrange polynomials up to 8th order
+ LAGRANGE = 2,
};
// Temporarily moved interpolation and associated helper functions over from States. Will be
- // moved to interpUtils in the future.
+ // move to 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
@@ -37,92 +36,10 @@ namespace ale {
double evaluateCubicHermiteFirstDeriv(const double interpTime, const std::vector<double>& deriv,
const std::vector<double>& times, const std::vector<double>& y);
- /**
- *@brief Get the position of the spacecraft at a given time based on a set of coordinates, and their associated times
- *@param coords A vector of double vectors of coordinates
- *@param times A double vector of times
- *@param time Time to observe the spacecraft's position at
- *@param interp Interpolation type
- *@return A vector double of the spacecrafts position
- */
- std::vector<double> getPosition(std::vector<std::vector<double>> coords,
- std::vector<double> times,
- double time, interpolation interp);
- /**
- *@brief Get the velocity of the spacecraft at a given time based on a set of coordinates, and their associated times
- *@param coords A vector of double vectors of coordinates
- *@param times A double vector of times
- *@param time Time to observe the spacecraft's velocity at
- *@param interp Interpolation type
- *@return A vector double of the spacecrafts velocity
- */
- std::vector<double> getVelocity(std::vector<std::vector<double>> coords,
- std::vector<double> times,
- double time, const interpolation interp);
- /**
- *@brief Get the position of the spacecraft at a given time based on a derived function from a set of coeffcients
- *@param coeffs A vector of double vector of coeffcients
- *@param time Time to observe the spacecraft's position at
- *@return A vector double of the spacecrafts position
- */
- std::vector<double> getPosition(std::vector<std::vector<double>> coeffs, double time);
-
- /**
- *@brief Get the velocity of the spacecraft at a given time based on a derived function from a set of coeffcients
- *@param coeffs A vector of double vector of coeffcients
- *@param time Time to observe the spacecraft's velocity at
- *@return A vector double of the spacecrafts velocity
- */
- std::vector<double> getVelocity(std::vector<std::vector<double>> coeffs, double time);
-
- /**
- *@brief Get the rotation of the spacecraft at a given time based on a set of rotations, and their associated times
- *@param rotations A vector of double vector of rotations
- *@param times A double vector of times
- *@param time Time to observe the spacecraft's rotation at
- *@param interp Interpolation type
- *@return A vector double of the spacecrafts rotation
- */
- std::vector<double> getRotation(std::vector<std::vector<double>> rotations,
- std::vector<double> times,
- double time, interpolation interp);
-
- /**
- *@brief Get the angular velocity of the spacecraft at a given time based on a set of rotations, and their associated times
- *@param rotations A vector of double vector of rotations
- *@param times A double vector of times
- *@param time Time to observe the spacecraft's angular velocity at
- *@param interp Interpolation type
- *@return A vector double of the spacecrafts angular velocity
- */
- std::vector<double> getAngularVelocity(std::vector<std::vector<double>> rotations,
- std::vector<double> times,
- double time, interpolation interp);
-
- /**
- *@brief Get the rotation of the spacecraft at a given time based on a derived function from a set of coeffcients
- *@param coeffs A vector of double vector of coeffcients
- *@param time Time to observe the spacecraft's rotation at
- *@return A vector double of the spacecrafts rotation
- */
- std::vector<double> getRotation(std::vector<std::vector<double>> coeffs, double time);
-
- /**
- *@brief Get the angular velocity of the spacecraft at a given time based on a derived function from a set of coeffcients
- *@param coeffs A vector of double vector of coeffcients
- *@param time Time to observe the spacecraft's angular velocity at
- *@return A vector double of the spacecrafts angular velocity
- */
- std::vector<double> getAngularVelocity(std::vector<std::vector<double>> coeffs, double time);
-
- /**
- *@brief Generates a derivatives in respect to time from a polynomial constructed using the given coeffcients, time, and derivation number
- *@param coeffs A double vector of coefficients can be any number of coefficients
- *@param time Time to use when deriving
- *@param d The order of the derivative to generate (Currently supports 0, 1, and 2)
- *@return Evalutation of the given polynomial as a double
- */
- double evaluatePolynomial(std::vector<double> coeffs, double time, int d);
+ double lagrangeInterpolate(const std::vector<double>& times, const std::vector<double>& values,
+ double time, int order=8);
+ double lagrangeInterpolateDerivative(const std::vector<double>& times, const std::vector<double>& values,
+ double time, int order=8);
/**
*@brief Interpolates the spacecrafts position along a path generated from a set of points,
@@ -138,7 +55,7 @@ namespace ale {
double interpolate(std::vector<double> points, std::vector<double> times, double time, interpolation interp, int d);
std::string loads(std::string filename, std::string props="", std::string formatter="usgscsm", bool verbose=true);
- json load(std::string filename, std::string props="", std::string formatter="usgscsm", bool verbose=true);
+ nlohmann::json load(std::string filename, std::string props="", std::string formatter="usgscsm", bool verbose=true);
}
diff --git a/recipe/meta.yaml b/recipe/meta.yaml
index bab606420e09216f15107bfbb782ac35210c75fb..dae59eb2c5df55ae9ba30da880db4a5f36d0528f 100644
--- a/recipe/meta.yaml
+++ b/recipe/meta.yaml
@@ -26,10 +26,8 @@ requirements:
build:
- cmake>=3.10
- eigen
- - gsl
run:
- eigen
- - gsl
- networkx
- numpy
- openblas
diff --git a/src/States.cpp b/src/States.cpp
index 9cf78300c9889773f8f85b9036485efcb1af3e8b..5c984617bc2dc4f6439a44ae9993817711a2e9b8 100644
--- a/src/States.cpp
+++ b/src/States.cpp
@@ -2,9 +2,6 @@
#include <iostream>
#include <algorithm>
-#include <gsl/gsl_interp.h>
-#include <gsl/gsl_spline.h>
-#include <gsl/gsl_poly.h>
#include <cmath>
#include <float.h>
@@ -105,29 +102,9 @@ namespace ale {
ale::State States::getState(double time, ale::interpolation interp) const {
int lowerBound = ale::interpolationIndex(m_ephemTimes, time);
- int interpStart;
- int interpStop;
-
- if (m_ephemTimes.size() <= 3) {
- interpStart = 0;
- interpStop = m_ephemTimes.size() - 1;
- }
- else if (lowerBound == 0) {
- interpStart = lowerBound;
- interpStop = lowerBound + 3;
- }
- else if (lowerBound == m_ephemTimes.size() - 1) {
- interpStart = lowerBound - 3;
- interpStop = lowerBound;
- }
- else if (lowerBound == m_ephemTimes.size() - 2) {
- interpStart = lowerBound - 2;
- interpStop = lowerBound + 1;
- }
- else {
- interpStart = lowerBound - 1;
- interpStop = lowerBound + 2;
- }
+ // try to copy the surrounding 8 points as that's the most possibly needed
+ int interpStart = std::max(0, lowerBound - 3);
+ int interpStop = std::min(lowerBound + 4, (int) m_ephemTimes.size() - 1);
ale::State state;
std::vector<double> xs, ys, zs, vxs, vys, vzs, interpTimes;
diff --git a/src/ale.cpp b/src/ale.cpp
index 571d8b2f875093d165149a4ebe14aa9d84c81a8b..efb41c2924589d19ff695dc93e83996070e34ebc 100644
--- a/src/ale.cpp
+++ b/src/ale.cpp
@@ -2,16 +2,10 @@
#include <nlohmann/json.hpp>
-#include <gsl/gsl_interp.h>
-#include <gsl/gsl_spline.h>
-#include <gsl/gsl_poly.h>
-
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-
#include <iostream>
#include <Python.h>
+#include <algorithm>
#include <string>
#include <iostream>
#include <stdexcept>
@@ -93,229 +87,70 @@ namespace ale {
}
}
- // Position Data Functions
- vector<double> getPosition(vector<vector<double>> coords, vector<double> times, double time,
- interpolation interp) {
- // Check that all of the data sizes are okay
- // TODO is there a cleaner way to do this? We're going to have to do this a lot.
- if (coords.size() != 3) {
- throw invalid_argument("Invalid input positions, expected three vectors.");
- }
-
- // GSL setup
- vector<double> coordinate = {0.0, 0.0, 0.0};
-
- coordinate = { interpolate(coords[0], times, time, interp, 0),
- interpolate(coords[1], times, time, interp, 0),
- interpolate(coords[2], times, time, interp, 0) };
-
- return coordinate;
- }
-
- vector<double> getVelocity(vector<vector<double>> coords, vector<double> times,
- double time, interpolation interp) {
- // Check that all of the data sizes are okay
- // TODO is there a cleaner way to do this? We're going to have to do this a lot.
- if (coords.size() != 3) {
- throw invalid_argument("Invalid input positions, expected three vectors.");
- }
-
- // GSL setup
- vector<double> coordinate = {0.0, 0.0, 0.0};
-
- coordinate = { interpolate(coords[0], times, time, interp, 1),
- interpolate(coords[1], times, time, interp, 1),
- interpolate(coords[2], times, time, interp, 1) };
-
- return coordinate;
- }
-
- // Postion Function Functions
- // vector<double> coeffs = [[cx_0, cx_1, cx_2 ..., cx_n],
- // [cy_0, cy_1, cy_2, ... cy_n],
- // [cz_0, cz_1, cz_2, ... cz_n]]
- // The equations evaluated by this function are:
- // x = cx_n * t^n + cx_n-1 * t^(n-1) + ... + cx_0
- // y = cy_n * t^n + cy_n-1 * t^(n-1) + ... + cy_0
- // z = cz_n * t^n + cz_n-1 * t^(n-1) + ... + cz_0
- vector<double> getPosition(vector<vector<double>> coeffs, double time) {
-
- if (coeffs.size() != 3) {
- throw invalid_argument("Invalid input coeffs, expected three vectors.");
- }
-
- vector<double> coordinate = {0.0, 0.0, 0.0};
- coordinate[0] = evaluatePolynomial(coeffs[0], time, 0); // X
- coordinate[1] = evaluatePolynomial(coeffs[1], time, 0); // Y
- coordinate[2] = evaluatePolynomial(coeffs[2], time, 0); // Z
-
- return coordinate;
- }
-
-
- // Velocity Function
- // Takes the coefficients from the position equation
- vector<double> getVelocity(vector<vector<double>> coeffs, double time) {
-
- if (coeffs.size() != 3) {
- throw invalid_argument("Invalid input coeffs, expected three vectors.");
- }
-
- vector<double> coordinate = {0.0, 0.0, 0.0};
- coordinate[0] = evaluatePolynomial(coeffs[0], time, 1); // X
- coordinate[1] = evaluatePolynomial(coeffs[1], time, 1); // Y
- coordinate[2] = evaluatePolynomial(coeffs[2], time, 1); // Z
-
- return coordinate;
- }
-
-
- // Rotation Data Functions
- vector<double> getRotation(vector<vector<double>> rotations,
- vector<double> times, double time, interpolation interp) {
- // Check that all of the data sizes are okay
- // TODO is there a cleaner way to do this? We're going to have to do this a lot.
- if (rotations.size() != 4) {
- throw invalid_argument("Invalid input rotations, expected four vectors.");
- }
-
- // Alot of copying and reassignment becuase conflicting data types
- // probably should rethink our vector situation to guarentee contiguous
- // memory. Should be easy to switch to a contiguous column-major format
- // if we stick with Eigen.
- for (size_t i = 0; i<rotations[0].size(); i++) {
- Eigen::Quaterniond quat(rotations[0][i], rotations[1][i], rotations[2][i], rotations[3][i]);
- quat.normalize();
-
- rotations[0][i] = quat.w();
- rotations[1][i] = quat.x();
- rotations[2][i] = quat.y();
- rotations[3][i] = quat.z();
- }
-
- // GSL setup
- vector<double> coordinate = {0.0, 0.0, 0.0, 0.0};
-
- coordinate = { interpolate(rotations[0], times, time, interp, 0),
- interpolate(rotations[1], times, time, interp, 0),
- interpolate(rotations[2], times, time, interp, 0),
- interpolate(rotations[3], times, time, interp, 0)};
-
- // Eigen::Map to ensure the array isn't copied, only the pointer is
- Eigen::Map<Eigen::MatrixXd> quat(coordinate.data(), 4, 1);
- quat.normalize();
- return coordinate;
- }
-
- vector<double> getAngularVelocity(vector<vector<double>> rotations,
- vector<double> times, double time, interpolation interp) {
- // Check that all of the data sizes are okay
- // TODO is there a cleaner way to do this? We're going to have to do this a lot.
- if (rotations.size() != 4) {
- throw invalid_argument("Invalid input rotations, expected four vectors.");
- }
-
- double data[] = {0,0,0,0};
- for (size_t i = 0; i<rotations[0].size(); i++) {
- Eigen::Quaterniond quat(rotations[0][i], rotations[1][i], rotations[2][i], rotations[3][i]);
- quat.normalize();
- rotations[0][i] = quat.w();
- rotations[1][i] = quat.x();
- rotations[2][i] = quat.y();
- rotations[3][i] = quat.z();
+ double lagrangeInterpolate(const std::vector<double>& times,
+ const std::vector<double>& values,
+ double time, int order) {
+ // Ensure the times and values have the same length
+ if (times.size() != values.size()) {
+ throw invalid_argument("Times and values must have the same length.");
}
- // GSL setup
-
-
- Eigen::Quaterniond quat(interpolate(rotations[0], times, time, interp, 0),
- interpolate(rotations[1], times, time, interp, 0),
- interpolate(rotations[2], times, time, interp, 0),
- interpolate(rotations[3], times, time, interp, 0));
- quat.normalize();
-
- Eigen::Quaterniond dQuat(interpolate(rotations[0], times, time, interp, 1),
- interpolate(rotations[1], times, time, interp, 1),
- interpolate(rotations[2], times, time, interp, 1),
- interpolate(rotations[3], times, time, interp, 1));
-
- Eigen::Quaterniond avQuat = quat.conjugate() * dQuat;
-
- vector<double> coordinate = {-2 * avQuat.x(), -2 * avQuat.y(), -2 * avQuat.z()};
- return coordinate;
- }
-
- // Rotation Function Functions
- std::vector<double> getRotation(vector<vector<double>> coeffs, double time) {
-
- if (coeffs.size() != 3) {
- throw invalid_argument("Invalid input coefficients, expected three vectors.");
- }
-
- vector<double> rotation = {0.0, 0.0, 0.0};
-
- rotation[0] = evaluatePolynomial(coeffs[0], time, 0); // X
- rotation[1] = evaluatePolynomial(coeffs[1], time, 0); // Y
- rotation[2] = evaluatePolynomial(coeffs[2], time, 0); // Z
-
- Eigen::Quaterniond quat;
- quat = Eigen::AngleAxisd(rotation[0] * M_PI / 180, Eigen::Vector3d::UnitZ())
- * Eigen::AngleAxisd(rotation[1] * M_PI / 180, Eigen::Vector3d::UnitX())
- * Eigen::AngleAxisd(rotation[2] * M_PI / 180, Eigen::Vector3d::UnitZ());
-
- quat.normalize();
-
- vector<double> rotationQ = {quat.w(), quat.x(), quat.y(), quat.z()};
- return rotationQ;
- }
-
- vector<double> getAngularVelocity(vector<vector<double>> coeffs, double time) {
-
- if (coeffs.size() != 3) {
- throw invalid_argument("Invalid input coefficients, expected three vectors.");
+ // Get the correct interpolation window
+ int index = interpolationIndex(times, time);
+ int windowSize = min(index + 1, (int) times.size() - index - 1);
+ windowSize = min(windowSize, (int) order / 2);
+ int startIndex = index - windowSize + 1;
+ int endIndex = index + windowSize + 1;
+
+ // Interpolate
+ double result = 0;
+ for (int i = startIndex; i < endIndex; i++) {
+ double weight = 1;
+ double numerator = 1;
+ for (int j = startIndex; j < endIndex; j++) {
+ if (i == j) {
+ continue;
+ }
+ weight *= times[i] - times[j];
+ numerator *= time - times[j];
+ }
+ result += numerator * values[i] / weight;
}
-
- double phi = evaluatePolynomial(coeffs[0], time, 0); // X
- double theta = evaluatePolynomial(coeffs[1], time, 0); // Y
- double psi = evaluatePolynomial(coeffs[2], time, 0); // Z
-
- double phi_dt = evaluatePolynomial(coeffs[0], time, 1);
- double theta_dt = evaluatePolynomial(coeffs[1], time, 1);
- double psi_dt = evaluatePolynomial(coeffs[2], time, 1);
-
- Eigen::Quaterniond quat1, quat2;
- quat1 = Eigen::AngleAxisd(phi * M_PI / 180, Eigen::Vector3d::UnitZ());
- quat2 = Eigen::AngleAxisd(theta * M_PI / 180, Eigen::Vector3d::UnitX());
-
- Eigen::Vector3d velocity = phi_dt * Eigen::Vector3d::UnitZ();
- velocity += theta_dt * (quat1 * Eigen::Vector3d::UnitX());
- velocity += psi_dt * (quat1 * quat2 * Eigen::Vector3d::UnitZ());
-
- return {velocity[0], velocity[1], velocity[2]};
+ return result;
}
- // Polynomial evaluation helper function
- // The equation evaluated by this function is:
- // x = cx_0 + cx_1 * t^(1) + ... + cx_n * t^n
- // The d parameter is for which derivative of the polynomial to compute.
- // Supported options are
- // 0: no derivative
- // 1: first derivative
- // 2: second derivative
- double evaluatePolynomial(vector<double> coeffs, double time, int d){
- if (coeffs.empty()) {
- throw invalid_argument("Invalid input coeffs, must be non-empty.");
+ double lagrangeInterpolateDerivative(const std::vector<double>& times,
+ const std::vector<double>& values,
+ double time, int order) {
+ // Ensure the times and values have the same length
+ if (times.size() != values.size()) {
+ throw invalid_argument("Times and values must have the same length.");
}
- if (d < 0) {
- throw invalid_argument("Invalid derivative degree, must be non-negative.");
+ // Get the correct interpolation window
+ int index = interpolationIndex(times, time);
+ int windowSize = min(index + 1, (int) times.size() - index - 1);
+ windowSize = min(windowSize, (int) order / 2);
+ int startIndex = index - windowSize + 1;
+ int endIndex = index + windowSize + 1;
+
+ // Interpolate
+ double result = 0;
+ for (int i = startIndex; i < endIndex; i++) {
+ double weight = 1;
+ double derivativeWeight = 0;
+ double numerator = 1;
+ for (int j = startIndex; j < endIndex; j++) {
+ if (i == j) {
+ continue;
+ }
+ weight *= times[i] - times[j];
+ numerator *= time - times[j];
+ derivativeWeight += 1.0 / (time - times[j]);
+ }
+ result += numerator * values[i] * derivativeWeight / weight;
}
-
- vector<double> derivatives(d + 1);
- gsl_poly_eval_derivs(coeffs.data(), coeffs.size(), time,
- derivatives.data(), derivatives.size());
-
- return derivatives.back();
+ return result;
}
double interpolate(vector<double> points, vector<double> times, double time, interpolation interp, int d) {
@@ -324,41 +159,36 @@ namespace ale {
throw invalid_argument("At least two points must be input to interpolate over.");
}
if (points.size() != times.size()) {
- throw invalid_argument("Invalid gsl_interp_type data, must have the same number of points as times.");
- }
- if (time < times.front() || time > times.back()) {
- throw invalid_argument("Invalid gsl_interp_type time, outside of input times.");
+ throw invalid_argument("Must have the same number of points as times.");
}
- // convert our interp enum into a GSL one,
- // should be easy to add non GSL interp methods here later
- const gsl_interp_type *interp_methods[] = {gsl_interp_linear, gsl_interp_cspline};
-
- gsl_interp *interpolator = gsl_interp_alloc(interp_methods[interp], numPoints);
- gsl_interp_init(interpolator, ×[0], &points[0], numPoints);
- gsl_interp_accel *acc = gsl_interp_accel_alloc();
+ int order;
+ switch(interp) {
+ case LINEAR:
+ order = 2;
+ break;
+ case SPLINE:
+ order = 4;
+ break;
+ case LAGRANGE:
+ order = 8;
+ break;
+ default:
+ throw invalid_argument("Invalid interpolation option, must be LINEAR, SPLINE, or LAGRANGE.");
+ }
- // GSL evaluate
double result;
switch(d) {
case 0:
- result = gsl_interp_eval(interpolator, ×[0], &points[0], time, acc);
+ result = lagrangeInterpolate(times, points, time, order);
break;
case 1:
- result = gsl_interp_eval_deriv(interpolator, ×[0], &points[0], time, acc);
- break;
- case 2:
- result = gsl_interp_eval_deriv2(interpolator, ×[0], &points[0], time, acc);
+ result = lagrangeInterpolateDerivative(times, points, time, order);
break;
default:
- throw invalid_argument("Invalid derivitive option, must be 0, 1 or 2.");
- break;
+ throw invalid_argument("Invalid derivitive option, must be 0 or 1.");
}
- // GSL clean up
- gsl_interp_free(interpolator);
- gsl_interp_accel_free(acc);
-
return result;
}
diff --git a/tests/ctests/AleTest.cpp b/tests/ctests/AleTest.cpp
index 9ee7d1345f9d3e65ad09891fc713c1ec811a9794..2afe618d0729a1471ad970e643a5422185e2ce2e 100644
--- a/tests/ctests/AleTest.cpp
+++ b/tests/ctests/AleTest.cpp
@@ -6,41 +6,12 @@
#include <stdexcept>
#include <cmath>
-#include <gsl/gsl_interp.h>
-
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
-TEST(PositionInterpTest, LinearInterp) {
- vector<double> times = { -3, -2, -1, 0, 1, 2};
- vector<vector<double>> data = {{ -3, -2, -1, 0, 1, 2},
- { 9, 4, 1, 0, 1, 4},
- {-27, -8, -1, 0, 1, 8}};
-
- vector<double> coordinate = ale::getPosition(data, times, -1.5, ale::LINEAR);
-
- ASSERT_EQ(3, coordinate.size());
- EXPECT_DOUBLE_EQ(-1.5, coordinate[0]);
- EXPECT_DOUBLE_EQ(2.5, coordinate[1]);
- EXPECT_DOUBLE_EQ(-4.5, coordinate[2]);
-}
-
-
-TEST(PositionInterpTest, FourCoordinates) {
- vector<double> times = { -3, -2, -1, 0, 1, 2};
- vector<vector<double>> data = {{ -3, -2, -1, 0, 1, 2},
- { 9, 4, 1, 0, 1, 4},
- {-27, -8, -1, 0, 1, 8},
- { 25, 0, -5, 25, 3, 6}};
-
- EXPECT_THROW(ale::getPosition(data, times, 0.0, ale::LINEAR),
- invalid_argument);
-}
-
-
TEST(LinearInterpTest, ExampleInterpolation) {
vector<double> times = {0, 1, 2, 3};
vector<double> data = {0, 2, 1, 0};
@@ -59,8 +30,7 @@ TEST(LinearInterpTest, NoPoints) {
vector<double> times = {};
vector<double> data = {};
- EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::LINEAR, 0),
- invalid_argument);
+ EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::LINEAR, 0), invalid_argument);
}
@@ -68,8 +38,7 @@ TEST(LinearInterpTest, DifferentCounts) {
vector<double> times = { -3, -2, -1, 0, 2};
vector<double> data = { -3, -2, 1, 2};
- EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::LINEAR, 0),
- invalid_argument);
+ EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::LINEAR, 0), invalid_argument);
}
@@ -77,34 +46,24 @@ TEST(LinearInterpTest, Extrapolate) {
vector<double> times = {0, 1, 2, 3};
vector<double> data = {0, 2, 1, 0};
- EXPECT_THROW(ale::interpolate(data, times, -1.0, ale::LINEAR, 0),
- invalid_argument);
- EXPECT_THROW(ale::interpolate(data, times, 4.0, ale::LINEAR, 0),
- invalid_argument);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, -1.0, ale::LINEAR, 0), -2);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 4.0, ale::LINEAR, 0), -1);
}
TEST(SplineInterpTest, ExampleInterpolation) {
// From http://www.maths.nuigalway.ie/~niall/teaching/Archive/1617/MA378/2-2-CubicSplines.pdf
- vector<double> times = {0, 1, 2, 3};
- vector<double> data = {0, 2, 1, 0};
- // Spline functions is:
- // 2.8x - 0.8x^3, x in [0, 1]
- // S(x) = x^3 - 5.4x^2 + 8.2x - 1.8, x in [1, 2]
- // -0.2x^3 + 1.8x^2 - 6.2x + 7.8, x in [2, 3]
-
- // The spline interpolation is only ~1e-10 so we have to define a tolerance
- double tolerance = 1e-10;
- EXPECT_NEAR(0.0, ale::interpolate(data, times, 0.0, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(2.8 * 0.5 - 0.8 * 0.125,
- ale::interpolate(data, times, 0.5, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(2.0, ale::interpolate(data, times, 1.0, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(3.375 - 5.4 * 2.25 + 8.2 * 1.5 - 1.8,
- ale::interpolate(data, times, 1.5, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(1.0, ale::interpolate(data, times, 2.0, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(-0.2 * 15.625 + 1.8 * 6.25 - 6.2 * 2.5 + 7.8,
- ale::interpolate(data, times, 2.5, ale::SPLINE, 0), tolerance);
- EXPECT_NEAR(0.0, ale::interpolate(data, times, 3.0, ale::SPLINE, 0), tolerance);
+ vector<double> times = {0, 1, 2, 3};
+ vector<double> data = {2, 4, 3, 2};
+ // function is f(t) = 0.5t^3 - 3t^2 + 4.5t + 2
+
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 0.0, ale::SPLINE, 0), 2.0);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 0.5, ale::SPLINE, 0), 3.0);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 1.0, ale::SPLINE, 0), 4.0);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 1.5, ale::SPLINE, 0), 3.6875);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 2.0, ale::SPLINE, 0), 3.0);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 2.5, ale::SPLINE, 0), 2.5);
+ EXPECT_DOUBLE_EQ(ale::interpolate(data, times, 3.0, ale::SPLINE, 0), 2.0);
}
@@ -112,8 +71,7 @@ TEST(SplineInterpTest, NoPoints) {
vector<double> times = {};
vector<double> data = {};
- EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::SPLINE, 0),
- invalid_argument);
+ EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::SPLINE, 0), invalid_argument);
}
@@ -121,247 +79,36 @@ TEST(SplineInterpTest, DifferentCounts) {
vector<double> times = { -3, -2, -1, 0, 2};
vector<double> data = { -3, -2, 1, 2};
- EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::SPLINE, 0),
- invalid_argument);
-}
-
-
-TEST(SplineInterpTest, Extrapolate) {
- vector<double> times = {0, 1, 2, 3};
- vector<double> data = {0, 2, 1, 0};
-
- EXPECT_THROW(ale::interpolate(data, times, -1.0, ale::SPLINE, 0),
- invalid_argument);
- EXPECT_THROW(ale::interpolate(data, times, 4.0, ale::SPLINE, 0),
- invalid_argument);
-}
-
-
-TEST(PolynomialTest, Evaluate) {
- vector<double> coeffs = {1.0, 2.0, 3.0}; // 1 + 2x + 3x^2
- EXPECT_EQ(2.0, ale::evaluatePolynomial(coeffs, -1, 0));
-}
-
-
-TEST(PolynomialTest, Derivatives) {
- vector<double> coeffs = {1.0, 2.0, 3.0}; // 1 + 2x + 3x^2
- EXPECT_EQ(-4.0, ale::evaluatePolynomial(coeffs, -1, 1));
- EXPECT_EQ(6.0, ale::evaluatePolynomial(coeffs, -1, 2));
-}
-
-
-TEST(PolynomialTest, EmptyCoeffs) {
- vector<double> coeffs = {};
- EXPECT_THROW(ale::evaluatePolynomial(coeffs, -1, 1), invalid_argument);
-}
-
-TEST(PolynomialTest, BadDerivative) {
- vector<double> coeffs = {1.0, 2.0, 3.0};
- EXPECT_THROW(ale::evaluatePolynomial(coeffs, -1, -1), invalid_argument);
-}
-
-
-TEST(PoisitionCoeffTest, SecondOrderPolynomial) {
- double time = 2.0;
- vector<vector<double>> coeffs = {{1.0, 2.0, 3.0},
- {1.0, 3.0, 2.0},
- {3.0, 2.0, 1.0}};
-
- vector<double> coordinate = ale::getPosition(coeffs, time);
-
- ASSERT_EQ(3, coordinate.size());
- EXPECT_DOUBLE_EQ(17.0, coordinate[0]);
- EXPECT_DOUBLE_EQ(15.0, coordinate[1]);
- EXPECT_DOUBLE_EQ(11.0, coordinate[2]);
-}
-
-
-TEST(PoisitionCoeffTest, DifferentPolynomialDegrees) {
- double time = 2.0;
- vector<vector<double>> coeffs = {{1.0},
- {1.0, 2.0},
- {1.0, 2.0, 3.0}};
-
- vector<double> coordinate = ale::getPosition(coeffs, time);
-
- ASSERT_EQ(3, coordinate.size());
- EXPECT_DOUBLE_EQ(1.0, coordinate[0]);
- EXPECT_DOUBLE_EQ(5.0, coordinate[1]);
- EXPECT_DOUBLE_EQ(17.0, coordinate[2]);
-}
-
-
-TEST(PoisitionCoeffTest, NegativeInputs) {
- double time = -2.0;
- vector<vector<double>> coeffs = {{-1.0, -2.0, -3.0},
- {1.0, -2.0, 3.0},
- {-1.0, 2.0, -3.0}};
-
- vector<double> coordinate = ale::getPosition(coeffs, time);
-
- ASSERT_EQ(3, coordinate.size());
- EXPECT_DOUBLE_EQ(-9.0, coordinate[0]);
- EXPECT_DOUBLE_EQ(17.0, coordinate[1]);
- EXPECT_DOUBLE_EQ(-17.0, coordinate[2]);
-}
-
-
-TEST(PoisitionCoeffTest, InvalidInput) {
- double valid_time = 0.0;
- vector<vector<double>> invalid_coeffs_sizes = {{3.0, 2.0, 1.0},
- {1.0, 2.0, 3.0}};
-
- EXPECT_THROW(ale::getPosition(invalid_coeffs_sizes, valid_time), invalid_argument);
-}
-
-
-TEST(VelocityCoeffTest, SecondOrderPolynomial) {
- double time = 2.0;
- vector<vector<double>> coeffs = {{1.0, 2.0, 3.0},
- {1.0, 3.0, 2.0},
- {3.0, 2.0, 1.0}};
-
- vector<double> coordinate = ale::getVelocity(coeffs, time);
-
- ASSERT_EQ(3, coordinate.size());
- EXPECT_DOUBLE_EQ(14.0, coordinate[0]);
- EXPECT_DOUBLE_EQ(11.0, coordinate[1]);
- EXPECT_DOUBLE_EQ(6.0, coordinate[2]);
-}
-
-
-TEST(VelocityCoeffTest, InvalidInput) {
- double valid_time = 0.0;
- vector<vector<double>> invalid_coeffs_sizes = {{3.0, 2.0, 1.0},
- {1.0, 2.0, 3.0}};
-
- EXPECT_THROW(ale::getVelocity(invalid_coeffs_sizes, valid_time), invalid_argument);
-}
-
-TEST(RotationCoeffTest, ZeroOrderPolynomial) {
- double time = 1.0;
- vector<vector<double>> coeffs = {{90},
- {0},
- {0}};
-
- vector<double> coordinate = ale::getRotation(coeffs, time);
-
- ASSERT_EQ(4, coordinate.size());
- EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[0]);
- EXPECT_DOUBLE_EQ(0, coordinate[1]);
- EXPECT_DOUBLE_EQ(0, coordinate[2]);
- EXPECT_DOUBLE_EQ(1 / sqrt(2), coordinate[3]);
-}
-
-TEST(RotationCoeffTest, InvalidInput) {
- double time = 1.0;
- vector<vector<double>> coeffs = {{90},
- {0}};
-
- EXPECT_THROW(ale::getRotation(coeffs, time), invalid_argument);
-}
-
-TEST(AngularVelocityCoeffTest, DefaultAngularExample) {
- vector<vector<double>> coeffs = {{0, 90}, {0, 90}, {0, 90}};
- double time = 1.0;
-
- vector<double> av = ale::getAngularVelocity(coeffs, time);
-
- ASSERT_EQ(3, av.size());
- EXPECT_DOUBLE_EQ(90, av[0]);
- EXPECT_DOUBLE_EQ(90, av[1]);
- EXPECT_DOUBLE_EQ(90, av[2]);
-}
-
-TEST(AngularVelocityCoeffTest, InvalidInput) {
- vector<vector<double>> coeffs = {{0, 90}, {0, 90}};
- double time = 2.0;
-
- EXPECT_THROW(ale::getAngularVelocity(coeffs, time), invalid_argument);
+ EXPECT_THROW(ale::interpolate(data, times, 0.0, ale::SPLINE, 0), invalid_argument);
}
-TEST(RotationInterpTest, ExampleGetRotation) {
- // simple test, only checks if API hit correctly and output is normalized
- vector<double> times = {0, 1, 2, 3};
- vector<vector<double>> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}});
- vector<double> r = ale::getRotation(rots, times, 2, ale::LINEAR);
- Eigen::Quaterniond quat(r[0], r[1], r[2], r[3]);
-
- EXPECT_DOUBLE_EQ(1, quat.norm());
-}
-
-TEST(RotationInterpTest, GetRotationDifferentCounts) {
- // incorrect params
- vector<double> times = {0, 1, 2};
- vector<vector<double>> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}});
- EXPECT_THROW(ale::getRotation(rots, times, 2, ale::LINEAR), invalid_argument);
-}
-
-TEST(RotationInterpTest, InvalidTime) {
- vector<double> times = {0, 1, 2};
- vector<vector<double>> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}});
- EXPECT_THROW(ale::getRotation(rots, times, 3, ale::LINEAR), invalid_argument);
-}
-
TEST(PyInterfaceTest, LoadInvalidLabel) {
std::string label = "Not a Real Label";
EXPECT_THROW(ale::load(label), invalid_argument);
}
+
TEST(PyInterfaceTest, LoadValidLabel) {
std::string label = "../pytests/data/EN1072174528M/EN1072174528M_spiceinit.lbl";
ale::load(label, "", "isis");
}
-TEST(AngularVelocityInterpTest, ExampleGetRotation) {
- vector<double> times = {0, 1};
- vector<vector<double>> rots({{0,0}, {1,0}, {0,1}, {0,0}});
- vector<double> av = ale::getAngularVelocity(rots, times, 0.5, ale::LINEAR);
-
- EXPECT_DOUBLE_EQ(0, av[0]);
- EXPECT_DOUBLE_EQ(0, av[1]);
- EXPECT_DOUBLE_EQ(2 * sqrt(2), av[2]);
-}
-
-TEST(AngularVelocityInterpTest, InvalidTime) {
- vector<double> times = {0, 1, 2};
- vector<vector<double>> rots({{0,0}, {1,0}, {0,1}, {0,0}});
- EXPECT_THROW(ale::getAngularVelocity(rots, times, 3, ale::LINEAR), invalid_argument);
-}
-
-TEST(VelocityInterpTest, ExampleGetVelocity) {
- vector<double> times = {0, 1, 2};
- vector<vector<double>> coords({{1, 1, 1}, {2, 2, 2}, {3, 3, 3}});
- vector<double> v = ale::getVelocity(coords, times, 1, ale::LINEAR);
-
- //not sure what the values are expected to look like
- EXPECT_DOUBLE_EQ(v[0], 0);
- EXPECT_DOUBLE_EQ(v[1], 0);
- EXPECT_DOUBLE_EQ(v[2], 0);
-}
-
-TEST(VelocityInterpTest, InvalidTime) {
- vector<double> times = {0, 1, 2};
- vector<vector<double>> coords({{1, 1, 1}, {2, 2, 2}, {3, 3, 3}});
- EXPECT_THROW(ale::getVelocity(coords, times, 3, ale::LINEAR), invalid_argument);
-}
TEST(Interpolation, Derivative1) {
vector<double> points = {0, 2, 4};
vector<double> times = {0, 1, 2};
-
- EXPECT_DOUBLE_EQ(ale::interpolate(points, times, 1, ale::LINEAR, 1), 2);
+ EXPECT_NO_THROW(ale::interpolate(points, times, 1, ale::LINEAR, 1));
}
+
TEST(Interpolation, Derivative2) {
- //only checks that case 2 of the switch block is hit
vector<double> points = {0, 0, 0};
vector<double> times = {0, 1, 2};
-
- EXPECT_DOUBLE_EQ(ale::interpolate(points, times, 1, ale::LINEAR, 2), 0);
+ EXPECT_THROW(ale::interpolate(points, times, 1, ale::LINEAR, 2), invalid_argument);
}
+
TEST(Interpolation, InvalidDerivative) {
vector<double> points = {0, 0, 0};
vector<double> times = {0, 1, 2};
@@ -369,13 +116,15 @@ TEST(Interpolation, InvalidDerivative) {
EXPECT_THROW(ale::interpolate(points, times, 1, ale::LINEAR, 3), invalid_argument);
}
+
TEST(InterpIndex, InvalidTimes) {
std::vector<double> times = {};
EXPECT_THROW(ale::interpolationIndex(times, 0), invalid_argument);
}
-TEST(EvaluateCubicHermite, SimplyPolynomial) {
+
+TEST(EvaluateCubicHermite, SimplePolynomial) {
// Cubic function is y = x^3 - 2x^2 + 1
// derivative is dy/dx = 3x^2 - 4x
std::vector<double> derivs = {7.0, -1.0};
@@ -393,6 +142,7 @@ TEST(EvaluateCubicHermite, InvalidDervisXY) {
EXPECT_THROW(ale::evaluateCubicHermite(0.0, derivs, x, y), invalid_argument);
}
+
TEST(EvaluateCubicHermiteFirstDeriv, SimplyPolynomial) {
// Cubic function is y = x^3 - 2x^2 + 1
// derivative is dy/dx = 3x^2 - 4x
@@ -403,6 +153,7 @@ TEST(EvaluateCubicHermiteFirstDeriv, SimplyPolynomial) {
EXPECT_DOUBLE_EQ(ale::evaluateCubicHermiteFirstDeriv(0.5, derivs, x, y), -1.25);
}
+
TEST(EvaluateCubicHermiteFirstDeriv, InvalidDervisTimes) {
std::vector<double> derivs = {};
std::vector<double> times = {1.0};
@@ -411,6 +162,7 @@ TEST(EvaluateCubicHermiteFirstDeriv, InvalidDervisTimes) {
EXPECT_THROW(ale::evaluateCubicHermiteFirstDeriv(0.0, derivs, times, y), invalid_argument);
}
+
TEST(EvaluateCubicHermiteFirstDeriv, InvalidVelocities) {
std::vector<double> derivs = {5.0, 6.0};
std::vector<double> times = {1.0, 1.0};
@@ -418,3 +170,84 @@ TEST(EvaluateCubicHermiteFirstDeriv, InvalidVelocities) {
EXPECT_THROW(ale::evaluateCubicHermiteFirstDeriv(0.0, derivs, times, y), invalid_argument);
}
+
+// The following tests all use the following equations
+//
+// v = -t^3 - 7x^2 + 3x + 16
+//
+// The full set of values is:
+// t v
+// -3 -83
+// -2 -26
+// -1 5
+// 0 16
+// 1 13
+// 2 2
+// 3 -11
+// 4 -20
+// 5 -19
+// 6 -2
+// 7 37
+
+TEST(LagrangeInterpolate, SecondOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolate(times, values, 1.5, 2), 7);
+}
+
+
+TEST(LagrangeInterpolate, FourthOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolate(times, values, 1.5, 4), 8.125);
+}
+
+
+TEST(LagrangeInterpolate, ReducedOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolate(times, values, 3.5, 4), -13);
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolate(times, values, -2.5, 4), -54.5);
+}
+
+
+TEST(LagrangeInterpolate, InvalidArguments) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13};
+ EXPECT_THROW(ale::lagrangeInterpolate(times, values, 3.5, 4), invalid_argument);
+}
+
+
+TEST(lagrangeInterpolateDerivative, SecondOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolateDerivative(times, values, 1.5, 2), -12);
+}
+
+
+TEST(LagrangeInterpolateDerivative, FourthOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolateDerivative(times, values, 1.5, 4), -11.25);
+}
+
+
+TEST(LagrangeInterpolateDerivative, ReducedOrder) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13, -11, -19};
+
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolateDerivative(times, values, 3.5, 4), -4);
+ EXPECT_DOUBLE_EQ(ale::lagrangeInterpolateDerivative(times, values, -2.5, 4), 57);
+}
+
+
+TEST(LagrangeInterpolateDerivative, InvalidArguments) {
+ std::vector<double> times = {-3, -2, -1, 0, 1, 3, 5};
+ std::vector<double> values = {-83, -26, 5, 16, 13};
+ EXPECT_THROW(ale::lagrangeInterpolateDerivative(times, values, 3.5, 4), invalid_argument);
+}
diff --git a/tests/ctests/CMakeLists.txt b/tests/ctests/CMakeLists.txt
index 85a7ab1cfd2450e6482a48b160d4d69f2c84efeb..226f9292ea8e74766c3ec78054021d02dfa24fb3 100644
--- a/tests/ctests/CMakeLists.txt
+++ b/tests/ctests/CMakeLists.txt
@@ -8,8 +8,6 @@ add_executable(runAleTests ${test_source})
target_link_libraries(runAleTests
PRIVATE
ale
- GSL::gsl
- GSL::gslcblas
Eigen3::Eigen
gtest
gmock
diff --git a/tests/ctests/StatesTest.cpp b/tests/ctests/StatesTest.cpp
index b96e6650b910eb9a5376c6f0cd2deab4d4cc146f..224aec7103dac780a0add23756a5fe2a3a0c1395 100644
--- a/tests/ctests/StatesTest.cpp
+++ b/tests/ctests/StatesTest.cpp
@@ -163,7 +163,7 @@ protected:
};
-// Tests GSL spline and linear interp - force to use GSL spline by omitting velocity
+// Tests spline, linear, and lagrange interp - force to use spline by omitting velocity
TEST_F(TestState, getPosition) {
double time = 1.5;
@@ -186,8 +186,8 @@ TEST_F(TestState, getPosition) {
EXPECT_NEAR(linear_no_vel_position.y, linear_y, 1e-10);
EXPECT_NEAR(linear_no_vel_position.z, linear_z, 1e-10);
EXPECT_NEAR(spline_no_vel_position.x, 5.5, 1e-10);
- EXPECT_NEAR(spline_no_vel_position.y, 3.8, 1e-10);
- EXPECT_NEAR(spline_no_vel_position.z, 0.2016, 1e-10);
+ EXPECT_NEAR(spline_no_vel_position.y, 3.75, 1e-10);
+ EXPECT_NEAR(spline_no_vel_position.z, 0.216, 1e-10);
}
@@ -209,7 +209,7 @@ TEST_F(TestState, getVelocity) {
EXPECT_NEAR(linear_no_vel_velocity.z, 0.448, 1e-10);
EXPECT_NEAR(spline_no_vel_velocity.x, 1, 1e-10);
EXPECT_NEAR(spline_no_vel_velocity.y, 1, 1e-10);
- EXPECT_NEAR(spline_no_vel_velocity.z, 0.416, 1e-10);
+ EXPECT_NEAR(spline_no_vel_velocity.z, 0.432, 1e-10);
}
// getState() and interpolateState() are tested when testing getPosition and