Finished implimenting quaternion basics

.vscode/launch.json

@@ -8,7 +8,7 @@
             "name": "Debug Matrix Unit Tests",
             "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/build/unit-tests/matrix-tests", 
+            "program": "${workspaceFolder}/build/src/Matrix/matrix-tests",
             "args": [],
             "stopAtEntry": false,
             "cwd": "${workspaceFolder}",
@@ -27,16 +27,25 @@
             "internalConsoleOptions": "openOnSessionStart"
         },
         {
-            "name": "Run Matrix Unit Tests",
+            "name": "Debug Quaternion Unit Tests",
-            "type": "cpp",
+            "type": "cppdbg",
             "request": "launch",
-            "program": "${workspaceFolder}/build/unit-tests/matrix-tests",
+            "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"
         }
     ]

.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"
         }
     ]

src/Quaternion/Quaternion.cpp

@@ -1,6 +1,24 @@
 #include "Quaternion.h"
 #include <cmath>
 
+/**
+ * @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<float>(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};
+    Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
-    static_cast<Matrix<1, 4>>(buffer) = static_cast<Matrix<1, 4>>(other);
+    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> &
+void Quaternion::Normalize()
-Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) const
 {
-    // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+    float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
-    // rotation sequence R = Rx * Ry * Rz
+    this->v1 /= magnitude;
-    // roll (x-axis rotation)
+    this->v2 /= magnitude;
-    float sinr_cosp = 2 * (this->v2 * this->v3 - this->w * this->v1);
+    this->v3 /= magnitude;
-    float cosr_cosp = 1 - 2 * (this->v1 * this->v1 + this->v2 * this->v2);
+    this->w /= magnitude;
-    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;
 }

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(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, v3) {}
-    Quaternion(const Quaternion &q) : Matrix<1, 4>(q.v1, q.v2, q.v3, q.w) {}
+    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<float, 4> &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
+     * @brief Normalize the quaternion to a magnitude of 1
-     * @param angleBuffer The buffer to store the angles in
-     * @return A reference to the buffer
      */
-    Matrix<1, 3> &ToEulerAngles(Matrix<1, 3> &angleBuffer) const;
+    void Normalize();
-
-    /**
-     * @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;
 
     // Give people an easy way to access the elements
-    float &v1{matrix[0]};
+    float &w{matrix[0]};
-    float &v2{matrix[1]};
+    float &v1{matrix[1]};
-    float &v3{matrix[2]};
+    float &v2{matrix[2]};
-    float &w{matrix[3]};
+    float &v3{matrix[3]};
 };
 
 #endif // QUATERNION_H_

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.w == 1);
-        REQUIRE(q1.v2 == 2);
+        REQUIRE(q1.v1 == 2);
-        REQUIRE(q1.v3 == 3);
+        REQUIRE(q1.v2 == 3);
-        REQUIRE(q1.w == 4);
+        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.w == 1);
-        REQUIRE(q4.v2 == 2);
+        REQUIRE(q4.v1 == 2);
-        REQUIRE(q4.v3 == 3);
+        REQUIRE(q4.v2 == 3);
-        REQUIRE(q4.w == 4);
+        REQUIRE(q4.v3 == 4);
 
         // matrix initialization
         Matrix<1, 4> m1{1, 2, 3, 4};
         Quaternion q5{m1};
-        REQUIRE(q5.v1 == 1);
+        REQUIRE(q5.w == 1);
-        REQUIRE(q5.v2 == 2);
+        REQUIRE(q5.v1 == 2);
-        REQUIRE(q5.v3 == 3);
+        REQUIRE(q5.v2 == 3);
-        REQUIRE(q5.w == 4);
+        REQUIRE(q5.v3 == 4);
 
         // array initialization
         Quaternion q6{std::array<float, 4>{1, 2, 3, 4}};
-        REQUIRE(q6.v1 == 1);
+        REQUIRE(q6.w == 1);
-        REQUIRE(q6.v2 == 2);
+        REQUIRE(q6.v1 == 2);
-        REQUIRE(q6.v3 == 3);
+        REQUIRE(q6.v2 == 3);
-        REQUIRE(q6.w == 4);
+        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.w == 5);
-        REQUIRE(q3.v2 == 12);
+        REQUIRE(q3.v1 == 12);
-        REQUIRE(q3.v3 == 21);
+        REQUIRE(q3.v2 == 21);
-        REQUIRE(q3.w == 32);
+        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;
+        Quaternion q3{Quaternion::FromAngleAndAxis(M_PI / 2, Matrix<1, 3>{0, 0, 1})};
-        q1.Q_Mult(q2, q3);
+        Quaternion q4{0, 1, 0, 0};
-        REQUIRE(q3.v1 == 24);
+        Quaternion q5;
-        REQUIRE(q3.v2 == 48);
+        q3.Rotate(q4, q5);
-        REQUIRE(q3.v3 == 48);
+        REQUIRE_THAT(q5.v1, Catch::Matchers::WithinRel(0.0f, 1e-6f));
-        REQUIRE(q3.w == -6);
+        REQUIRE_THAT(q5.v2, Catch::Matchers::WithinRel(1.0f, 1e-6f));
-    }
+        REQUIRE_THAT(q5.v3, Catch::Matchers::WithinRel(0.0f, 1e-6f));
-
-    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));
     }
 }