From 7b57668c937c2830416e7beb5d5dfcbb97c9599a Mon Sep 17 00:00:00 2001 From: Kelvin Rodriguez Date: Tue, 4 Dec 2018 16:48:29 -0700 Subject: [PATCH] implemented getVelocity (#45) * implemented getVelocity * removed debug print in cmake script * updated as per comments --- include/eal.h | 16 ++++-- src/eal.cpp | 133 ++++++++++++++++++++----------------------- tests/CMakeLists.txt | 8 ++- tests/EalTest.cpp | 63 +++++++++----------- 4 files changed, 106 insertions(+), 114 deletions(-) diff --git a/include/eal.h b/include/eal.h index c33665a..5858e7a 100644 --- a/include/eal.h +++ b/include/eal.h @@ -5,6 +5,8 @@ #include #include +#include + namespace eal { enum interpolation { @@ -16,10 +18,11 @@ namespace eal { std::vector getPosition(std::vector> coords, std::vector times, - interpolation interp, double time); + double time, interpolation interp); + std::vector getVelocity(std::vector> coords, std::vector times, - interpolation interp, double time); + double time, const interpolation interp); std::vector getPosition(std::vector coeffs, double time); std::vector getVelocity(std::vector coeffs, double time); @@ -27,19 +30,20 @@ namespace eal { std::vector getRotation(std::string from, std::string to, std::vector> rotations, std::vector times, - interpolation interp, double time); + double time, interpolation interp); std::vector getAngularVelocity(std::string from, std::string to, std::vector> rotations, std::vector times, - interpolation interp, double time); + double time, interpolation interp); std::vector getRotation(std::string from, std::string to, std::vector coefficients, double time); std::vector getAngularVelocity(std::string from, std::string to, std::vector coefficients, double time); - double linearInterpolate(std::vector points, std::vector times, double time); - double splineInterpolate(std::vector points, std::vector times, double time); + double interpolate(std::vector points, std::vector times, double time, interpolation interp, int d); + + } #endif // EAL_H diff --git a/src/eal.cpp b/src/eal.cpp index 8da683f..f02078c 100644 --- a/src/eal.cpp +++ b/src/eal.cpp @@ -30,8 +30,8 @@ namespace eal { // Positional Functions // Position Data Functions - vector getPosition(vector> coords, vector times, - interpolation interp, double time) { + vector getPosition(vector> coords, vector 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) { @@ -40,30 +40,29 @@ namespace eal { // GSL setup vector coordinate = {0.0, 0.0, 0.0}; - switch(interp) { - case linear: - coordinate = { linearInterpolate(coords[0], times, time), - linearInterpolate(coords[1], times, time), - linearInterpolate(coords[2], times, time) }; - break; - - case spline: - coordinate = { splineInterpolate(coords[0], times, time), - splineInterpolate(coords[1], times, time), - splineInterpolate(coords[2], times, time) }; - break; - - default: - throw invalid_argument("Invalid interpolation method."); - break; - } + + 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 getVelocity(vector> coords, vector times, - interpolation interp, double time) { + 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 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; } @@ -82,13 +81,13 @@ namespace eal { // Rotation Data Functions vector getRotation(string from, string to, vector> rotations, - vector times, interpolation interp, double time) { + vector times, double time, interpolation interp) { vector coordinate = {0.0, 0.0, 0.0}; return coordinate; } vector getAngularVelocity(string from, string to, vector> rotations, - vector times, interpolation interp, double time) { + vector times, double time, interpolation interp) { vector coordinate = {0.0, 0.0, 0.0}; return coordinate; } @@ -106,59 +105,49 @@ namespace eal { return coordinate; } - // Interpolation helper functions - double linearInterpolate(vector points, vector times, double time) { - size_t numPoints = points.size(); - if (numPoints < 2) { - throw invalid_argument("At least two points must be input to interpolate over."); - } - if (points.size() != times.size()) { - throw invalid_argument("Invalid interpolation data, must have the same number of points as times."); - } - if (time < times.front() || time > times.back()) { - throw invalid_argument("Invalid interpolation time, outside of input times."); - } - - // GSL setup - gsl_interp *interpolator = gsl_interp_alloc(gsl_interp_linear, numPoints); - gsl_interp_init(interpolator, ×[0], &points[0], numPoints); - gsl_interp_accel *acc = gsl_interp_accel_alloc(); - // GSL evaluate - double result = gsl_interp_eval(interpolator, ×[0], &points[0], time, acc); - - // GSL clean up - gsl_interp_free(interpolator); - gsl_interp_accel_free(acc); - - return result; - } - - double splineInterpolate(vector points, vector times, double time) { - size_t numPoints = points.size(); - if (numPoints < 2) { - throw invalid_argument("Invalid interpolation data, at least two points are required to interpolate."); - } - if (points.size() != times.size()) { - throw invalid_argument("Invalid interpolation data, must have the same number of points as times."); - } - if (time < times.front() || time > times.back()) { - throw invalid_argument("Invalid interpolation time, outside of input times."); - } - - // GSL setup - gsl_interp *interpolator = gsl_interp_alloc(gsl_interp_cspline, numPoints); - gsl_interp_init(interpolator, ×[0], &points[0], numPoints); - gsl_interp_accel *acc = gsl_interp_accel_alloc(); + double interpolate(vector points, vector times, double time, interpolation interp, int d) { + size_t numPoints = points.size(); + if (numPoints < 2) { + 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."); + } - // GSL evaluate - double result = gsl_interp_eval(interpolator, ×[0], &points[0], time, acc); + // 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(); + + // GSL evaluate + double result; + switch(d) { + case 0: + result = gsl_interp_eval(interpolator, ×[0], &points[0], time, acc); + 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); + break; + default: + throw invalid_argument("Invalid derivitive option, must be 0, 1 or 2."); + break; + } - // GSL clean up - gsl_interp_free(interpolator); - gsl_interp_accel_free(acc); + // GSL clean up + gsl_interp_free(interpolator); + gsl_interp_accel_free(acc); - return result; - } + return result; + } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4755fb7..1059ba0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -5,6 +5,12 @@ file(GLOB test_source "${CMAKE_SOURCE_DIR}/tests/*.cpp") # setup test executable add_executable(runEALTests ${test_source}) -target_link_libraries(runEALTests eal ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} pthread) +target_link_libraries(runEALTests eal ${GSL_LIBRARIES} ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} pthread) + +target_include_directories(runEALTests + PRIVATE + ${GSL_INCLUDE_DIRS} + PUBLIC + ${EAL_INCLUDE_DIRS}) gtest_discover_tests(runEALTests WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/tests) diff --git a/tests/EalTest.cpp b/tests/EalTest.cpp index 7747784..fd669b6 100644 --- a/tests/EalTest.cpp +++ b/tests/EalTest.cpp @@ -3,6 +3,8 @@ #include "eal.h" #include +#include + using namespace std; @@ -12,10 +14,10 @@ TEST(PositionInterpTest, LinearInterp) { { 9, 4, 1, 0, 1, 4}, {-27, -8, -1, 0, 1, 8}}; - vector coordinate = eal::getPosition(data, times, eal::linear, -1.5); + vector coordinate = eal::getPosition(data, times, -1.5, eal::linear); ASSERT_EQ(3, coordinate.size()); - EXPECT_DOUBLE_EQ(-1.5, coordinate[0]); + EXPECT_DOUBLE_EQ(-1.5, coordinate[0]); EXPECT_DOUBLE_EQ(2.5, coordinate[1]); EXPECT_DOUBLE_EQ(-4.5, coordinate[2]); } @@ -26,7 +28,7 @@ TEST(PositionInterpTest, SplineInterp) { {0, 1, 2, 3}, {0, 2, 1, 0}}; - vector coordinate = eal::getPosition(data, times, eal::spline, 0.5); + vector coordinate = eal::getPosition(data, times, 0.5, eal::spline); ASSERT_EQ(3, coordinate.size()); EXPECT_DOUBLE_EQ(0, coordinate[0]); @@ -41,38 +43,29 @@ TEST(PositionInterpTest, FourCoordinates) { {-27, -8, -1, 0, 1, 8}, { 25, 0, -5, 25, 3, 6}}; - EXPECT_THROW(eal::getPosition(data, times, eal::linear, 0.0), + EXPECT_THROW(eal::getPosition(data, times, 0.0, eal::linear), invalid_argument); } -TEST(PositionInterpTest, BadInterpolation) { - vector times = { -3, -2, -1, 0, 1, 2}; - vector> data = {{ -3, -2, -1, 0, 1, 2}, - { 9, 4, 1, 0, 1, 4}, - {-27, -8, -1, 0, 1, 8}}; - - EXPECT_THROW(eal::getPosition(data, times, (eal::interpolation)1000, 0.0), - invalid_argument); -} TEST(LinearInterpTest, ExampleInterpolation) { vector times = {0, 1, 2, 3}; vector data = {0, 2, 1, 0}; - EXPECT_DOUBLE_EQ(0.0, eal::linearInterpolate(data, times, 0.0)); - EXPECT_DOUBLE_EQ(1.0, eal::linearInterpolate(data, times, 0.5)); - EXPECT_DOUBLE_EQ(2.0, eal::linearInterpolate(data, times, 1.0)); - EXPECT_DOUBLE_EQ(1.5, eal::linearInterpolate(data, times, 1.5)); - EXPECT_DOUBLE_EQ(1.0, eal::linearInterpolate(data, times, 2.0)); - EXPECT_DOUBLE_EQ(0.5, eal::linearInterpolate(data, times, 2.5)); - EXPECT_DOUBLE_EQ(0.0, eal::linearInterpolate(data, times, 3.0)); + EXPECT_DOUBLE_EQ(0.0, eal::interpolate(data, times, 0.0, eal::linear, 0)); + EXPECT_DOUBLE_EQ(1.0, eal::interpolate(data, times, 0.5, eal::linear, 0)); + EXPECT_DOUBLE_EQ(2.0, eal::interpolate(data, times, 1.0, eal::linear, 0)); + EXPECT_DOUBLE_EQ(1.5, eal::interpolate(data, times, 1.5, eal::linear, 0)); + EXPECT_DOUBLE_EQ(1.0, eal::interpolate(data, times, 2.0, eal::linear, 0)); + EXPECT_DOUBLE_EQ(0.5, eal::interpolate(data, times, 2.5, eal::linear, 0)); + EXPECT_DOUBLE_EQ(0.0, eal::interpolate(data, times, 3.0, eal::linear, 0)); } TEST(LinearInterpTest, NoPoints) { vector times = {}; vector data = {}; - EXPECT_THROW(eal::linearInterpolate(data, times, 0.0), + EXPECT_THROW(eal::interpolate(data, times, 0.0, eal::linear, 0), invalid_argument); } @@ -80,7 +73,7 @@ TEST(LinearInterpTest, DifferentCounts) { vector times = { -3, -2, -1, 0, 2}; vector data = { -3, -2, 1, 2}; - EXPECT_THROW(eal::linearInterpolate(data, times, 0.0), + EXPECT_THROW(eal::interpolate(data, times, 0.0, eal::linear, 0), invalid_argument); } @@ -88,9 +81,9 @@ TEST(LinearInterpTest, Extrapolate) { vector times = {0, 1, 2, 3}; vector data = {0, 2, 1, 0}; - EXPECT_THROW(eal::linearInterpolate(data, times, -1.0), + EXPECT_THROW(eal::interpolate(data, times, -1.0, eal::linear, 0), invalid_argument); - EXPECT_THROW(eal::linearInterpolate(data, times, 4.0), + EXPECT_THROW(eal::interpolate(data, times, 4.0, eal::linear, 0), invalid_argument); } @@ -105,23 +98,23 @@ TEST(SplineInterpTest, ExampleInterpolation) { // The spline interpolation is only ~1e-10 so we have to define a tolerance double tolerance = 1e-10; - EXPECT_NEAR(0.0, eal::splineInterpolate(data, times, 0.0), tolerance); + EXPECT_NEAR(0.0, eal::interpolate(data, times, 0.0, eal::spline, 0), tolerance); EXPECT_NEAR(2.8 * 0.5 - 0.8 * 0.125, - eal::splineInterpolate(data, times, 0.5), tolerance); - EXPECT_NEAR(2.0, eal::splineInterpolate(data, times, 1.0), tolerance); + eal::interpolate(data, times, 0.5, eal::spline, 0), tolerance); + EXPECT_NEAR(2.0, eal::interpolate(data, times, 1.0, eal::spline, 0), tolerance); EXPECT_NEAR(3.375 - 5.4 * 2.25 + 8.2 * 1.5 - 1.8, - eal::splineInterpolate(data, times, 1.5), tolerance); - EXPECT_NEAR(1.0, eal::splineInterpolate(data, times, 2.0), tolerance); + eal::interpolate(data, times, 1.5, eal::spline, 0), tolerance); + EXPECT_NEAR(1.0, eal::interpolate(data, times, 2.0, eal::spline, 0), tolerance); EXPECT_NEAR(-0.2 * 15.625 + 1.8 * 6.25 - 6.2 * 2.5 + 7.8, - eal::splineInterpolate(data, times, 2.5), tolerance); - EXPECT_NEAR(0.0, eal::splineInterpolate(data, times, 3.0), tolerance); + eal::interpolate(data, times, 2.5, eal::spline, 0), tolerance); + EXPECT_NEAR(0.0, eal::interpolate(data, times, 3.0, eal::spline, 0), tolerance); } TEST(SplineInterpTest, NoPoints) { vector times = {}; vector data = {}; - EXPECT_THROW(eal::splineInterpolate(data, times, 0.0), + EXPECT_THROW(eal::interpolate(data, times, 0.0, eal::spline, 0), invalid_argument); } @@ -129,7 +122,7 @@ TEST(SplineInterpTest, DifferentCounts) { vector times = { -3, -2, -1, 0, 2}; vector data = { -3, -2, 1, 2}; - EXPECT_THROW(eal::splineInterpolate(data, times, 0.0), + EXPECT_THROW(eal::interpolate(data, times, 0.0, eal::spline, 0), invalid_argument); } @@ -137,8 +130,8 @@ TEST(SplineInterpTest, Extrapolate) { vector times = {0, 1, 2, 3}; vector data = {0, 2, 1, 0}; - EXPECT_THROW(eal::splineInterpolate(data, times, -1.0), + EXPECT_THROW(eal::interpolate(data, times, -1.0, eal::spline, 0), invalid_argument); - EXPECT_THROW(eal::splineInterpolate(data, times, 4.0), + EXPECT_THROW(eal::interpolate(data, times, 4.0, eal::spline, 0), invalid_argument); } -- GitLab