diff --git a/src/Matrix/CMakeLists.txt b/src/Matrix/CMakeLists.txt index cbf86f0..486f8f9 100644 --- a/src/Matrix/CMakeLists.txt +++ b/src/Matrix/CMakeLists.txt @@ -1,4 +1,4 @@ -# Vector 3D Interface +# Matrix Interface add_library(matrix-intf INTERFACE ) @@ -16,7 +16,7 @@ add_library(matrix target_link_libraries(matrix PUBLIC - vector-3d-intf + matrix-intf PRIVATE ) diff --git a/src/Matrix/Matrix.hpp b/src/Matrix/Matrix.hpp index 814cd66..2f7bb68 100644 --- a/src/Matrix/Matrix.hpp +++ b/src/Matrix/Matrix.hpp @@ -3,6 +3,7 @@ #include #include +#include // TODO: Add a function to calculate eigenvalues/vectors // TODO: Add a function to compute RREF @@ -186,6 +187,9 @@ public: Matrix operator*(float scalar) const; +protected: + std::array matrix; + private: /** * @brief take the dot product of the two vectors @@ -201,8 +205,6 @@ private: Matrix &adjugate(Matrix &result) const; void setMatrixToArray(const std::array &array); - - std::array matrix; }; #include "Matrix.cpp" diff --git a/src/Quaternion/CMakeLists.txt b/src/Quaternion/CMakeLists.txt index e69de29..79d815a 100644 --- a/src/Quaternion/CMakeLists.txt +++ b/src/Quaternion/CMakeLists.txt @@ -0,0 +1,31 @@ +# Quaternion Interface +add_library(quaternion-intf + INTERFACE +) + +target_include_directories(quaternion-intf + INTERFACE + . +) + +target_link_libraries(quaternion-intf + INTERFACE + matrix-intf +) + +# Quaternion +add_library(quaternion + STATIC + Quaternion.cpp +) + +target_link_libraries(quaternion + PUBLIC + quaternion-intf + PRIVATE +) + +set_target_properties(quaternion + PROPERTIES + LINKER_LANGUAGE CXX +) \ No newline at end of file diff --git a/src/Quaternion/Quaternion.cpp b/src/Quaternion/Quaternion.cpp new file mode 100644 index 0000000..19607ec --- /dev/null +++ b/src/Quaternion/Quaternion.cpp @@ -0,0 +1,66 @@ +#include "Quaternion.h" +#include + +float Quaternion::operator[](uint8_t index) +{ + if (index < 4) + { + return this->matrix[index]; + } + + // index out of bounds + return 1e+6; +} + +Quaternion Quaternion::operator+(const Quaternion &other) +{ + return Quaternion{this->v1 * other.v1, this->v2 * other.v2, this->v3 * other.v3, this->w * other.w}; +} + +Quaternion & +Quaternion::Rotate(Quaternion &other, Quaternion &buffer) +{ + // eq. 6 + buffer.v1 = this->w * other.v1 + other.w * this->v1 - this->v2 * other.v3 + this->v3 * other.v2; + buffer.v2 = this->w * other.v2 + other.w * this->v2 - this->v3 * other.v1 + this->v1 * other.v3; + buffer.v3 = this->w * other.v3 + other.w * this->v3 - this->v1 * other.v2 + this->v2 * other.v1; + buffer.w = this->w * other.w - this->v1 * other.v1 - this->v2 * other.v2 - this->v3 * other.v3; + return buffer; +} + +Matrix<1, 3> & +Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) +{ + // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + // rotation sequence R = Rx * Ry * Rz + // roll (x-axis rotation) + float sinr_cosp = 2 * (this->v2 * this->v3 - this->w * this->v1); + float cosr_cosp = 1 - 2 * (this->v1 * this->v1 + this->v2 * this->v2); + angleBuffer[0][0] = atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + float sinp = -2 * (this->w * this->v2 + this->v3 * this->v1); + if (abs(sinp) >= 1) + angleBuffer[0][1] = copysign(M_PI / 2, sinp); // use 90 degrees if out of range + else + angleBuffer[0][1] = asin(sinp); + + // yaw (z-axis rotation) + float siny_cosp = 2 * (this->v1 * this->v2 - this->w * this->v3); + float cosy_cosp = 1 - 2 * (this->v2 * this->v2 + this->v3 * this->v3); + angleBuffer[0][2] = atan2(siny_cosp, cosy_cosp); + + return angleBuffer; +} + +Matrix<3, 3> &Quaternion::ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) +{ + // eq. 4 + // from https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm + Matrix<3, 3> temp{1.0 - 2.0 * this->v2 * this->v2 - 2.0 * this->v3 * this->v3, 2.0 * this->v1 * this->v2 - 2.0 * this->v3 * this->w, 2.0 * this->v1 * this->v3 + 2.0 * this->v2 * this->w, + 2.0 * this->v1 * this->v2 + 2.0 * this->v3 * this->w, 1.0 - 2.0 * this->v1 * this->v1 - 2.0 * this->v3 * this->v3, 2.0 * this->v2 * this->v3 - 2.0 * this->v1 * this->w, + 2.0 * this->v1 * this->v3 - 2.0 * this->v2 * this->w, 2.0 * this->v2 * this->v3 + 2.0 * this->v1 * this->w, 1.0 - 2.0 * this->v1 * this->v1 - 2.0 * this->v2 * this->v2}; + + temp.Transpose(rotationMatrixBuffer); + return rotationMatrixBuffer; +} \ No newline at end of file diff --git a/src/Quaternion/Quaternion.h b/src/Quaternion/Quaternion.h new file mode 100644 index 0000000..a1641a5 --- /dev/null +++ b/src/Quaternion/Quaternion.h @@ -0,0 +1,31 @@ +#ifndef QUATERNION_H_ +#define QUATERNION_H_ + +#include "Matrix.hpp" + +class Quaternion : public Matrix<1, 4> +{ +public: + Quaternion() : Matrix<1, 4>() {} + Quaternion(float fillValue) : Matrix<1, 4>(fillValue) {} + Quaternion(float v1, float v2, float v3, float w) : Matrix<1, 4>(v1, v2, v3, w) {} + Quaternion(const Quaternion &q) : Matrix<1, 4>(q.v1, q.v2, q.v3, q.w) {} + Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {} + Quaternion(const std::array &array) : Matrix<1, 4>(array) {} + + float operator[](uint8_t index); + Quaternion operator+(const Quaternion &other); + Quaternion &Rotate(Quaternion &other, Quaternion &buffer); + Matrix<1, 3> &ToEulerAngles(Matrix<1, 3> &angleBuffer); + Matrix<3, 3> &ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer); + + // Give people an easy way to access the elements + float &v1{matrix[0]}; + float &v2{matrix[1]}; + float &v3{matrix[2]}; + float &w{matrix[3]}; + +private: +}; + +#endif // QUATERNION_H_ \ No newline at end of file diff --git a/unit-tests/CMakeLists.txt b/unit-tests/CMakeLists.txt index 5cb60ea..ab34785 100644 --- a/unit-tests/CMakeLists.txt +++ b/unit-tests/CMakeLists.txt @@ -29,4 +29,13 @@ target_link_libraries(vector-tests matrix-intf vector-3d-intf Catch2::Catch2WithMain +) + +# quaternion tests +add_executable(quaternion-tests quaternion-tests.cpp) + +target_link_libraries(quaternion-tests + PRIVATE + quaternion-intf + Catch2::Catch2WithMain ) \ No newline at end of file diff --git a/unit-tests/quaternion-tests.cpp b/unit-tests/quaternion-tests.cpp new file mode 100644 index 0000000..6737548 --- /dev/null +++ b/unit-tests/quaternion-tests.cpp @@ -0,0 +1,55 @@ +// include the unit test framework first +#include +#include + +// include the module you're going to test next +#include "Quaternion.h" + +// any other libraries +#include +#include +#include + +TEST_CASE("Vector Math", "Vector") +{ + Quaternion q1{1, 2, 3, 4}; + Quaternion q2{5, 6, 7, 8}; + + SECTION("Initialization") + { + // explicit initialization + REQUIRE(q1.v1 == 1); + REQUIRE(q1.v2 == 2); + REQUIRE(q1.v3 == 3); + REQUIRE(q1.w == 4); + + // fill initialization + Quaternion q3{0}; + REQUIRE(q3.v1 == 0); + REQUIRE(q3.v2 == 0); + REQUIRE(q3.v3 == 0); + REQUIRE(q3.w == 0); + + // copy initialization + Quaternion q4{q1}; + REQUIRE(q4.v1 == 1); + REQUIRE(q4.v2 == 2); + REQUIRE(q4.v3 == 3); + REQUIRE(q4.w == 4); + + // matrix initialization + Matrix<1, 4> m1{1, 2, 3, 4}; + Quaternion q5{m1}; + REQUIRE(q5.v1 == 1); + REQUIRE(q5.v2 == 2); + REQUIRE(q5.v3 == 3); + REQUIRE(q5.w == 4); + + // array initialization + Quaternion q6{std::array{1, 2, 3, 4}}; + REQUIRE(q6.v1 == 1); + REQUIRE(q6.v2 == 2); + REQUIRE(q6.v3 == 3); + REQUIRE(q6.w == 4); + } +} \ No newline at end of file