From 4cf469308802dec21aeabb31d14d0c4b0a827ac2 Mon Sep 17 00:00:00 2001 From: Kyle Isom Date: Mon, 5 Aug 2019 00:12:03 -0700 Subject: [PATCH] Start quaternions. --- CMakeLists.txt | 5 +- include/wrmath/geom/quaternion.h | 89 ++++++++++++++++++++++++++++++++ include/wrmath/geom/vector.h | 38 ++++++++++---- test/quaternion_test.cc | 23 +++++++++ 4 files changed, 143 insertions(+), 12 deletions(-) create mode 100644 include/wrmath/geom/quaternion.h create mode 100644 test/quaternion_test.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 97be40b..be28108 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,8 +66,9 @@ macro(package_add_gtest TESTNAME) endmacro() # define the tests -package_add_gtest(vector_test test/vector_test.cc) -package_add_gtest(orientation_test test/orientation_test.cc) +package_add_gtest(vector_test test/vector_test.cc) +package_add_gtest(orientation_test test/orientation_test.cc) +package_add_gtest(quaternion_test test/quaternion_test.cc) add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --verbose DEPENDS ${TEST_EXECS}) diff --git a/include/wrmath/geom/quaternion.h b/include/wrmath/geom/quaternion.h new file mode 100644 index 0000000..33ca3f9 --- /dev/null +++ b/include/wrmath/geom/quaternion.h @@ -0,0 +1,89 @@ +#ifndef __WRMATH_QUATERNION_H +#define __WRMATH_QUATERNION_H + + +#include +#include +#include +#include +#include + + +namespace wr { +namespace geom { + + +template +class Quaternion { +public: + // The default constructor returns the identity quaternion. + Quaternion() : v(Vector()), w(1.0) + { + wr::math::DefaultEpsilon(this->eps); + }; + + + Quaternion(Vector axis, T angle) : v(axis), w(angle) + { + wr::math::DefaultEpsilon(this->eps); + }; + + + Quaternion + operator+(const Quaternion &other) const + { + return Quaternion(this->v + other.v, (this->w + other.w) % this->maxRotation); + } + + + Quaternion + operator-(const Quaternion &other) const + { + return Quaternion(this->v - other.v, (this->w - other.w) % this->maxRotation); + } + + + Quaternion + operator*(const Quaternion &other) const + { + T angle = (this->w * other.w) - + (this->v * other.v); + Vector axis = (other.v * this->w) + + (this->v * other.w) + + (this->v.cross(other.v)); + return Quaternion(axis, angle); + } + + + bool + operator==(const Quaternion &other) const + { + return (this->v == other.v) && + (wr::math::WithinTolerance(this->w, other.w, this->eps)); + } + + + bool + operator!=(const Quaternion &other) const + { + return !(*this == other); + } + +private: + static constexpr T maxRotation = 4 * M_PI; + + Vector v; // axis of rotation + T w; // angle of rotation + T eps; +}; + + +typedef Quaternion Quaternionf; +typedef Quaternion Quaterniond; + + +} // namespace geom +} // namespace wr + + +#endif // WRMATH_QUATERNION_H diff --git a/include/wrmath/geom/vector.h b/include/wrmath/geom/vector.h index 89e39fb..5169513 100644 --- a/include/wrmath/geom/vector.h +++ b/include/wrmath/geom/vector.h @@ -211,7 +211,7 @@ public: * @return The cross product vector. */ Vector - cross(const Vector &other) + cross(const Vector &other) const { assert(N == 3); return Vector { @@ -228,7 +228,9 @@ public: * @return A new vector that is the result of adding this and the * other vector. */ - Vector operator+(const Vector &other) const { + Vector + operator+(const Vector &other) const + { Vector vec; for (size_t i = 0; i < N; i++) { @@ -245,7 +247,9 @@ public: * @return A new vector that is the result of subtracting the * other vector from this one. */ - Vector operator-(const Vector &other) const { + Vector + operator-(const Vector &other) const + { Vector vec; for (size_t i = 0; i < N; i++) { @@ -261,7 +265,9 @@ public: * @param k The scaling value. * @return A new vector that is this vector scaled by k. */ - Vector operator*(const T k) const { + Vector + operator*(const T k) const + { Vector vec; for (size_t i = 0; i < N; i++) { @@ -277,7 +283,9 @@ public: * @param k The scaling value * @return A new vector that is this vector scaled by 1/k. */ - Vector operator/(const T k) const { + Vector + operator/(const T k) const + { Vector vec; for (size_t i = 0; i < N; i++) { @@ -293,7 +301,9 @@ public: * @param other The other vector. * @return A scalar value that is the dot product of the two vectors. */ - T operator*(const Vector &other) const { + T + operator*(const Vector &other) const + { T result = 0; for (size_t i = 0; i < N; i++) { @@ -310,7 +320,9 @@ public: * @return Return true if all the components of both vectors are * within the tolerance value. */ - bool operator==(const Vector &other) const { + bool + operator==(const Vector &other) const + { for (size_t i = 0; iarr[i], other.arr[i], this->epsilon)) { return false; @@ -326,7 +338,9 @@ public: * @return Return true if any of the components of both vectors are * not within the tolerance value. */ - bool operator!=(const Vector &other) const { + bool + operator!=(const Vector &other) const + { return !(*this == other); } @@ -336,7 +350,9 @@ public: * @param i The component index. * @return The value of the vector component at i. */ - T operator[](size_t i) const { + T + operator[](size_t i) const + { return this->arr[i]; } @@ -347,7 +363,9 @@ public: * @param vec The vector to be formatted. * @return The output stream. */ - friend std::ostream& operator<<(std::ostream& outs, const Vector& vec) { + friend std::ostream& + operator<<(std::ostream& outs, const Vector& vec) + { outs << "<"; for (size_t i = 0; i < N; i++) { outs << vec.arr[i]; diff --git a/test/quaternion_test.cc b/test/quaternion_test.cc new file mode 100644 index 0000000..9b0ee18 --- /dev/null +++ b/test/quaternion_test.cc @@ -0,0 +1,23 @@ +#include +#include + +using namespace std; +using namespace wr; + + +TEST(Quaterniond, Addition) +{ + geom::Quaterniond p(geom::Vector3d {1.0, -2.0, 1.0}, 3.0); + geom::Quaterniond q(geom::Vector3d {-1.0, 2.0, 3.0}, 2.0); + geom::Quaterniond expected(geom::Vector3d{-9.0, -2.0, 11.0}, 8.0); + + EXPECT_EQ(p * q, expected); +} + + +int +main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}