From aa8056240a61b42aecf8def400ad0238720e702f Mon Sep 17 00:00:00 2001
From: Quinn <quinn.henthorne@gmail.com>
Date: Fri, 7 Feb 2025 19:19:06 -0500
Subject: [PATCH] Fixed quaternion multiplication

---
 src/Quaternion/Quaternion.cpp                 | 30 +++++++++++++------
 src/Quaternion/Quaternion.h                   | 22 +++++++++-----
 .../unit-tests/quaternion-tests.cpp           |  4 +--
 3 files changed, 37 insertions(+), 19 deletions(-)

diff --git a/src/Quaternion/Quaternion.cpp b/src/Quaternion/Quaternion.cpp
index 19607ec..3d15337 100644
--- a/src/Quaternion/Quaternion.cpp
+++ b/src/Quaternion/Quaternion.cpp
@@ -1,7 +1,7 @@
 #include "Quaternion.h"
 #include <cmath>
 
-float Quaternion::operator[](uint8_t index)
+float Quaternion::operator[](uint8_t index) const
 {
     if (index < 4)
     {
@@ -12,24 +12,36 @@ float Quaternion::operator[](uint8_t index)
     return 1e+6;
 }
 
-Quaternion Quaternion::operator+(const Quaternion &other)
+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};
 }
 
 Quaternion &
-Quaternion::Rotate(Quaternion &other, Quaternion &buffer)
+Quaternion::Q_Mult(Quaternion &other, Quaternion &buffer) const
 {
+
     // 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;
+    buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3);
+    buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2);
+    buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1);
+    buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w);
+    return buffer;
+}
+
+Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
+{
+    Quaternion prime{-this->v1, -this->v2, -this->v3, this->w};
+    static_cast<Matrix<1, 4>>(buffer) = static_cast<Matrix<1, 4>>(other);
+    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)
+Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer) const
 {
     // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
     // rotation sequence R = Rx * Ry * Rz
@@ -53,7 +65,7 @@ Quaternion::ToEulerAngles(Matrix<1, 3> &angleBuffer)
     return angleBuffer;
 }
 
-Matrix<3, 3> &Quaternion::ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer)
+Matrix<3, 3> &Quaternion::ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) const
 {
     // eq. 4
     // from https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
diff --git a/src/Quaternion/Quaternion.h b/src/Quaternion/Quaternion.h
index b546b61..2a3ce5c 100644
--- a/src/Quaternion/Quaternion.h
+++ b/src/Quaternion/Quaternion.h
@@ -18,44 +18,50 @@ public:
      * @param index The index of the element to access
      * @return The value of the element at the index
      */
-    float operator[](uint8_t index);
+    float operator[](uint8_t index) const;
 
     /**
      * @brief Add two quaternions together
      * @param other The quaternion to add to this one
      * @return The net quaternion
      */
-    Quaternion operator+(const Quaternion &other);
+    Quaternion operator+(const Quaternion &other) const;
 
     /**
-     * @brief Rotate a quaternion by another quaternion
+     * @brief Q_Mult a quaternion by another quaternion
      * @param other The quaternion to rotate by
      * @param buffer The buffer to store the result in
      * @return A reference to the buffer
      */
-    Quaternion &Rotate(Quaternion &other, Quaternion &buffer);
+    Quaternion &Q_Mult(Quaternion &other, Quaternion &buffer) const;
+
+    /**
+     * @brief Rotate a quaternion by another quaternion
+     * @param other The quaternion to rotate
+     * @param buffer The buffer to store the result in
+     *
+     */
+    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
      */
-    Matrix<1, 3> &ToEulerAngles(Matrix<1, 3> &angleBuffer);
+    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);
+    Matrix<3, 3> &ToRotationMatrix(Matrix<3, 3> &rotationMatrixBuffer) const;
 
     // 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/src/Quaternion/unit-tests/quaternion-tests.cpp b/src/Quaternion/unit-tests/quaternion-tests.cpp
index 0b5f1ad..4c90752 100644
--- a/src/Quaternion/unit-tests/quaternion-tests.cpp
+++ b/src/Quaternion/unit-tests/quaternion-tests.cpp
@@ -73,8 +73,8 @@ TEST_CASE("Vector Math", "Vector")
     SECTION("Rotation")
     {
         Quaternion q3;
-        q1.Rotate(q2, q3);
-        REQUIRE(q3.v1 == -24);
+        q1.Q_Mult(q2, q3);
+        REQUIRE(q3.v1 == 24);
         REQUIRE(q3.v2 == 48);
         REQUIRE(q3.v3 == 48);
         REQUIRE(q3.w == -6);