From be0d5f9b714664b4f74f3197fe14f622b2716cdc Mon Sep 17 00:00:00 2001 From: Kyle Isom Date: Tue, 6 Aug 2019 00:46:02 -0700 Subject: [PATCH] Add SLERP test and euler2quat. --- CMakeLists.txt | 11 ++++-- docs/CMakeLists.txt | 2 +- include/wrmath/geom/quaternion.h | 54 +++++++++++++++++++++++++-- src/quaternion.cc | 20 ---------- test/quaternion_test.cc | 13 +++++++ tools/euler2quat.cc | 64 ++++++++++++++++++++++++++++++++ 6 files changed, 137 insertions(+), 27 deletions(-) create mode 100644 tools/euler2quat.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index b1b3685..7f1076b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,11 +29,16 @@ include_directories(include) file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**.h) file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cc) -message("${PROJECT_NAME}_SOURCES -> libwrmath") +message("${${PROJECT_NAME}_SOURCES} -> libwrmath") ## BUILD -add_library(lib${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES}) +add_executable(euler2quat tools/euler2quat.cc) +target_link_libraries(euler2quat ${PROJECT_NAME}) +set_target_properties(${TESTNAME} PROPERTIES + FOLDER bin + RUNTIME_OUTPUT_DIRECTORY bin) ## INSTALL @@ -62,7 +67,7 @@ include(CTest) set(TEST_EXECS) macro(package_add_gtest TESTNAME) add_executable(${TESTNAME} ${ARGN}) - target_link_libraries(${TESTNAME} gtest_main lib${PROJECT_NAME}) + target_link_libraries(${TESTNAME} gtest_main ${PROJECT_NAME}) target_compile_options(${TESTNAME} PUBLIC ${GTEST_CFLAGS}) add_test(NAME ${TESTNAME} COMMAND ${TESTNAME}) set_target_properties(${TESTNAME} PROPERTIES diff --git a/docs/CMakeLists.txt b/docs/CMakeLists.txt index bba8fe9..8e06e53 100644 --- a/docs/CMakeLists.txt +++ b/docs/CMakeLists.txt @@ -1,7 +1,7 @@ find_package(Doxygen REQUIRED) # Find all the public headers -get_target_property(WRMATH_PUBLIC_HEADER_DIR libwrmath INTERFACE_INCLUDE_DIRECTORIES) +get_target_property(WRMATH_PUBLIC_HEADER_DIR wrmath INTERFACE_INCLUDE_DIRECTORIES) file(GLOB_RECURSE WRMATH_PUBLIC_HEADERS ${WRMATH_PUBLIC_HEADER_DIR}/*.h) #This will be the main output of our command diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h index 044afa7..223634a 100644 --- a/include/wrmath/geom/quaternion.h +++ b/include/wrmath/geom/quaternion.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -123,8 +124,25 @@ public: } + /// Compute the dot product of two quaternions. + /// + /// \param other Another quaternion. + /// \return The dot product between the two quaternions. + T + dot(const Quaternion &other) const + { + double innerProduct = this->v[0] * other.v[0]; + + innerProduct += (this->v[1] * other.v[1]); + innerProduct += (this->v[2] * other.v[2]); + innerProduct += (this->w * other.w); + return innerProduct; + } + + /// Compute the norm of a quaternion. Treating the Quaternion as a /// Vector, it's the same as computing the magnitude. + /// /// @return A non-negative real number. T norm() const @@ -140,6 +158,15 @@ public: } + /// Return the unit quaternion. + /// + /// \return The unit quaternion. + Quaternion + unitQuaternion() + { + return *this / this->norm(); + } + /// Compute the conjugate of a quaternion. /// @return The conjugate of this quaternion. Quaternion @@ -393,7 +420,11 @@ Quaterniond quaterniond_from_euler(Vector3d euler); /// \return A Quaternion representing the linear interpolation of the /// two quaternions. template -Quaternion LERP(Quaternion p, Quaternion q, T t); +Quaternion +LERP(Quaternion p, Quaternion q, T t) +{ + return (p + (q - p) * t).unitQuaternion(); +} /// ShortestSLERP computes the shortest distance spherical linear @@ -402,13 +433,30 @@ Quaternion LERP(Quaternion p, Quaternion q, T t); /// /// \tparam T /// \param p The starting quaternion. -/// \param q The ending quaternion. +/// \param q The ending quaternion.Short /// \param t The fraction of the distance between the two quaternions /// to interpolate. /// \return A Quaternion representing the shortest path between two /// quaternions. template -Quaternion ShortestSLERP(Quaternion p, Quaternion q, T t); +Quaternion +ShortestSLERP(Quaternion p, Quaternion q, T t) +{ + assert(p.isUnitQuaternion()); + assert(q.isUnitQuaternion()); + + T dp = p.dot(q); + T sign = dp < 0.0 ? -1.0 : 1.0; + T omega = std::acos(dp * sign); + T sin_omega = std::sin(omega); // Compute once. + + if (dp > 0.99999) { + return LERP(p, q * sign, t); + } + + return (p * std::sin((1.0 - t) * omega) / sin_omega) + + (q * sign * std::sin(omega*t) / sin_omega); +} /// Run a quick self test to exercise basic functionality of the Quaternion diff --git a/src/quaternion.cc b/src/quaternion.cc index 5a568cb..3cfa4ae 100644 --- a/src/quaternion.cc +++ b/src/quaternion.cc @@ -65,26 +65,6 @@ quaterniond_from_euler(Vector3d euler) } -template -Quaternion -LERP(Quaternion p, Quaternion q, T t) -{ - return p + (q - p) * t; -} - - -template -Quaternion -ShortestSLERP(Quaternion p, Quaternion q, T t) -{ - T innerProduct = p.dot(q); - T sign = innerProduct >= 0.0 ? -1.0 : 1.0; - T acip = std::acos(innerProduct); - - return (p * std::sin((T)1.0 - t) * acip + p * sign * std::sin(t * acip)) / std::sin(acip); -} - - void Quaternion_SelfTest() { diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc index d935eb9..25d50e0 100644 --- a/test/quaternion_test.cc +++ b/test/quaternion_test.cc @@ -107,6 +107,19 @@ TEST(Quaterniond, Rotate) } +TEST(Quaterniond, ShortestSLERP) +{ + geom::Quaterniond p = geom::Quaterniond {0.382683, 0, 0, 0.92388}; + geom::Quaterniond q = geom::Quaterniond {-0.382683, 0, 0, 0.92388}; + geom::Quaterniond r = geom::Quaterniond {0, 0, 0, 1}; + + + EXPECT_EQ(geom::ShortestSLERP(p, q, 0.0), p); + EXPECT_EQ(geom::ShortestSLERP(p, q, 1.0), q); + EXPECT_EQ(geom::ShortestSLERP(p, q, 0.5), r); +} + + TEST(Quaterniond, Unit) { geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0}); diff --git a/tools/euler2quat.cc b/tools/euler2quat.cc new file mode 100644 index 0000000..210be94 --- /dev/null +++ b/tools/euler2quat.cc @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include + +using namespace std; +using namespace wr; + + +static void +usage(ostream& outs) +{ + outs << "Print conversions between Euler angles and quaternions." << endl; + outs << "Usage: euler2quat yaw pitch roll" << endl; + outs << " euler2quat x y z w" << endl; +} + + +static void +convertEulerToQuat(char **argv) +{ + double yaw = math::DegreesToRadiansD(stod(string(argv[0]))); + double pitch = math::DegreesToRadiansD(stod(string(argv[1]))); + double roll = math::DegreesToRadiansD(stod(string(argv[2]))); + + geom::Vector3d euler {yaw, pitch, roll}; + auto quaternion = geom::quaterniond_from_euler(euler); + + cout << "Quaternion: " << quaternion.asVector() << endl; +} + + +static void +convertQuatToEuler(char **argv) +{ + double x = stod(string(argv[0])); + double y = stod(string(argv[1])); + double z = stod(string(argv[1])); + double w = stod(string(argv[1])); + + geom::Quaterniond quaternion {x, y, z, w}; + auto euler = quaternion.euler() * (180.0 / M_PI); + + cout << "Euler ZYX: " << euler << endl; +} + + +int +main(int argc, char **argv) +{ + if ((argc != 4) && (argc != 5)) { + usage(cerr); + return EXIT_FAILURE; + } + + argv++; + if (argc == 4) { + convertEulerToQuat(argv); + } + else { + convertQuatToEuler(argv); + } +} \ No newline at end of file