Added quaternion class

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
 )
 

src/Matrix/Matrix.hpp

@@ -3,6 +3,7 @@
 
 #include <array>
 #include <cstdint>
+#include <string>
 
 // TODO: Add a function to calculate eigenvalues/vectors
 // TODO: Add a function to compute RREF
@@ -186,6 +187,9 @@ public:
 
   Matrix<rows, columns> operator*(float scalar) const;
 
+protected:
+  std::array<float, rows * columns> matrix;
+
 private:
   /**
    * @brief take the dot product of the two vectors
@@ -201,8 +205,6 @@ private:
   Matrix<rows, columns> &adjugate(Matrix<rows, columns> &result) const;
 
   void setMatrixToArray(const std::array<float, rows * columns> &array);
-
-  std::array<float, rows * columns> matrix;
 };
 
 #include "Matrix.cpp"

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
+)

src/Quaternion/Quaternion.cpp

@@ -0,0 +1,66 @@
+#include "Quaternion.h"
+#include <cmath>
+
+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;
+}

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<float, 4> &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_

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
 )

unit-tests/quaternion-tests.cpp

@@ -0,0 +1,55 @@
+// include the unit test framework first
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
+
+// include the module you're going to test next
+#include "Quaternion.h"
+
+// any other libraries
+#include <array>
+#include <cmath>
+#include <iostream>
+
+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<float, 4>{1, 2, 3, 4}};
+        REQUIRE(q6.v1 == 1);
+        REQUIRE(q6.v2 == 2);
+        REQUIRE(q6.v3 == 3);
+        REQUIRE(q6.w == 4);
+    }
+}