Skip to content
Snippets Groups Projects
Commit 7b57668c authored by Kelvin Rodriguez's avatar Kelvin Rodriguez Committed by Jesse Mapel
Browse files

implemented getVelocity (#45)

* implemented getVelocity

* removed debug print in cmake script

* updated as per comments
parent de07000c
No related branches found
No related tags found
No related merge requests found
......@@ -5,6 +5,8 @@
#include <string>
#include <vector>
#include <gsl/gsl_interp.h>
namespace eal {
enum interpolation {
......@@ -16,10 +18,11 @@ namespace eal {
std::vector<double> getPosition(std::vector<std::vector<double>> coords,
std::vector<double> times,
interpolation interp, double time);
double time, interpolation interp);
std::vector<double> getVelocity(std::vector<std::vector<double>> coords,
std::vector<double> times,
interpolation interp, double time);
double time, const interpolation interp);
std::vector<double> getPosition(std::vector<double> coeffs, double time);
std::vector<double> getVelocity(std::vector<double> coeffs, double time);
......@@ -27,19 +30,20 @@ namespace eal {
std::vector<double> getRotation(std::string from, std::string to,
std::vector<std::vector<double>> rotations,
std::vector<double> times,
interpolation interp, double time);
double time, interpolation interp);
std::vector<double> getAngularVelocity(std::string from, std::string to,
std::vector<std::vector<double>> rotations,
std::vector<double> times,
interpolation interp, double time);
double time, interpolation interp);
std::vector<double> getRotation(std::string from, std::string to,
std::vector<double> coefficients, double time);
std::vector<double> getAngularVelocity(std::string from, std::string to,
std::vector<double> coefficients, double time);
double linearInterpolate(std::vector<double> points, std::vector<double> times, double time);
double splineInterpolate(std::vector<double> points, std::vector<double> times, double time);
double interpolate(std::vector<double> points, std::vector<double> times, double time, interpolation interp, int d);
}
#endif // EAL_H
......@@ -30,8 +30,8 @@ namespace eal {
// Positional Functions
// Position Data Functions
vector<double> getPosition(vector<vector<double>> coords, vector<double> times,
interpolation interp, double time) {
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) {
......@@ -40,30 +40,29 @@ namespace eal {
// GSL setup
vector<double> 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<double> getVelocity(vector<vector<double>> coords, vector<double> 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<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;
}
......@@ -82,13 +81,13 @@ namespace eal {
// Rotation Data Functions
vector<double> getRotation(string from, string to, vector<vector<double>> rotations,
vector<double> times, interpolation interp, double time) {
vector<double> times, double time, interpolation interp) {
vector<double> coordinate = {0.0, 0.0, 0.0};
return coordinate;
}
vector<double> getAngularVelocity(string from, string to, vector<vector<double>> rotations,
vector<double> times, interpolation interp, double time) {
vector<double> times, double time, interpolation interp) {
vector<double> coordinate = {0.0, 0.0, 0.0};
return coordinate;
}
......@@ -106,53 +105,43 @@ namespace eal {
return coordinate;
}
// Interpolation helper functions
double linearInterpolate(vector<double> points, vector<double> times, double time) {
double interpolate(vector<double> points, vector<double> 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 interpolation data, must have the same number of points as times.");
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 interpolation time, outside of input times.");
throw invalid_argument("Invalid gsl_interp_type time, outside of input times.");
}
// GSL setup
gsl_interp *interpolator = gsl_interp_alloc(gsl_interp_linear, numPoints);
gsl_interp_init(interpolator, &times[0], &points[0], numPoints);
gsl_interp_accel *acc = gsl_interp_accel_alloc();
// 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 evaluate
double result = gsl_interp_eval(interpolator, &times[0], &points[0], time, acc);
// GSL clean up
gsl_interp_free(interpolator);
gsl_interp_accel_free(acc);
return result;
}
double splineInterpolate(vector<double> points, vector<double> 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 *interpolator = gsl_interp_alloc(interp_methods[interp], numPoints);
gsl_interp_init(interpolator, &times[0], &points[0], numPoints);
gsl_interp_accel *acc = gsl_interp_accel_alloc();
// GSL evaluate
double result = gsl_interp_eval(interpolator, &times[0], &points[0], time, acc);
double result;
switch(d) {
case 0:
result = gsl_interp_eval(interpolator, &times[0], &points[0], time, acc);
break;
case 1:
result = gsl_interp_eval_deriv(interpolator, &times[0], &points[0], time, acc);
break;
case 2:
result = gsl_interp_eval_deriv2(interpolator, &times[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);
......
......@@ -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)
......@@ -3,6 +3,8 @@
#include "eal.h"
#include <stdexcept>
#include <gsl/gsl_interp.h>
using namespace std;
......@@ -12,7 +14,7 @@ TEST(PositionInterpTest, LinearInterp) {
{ 9, 4, 1, 0, 1, 4},
{-27, -8, -1, 0, 1, 8}};
vector<double> coordinate = eal::getPosition(data, times, eal::linear, -1.5);
vector<double> coordinate = eal::getPosition(data, times, -1.5, eal::linear);
ASSERT_EQ(3, coordinate.size());
EXPECT_DOUBLE_EQ(-1.5, coordinate[0]);
......@@ -26,7 +28,7 @@ TEST(PositionInterpTest, SplineInterp) {
{0, 1, 2, 3},
{0, 2, 1, 0}};
vector<double> coordinate = eal::getPosition(data, times, eal::spline, 0.5);
vector<double> 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<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}};
EXPECT_THROW(eal::getPosition(data, times, (eal::interpolation)1000, 0.0),
invalid_argument);
}
TEST(LinearInterpTest, ExampleInterpolation) {
vector<double> times = {0, 1, 2, 3};
vector<double> 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<double> times = {};
vector<double> 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<double> times = { -3, -2, -1, 0, 2};
vector<double> 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<double> times = {0, 1, 2, 3};
vector<double> 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<double> times = {};
vector<double> 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<double> times = { -3, -2, -1, 0, 2};
vector<double> 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<double> times = {0, 1, 2, 3};
vector<double> 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);
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment