From 55ff4aa6930dc8c5a4e224e03512b24a2bc4ed41 Mon Sep 17 00:00:00 2001 From: Quinn Date: Fri, 7 Feb 2025 23:40:56 -0500 Subject: [PATCH] Finished implimenting quaternion basics --- .vscode/launch.json | 29 +++--- .vscode/tasks.json | 6 +- src/Quaternion/Quaternion.cpp | 67 +++++++------- src/Quaternion/Quaternion.h | 34 ++++--- .../unit-tests/quaternion-tests.cpp | 89 ++++++++----------- 5 files changed, 107 insertions(+), 118 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index d860e76..bf41fff 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -8,14 +8,14 @@ "name": "Debug Matrix Unit Tests", "type": "cppdbg", "request": "launch", - "program": "${workspaceFolder}/build/unit-tests/matrix-tests", - "args": [], + "program": "${workspaceFolder}/build/src/Matrix/matrix-tests", + "args": [], "stopAtEntry": false, "cwd": "${workspaceFolder}", "environment": [], "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path + "MIMode": "gdb", + "miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path "setupCommands": [ { "description": "Enable pretty-printing for gdb", @@ -23,20 +23,29 @@ "ignoreFailures": true } ], - "preLaunchTask": "build_tests", // Task to compile unit tests + "preLaunchTask": "build_tests", // Task to compile unit tests "internalConsoleOptions": "openOnSessionStart" }, { - "name": "Run Matrix Unit Tests", - "type": "cpp", + "name": "Debug Quaternion Unit Tests", + "type": "cppdbg", "request": "launch", - "program": "${workspaceFolder}/build/unit-tests/matrix-tests", - "args": [], + "program": "${workspaceFolder}/build/src/Quaternion/quaternion-tests", + "args": [], "stopAtEntry": false, "cwd": "${workspaceFolder}", "environment": [], "externalConsole": false, - "preLaunchTask": "build_tests", // Compile unit tests before running + "MIMode": "gdb", + "miDebuggerPath": "/usr/bin/gdb", // Adjust to your debugger path + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ], + "preLaunchTask": "build_tests", // Task to compile unit tests "internalConsoleOptions": "openOnSessionStart" } ] diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 45f53aa..793d575 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,12 +4,14 @@ { "label": "build_tests", "type": "shell", - "command": "cd build && ninja matrix-tests", + "command": "cd build && ninja", "group": { "kind": "build", "isDefault": true }, - "problemMatcher": ["$gcc"], + "problemMatcher": [ + "$gcc" + ], "detail": "Generated task to build unit test executable" } ] diff --git a/src/Quaternion/Quaternion.cpp b/src/Quaternion/Quaternion.cpp index 3d15337..6915ed9 100644 --- a/src/Quaternion/Quaternion.cpp +++ b/src/Quaternion/Quaternion.cpp @@ -1,6 +1,24 @@ #include "Quaternion.h" #include +/** + * @brief Create a quaternion from an angle and axis + * @param angle The angle to rotate by + * @param axis The axis to rotate around + */ +Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis) +{ + const float halfAngle = angle / 2; + const float sinHalfAngle = sin(halfAngle); + Matrix<1, 3> normalizedAxis{}; + axis.Normalize(normalizedAxis); + return Quaternion{ + static_cast(cos(halfAngle)), + normalizedAxis.Get(0, 0) * sinHalfAngle, + normalizedAxis.Get(0, 1) * sinHalfAngle, + normalizedAxis.Get(0, 2) * sinHalfAngle}; +} + float Quaternion::operator[](uint8_t index) const { if (index < 4) @@ -14,7 +32,7 @@ float Quaternion::operator[](uint8_t index) const Quaternion Quaternion::operator+(const Quaternion &other) const { - return Quaternion{this->v1 * other.v1, this->v2 * other.v2, this->v3 * other.v3, this->w * other.w}; + return Quaternion{this->w * other.w, this->v1 * other.v1, this->v2 * other.v2, this->v3 * other.v3}; } Quaternion & @@ -31,48 +49,23 @@ Quaternion::Q_Mult(Quaternion &other, Quaternion &buffer) const Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const { - Quaternion prime{-this->v1, -this->v2, -this->v3, this->w}; - static_cast>(buffer) = static_cast>(other); + Quaternion prime{this->w, -this->v1, -this->v2, -this->v3}; + buffer.v1 = other.v1; + buffer.v2 = other.v2; + buffer.v3 = other.v3; buffer.w = 0; + Quaternion temp{}; this->Q_Mult(buffer, temp); temp.Q_Mult(prime, buffer); return buffer; } -Matrix<1, 3> & -Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) const +void Quaternion::Normalize() { - // 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) const -{ - // 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; + float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w); + this->v1 /= magnitude; + this->v2 /= magnitude; + this->v3 /= magnitude; + this->w /= magnitude; } \ No newline at end of file diff --git a/src/Quaternion/Quaternion.h b/src/Quaternion/Quaternion.h index 2a3ce5c..e3c49c8 100644 --- a/src/Quaternion/Quaternion.h +++ b/src/Quaternion/Quaternion.h @@ -8,11 +8,18 @@ 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(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, v3) {} + Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {} Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {} Quaternion(const std::array &array) : Matrix<1, 4>(array) {} + /** + * @brief Create a quaternion from an angle and axis + * @param angle The angle to rotate by + * @param axis The axis to rotate around + */ + static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis); + /** * @brief Access the elements of the quaternion * @param index The index of the element to access @@ -36,7 +43,7 @@ public: Quaternion &Q_Mult(Quaternion &other, Quaternion &buffer) const; /** - * @brief Rotate a quaternion by another quaternion + * @brief Rotate a quaternion by this quaternion * @param other The quaternion to rotate * @param buffer The buffer to store the result in * @@ -44,24 +51,15 @@ public: Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const; /** - * @brief Calculate the Euler angles from the quaternion - * @param angleBuffer The buffer to store the angles in - * @return A reference to the buffer + * @brief Normalize the quaternion to a magnitude of 1 */ - Matrix<1, 3> &ToEulerAngles(Matrix<1, 3> &angleBuffer) const; - - /** - * @brief Convert the quaternion to a rotation matrix - * @param rotationMatrixBuffer The buffer to store the rotation matrix in - * @return A reference to the buffer - */ - Matrix<3, 3> &ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) const; + void Normalize(); // 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]}; + float &w{matrix[0]}; + float &v1{matrix[1]}; + float &v2{matrix[2]}; + float &v3{matrix[3]}; }; #endif // QUATERNION_H_ \ No newline at end of file diff --git a/src/Quaternion/unit-tests/quaternion-tests.cpp b/src/Quaternion/unit-tests/quaternion-tests.cpp index 4c90752..2551754 100644 --- a/src/Quaternion/unit-tests/quaternion-tests.cpp +++ b/src/Quaternion/unit-tests/quaternion-tests.cpp @@ -18,39 +18,39 @@ TEST_CASE("Vector Math", "Vector") SECTION("Initialization") { // explicit initialization - REQUIRE(q1.v1 == 1); - REQUIRE(q1.v2 == 2); - REQUIRE(q1.v3 == 3); - REQUIRE(q1.w == 4); + REQUIRE(q1.w == 1); + REQUIRE(q1.v1 == 2); + REQUIRE(q1.v2 == 3); + REQUIRE(q1.v3 == 4); // fill initialization Quaternion q3{0}; + REQUIRE(q3.w == 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); + REQUIRE(q4.w == 1); + REQUIRE(q4.v1 == 2); + REQUIRE(q4.v2 == 3); + REQUIRE(q4.v3 == 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); + REQUIRE(q5.w == 1); + REQUIRE(q5.v1 == 2); + REQUIRE(q5.v2 == 3); + REQUIRE(q5.v3 == 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); + REQUIRE(q6.w == 1); + REQUIRE(q6.v1 == 2); + REQUIRE(q6.v2 == 3); + REQUIRE(q6.v3 == 4); } SECTION("Array access") @@ -64,43 +64,30 @@ TEST_CASE("Vector Math", "Vector") SECTION("Addition") { Quaternion q3 = q1 + q2; - REQUIRE(q3.v1 == 5); - REQUIRE(q3.v2 == 12); - REQUIRE(q3.v3 == 21); - REQUIRE(q3.w == 32); + REQUIRE(q3.w == 5); + REQUIRE(q3.v1 == 12); + REQUIRE(q3.v2 == 21); + REQUIRE(q3.v3 == 32); + } + + SECTION("Multiplication") + { + Quaternion q3; + q1.Q_Mult(q2, q3); + REQUIRE(q3.w == -60); + REQUIRE(q3.v1 == 12); + REQUIRE(q3.v2 == 30); + REQUIRE(q3.v3 == 24); } SECTION("Rotation") { - Quaternion q3; - q1.Q_Mult(q2, q3); - REQUIRE(q3.v1 == 24); - REQUIRE(q3.v2 == 48); - REQUIRE(q3.v3 == 48); - REQUIRE(q3.w == -6); - } - - SECTION("Euler Angles") - { - Matrix<1, 3> angles; - q1.ToEulerAngles(angles); - REQUIRE_THAT(angles.Get(0, 0), Catch::Matchers::WithinRel(-0.1973956f, 1e-6f)); - REQUIRE_THAT(angles.Get(0, 1), Catch::Matchers::WithinRel(0.823212f, 1e-6f)); - REQUIRE_THAT(angles.Get(0, 2), Catch::Matchers::WithinRel(1.3734008f, 1e-6f)); - } - - SECTION("Rotation Matrix") - { - Matrix<3, 3> rotationMatrix; - q1.ToRotationMatrix(rotationMatrix); - REQUIRE_THAT(rotationMatrix.Get(0, 0), Catch::Matchers::WithinRel(0.1333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(0, 1), Catch::Matchers::WithinRel(-0.6666667f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(0, 2), Catch::Matchers::WithinRel(0.7333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(1, 0), Catch::Matchers::WithinRel(0.9333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(1, 1), Catch::Matchers::WithinRel(0.3333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(1, 2), Catch::Matchers::WithinRel(0.1333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(2, 0), Catch::Matchers::WithinRel(-0.3333333f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(2, 1), Catch::Matchers::WithinRel(0.6666667f, 1e-6f)); - REQUIRE_THAT(rotationMatrix.Get(2, 2), Catch::Matchers::WithinRel(0.6666667f, 1e-6f)); + Quaternion q3{Quaternion::FromAngleAndAxis(M_PI / 2, Matrix<1, 3>{0, 0, 1})}; + Quaternion q4{0, 1, 0, 0}; + Quaternion q5; + q3.Rotate(q4, q5); + REQUIRE_THAT(q5.v1, Catch::Matchers::WithinRel(0.0f, 1e-6f)); + REQUIRE_THAT(q5.v2, Catch::Matchers::WithinRel(1.0f, 1e-6f)); + REQUIRE_THAT(q5.v3, Catch::Matchers::WithinRel(0.0f, 1e-6f)); } } \ No newline at end of file