diff --git a/CMakeLists.txt b/CMakeLists.txt index 507499d69461de997adfb22b811005a842d1a09c..7322103b8ff2538a08455e698c6978986c78ad1f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,8 @@ include(CTest) set(CMAKE_CXX_STANDARD 11) # Third Party Dependencies -find_package(GSL) +find_package(GSL REQUIRED) +find_package(Eigen3 3.3 REQUIRED NO_MODULE) # Library setup add_library(eal SHARED @@ -28,9 +29,11 @@ set_target_properties(eal PROPERTIES SOVERSION 1) set(EAL_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include/" "${CMAKE_CURRENT_SOURCE_DIR}/include/json") + target_include_directories(eal PRIVATE ${GSL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} PUBLIC ${EAL_INCLUDE_DIRS}) diff --git a/environment.yml b/environment.yml index 430f7de3dd76d5c96bf150cb48746f7858946885..a318f4895d9b2e7ed16c03e699da6e863a5dd88b 100644 --- a/environment.yml +++ b/environment.yml @@ -8,3 +8,4 @@ dependencies: - cmake>=3.10 - gsl - openblas>=0.3.0 + - eigen diff --git a/include/eal.h b/include/eal.h index 4d68e22144e14792ebbd353585a4d4f338a6afd1..423be9dcfa105290411a36976fe0b600047debfd 100644 --- a/include/eal.h +++ b/include/eal.h @@ -27,12 +27,10 @@ namespace eal { std::vector getPosition(std::vector> coeffs, double time); std::vector getVelocity(std::vector> coeffs, double time); - std::vector getRotation(std::string from, std::string to, - std::vector> rotations, + std::vector getRotation(std::vector> rotations, std::vector times, double time, interpolation interp); - std::vector getAngularVelocity(std::string from, std::string to, - std::vector> rotations, + std::vector getAngularVelocity(std::vector> rotations, std::vector times, double time, interpolation interp); diff --git a/src/eal.cpp b/src/eal.cpp index 952c6089f8c6f633c0e3080eda4d2d18b40f14e6..7d5f34cfa95963ccb871978b541fdd6887639657 100644 --- a/src/eal.cpp +++ b/src/eal.cpp @@ -6,7 +6,11 @@ #include #include +#include +#include + #include +#include #include using json = nlohmann::json; @@ -80,8 +84,8 @@ namespace eal { throw invalid_argument("Invalid input coeffs, expected three vectors."); } - vector coordinate = {0.0, 0.0, 0.0}; - coordinate[0] = evaluatePolynomial(coeffs[0], time); // X + vector coordinate = {0.0, 0.0, 0.0}; + coordinate[0] = evaluatePolynomial(coeffs[0], time); // X coordinate[1] = evaluatePolynomial(coeffs[1], time); // Y coordinate[2] = evaluatePolynomial(coeffs[2], time); // Z @@ -89,26 +93,81 @@ namespace eal { } - // Velocity Function + // Velocity Function // Takes the coefficients from the position equation vector getVelocity(vector> coeffs, double time) { vector coordinate = {0.0, 0.0, 0.0}; return coordinate; } - // Rotation Data Functions // Rotation Data Functions - vector getRotation(string from, string to, vector> rotations, + vector getRotation(vector> rotations, vector times, double time, interpolation interp) { - vector coordinate = {0.0, 0.0, 0.0}; + // 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 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 quat(coordinate.data(), 4, 1); + quat.normalize(); return coordinate; } - vector getAngularVelocity(string from, string to, vector> rotations, + vector getAngularVelocity(vector> rotations, vector times, double time, interpolation interp) { - vector coordinate = {0.0, 0.0, 0.0}; - return coordinate; + // 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 coordinate = {0.0, 0.0, 0.0, 0.0}; + + coordinate = { 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::Map to ensure the array isn't copied, only the pointer is + Eigen::Map quat(coordinate.data(), 4, 1); + quat.normalize(); + return coordinate; } // Rotation Function Functions @@ -132,7 +191,7 @@ namespace eal { throw invalid_argument("Invalid input coeffs, must be non-empty."); } - const double *coeffsArray = coeffs.data(); + const double *coeffsArray = coeffs.data(); return gsl_poly_eval(coeffsArray, coeffs.size(), time); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 1059ba0b923a330aa5a0df5f44bac110a314ae6a..9271b2cb532ca19cc93e4295c1833e3689ef227c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -13,4 +13,5 @@ target_include_directories(runEALTests 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 598b109539f5a77352feb194e35e82094f2fedef..37be2dfe55e79928a61872f3394b38ef4ee53102 100644 --- a/tests/EalTest.cpp +++ b/tests/EalTest.cpp @@ -137,26 +137,26 @@ TEST(SplineInterpTest, Extrapolate) { } TEST(PoisitionCoeffTest, SecondOrderPolynomial) { - double time = 2.0; + double time = 2.0; vector> coeffs = {{1.0, 2.0, 3.0}, {1.0, 3.0, 2.0}, {3.0, 2.0, 1.0}}; - vector coordinate = eal::getPosition(coeffs, time); + vector coordinate = eal::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(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; + double time = 2.0; vector> coeffs = {{1.0}, {1.0, 2.0}, {1.0, 2.0, 3.0}}; - vector coordinate = eal::getPosition(coeffs, time); + vector coordinate = eal::getPosition(coeffs, time); ASSERT_EQ(3, coordinate.size()); EXPECT_DOUBLE_EQ(1.0, coordinate[0]); @@ -165,12 +165,12 @@ TEST(PoisitionCoeffTest, DifferentPolynomialDegrees) { } TEST(PoisitionCoeffTest, NegativeInputs) { - double time = -2.0; + double time = -2.0; vector> coeffs = {{-1.0, -2.0, -3.0}, {1.0, -2.0, 3.0}, {-1.0, 2.0, -3.0}}; - - vector coordinate = eal::getPosition(coeffs, time); + + vector coordinate = eal::getPosition(coeffs, time); ASSERT_EQ(3, coordinate.size()); EXPECT_DOUBLE_EQ(-9.0, coordinate[0]); @@ -186,3 +186,25 @@ TEST(PoisitionCoeffTest, InvalidInput) { EXPECT_THROW(eal::getPosition(invalid_coeffs_sizes, valid_time), invalid_argument); } + + +TEST(LinearInterpTest, ExmapleGetRotation) { + // simple test, only checks if API hit correctly and output is normalized + vector times = {0, 1, 2, 3}; + vector> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}}); + vector r = eal::getRotation(rots, times, 2, eal::linear); + + ASSERT_NEAR(0.707107, r[0], 0.000001); + EXPECT_DOUBLE_EQ(0, r[1]); + ASSERT_NEAR(0.707107, r[2], 0.000001); + EXPECT_DOUBLE_EQ(0, r[3]); +} + + +TEST(LinearInterpTest, GetRotationDifferentCounts) { + // incorrect params + vector times = {0, 1, 2}; + vector> rots({{1,1,1,1}, {0,0,0,0}, {1,1,1,1}, {0,0,0,0}}); + EXPECT_THROW(eal::getRotation(rots, times, 2, eal::linear), invalid_argument); + +}