Merge pull request 'Updating the readme' (#7) from update-readme into main

Reviewed-on: #7

.gitea/workflows/Merge-Checker.yaml

@@ -0,0 +1,102 @@
+name: Merge-Checker
+
+on:
+  pull_request:
+    branches: ["**"]
+
+jobs:
+  build_and_test:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout source code
+        uses: actions/checkout@v3
+        with:
+          persist-credentials: true
+          fetch-depth: 0
+
+      - name: Install dependencies (CMake + Ninja + build tools)
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y cmake ninja-build build-essential time git
+
+      - name: Configure project with CMake
+        run: cmake -G Ninja -S . -B build/
+
+      - name: Build with Ninja
+        run: ninja -C build/
+
+      - name: Run all unit tests except matrix-timing-tests
+        run: |
+          for test_exec in build/unit-tests/matrix-tests build/unit-tests/quaternion-tests build/unit-tests/vector-3d-tests; do
+            if [ -x "$test_exec" ]; then
+              echo "Running $test_exec"
+              "$test_exec"
+            else
+              echo "Warning: $test_exec not found or not executable"
+            fi
+          done
+      - name: Run matrix-timing-tests
+        run: |
+          mkdir -p unit-tests/timing-results
+          if [ -x build/unit-tests/matrix-timing-tests ]; then
+            echo "Running matrix-timing-tests with timing"
+            /usr/bin/time -v build/unit-tests/matrix-timing-tests -d yes &> unit-tests/timing-results/matrix-timing-tests.txt
+            cat unit-tests/timing-results/matrix-timing-tests.txt
+          else
+            echo "matrix-timing-tests executable not found or not executable"
+            exit 1
+          fi
+
+      - name: Compare timing results
+        id: check_diff
+        run: |
+          git show origin/${{ github.event.pull_request.head.ref }}:unit-tests/timing-results/matrix-timing-tests.txt > old.txt || echo "" > old.txt
+          cp unit-tests/timing-results/matrix-timing-tests.txt new.txt
+
+          echo "Comparing timing results for changes ≥ 0.1s (ignoring 'Timing Tests' lines)..."
+
+          changed=0
+
+          awk -v changed_ref=/tmp/timings_changed.flag '
+            BEGIN {
+              change_threshold = 0.1
+            }
+            FILENAME == "old.txt" && /^[0-9]+\.[0-9]+ s: / {
+              label = substr($0, index($0, ":") + 2)
+              if (label != "Timing Tests") {
+                label_times[label] = $1
+              }
+            }
+            FILENAME == "new.txt" && /^[0-9]+\.[0-9]+ s: / {
+              new_time = $1
+              label = substr($0, index($0, ":") + 2)
+              if (label == "Timing Tests") next
+
+              old_time = label_times[label]
+              delta = new_time - old_time
+              if (delta < 0) delta = -delta
+
+              if (old_time != "" && delta >= change_threshold) {
+                printf "⚠️  %.3f s → %.3f s: %s (Δ=%.3f s)\n", old_time, new_time, label, delta
+                system("touch " changed_ref)
+              } else if (old_time == "") {
+                printf "🆕 New timing entry: %.3f s: %s\n", new_time, label
+                system("touch " changed_ref)
+              }
+            }
+            END {
+              if (!system("test -f " changed_ref)) {
+                exit 0
+              } else {
+                print "✅ Timings haven’t changed significantly (Δ < 0.1s)."
+                exit 0
+              }
+            }
+          ' old.txt new.txt
+
+          if [ -f /tmp/timings_changed.flag ]; then
+            echo "timings_changed=true" >> $GITHUB_OUTPUT
+          else
+            echo "timings_changed=false" >> $GITHUB_OUTPUT
+          fi

.gitignore

@@ -1,2 +1,2 @@
 build/
-venv/
+.cache/

.vscode/launch.json

@@ -27,16 +27,25 @@
             "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",
+            "program": "${workspaceFolder}/build/unit-tests/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/settings.json

@@ -1,8 +1,5 @@
 {
     "C_Cpp.intelliSenseEngine": "default",
-    "clangd.arguments": [
-        "--include-directory=build/unit-tests"
-    ],
     "C_Cpp.default.intelliSenseMode": "linux-gcc-x64",
     "files.associations": {
         "*.h": "cpp",
@@ -71,8 +68,16 @@
         "typeinfo": "cpp",
         "variant": "cpp",
         "shared_mutex": "cpp",
-        "complex": "cpp"
+        "charconv": "cpp",
+        "format": "cpp",
+        "csignal": "cpp",
+        "span": "cpp"
     },
-    "clangd.enable": false,
-    "C_Cpp.dimInactiveRegions": false
+    "clangd.enable": true,
+    "C_Cpp.dimInactiveRegions": false,
+    "editor.defaultFormatter": "xaver.clang-format",
+    "clangd.inactiveRegions.useBackgroundHighlight": false,
+    "clangd.arguments": [
+        "--compile-commands-dir=${workspaceFolder}/build"
+    ],
 }

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

CMakeLists.txt

@@ -1,40 +1,21 @@
-cmake_minimum_required(VERSION 3.6)
+cmake_minimum_required (VERSION 3.11)
 project(Vector3D)
 
+add_subdirectory(src)
 add_subdirectory(unit-tests)
 
 set(CMAKE_CXX_STANDARD 11)
 
+add_compile_options(-Wall -Wextra -Wpedantic)
 add_compile_options (-fdiagnostics-color=always)
+set(CMAKE_COLOR_DIAGNOSTICS ON)
 
-# Vector3d
-add_library(Vector3D 
-    STATIC
-    Vector3D.hpp
+include(FetchContent)
+ 
+FetchContent_Declare(
+    Catch2
+    GIT_REPOSITORY https://github.com/catchorg/Catch2.git
+    GIT_TAG v3.8.0 # or a later release
 )
  
-set_target_properties(Vector3D
-    PROPERTIES
-    LINKER_LANGUAGE CXX
-)
-
-target_include_directories(Vector3D PUBLIC
-    include
-)
-
-# Matrix
-add_library(Matrix 
-    STATIC
-    Matrix.hpp
-    Matrix.cpp
-)
-
-set_target_properties(Matrix
-    PROPERTIES
-    LINKER_LANGUAGE CXX
-)
-
-target_include_directories(Matrix 
-    PUBLIC
-    .
-)
+FetchContent_MakeAvailable(Catch2)

README.md

@@ -1 +1,12 @@
-A Simple matrix math library focused on embedded development which avoids and heap memory allocation unless you explicitly ask for it.
+# Introduction
+This matrix math library is focused on embedded development and avoids any heap memory allocation unless you explicitly ask for it.
+It uses templates to pre-allocate matrices on the stack.
+
+# Building
+1. Initialize the repositiory with the command:
+```bash
+cmake -S . -B build -G Ninja
+```
+
+2. Go into the build folder and run `ninja`
+3. That's it. You can test out the build by running `./unit-tests/matrix-tests`

Vector3D.hpp

@@ -1,81 +0,0 @@
-#pragma once
-
-#include <cstdint>
-#include <cmath>
-#include <type_traits>
-
-template <typename Type>
-class V3D{
-    public:
-    constexpr V3D(const V3D& other):
-    x(other.x),
-    y(other.y),
-    z(other.z){
-        static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
-    }
-    
-    constexpr V3D(Type x=0, Type y=0, Type z=0): 
-    x(x), 
-    y(y), 
-    z(z){
-        static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
-    }
-
-    template <typename OtherType>
-    constexpr V3D(const V3D<OtherType> other):
-    x(static_cast<Type>(other.x)),
-    y(static_cast<Type>(other.y)),
-    z(static_cast<Type>(other.z)){
-        static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
-        static_assert(std::is_arithmetic<OtherType>::value, "OtherType must be a number");
-    }
-
-    V3D& operator=(const V3D &other){
-        this->x = other.x;
-        this->y = other.y;
-        this->z = other.z;
-        return *this;
-    }
-
-    V3D& operator+=(const V3D &other){
-        this->x += other.x;
-        this->y += other.y;
-        this->z += other.z;
-        return *this;
-    }
-
-    V3D& operator-=(const V3D &other){
-        this->x -= other.x;
-        this->y -= other.y;
-        this->z -= other.z;
-        return *this;
-    }
-
-    V3D& operator/=(const Type scalar){
-        if(scalar == 0){
-            return *this;
-        }
-        this->x /= scalar;
-        this->y /= scalar;
-        this->z /= scalar;
-        return *this;
-    }
-
-    V3D& operator*=(const Type scalar){
-        this->x *= scalar;
-        this->y *= scalar;
-        this->z *= scalar;
-        return *this;
-    }
-
-    bool operator==(const V3D &other){
-        return this->x == other.x && this->y == other.y && this->z == other.z;
-    }
-
-    float magnitude(){
-        return std::sqrt(static_cast<float>(this->x * this->x + this->y * this->y + this->z * this->z));
-    }
-    Type x;
-    Type y;
-    Type z;
-};

library.json

@@ -0,0 +1,20 @@
+{
+    "name": "Vector3D",
+    "version": "1.0.0",
+    "description": "Contains a V3D object for easy 3d vector math and a Matrix object for more complicated linear algebra operations.",
+    "keywords": "linear algebra, vector, matrix, 3D",
+    "repository": {
+        "type": "git",
+        "url": "https://github.com/Cynopolis/Vector3D.git"
+    },
+    "authors": [
+        {
+            "name": "Cynopolis",
+            "email": "megaveganzombie@gmail.com",
+            "url": "https://github.com/Cynopolis"
+        }
+    ],
+    "license": "None Yet",
+    "frameworks": "*",
+    "platforms": "*"
+}

qr-decom.py

@@ -1,159 +0,0 @@
-import numpy as np
-
-# QR decomposition using the householder reflection method
-def householder_reflection(A):
-    """
-    Perform QR decomposition using Householder reflection.
-    
-    Arguments:
-    A -- A matrix to be decomposed (m x n).
-    
-    Returns:
-    Q -- Orthogonal matrix (m x m).
-    R -- Upper triangular matrix (m x n).
-    """
-    A = A.astype(float)  # Ensure the matrix is of type float
-    m, n = A.shape
-    Q = np.eye(m)  # Initialize Q as an identity matrix
-    R = A.copy()  # R starts as a copy of A
-
-    # Apply Householder reflections for each column
-    for k in range(n):
-        # Step 1: Compute the Householder vector
-        x = R[k:m, k]
-        e1 = np.zeros_like(x)
-        e1[0] = np.linalg.norm(x) if x[0] >= 0 else -np.linalg.norm(x)
-        v = x + e1
-        v = v / np.linalg.norm(v)  # Normalize v
-        
-        # Step 2: Apply the reflection to the matrix
-        R[k:m, k:n] = R[k:m, k:n] - 2 * np.outer(v, v.T @ R[k:m, k:n])
-        
-        # Step 3: Apply the reflection to Q
-        Q[:, k:m] = Q[:, k:m] - 2 * np.outer(Q[:, k:m] @ v, v)
-
-    # The resulting Q and R are the QR decomposition
-    return Q, R
-
-# Example usage
-A = np.array([[12, -51, 4], 
-              [6, 167, -68], 
-              [-4, 24, -41]])
-
-Q, R = householder_reflection(A)
-print("Q matrix:")
-print(Q)
-print("\nR matrix:")
-print(R)
-print("Multiplied Together:")
-print(Q@R)
-
-def svd_decomposition(A):
-    """
-    Perform Singular Value Decomposition (SVD) from scratch.
-    
-    Arguments:
-    A -- The matrix to be decomposed (m x n).
-    
-    Returns:
-    U -- Orthogonal matrix of left singular vectors (m x m).
-    Sigma -- Diagonal matrix of singular values (m x n).
-    Vt -- Orthogonal matrix of right singular vectors (n x n).
-    """
-    # Step 1: Compute A^T A
-    AtA = np.dot(A.T, A)  # A transpose multiplied by A
-    
-    # Step 2: Compute the eigenvalues and eigenvectors of A^T A
-    eigenvalues, V = np.linalg.eig(AtA)
-    
-    # Step 3: Sort eigenvalues in descending order and sort V accordingly
-    sorted_indices = np.argsort(eigenvalues)[::-1]  # Indices to sort eigenvalues in descending order
-    eigenvalues = eigenvalues[sorted_indices]
-    V = V[:, sorted_indices]
-    
-    # Step 4: Compute the singular values (sqrt of eigenvalues)
-    singular_values = np.sqrt(eigenvalues)
-    
-    # Step 5: Construct the Sigma matrix
-    m, n = A.shape
-    Sigma = np.zeros((m, n))  # Initialize Sigma as a zero matrix
-    for i in range(min(m, n)):
-        Sigma[i, i] = singular_values[i]  # Place the singular values on the diagonal
-    
-    # Step 6: Compute the U matrix using A * V = U * Sigma
-    U = np.dot(A, V)  # A * V gives us the unnormalized U
-    # Normalize the columns of U
-    for i in range(U.shape[1]):
-        U[:, i] = U[:, i] / singular_values[i]  # Normalize each column by the corresponding singular value
-    
-    # Step 7: Return U, Sigma, Vt
-    return U, Sigma, V.T  # V.T is the transpose of V
-
-# Example usage
-A = np.array([[12, -51, 4],
-              [6, 167, -68],
-              [-4, 24, -41]])
-
-U, Sigma, Vt = svd_decomposition(A)
-
-print("\nSVD DECOMPOSITION\nU matrix:")
-print(U)
-print("\nSigma matrix:")
-print(Sigma)
-print("\nVt matrix:")
-print(Vt)
-print("Multiplied together:")
-print(U@Sigma@Vt)
-
-def eigen_decomposition_qr(A, max_iter=1000, tol=1e-9):
-    """
-    Compute the eigenvalues and eigenvectors of a matrix A using the QR algorithm
-    with QR decomposition.
-
-    Arguments:
-    A -- A square matrix (n x n).
-    max_iter -- Maximum number of iterations for convergence (default 1000).
-    tol -- Tolerance for convergence (default 1e-9).
-    
-    Returns:
-    eigenvalues -- List of eigenvalues.
-    eigenvectors -- Matrix of eigenvectors.
-    """
-    # Make a copy of A to perform the iteration
-    A_copy = A.copy()
-    n = A_copy.shape[0]
-    
-    # Initialize the matrix for eigenvectors (this will accumulate the Q matrices)
-    eigenvectors = np.eye(n)
-    
-    # Perform QR iterations
-    for _ in range(max_iter):
-        # Perform QR decomposition on A_copy
-        Q, R = householder_reflection(A_copy)
-        
-        # Update A_copy to be R * Q (QR algorithm step)
-        A_copy = R @ Q
-        
-        # Accumulate the eigenvectors
-        eigenvectors = eigenvectors @ Q
-        
-        # Check for convergence: if the off-diagonal elements are small enough, we stop
-        off_diagonal_norm = np.linalg.norm(np.tril(A_copy, -1))  # Norm of the lower triangle (off-diagonal)
-        if off_diagonal_norm < tol:
-            break
-    
-    # The eigenvalues are the diagonal elements of the matrix A_copy
-    eigenvalues = np.diag(A_copy)
-    
-    return eigenvalues, eigenvectors
-
-# Example usage
-A = np.array([[12, -51, 4], 
-              [6, 167, -68], 
-              [-4, 24, -41]])
-
-eigenvalues, eigenvectors = eigen_decomposition_qr(A)
-
-
-print("\n\nEigenvalues:", eigenvalues)
-print("Eigenvectors:\n", eigenvectors)

src/CMakeLists.txt

@@ -0,0 +1,59 @@
+# Quaternion Interface
+add_library(vector-3d-intf
+    INTERFACE
+)
+
+target_include_directories(vector-3d-intf
+    INTERFACE
+    .
+)
+
+target_link_libraries(vector-3d-intf
+    INTERFACE
+)
+
+# Quaternion
+add_library(quaternion 
+    STATIC
+    Quaternion.cpp
+)
+
+target_link_libraries(quaternion
+    PUBLIC
+    vector-3d-intf
+    PRIVATE
+)
+
+set_target_properties(quaternion
+    PROPERTIES
+    LINKER_LANGUAGE CXX
+)
+
+# Vector3d
+add_library(vector-3d 
+    STATIC
+    Vector3D.cpp
+)
+
+target_link_libraries(vector-3d
+    PUBLIC
+    vector-3d-intf
+    PRIVATE
+)
+
+# Matrix
+add_library(matrix 
+    STATIC
+    Matrix.cpp
+)
+
+target_link_libraries(matrix
+    PUBLIC
+    vector-3d-intf
+    PRIVATE
+)
+
+set_target_properties(matrix
+    PROPERTIES
+    LINKER_LANGUAGE CXX
+)

src/Matrix.cpp

@@ -1,3 +1,10 @@
+// This #ifndef section makes clangd happy so that it can properly do type hints
+// in this file
+#ifndef MATRIX_H_
+#define MATRIX_H_
+#include "Matrix.hpp"
+#endif
+
 #ifdef MATRIX_H_ // since the .cpp file has to be included by the .hpp file this
                  // will evaluate to true
 #include "Matrix.hpp"
@@ -5,18 +12,44 @@
 #include <algorithm>
 #include <cmath>
 #include <cstdlib>
-#include <type_traits>
-
-template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns>::Matrix(float value) {
-  this->Fill(value);
-}
+#include <cstring>
 
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array) {
   this->setMatrixToArray(array);
 }
 
+template <uint8_t rows, uint8_t columns>
+template <typename... Args>
+Matrix<rows, columns>::Matrix(Args... args) {
+  constexpr uint16_t arraySize{static_cast<uint16_t>(rows) *
+                               static_cast<uint16_t>(columns)};
+
+  std::initializer_list<float> initList{static_cast<float>(args)...};
+  // if there is only one value, we actually want to do a fill
+  if (sizeof...(args) == 1) {
+    this->Fill(*initList.begin());
+  }
+  static_assert(sizeof...(args) == arraySize || sizeof...(args) == 1,
+                "You did not provide the right amount of initializers for this "
+                "matrix size");
+
+  // choose whichever buffer size is smaller for the copy length
+  uint32_t minSize =
+      std::min(arraySize, static_cast<uint16_t>(initList.size()));
+  memcpy(this->matrix.begin(), initList.begin(), minSize * sizeof(float));
+}
+
+template <uint8_t rows, uint8_t columns>
+Matrix<rows, columns> Matrix<rows, columns>::Identity() {
+  Matrix<rows, columns> identityMatrix{0};
+  uint32_t minDimension = std::min(rows, columns);
+  for (uint8_t idx{0}; idx < minDimension; idx++) {
+    identityMatrix[idx][idx] = 1;
+  }
+  return identityMatrix;
+}
+
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
   for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
@@ -27,19 +60,6 @@ Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
   }
 }
 
-template <uint8_t rows, uint8_t columns>
-template <typename... Args>
-Matrix<rows, columns>::Matrix(Args... args) {
-  constexpr uint16_t arraySize{static_cast<uint16_t>(rows) *
-                               static_cast<uint16_t>(columns)};
-
-  std::initializer_list<float> initList{static_cast<float>(args)...};
-  // choose whichever buffer size is smaller for the copy length
-  uint32_t minSize =
-      std::min(arraySize, static_cast<uint16_t>(initList.size()));
-  memcpy(this->matrix.begin(), initList.begin(), minSize * sizeof(float));
-}
-
 template <uint8_t rows, uint8_t columns>
 void Matrix<rows, columns>::setMatrixToArray(
     const std::array<float, rows * columns> &array) {
@@ -91,21 +111,18 @@ Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
                             Matrix<rows, other_columns> &result) const {
   // allocate some buffers for all of our dot products
   Matrix<1, columns> this_row;
-  Matrix<rows, 1> other_column;
-  Matrix<1, rows> other_column_t;
+  Matrix<columns, 1> other_column;
 
   for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
     // get our row
     this->GetRow(row_idx, this_row);
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+    for (uint8_t column_idx{0}; column_idx < other_columns; column_idx++) {
       // get the other matrix'ss column
       other.GetColumn(column_idx, other_column);
-      // transpose the other matrix's column
-      other_column.Transpose(other_column_t);
 
       // the result's index is equal to the dot product of these two vectors
       result[row_idx][column_idx] =
-          Matrix<rows, columns>::dotProduct(this_row, other_column_t);
+          Matrix<rows, columns>::DotProduct(this_row, other_column.Transpose());
     }
   }
 
@@ -125,13 +142,13 @@ Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> &
-Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const {
+Matrix<rows, columns> Matrix<rows, columns>::Invert() const {
   // since all matrix sizes have to be statically specified at compile time we
   // can do this
   static_assert(rows == columns,
                 "Your matrix isn't square and can't be inverted");
 
+  Matrix<rows, columns> result{};
   // unfortunately we can't calculate this at compile time so we'll just reurn
   // zeros
   float determinant{this->Det()};
@@ -160,8 +177,8 @@ Matrix<rows, columns>::Invert(Matrix<rows, columns> &result) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<columns, rows> &
-Matrix<rows, columns>::Transpose(Matrix<columns, rows> &result) const {
+Matrix<columns, rows> Matrix<rows, columns>::Transpose() const {
+  Matrix<columns, rows> result{};
   for (uint8_t column_idx{0}; column_idx < rows; column_idx++) {
     for (uint8_t row_idx{0}; row_idx < columns; row_idx++) {
       result[row_idx][column_idx] = this->Get(column_idx, row_idx);
@@ -173,9 +190,10 @@ Matrix<rows, columns>::Transpose(Matrix<columns, rows> &result) const {
 
 // explicitly define the determinant for a 2x2 matrix because it is definitely
 // the fastest way to calculate a 2x2 matrix determinant
-template <> float Matrix<0, 0>::Det() const { return 1e+6; }
-template <> float Matrix<1, 1>::Det() const { return this->matrix[0]; }
-template <> float Matrix<2, 2>::Det() const {
+// template <>
+// inline float Matrix<0, 0>::Det() const { return 1e+6; }
+template <> inline float Matrix<1, 1>::Det() const { return this->matrix[0]; }
+template <> inline float Matrix<2, 2>::Det() const {
   return this->matrix[0] * this->matrix[3] - this->matrix[1] * this->matrix[2];
 }
 
@@ -271,8 +289,13 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-std::array<float, columns> &Matrix<rows, columns>::
-operator[](uint8_t row_index) {
+const float *Matrix<rows, columns>::ToArray() const {
+  return this->matrix.data();
+}
+
+template <uint8_t rows, uint8_t columns>
+std::array<float, columns> &
+Matrix<rows, columns>::operator[](uint8_t row_index) {
   if (row_index > rows - 1) {
     // TODO: We should throw something here instead of failing quietly.
     row_index = 0;
@@ -284,38 +307,36 @@ operator[](uint8_t row_index) {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> &Matrix<rows, columns>::
-operator=(const Matrix<rows, columns> &other) {
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
-      this->matrix[row_idx * columns + column_idx] =
-          other.Get(row_idx, column_idx);
-    }
-  }
+Matrix<rows, columns> &
+Matrix<rows, columns>::operator=(const Matrix<rows, columns> &other) {
+  memcpy(this->matrix.begin(), other.matrix.begin(),
+         rows * columns * sizeof(float));
+
   // return a reference to ourselves so you can chain together these functions
   return *this;
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> Matrix<rows, columns>::
-operator+(const Matrix<rows, columns> &other) const {
+Matrix<rows, columns>
+Matrix<rows, columns>::operator+(const Matrix<rows, columns> &other) const {
   Matrix<rows, columns> buffer{};
   this->Add(other, buffer);
   return buffer;
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> Matrix<rows, columns>::
-operator-(const Matrix<rows, columns> &other) const {
+Matrix<rows, columns>
+Matrix<rows, columns>::operator-(const Matrix<rows, columns> &other) const {
   Matrix<rows, columns> buffer{};
   this->Sub(other, buffer);
   return buffer;
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> Matrix<rows, columns>::
-operator*(const Matrix<rows, columns> &other) const {
-  Matrix<rows, columns> buffer{};
+template <uint8_t other_columns>
+Matrix<rows, other_columns> Matrix<rows, columns>::operator*(
+    const Matrix<columns, other_columns> &other) const {
+  Matrix<rows, other_columns> buffer{};
   this->Mult(other, buffer);
   return buffer;
 }
@@ -327,9 +348,25 @@ Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const {
   return buffer;
 }
 
+template <uint8_t rows, uint8_t columns>
+Matrix<rows, columns> Matrix<rows, columns>::operator/(float scalar) const {
+  Matrix<rows, columns> buffer = *this;
+  if (scalar == 0) {
+    buffer.Fill(1e+10);
+    return buffer;
+  }
+
+  for (uint8_t row = 0; row < rows; row++) {
+    for (uint8_t column = 0; column < columns; column++) {
+      buffer[row][column] /= scalar;
+    }
+  }
+  return buffer;
+}
+
 template <uint8_t rows, uint8_t columns>
 template <uint8_t vector_size>
-float Matrix<rows, columns>::dotProduct(const Matrix<1, vector_size> &vec1,
+float Matrix<rows, columns>::DotProduct(const Matrix<1, vector_size> &vec1,
                                         const Matrix<1, vector_size> &vec2) {
   float sum{0};
   for (uint8_t i{0}; i < vector_size; i++) {
@@ -341,7 +378,7 @@ float Matrix<rows, columns>::dotProduct(const Matrix<1, vector_size> &vec1,
 
 template <uint8_t rows, uint8_t columns>
 template <uint8_t vector_size>
-float Matrix<rows, columns>::dotProduct(const Matrix<vector_size, 1> &vec1,
+float Matrix<rows, columns>::DotProduct(const Matrix<vector_size, 1> &vec1,
                                         const Matrix<vector_size, 1> &vec2) {
   float sum{0};
   for (uint8_t i{0}; i < vector_size; i++) {
@@ -353,7 +390,11 @@ float Matrix<rows, columns>::dotProduct(const Matrix<vector_size, 1> &vec1,
 
 template <uint8_t rows, uint8_t columns>
 void Matrix<rows, columns>::Fill(float value) {
-  this->matrix.fill(value);
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+      this->matrix[row_idx * columns + column_idx] = value;
+    }
+  }
 }
 
 template <uint8_t rows, uint8_t columns>
@@ -409,8 +450,8 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> &
-Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
+float Matrix<rows, columns>::EuclideanNorm() const {
+
   float sum{0};
   for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
     for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
@@ -419,90 +460,147 @@ Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const {
     }
   }
 
-  if (sum == 0) {
-    // this wouldn't do anything anyways
-    result.Fill(1e+6);
-    return result;
-  }
-
-  sum = sqrt(sum);
-
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
-      result[row_idx][column_idx] = this->Get(row_idx, column_idx) / sum;
-    }
-  }
-
-  return result;
+  return sqrt(sum);
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, rows> Matrix<rows, columns>::Eye() {
-  Matrix<rows, rows> i_matrix;
-  i_matrix.Fill(0);
-  for (uint8_t i{0}; i < rows; i++) {
-    i_matrix[i][i] = 1;
+template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
+          uint8_t column_offset>
+Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const {
+  // static assert that sub_rows + row_offset <= rows
+  // static assert that sub_columns + column_offset <= columns
+  static_assert(sub_rows + row_offset <= rows,
+                "The submatrix you're trying to get is out of bounds (rows)");
+  static_assert(
+      sub_columns + column_offset <= columns,
+      "The submatrix you're trying to get is out of bounds (columns)");
+
+  Matrix<sub_rows, sub_columns> buffer{};
+  for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++) {
+    for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++) {
+      buffer[row_idx][column_idx] =
+          this->Get(row_idx + row_offset, column_idx + column_offset);
     }
-  return i_matrix;
+  }
+  return buffer;
 }
 
 template <uint8_t rows, uint8_t columns>
-void Matrix<rows, columns>::QR_Decomposition(Matrix<rows, columns> &Q,
-                                             Matrix<rows, columns> &R) const {
-  Q = Matrix<rows, columns>::Eye(); // Q starts as the identity matrix
-  R = *this; // R starts as a copy of this matrix (For this algorithm we'll call
-             // this matrix A)
+template <uint8_t sub_rows, uint8_t sub_columns>
+void Matrix<rows, columns>::SetSubMatrix(
+    uint8_t rowOffset, uint8_t columnOffset,
+    const Matrix<sub_rows, sub_columns> &sub_matrix) {
+  int16_t adjustedSubRows = sub_rows;
+  int16_t adjustedSubColumns = sub_columns;
+  int16_t adjustedRowOffset = rowOffset;
+  int16_t adjustedColumnOffset = columnOffset;
 
-  for (uint8_t row{0}; row < rows; row++) {
-    // compute the householder vector
-    const uint8_t houseHoldVectorSize{rows - row};
-    const uint8_t subMatrixSize{columns - row};
-    Matrix<houseHoldVectorSize, 1> x{};
-    this->SubMatrix(row, row, x);
+  // a bunch of safety checks to make sure we don't overflow the matrix
+  if (sub_rows > rows) {
+    adjustedSubRows = rows;
+  }
+  if (sub_columns > columns) {
+    adjustedSubColumns = columns;
+  }
 
-    Matrix<houseHoldVectorSize, 1> e1{};
-    e1.Fill(0);
-    if (x[0][0] >= 0) {
-      e1[0][0] = x.Norm();
+  if (adjustedSubRows + adjustedRowOffset >= rows) {
+    adjustedRowOffset =
+        std::max(0, static_cast<int16_t>(rows) - adjustedSubRows);
+  }
+
+  if (adjustedSubColumns + adjustedColumnOffset >= columns) {
+    adjustedColumnOffset =
+        std::max(0, static_cast<int16_t>(columns) - adjustedSubColumns);
+  }
+
+  for (uint8_t row_idx{0}; row_idx < adjustedSubRows; row_idx++) {
+    for (uint8_t column_idx{0}; column_idx < adjustedSubColumns; column_idx++) {
+      this->matrix[(row_idx + adjustedRowOffset) * columns + column_idx +
+                   adjustedColumnOffset] = sub_matrix.Get(row_idx, column_idx);
+    }
+  }
+}
+
+// QR decomposition: decomposes this matrix A into Q and R
+// Assumes square matrix
+template <uint8_t rows, uint8_t columns>
+void Matrix<rows, columns>::QRDecomposition(Matrix<rows, columns> &Q,
+                                            Matrix<columns, columns> &R) const {
+  static_assert(columns <= rows, "QR decomposition requires columns <= rows");
+
+  Q.Fill(0);
+  R.Fill(0);
+  Matrix<rows, 1> a_col, e, u, Q_column_k{};
+  Matrix<1, rows> a_T, e_T{};
+
+  for (uint8_t column = 0; column < columns; column++) {
+    this->GetColumn(column, a_col);
+    u = a_col;
+    // -----------------------
+    // ----- CALCULATE Q -----
+    // -----------------------
+    for (uint8_t k = 0; k <= column; k++) {
+      Q.GetColumn(k, Q_column_k);
+      Matrix<1, rows> Q_column_k_T = Q_column_k.Transpose();
+      u = u - Q_column_k * (Q_column_k_T * a_col);
+    }
+    float norm = u.EuclideanNorm();
+    if (norm > 1e-4) {
+      u = u / norm;
     } else {
-      e1[0][0] = -x.Norm();
+      u.Fill(0);
+    }
+    Q.SetSubMatrix(0, column, u);
+
+    // -----------------------
+    // ----- CALCULATE R -----
+    // -----------------------
+    for (uint8_t k = 0; k <= column; k++) {
+      Q.GetColumn(k, e);
+      R[k][column] = (a_col.Transpose() * e).Get(0, 0);
+    }
+  }
 }
 
-    Matrix<houseHoldVectorSize, 1> v = x + e1;
-    v = v * (1 / v.Norm()); // normalize V
+template <uint8_t rows, uint8_t columns>
+void Matrix<rows, columns>::EigenQR(Matrix<rows, rows> &eigenVectors,
+                                    Matrix<rows, 1> &eigenValues,
+                                    uint32_t maxIterations,
+                                    float tolerance) const {
+  static_assert(rows > 1, "Matrix size must be > 1 for QR iteration");
+  static_assert(rows == columns, "Matrix size must be square for QR iteration");
 
-    // ************************************
-    // Apply the reflection to the R matrix
-    // ************************************
-    // initialize R's submatrix
-    Matrix<houseHoldVectorSize, subMatrixSize> R_subMatrix{};
-    R.SubMatrix(row, row, R_subMatrix);
-    // create some temporary buffers
-    Matrix<1, subMatrixSize> vR{};
-    Matrix<1, houseHoldVectorSize> v_T{};
-    v.Transpose(v_T);
-    Matrix<houseHoldVectorSize, subMatrixSize> vR_outer{};
-    // calculate the reflection
-    R_subMatrix =
-        R_subMatrix - 2 * Matrix<rows, columns>::OuterProduct(
-                              v_T, v_T.Mult(R_subMatrix, vR), vR_outer);
-    // save the reflection back to R
-    R.CopySubMatrixInto(row, row, R_subMatrix);
+  Matrix<rows, rows> Ak = *this; // Copy original matrix
+  Matrix<rows, rows> QQ{Matrix<rows, rows>::Identity()};
+  Matrix<rows, rows> shift{0};
 
-    // ************************************
-    // Apply the reflection to the Q matrix
-    // ************************************
-    // initialize Q's submatrix
-    Matrix<rows, houseHoldVectorSize> Q_subMatrix{};
-    Q.SubMatrix(0, row, Q_subMatrix);
-    // create some temporary buffers
-    Matrix<rows, 1> Qv{};
-    Matrix<rows, houseHoldVectorSize> Qv_outer{};
+  for (uint32_t iter = 0; iter < maxIterations; ++iter) {
+    Matrix<rows, rows> Q, R;
 
-    Q_subMatrix = Q_subMatrix - 2 * Matrix<rows, columns>::OuterProduct(
-                                        Q_subMatrix.Mult(v, Qv), v, Qv_outer);
-    Q.CopySubMatrixInto(0, row, Q_subMatrix);
+    // // QR shift lets us "attack" the first diagonal to speed up the algorithm
+    // shift = Matrix<rows, rows>::Identity() * Ak[rows - 1][rows - 1];
+    (Ak - shift).QRDecomposition(Q, R);
+    Ak = R * Q + shift;
+    QQ = QQ * Q;
+
+    // Check convergence: off-diagonal norm
+    float offDiagSum = 0.0f;
+    for (uint32_t row = 1; row < rows; row++) {
+      for (uint32_t column = 0; column < row; column++) {
+        offDiagSum += fabs(Ak[row][column]);
       }
     }
 
+    if (offDiagSum < tolerance) {
+      break;
+    }
+  }
+
+  // Diagonal elements are the eigenvalues
+  for (uint8_t i = 0; i < rows; i++) {
+    eigenValues[i][0] = Ak[i][i];
+  }
+  eigenVectors = QQ;
+}
+
 #endif // MATRIX_H_

src/Matrix.hpp

@@ -1,8 +1,8 @@
-#ifndef MATRIX_H_
-#define MATRIX_H_
+#pragma once
 
 #include <array>
 #include <cstdint>
+#include <string>
 
 // TODO: Add a function to calculate eigenvalues/vectors
 // TODO: Add a function to compute RREF
@@ -11,16 +11,13 @@
 
 template <uint8_t rows, uint8_t columns> class Matrix {
 public:
+  static_assert(rows > 0, "Template error: rows must be greater than 0.");
+  static_assert(columns > 0, "Template error: columns must be greater than 0.");
   /**
    * @brief create a matrix but leave all of its values unitialized
    */
   Matrix() = default;
 
-  /**
-   * @brief Create a matrix but fill all of its entries with one value
-   */
-  Matrix(float value);
-
   /**
    * @brief Initialize a matrix with an array
    */
@@ -36,6 +33,11 @@ public:
    */
   template <typename... Args> Matrix(Args... args);
 
+  /**
+   * @brief Create an identity matrix
+   */
+  static Matrix<rows, columns> Identity();
+
   /**
    * @brief Set all elements in this to value
    */
@@ -112,79 +114,20 @@ public:
    * @param result A buffer to store the result into
    * @warning this is super slow! Only call it if you absolutely have to!!!
    */
-  Matrix<rows, columns> &Invert(Matrix<rows, columns> &result) const;
+  Matrix<rows, columns> Invert() const;
 
   /**
    * @brief Transpose this matrix
    * @param result A buffer to store the result into
    */
-  Matrix<columns, rows> &Transpose(Matrix<columns, rows> &result) const;
+  Matrix<columns, rows> Transpose() const;
 
   /**
-   * @brief reduce the matrix so the sum of its elements equal 1
+   * @brief Returns the euclidean magnitude of the matrix. Also known as the L2
+   * norm
    * @param result a buffer to store the result into
    */
-  Matrix<rows, columns> &Normalize(Matrix<rows, columns> &result) const;
-
-  /**
-   * @brief return an identity matrix of the specified size
-   */
-  static Matrix<rows, rows> Eye();
-
-  /**
-   * @brief write a copy of a sub matrix into the given result matrix.
-   * @param rowIndex The row index to start the copy from
-   * @param columnIndex the column index to start the copy from
-   * @param result the matrix buffer to write the sub matrix into. The size of
-   * the matrix buffer allows the function to determine the end indices of the
-   * sub matrix
-   */
-  template <uint8_t subRows, uint8_t subColumns>
-  Matrix<subRows, subColumns> &
-  SubMatrix(uint8_t rowIndex, uint8_t columnIndex,
-            Matrix<subRows, subColumns> &result) const {
-    return result;
-  }
-
-  /**
-   * @brief write a copy of a sub matrix into this matrix starting at the given
-   * idnex.
-   * @param rowIndex The row index to start the copy from
-   * @param columnIndex the column index to start the copy from
-   * @param subMatrix The submatrix to copy into this matrix. The size of
-   * the matrix buffer allows the function to determine the end indices of the
-   * sub matrix
-   */
-  template <uint8_t subRows, uint8_t subColumns>
-  void CopySubMatrixInto(uint8_t rowIndex, uint8_t columnIndex,
-                         const Matrix<subRows, subColumns> &subMatrix) {}
-
-  /**
-   * @brief Returns the norm of the matrix
-   */
-  float Norm() { return 0; }
-
-  template <uint8_t vec1Length, uint8_t vec2Length>
-  static Matrix<vec1Length, vec2Length> &
-  OuterProduct(const Matrix<1, vec1Length> &vec1,
-               const Matrix<1, vec2Length> &vec2,
-               Matrix<vec1Length, vec2Length> &result) {
-    return result;
-  }
-
-  template <uint8_t vec1Length, uint8_t vec2Length>
-  static Matrix<vec1Length, vec2Length> &
-  OuterProduct(const Matrix<vec1Length, 1> &vec1,
-               const Matrix<vec2Length, 1> &vec2,
-               Matrix<vec1Length, vec2Length> &result) {
-    return result;
-  }
-  /**
-   * @brief Calulcate the QR decomposition of a matrix
-   * @param Q the
-   */
-  void QR_Decomposition(Matrix<rows, columns> &Q,
-                        Matrix<rows, columns> &R) const;
+  float EuclideanNorm() const;
 
   /**
    * @brief Get a row from the matrix
@@ -211,8 +154,16 @@ public:
    */
   constexpr uint8_t GetColumnSize() { return columns; }
 
+  /**
+   * @brief Write a string representation of the matrix into the buffer
+   */
   void ToString(std::string &stringBuffer) const;
 
+  /**
+   * @brief Returns the internal representation of the matrix as an array
+   */
+  const float *ToArray() const;
+
   /**
    * @brief Get an element from the matrix
    * @param row the row index of the element
@@ -221,10 +172,6 @@ public:
    */
   float Get(uint8_t row_index, uint8_t column_index) const;
 
-  // *******************************************************
-  // ************** OPERATOR OVERRIDES *********************
-  // *******************************************************
-
   /**
    * @brief get the specified row of the matrix returned as a reference to the
    * internal array
@@ -243,29 +190,68 @@ public:
 
   Matrix<rows, columns> operator-(const Matrix<rows, columns> &other) const;
 
-  Matrix<rows, columns> operator*(const Matrix<rows, columns> &other) const;
+  template <uint8_t other_columns>
+  Matrix<rows, other_columns>
+  operator*(const Matrix<columns, other_columns> &other) const;
 
   Matrix<rows, columns> operator*(float scalar) const;
 
-private:
+  Matrix<rows, columns> operator/(float scalar) const;
+
+  template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
+            uint8_t column_offset>
+  Matrix<sub_rows, sub_columns> SubMatrix() const;
+
+  template <uint8_t sub_rows, uint8_t sub_columns>
+  void SetSubMatrix(uint8_t rowOffset, uint8_t columnOffset,
+                    const Matrix<sub_rows, sub_columns> &sub_matrix);
+
   /**
    * @brief take the dot product of the two vectors
    */
   template <uint8_t vector_size>
-  static float dotProduct(const Matrix<1, vector_size> &vec1,
+  static float DotProduct(const Matrix<1, vector_size> &vec1,
                           const Matrix<1, vector_size> &vec2);
 
   template <uint8_t vector_size>
-  static float dotProduct(const Matrix<vector_size, 1> &vec1,
+  static float DotProduct(const Matrix<vector_size, 1> &vec1,
                           const Matrix<vector_size, 1> &vec2);
 
+  static float DotProduct(const Matrix<1, 1> &vec1, const Matrix<1, 1> &vec2) {
+    return vec1.Get(0, 0) * vec2.Get(0, 0);
+  }
+
+  /**
+   * @brief Performs QR decomposition on this matrix
+   * @param Q a buffer that will contain Q after the function completes
+   * @param R a buffer that will contain R after the function completes
+   */
+  void QRDecomposition(Matrix<rows, columns> &Q,
+                       Matrix<columns, columns> &R) const;
+
+  /**
+   * @brief Uses QR decomposition to efficiently calculate the eigenvectors
+   * and values of this matrix
+   * @param eigenVectors a buffer that will contain the eigenvectors fo this
+   * matrix
+   * @param eigenValues a buffer that will contain the eigenValues fo this
+   * matrix
+   * @param maxIterations the number of iterations to perform before giving
+   * up on reaching the given tolerance
+   * @param tolerance the level of accuracy to obtain before stopping.
+   */
+  void EigenQR(Matrix<rows, rows> &eigenVectors, Matrix<rows, 1> &eigenValues,
+               uint32_t maxIterations = 1000, float tolerance = 1e-6f) const;
+
+protected:
+  std::array<float, rows * columns> matrix;
+
+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;
 };
 
+#ifndef MATRIX_H_
 #include "Matrix.cpp"
-
 #endif // MATRIX_H_

src/Quaternion.cpp

@@ -0,0 +1,120 @@
+#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 / axis.EuclideanNorm();
+  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) {
+    return this->matrix[index];
+  }
+
+  // index out of bounds
+  return 1e+6;
+}
+
+void Quaternion::operator=(const Quaternion &other) {
+  memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
+}
+
+Quaternion Quaternion::operator*(const Quaternion &other) const {
+  Quaternion result{};
+  this->Q_Mult(other, result);
+  return result;
+}
+
+Quaternion Quaternion::operator*(float scalar) const {
+  return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar,
+                    this->v3 * scalar};
+}
+
+Quaternion Quaternion::operator+(const Quaternion &other) const {
+  return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2,
+                    this->v3 + other.v3};
+}
+
+Quaternion &Quaternion::Q_Mult(const Quaternion &other,
+                               Quaternion &buffer) const {
+
+  // eq. 6
+  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->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;
+}
+
+void Quaternion::Normalize() {
+  float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
+                         this->v3 * this->v3 + this->w * this->w);
+  if (magnitude == 0) {
+    return;
+  }
+  this->v1 /= magnitude;
+  this->v2 /= magnitude;
+  this->v3 /= magnitude;
+  this->w /= magnitude;
+}
+
+Matrix<3, 3> Quaternion::ToRotationMatrix() const {
+  float xx = this->v1 * this->v1;
+  float yy = this->v2 * this->v2;
+  float zz = this->v3 * this->v3;
+  Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
+                              2 * (this->v1 * this->v2 - this->v3 * this->w),
+                              2 * (this->v1 * this->v3 + this->v2 * this->w),
+                              2 * (this->v1 * this->v2 + this->v3 * this->w),
+                              1 - 2 * (xx - zz),
+                              2 * (this->v2 * this->v3 - this->v1 * this->w),
+                              2 * (this->v1 * this->v3 - this->v2 * this->w),
+                              2 * (this->v2 * this->v3 + this->v1 * this->w),
+                              1 - 2 * (xx - yy)};
+  return rotationMatrix;
+};
+
+Matrix<3, 1> Quaternion::ToEulerAngle() const {
+  float sqv1 = this->v1 * this->v1;
+  float sqv2 = this->v2 * this->v2;
+  float sqv3 = this->v3 * this->v3;
+  float sqw = this->w * this->w;
+
+  Matrix<3, 1> eulerAngle;
+  {
+    atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
+          (sqv1 - sqv2 - sqv3 + sqw));
+    asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) /
+         (sqv1 + sqv2 + sqv3 + sqw));
+    atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w),
+          (-sqv1 - sqv2 + sqv3 + sqw));
+  };
+  return eulerAngle;
+}

src/Quaternion.h

@@ -0,0 +1,90 @@
+#ifndef QUATERNION_H_
+#define QUATERNION_H_
+
+#include "Matrix.hpp"
+class Quaternion : public Matrix<1, 4> {
+public:
+  Quaternion() : Matrix<1, 4>() {}
+  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<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
+   * @return The value of the element at the index
+   */
+  float operator[](uint8_t index) const;
+
+  /**
+   * @brief Assign one quaternion to another
+   */
+  void operator=(const Quaternion &other);
+
+  /**
+   * @brief Do quaternion multiplication
+   */
+  Quaternion operator*(const Quaternion &other) const;
+
+  /**
+   * @brief Multiply the quaternion by a scalar
+   */
+  Quaternion operator*(float scalar) const;
+
+  /**
+   * @brief Add two quaternions together
+   * @param other The quaternion to add to this one
+   * @return The net quaternion
+   */
+  Quaternion operator+(const Quaternion &other) const;
+
+  /**
+   * @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 &Q_Mult(const Quaternion &other, Quaternion &buffer) const;
+
+  /**
+   * @brief Rotate a quaternion by this quaternion
+   * @param other The quaternion to rotate
+   * @param buffer The buffer to store the result in
+   *
+   */
+  Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
+
+  /**
+   * @brief Normalize the quaternion to a magnitude of 1
+   */
+  void Normalize();
+
+  /**
+   * @brief Convert the quaternion to a rotation matrix
+   * @return The rotation matrix
+   */
+  Matrix<3, 3> ToRotationMatrix() const;
+
+  /**
+   * @brief Convert the quaternion to an Euler angle representation
+   * @return The Euler angle representation of the quaternion
+   */
+  Matrix<3, 1> ToEulerAngle() const;
+
+  // Give people an easy way to access the elements
+  float &w{matrix[0]};
+  float &v1{matrix[1]};
+  float &v2{matrix[2]};
+  float &v3{matrix[3]};
+};
+
+#endif // QUATERNION_H_

src/Vector3D.cpp

@@ -0,0 +1,162 @@
+#ifdef VECTOR3D_H_ // since the .cpp file has to be included by the .hpp file this
+                   // will evaluate to true
+#include <cmath>
+#include <type_traits>
+#include <string>
+
+template <typename Type>
+V3D<Type>::V3D(const Matrix<1, 3> &other)
+{
+    this->x = other.Get(0, 0);
+    this->y = other.Get(0, 1);
+    this->z = other.Get(0, 2);
+}
+
+template <typename Type>
+V3D<Type>::V3D(const Matrix<3, 1> &other)
+{
+    this->x = other.Get(0, 0);
+    this->y = other.Get(1, 0);
+    this->z = other.Get(2, 0);
+}
+
+template <typename Type>
+V3D<Type>::V3D(const V3D &other) : x(other.x),
+                                   y(other.y),
+                                   z(other.z)
+{
+    static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
+}
+
+template <typename Type>
+V3D<Type>::V3D(Type x, Type y, Type z) : x(x),
+                                         y(y),
+                                         z(z)
+{
+    static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
+}
+
+template <typename Type>
+template <typename OtherType>
+V3D<Type>::V3D(const V3D<OtherType> &other)
+{
+    static_assert(std::is_arithmetic<Type>::value, "Type must be a number");
+    static_assert(std::is_arithmetic<OtherType>::value, "OtherType must be a number");
+    this->x = static_cast<Type>(other.x);
+    this->y = static_cast<Type>(other.y);
+    this->z = static_cast<Type>(other.z);
+}
+
+template <typename Type>
+std::array<Type, 3> V3D<Type>::ToArray() const
+{
+    return {this->x, this->y, this->z};
+}
+
+template <typename Type>
+void V3D<Type>::operator=(const V3D<Type> &other)
+{
+    this->x = other.x;
+    this->y = other.y;
+    this->z = other.z;
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator+(Type other) const
+{
+    return V3D<Type>{this->x + other, this->y + other, this->z + other};
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator+(const V3D<Type> &other) const
+{
+    return V3D<Type>{this->x + other.x, this->y + other.y, this->z + other.z};
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator-(Type other) const
+{
+    return V3D<Type>{this->x - other, this->y - other, this->z - other};
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator-(const V3D<Type> &other) const
+{
+    return V3D<Type>{this->x - other.x, this->y - other.y, this->z - other.z};
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator*(Type scalar) const
+{
+    return V3D<Type>{this->x * scalar, this->y * scalar, this->z * scalar};
+}
+
+template <typename Type>
+V3D<Type> V3D<Type>::operator/(Type scalar) const
+{
+    return V3D<Type>{this->x / scalar, this->y / scalar, this->z / scalar};
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator+=(Type other)
+{
+    *this = *this + other;
+    return *this;
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator+=(const V3D<Type> &other)
+{
+    *this = *this + other;
+    return *this;
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator-=(Type other)
+{
+    *this = *this - other;
+    return *this;
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator-=(const V3D<Type> &other)
+{
+    *this = *this - other;
+    return *this;
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator/=(Type scalar)
+{
+    if (scalar == 0)
+    {
+        return *this;
+    }
+    this->x /= scalar;
+    this->y /= scalar;
+    this->z /= scalar;
+    return *this;
+}
+
+template <typename Type>
+V3D<Type> &V3D<Type>::operator*=(Type scalar)
+{
+    this->x *= scalar;
+    this->y *= scalar;
+    this->z *= scalar;
+    return *this;
+}
+
+template <typename Type>
+bool V3D<Type>::operator==(const V3D<Type> &other)
+{
+    return this->x == other.x && this->y == other.y && this->z == other.z;
+}
+
+template <typename Type>
+float V3D<Type>::magnitude()
+{
+    return std::sqrt(static_cast<float>(this->x * this->x + this->y * this->y + this->z * this->z));
+}
+
+#endif // VECTOR3D_H_

src/Vector3D.hpp

@@ -0,0 +1,58 @@
+#ifndef VECTOR3D_H_
+#define VECTOR3D_H_
+
+#include <cstdint>
+#include "Matrix.hpp"
+
+template <typename Type>
+class V3D
+{
+public:
+    V3D(const Matrix<1, 3> &other);
+    V3D(const Matrix<3, 1> &other);
+
+    V3D(const V3D &other);
+
+    V3D(Type x = 0, Type y = 0, Type z = 0);
+
+    template <typename OtherType>
+    V3D(const V3D<OtherType> &other);
+
+    template <typename OtherType>
+    operator OtherType() const;
+
+    std::array<Type, 3> ToArray() const;
+
+    V3D<Type> operator+(Type other) const;
+    V3D<Type> operator+(const V3D<Type> &other) const;
+
+    V3D<Type> operator-(Type other) const;
+    V3D<Type> operator-(const V3D<Type> &other) const;
+
+    V3D<Type> operator*(Type scalar) const;
+
+    V3D<Type> operator/(Type scalar) const;
+
+    void operator=(const V3D<Type> &other);
+
+    V3D<Type> &operator+=(Type other);
+    V3D<Type> &operator+=(const V3D<Type> &other);
+
+    V3D<Type> &operator-=(Type other);
+    V3D<Type> &operator-=(const V3D<Type> &other);
+
+    V3D<Type> &operator/=(Type scalar);
+
+    V3D<Type> &operator*=(Type scalar);
+
+    bool operator==(const V3D<Type> &other);
+
+    float magnitude();
+
+    Type x;
+    Type y;
+    Type z;
+};
+
+#include "Vector3D.cpp"
+#endif // VECTOR3D_H_

unit-tests/CMakeLists.txt

@@ -1,21 +1,35 @@
-cmake_minimum_required (VERSION 3.11)
+# Quaternion tests
+add_executable(quaternion-tests quaternion-tests.cpp)
  
-project ("test_driver")
- 
-include(FetchContent)
- 
-FetchContent_Declare(
-    Catch2
-    GIT_REPOSITORY https://github.com/catchorg/Catch2.git
-    GIT_TAG v3.0.1 # or a later release
+target_link_libraries(quaternion-tests 
+    PRIVATE 
+    quaternion
+    Catch2::Catch2WithMain
 )
 
-FetchContent_MakeAvailable(Catch2)
- 
+# matrix tests
 add_executable(matrix-tests matrix-tests.cpp)
  
 target_link_libraries(matrix-tests 
     PRIVATE 
-    Matrix
+    matrix
+    Catch2::Catch2WithMain
+)
+
+# matrix timing tests
+add_executable(matrix-timing-tests matrix-timing-tests.cpp)
+ 
+target_link_libraries(matrix-timing-tests 
+    PRIVATE 
+    matrix
+    Catch2::Catch2WithMain
+)
+
+# Vector 3D Tests
+add_executable(vector-3d-tests vector-tests.cpp)
+
+target_link_libraries(vector-3d-tests
+    PRIVATE
+    vector-3d
     Catch2::Catch2WithMain
 )

unit-tests/matrix-test-timings.txt

@@ -1,14 +0,0 @@
-Addition: 0.419 s
-Subtraction: 0.421 s
-Multiplication: 3.297 s
-Scalar Multiplication: 0.329 s
-Element Multiply: 0.306 s
-Element Divide: 0.302 s
-Minor Matrix: 0.331 s
-Determinant: 0.177 s
-Matrix of Minors: 0.766 s
-Invert: 0.183 s
-Transpose: 0.215 s
-Normalize: 0.315 s
-GET ROW: 0.008 s
-GET COLUMN: 0.43 s

unit-tests/matrix-tests.cpp

@@ -10,41 +10,61 @@
 #include <cmath>
 #include <iostream>
 
+// Helper functions
+template <uint8_t rows, uint8_t columns>
+float matrixSum(const Matrix<rows, columns> &matrix) {
+  float sum = 0;
+  for (uint32_t i = 0; i < rows * columns; i++) {
+    float number = matrix.ToArray()[i];
+    sum += number * number;
+  }
+  return std::sqrt(sum);
+}
+
+template <uint8_t rows, uint8_t columns>
+void printLabeledMatrix(const std::string &label,
+                        const Matrix<rows, columns> &matrix) {
+  std::string strBuf = "";
+  matrix.ToString(strBuf);
+  std::cout << label << ":\n" << strBuf << std::endl;
+}
+
+TEST_CASE("Initialization", "Matrix") {
+  SECTION("Array Initialization") {
+    std::array<float, 4> arr2{5, 6, 7, 8};
+    Matrix<2, 2> mat2{arr2};
+    // array initialization
+    REQUIRE(mat2.Get(0, 0) == 5);
+    REQUIRE(mat2.Get(0, 1) == 6);
+    REQUIRE(mat2.Get(1, 0) == 7);
+    REQUIRE(mat2.Get(1, 1) == 8);
+  }
+
+  SECTION("Argument Pack Initialization") {
+    Matrix<2, 2> mat1{1, 2, 3, 4};
+    // template pack initialization
+    REQUIRE(mat1.Get(0, 0) == 1);
+    REQUIRE(mat1.Get(0, 1) == 2);
+    REQUIRE(mat1.Get(1, 0) == 3);
+    REQUIRE(mat1.Get(1, 1) == 4);
+  }
+
+  SECTION("Single Argument Pack Initialization") {
+    Matrix<2, 2> mat1{2};
+    // template pack initialization
+    REQUIRE(mat1.Get(0, 0) == 2);
+    REQUIRE(mat1.Get(0, 1) == 2);
+    REQUIRE(mat1.Get(1, 0) == 2);
+    REQUIRE(mat1.Get(1, 1) == 2);
+  }
+}
+
 TEST_CASE("Elementary Matrix Operations", "Matrix") {
   std::array<float, 4> arr2{5, 6, 7, 8};
   Matrix<2, 2> mat1{1, 2, 3, 4};
   Matrix<2, 2> mat2{arr2};
   Matrix<2, 2> mat3{};
 
-  SECTION("Initialization") {
-    // array initialization
-    REQUIRE(mat1.Get(0, 0) == 1);
-    REQUIRE(mat1.Get(0, 1) == 2);
-    REQUIRE(mat1.Get(1, 0) == 3);
-    REQUIRE(mat1.Get(1, 1) == 4);
-
-    // empty initialization
-    REQUIRE(mat3.Get(0, 0) == 0);
-    REQUIRE(mat3.Get(0, 1) == 0);
-    REQUIRE(mat3.Get(1, 0) == 0);
-    REQUIRE(mat3.Get(1, 1) == 0);
-
-    // template pack initialization
-    REQUIRE(mat2.Get(0, 0) == 5);
-    REQUIRE(mat2.Get(0, 1) == 6);
-    REQUIRE(mat2.Get(1, 0) == 7);
-    REQUIRE(mat2.Get(1, 1) == 8);
-
-    // large matrix
-    Matrix<255, 255> mat6{};
-    mat6.Fill(4);
-    for (uint8_t row{0}; row < 255; row++) {
-      for (uint8_t column{0}; column < 255; column++) {
-        REQUIRE(mat6.Get(row, column) == 4);
-      }
-    }
-  }
-
   SECTION("Fill") {
     mat1.Fill(0);
     REQUIRE(mat1.Get(0, 0) == 0);
@@ -66,10 +86,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
   }
 
   SECTION("Addition") {
-    std::string strBuf1 = "";
-    mat1.ToString(strBuf1);
-    std::cout << "Matrix 1:\n" << strBuf1 << std::endl;
-
     mat1.Add(mat2, mat3);
 
     REQUIRE(mat3.Get(0, 0) == 6);
@@ -118,6 +134,36 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(mat3.Get(0, 1) == 22);
     REQUIRE(mat3.Get(1, 0) == 43);
     REQUIRE(mat3.Get(1, 1) == 50);
+
+    // Non-square multiplication
+    Matrix<2, 4> mat4{1, 2, 3, 4, 5, 6, 7, 8};
+    Matrix<4, 2> mat5{9, 10, 11, 12, 13, 14, 15, 16};
+    Matrix<2, 2> mat6{};
+    mat6 = mat4 * mat5;
+    REQUIRE(mat6.Get(0, 0) == 130);
+    REQUIRE(mat6.Get(0, 1) == 140);
+    REQUIRE(mat6.Get(1, 0) == 322);
+    REQUIRE(mat6.Get(1, 1) == 348);
+
+    // One more non-square multiplicaiton
+    Matrix<4, 4> mat7{};
+    mat7 = mat5 * mat4;
+    REQUIRE(mat7.Get(0, 0) == 59);
+    REQUIRE(mat7.Get(0, 1) == 78);
+    REQUIRE(mat7.Get(0, 2) == 97);
+    REQUIRE(mat7.Get(0, 3) == 116);
+    REQUIRE(mat7.Get(1, 0) == 71);
+    REQUIRE(mat7.Get(1, 1) == 94);
+    REQUIRE(mat7.Get(1, 2) == 117);
+    REQUIRE(mat7.Get(1, 3) == 140);
+    REQUIRE(mat7.Get(2, 0) == 83);
+    REQUIRE(mat7.Get(2, 1) == 110);
+    REQUIRE(mat7.Get(2, 2) == 137);
+    REQUIRE(mat7.Get(2, 3) == 164);
+    REQUIRE(mat7.Get(3, 0) == 95);
+    REQUIRE(mat7.Get(3, 1) == 126);
+    REQUIRE(mat7.Get(3, 2) == 157);
+    REQUIRE(mat7.Get(3, 3) == 188);
   }
 
   SECTION("Scalar Multiplication") {
@@ -182,18 +228,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(minorMat4.Get(1, 0) == 4);
     REQUIRE(minorMat4.Get(1, 1) == 5);
   }
-  SECTION("Identity Matrix") {
-    Matrix<3, 3> mat4{Matrix<3, 3>::Eye()};
-    REQUIRE(mat4.Get(0, 0) == 1);
-    REQUIRE(mat4.Get(0, 1) == 0);
-    REQUIRE(mat4.Get(0, 2) == 0);
-    REQUIRE(mat4.Get(1, 0) == 0);
-    REQUIRE(mat4.Get(1, 1) == 1);
-    REQUIRE(mat4.Get(1, 2) == 0);
-    REQUIRE(mat4.Get(2, 0) == 0);
-    REQUIRE(mat4.Get(2, 1) == 0);
-    REQUIRE(mat4.Get(2, 2) == 1);
-  }
 
   SECTION("Determinant") {
     float det1 = mat1.Det();
@@ -234,7 +268,7 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
   }
 
   SECTION("Invert") {
-    mat1.Invert(mat3);
+    mat3 = mat1.Invert();
     REQUIRE_THAT(mat3.Get(0, 0), Catch::Matchers::WithinRel(-2.0F, 1e-6f));
     REQUIRE_THAT(mat3.Get(0, 1), Catch::Matchers::WithinRel(1.0F, 1e-6f));
     REQUIRE_THAT(mat3.Get(1, 0), Catch::Matchers::WithinRel(1.5F, 1e-6f));
@@ -243,7 +277,7 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
 
   SECTION("Transpose") {
     // transpose a square matrix
-    mat1.Transpose(mat3);
+    mat3 = mat1.Transpose();
 
     REQUIRE(mat3.Get(0, 0) == 1);
     REQUIRE(mat3.Get(0, 1) == 3);
@@ -254,7 +288,7 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     Matrix<2, 3> mat4{1, 2, 3, 4, 5, 6};
     Matrix<3, 2> mat5{};
 
-    mat4.Transpose(mat5);
+    mat5 = mat4.Transpose();
 
     REQUIRE(mat5.Get(0, 0) == 1);
     REQUIRE(mat5.Get(0, 1) == 4);
@@ -264,26 +298,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(mat5.Get(2, 1) == 6);
   }
 
-  SECTION("Normalize") {
-    mat1.Normalize(mat3);
-
-    float sqrt_30{sqrt(30)};
-
-    REQUIRE(mat3.Get(0, 0) == 1 / sqrt_30);
-    REQUIRE(mat3.Get(0, 1) == 2 / sqrt_30);
-    REQUIRE(mat3.Get(1, 0) == 3 / sqrt_30);
-    REQUIRE(mat3.Get(1, 1) == 4 / sqrt_30);
-
-    Matrix<2, 1> mat4{-0.878877044, 2.92092276};
-    Matrix<2, 1> mat5{};
-    mat4.Normalize(mat5);
-
-    REQUIRE_THAT(mat5.Get(0, 0),
-                 Catch::Matchers::WithinRel(-0.288129855179f, 1e-6f));
-    REQUIRE_THAT(mat5.Get(1, 0),
-                 Catch::Matchers::WithinRel(0.957591346325f, 1e-6f));
-  }
-
   SECTION("GET ROW") {
     Matrix<1, 2> mat1Rows{};
     mat1.GetRow(0, mat1Rows);
@@ -305,113 +319,322 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(mat1Columns.Get(0, 0) == 2);
     REQUIRE(mat1Columns.Get(1, 0) == 4);
   }
+
+  SECTION("Get Sub-Matrices") {
+    Matrix<3, 3> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9};
+
+    Matrix<2, 2> mat5 = mat4.SubMatrix<2, 2, 0, 0>();
+
+    REQUIRE(mat5.Get(0, 0) == 1);
+    REQUIRE(mat5.Get(0, 1) == 2);
+    REQUIRE(mat5.Get(1, 0) == 4);
+    REQUIRE(mat5.Get(1, 1) == 5);
+
+    mat5 = mat4.SubMatrix<2, 2, 1, 1>();
+    REQUIRE(mat5.Get(0, 0) == 5);
+    REQUIRE(mat5.Get(0, 1) == 6);
+    REQUIRE(mat5.Get(1, 0) == 8);
+    REQUIRE(mat5.Get(1, 1) == 9);
+
+    Matrix<3, 1> mat6 = mat4.SubMatrix<3, 1, 0, 0>();
+    REQUIRE(mat6.Get(0, 0) == 1);
+    REQUIRE(mat6.Get(1, 0) == 4);
+    REQUIRE(mat6.Get(2, 0) == 7);
+
+    Matrix<1, 3> mat7 = mat4.SubMatrix<1, 3, 0, 0>();
+    REQUIRE(mat7.Get(0, 0) == 1);
+    REQUIRE(mat7.Get(0, 1) == 2);
+    REQUIRE(mat7.Get(0, 2) == 3);
   }
 
-// basically re-run all of the previous tests with huge matrices and time the
-// results.
-TEST_CASE("Timing Tests", "Matrix") {
-  std::array<float, 50 * 50> arr1{};
-  for (uint16_t i{0}; i < 50 * 50; i++) {
-    arr1[i] = i;
-  }
-  std::array<float, 50 * 50> arr2{5, 6, 7, 8};
-  for (uint16_t i{50 * 50}; i < 2 * 50 * 50; i++) {
-    arr2[i] = i;
-  }
-  Matrix<50, 50> mat1{arr1};
-  Matrix<50, 50> mat2{arr2};
-  Matrix<50, 50> mat3{};
+  SECTION("Set Sub-Matrices") {
+    Matrix<3, 3> startMatrix{1, 2, 3, 4, 5, 6, 7, 8, 9};
+    Matrix<3, 3> mat4 = startMatrix;
 
-  // A smaller matrix to use for really badly optimized operations
-  Matrix<4, 4> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
-  Matrix<4, 4> mat5{};
+    Matrix<2, 2> mat5{10, 11, 12, 13};
+    mat4.SetSubMatrix(0, 0, mat5);
+    REQUIRE(mat4.Get(0, 0) == 10);
+    REQUIRE(mat4.Get(0, 1) == 11);
+    REQUIRE(mat4.Get(1, 0) == 12);
+    REQUIRE(mat4.Get(1, 1) == 13);
 
-  SECTION("Addition") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat3 = mat1 + mat2;
+    mat4 = startMatrix;
+    mat4.SetSubMatrix(1, 1, mat5);
+    REQUIRE(mat4.Get(1, 1) == 10);
+    REQUIRE(mat4.Get(1, 2) == 11);
+    REQUIRE(mat4.Get(2, 1) == 12);
+    REQUIRE(mat4.Get(2, 2) == 13);
+
+    Matrix<3, 1> mat6{10, 11, 12};
+    mat4.SetSubMatrix(0, 0, mat6);
+    REQUIRE(mat4.Get(0, 0) == 10);
+    REQUIRE(mat4.Get(1, 0) == 11);
+    REQUIRE(mat4.Get(2, 0) == 12);
+
+    Matrix<1, 3> mat7{10, 11, 12};
+    mat4.SetSubMatrix(0, 0, mat7);
+    REQUIRE(mat4.Get(0, 0) == 10);
+    REQUIRE(mat4.Get(0, 1) == 11);
+    REQUIRE(mat4.Get(0, 2) == 12);
   }
 }
 
-  SECTION("Subtraction") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat3 = mat1 - mat2;
+TEST_CASE("Identity Matrix", "Matrix") {
+  SECTION("Square Matrix") {
+    Matrix<5, 5> matrix = Matrix<5, 5>::Identity();
+    uint32_t oneColumnIndex{0};
+    for (uint32_t row = 0; row < 5; row++) {
+      for (uint32_t column = 0; column < 5; column++) {
+        float value = matrix[row][column];
+        if (oneColumnIndex == column) {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+        } else {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+        }
+      }
+      oneColumnIndex++;
     }
   }
 
-  SECTION("Multiplication") {
-    for (uint32_t i{0}; i < 1000; i++) {
-      mat3 = mat1 * mat2;
+  SECTION("Wide Matrix") {
+    Matrix<2, 5> matrix = Matrix<2, 5>::Identity();
+
+    uint32_t oneColumnIndex{0};
+    for (uint32_t row = 0; row < 2; row++) {
+      for (uint32_t column = 0; column < 5; column++) {
+        float value = matrix[row][column];
+        if (oneColumnIndex == column && row < 3) {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+        } else {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+        }
+      }
+      oneColumnIndex++;
     }
   }
 
-  SECTION("Scalar Multiplication") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat3 = mat1 * 3;
+  SECTION("Tall Matrix") {
+    Matrix<5, 2> matrix = Matrix<5, 2>::Identity();
+    uint32_t oneColumnIndex{0};
+    for (uint32_t row = 0; row < 5; row++) {
+      for (uint32_t column = 0; column < 2; column++) {
+        float value = matrix[row][column];
+        if (oneColumnIndex == column) {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+        } else {
+          REQUIRE_THAT(value, Catch::Matchers::WithinRel(0.0f, 1e-6f));
+        }
+      }
+      oneColumnIndex++;
+    }
   }
 }
 
-  SECTION("Element Multiply") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat1.ElementMultiply(mat2, mat3);
+// TODO: Add test for scalar division
+TEST_CASE("Euclidean Norm", "Matrix") {
+
+  SECTION("2x2 Normalize") {
+    Matrix<2, 2> mat1{1, 2, 3, 4};
+    Matrix<2, 2> mat2{};
+
+    mat2 = mat1 / mat1.EuclideanNorm();
+
+    float sqrt_30{static_cast<float>(sqrt(30.0f))};
+
+    REQUIRE(mat2.Get(0, 0) == 1 / sqrt_30);
+    REQUIRE(mat2.Get(0, 1) == 2 / sqrt_30);
+    REQUIRE(mat2.Get(1, 0) == 3 / sqrt_30);
+    REQUIRE(mat2.Get(1, 1) == 4 / sqrt_30);
+
+    REQUIRE_THAT(matrixSum(mat2), Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+
+  SECTION("2x1 (Vector) Normalize") {
+    Matrix<2, 1> mat1{-0.878877044, 2.92092276};
+    Matrix<2, 1> mat2{};
+    mat2 = mat1 / mat1.EuclideanNorm();
+
+    REQUIRE_THAT(mat2.Get(0, 0),
+                 Catch::Matchers::WithinRel(-0.288129855179f, 1e-6f));
+    REQUIRE_THAT(mat2.Get(1, 0),
+                 Catch::Matchers::WithinRel(0.957591346325f, 1e-6f));
+
+    float sum = matrixSum(mat2);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+  }
+
+  SECTION("Normalized vectors sum to 1") {
+    Matrix<9, 1> mat1{1, 2, 3, 4, 5, 6, 7, 8, 9};
+    Matrix<9, 1> mat2;
+    mat2 = mat1 / mat1.EuclideanNorm();
+    float sum = matrixSum(mat2);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
+
+    Matrix<2, 3> mat3{1, 2, 3, 4, 5, 6};
+    Matrix<2, 3> mat4{};
+    mat4 = mat3 / mat3.EuclideanNorm();
+    sum = matrixSum(mat4);
+    REQUIRE_THAT(sum, Catch::Matchers::WithinRel(1.0f, 1e-6f));
   }
 }
 
-  SECTION("Element Divide") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat1.ElementDivide(mat2, mat3);
+TEST_CASE("QR Decompositions", "Matrix") {
+  SECTION("2x2 QRDecomposition") {
+    Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
+    Matrix<2, 2> Q{}, R{};
+    A.QRDecomposition(Q, R);
+
+    // Check that Q * R ≈ A
+    Matrix<2, 2> QR{};
+    Q.Mult(R, QR);
+    for (int i = 0; i < 2; ++i) {
+      for (int j = 0; j < 2; ++j) {
+        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
       }
     }
 
-  SECTION("Minor Matrix") {
-    // what about matrices of 0,0 or 1,1?
-    // minor matrix for 2x2 matrix
-    Matrix<49, 49> minorMat1{};
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat1.MinorMatrix(minorMat1, 0, 0);
+    // Check that Q is orthonormal: Qᵀ * Q ≈ I
+    Matrix<2, 2> Qt = Q.Transpose();
+    Matrix<2, 2> QtQ{};
+    Qt.Mult(Q, QtQ);
+    for (int i = 0; i < 2; ++i) {
+      for (int j = 0; j < 2; ++j) {
+        if (i == j)
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
+        else
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
       }
     }
 
-  SECTION("Determinant") {
-    for (uint32_t i{0}; i < 100000; i++) {
-      float det1 = mat4.Det();
+    // Optional: R should be upper triangular
+    REQUIRE(std::fabs(R[1][0]) < 1e-4f);
+
+    // check that all Q values are correct
+    REQUIRE_THAT(Q[0][0], Catch::Matchers::WithinRel(0.3162f, 1e-4f));
+    REQUIRE_THAT(Q[0][1], Catch::Matchers::WithinRel(0.94868f, 1e-4f));
+    REQUIRE_THAT(Q[1][0], Catch::Matchers::WithinRel(0.94868f, 1e-4f));
+    REQUIRE_THAT(Q[1][1], Catch::Matchers::WithinRel(-0.3162f, 1e-4f));
+
+    // check that all R values are correct
+    REQUIRE_THAT(R[0][0], Catch::Matchers::WithinRel(3.16228f, 1e-4f));
+    REQUIRE_THAT(R[0][1], Catch::Matchers::WithinRel(4.42719f, 1e-4f));
+    REQUIRE_THAT(R[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
+    REQUIRE_THAT(R[1][1], Catch::Matchers::WithinRel(0.63246f, 1e-4f));
+  }
+
+  SECTION("3x3 QRDecomposition") {
+    // this symmetrix tridiagonal matrix is well behaved for testing
+    Matrix<3, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9};
+
+    Matrix<3, 3> Q{}, R{};
+    A.QRDecomposition(Q, R);
+
+    // Check that Q * R ≈ A
+    Matrix<3, 3> QR{};
+    QR = Q * R;
+    for (int i = 0; i < 3; ++i) {
+      for (int j = 0; j < 3; ++j) {
+        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
       }
     }
 
-  SECTION("Matrix of Minors") {
-    for (uint32_t i{0}; i < 100000; i++) {
-      mat4.MatrixOfMinors(mat5);
+    // Check that Qᵀ * Q ≈ I
+    // Since the rank of this matrix is 2, only the top left 2x2 sub-matrix will
+    // equal I.
+    Matrix<3, 3> Qt = Q.Transpose();
+    Matrix<3, 3> QtQ{};
+    QtQ = Qt * Q;
+    for (int i = 0; i < 2; ++i) {
+      for (int j = 0; j < 2; ++j) {
+        if (i == j)
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
+        else
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
       }
     }
 
-  SECTION("Invert") {
-    for (uint32_t i{0}; i < 100000; i++) {
-      mat4.Invert(mat5);
+    // Optional: Check R is upper triangular
+    for (int i = 1; i < 3; ++i) {
+      for (int j = 0; j < i; ++j) {
+        REQUIRE(std::fabs(R[i][j]) < 1e-4f);
       }
-  };
-
-  SECTION("Transpose") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat1.Transpose(mat3);
     }
   }
 
-  SECTION("Normalize") {
-    for (uint32_t i{0}; i < 10000; i++) {
-      mat1.Normalize(mat3);
+  SECTION("4x2 QRDecomposition") {
+    // A simple 4x2 matrix
+    Matrix<4, 2> A{1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f};
+
+    Matrix<4, 2> Q{};
+    Matrix<2, 2> R{};
+    A.QRDecomposition(Q, R);
+
+    // Check that Q * R ≈ A
+    Matrix<4, 2> QR{};
+    Q.Mult(R, QR);
+    for (int i = 0; i < 4; ++i) {
+      for (int j = 0; j < 2; ++j) {
+        REQUIRE_THAT(QR[i][j], Catch::Matchers::WithinRel(A[i][j], 1e-4f));
       }
     }
 
-  SECTION("GET ROW") {
-    Matrix<1, 50> mat1Rows{};
-    for (uint32_t i{0}; i < 1000000; i++) {
-      mat1.GetRow(0, mat1Rows);
+    // Check that Qᵀ * Q ≈ I₂
+    Matrix<2, 4> Qt = Q.Transpose();
+    Matrix<2, 2> QtQ{};
+    Qt.Mult(Q, QtQ);
+    for (int i = 0; i < 2; ++i) {
+      for (int j = 0; j < 2; ++j) {
+        if (i == j)
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinRel(1.0f, 1e-4f));
+        else
+          REQUIRE_THAT(QtQ[i][j], Catch::Matchers::WithinAbs(0.0f, 1e-4f));
       }
     }
 
-  SECTION("GET COLUMN") {
-    Matrix<50, 1> mat1Columns{};
-    for (uint32_t i{0}; i < 1000000; i++) {
-      mat1.GetColumn(0, mat1Columns);
+    // Check R is upper triangular (i > j ⇒ R[i][j] ≈ 0)
+    for (int i = 1; i < 2; ++i) {
+      for (int j = 0; j < i; ++j) {
+        REQUIRE(std::fabs(R[i][j]) < 1e-4f);
       }
     }
   }
+}
+
+TEST_CASE("Eigenvalues and Vectors", "Matrix") {
+  SECTION("2x2 Eigen") {
+    Matrix<2, 2> A{1.0f, 2.0f, 3.0f, 4.0f};
+    Matrix<2, 2> vectors{};
+    Matrix<2, 1> values{};
+
+    A.EigenQR(vectors, values, 1000000, 1e-20f);
+
+    REQUIRE_THAT(vectors[0][0], Catch::Matchers::WithinRel(0.41597f, 1e-4f));
+    REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.90938f, 1e-4f));
+    REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(5.372282f, 1e-4f));
+    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinRel(-0.372281f, 1e-4f));
+  }
+
+  SECTION("3x3 Rank Defficient Eigen") {
+    SKIP("Skipping this because QR decomposition isn't ready for it");
+    // this symmetrix tridiagonal matrix is well behaved for testing
+    Matrix<3, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9};
+
+    Matrix<3, 3> vectors{};
+    Matrix<3, 1> values{};
+    A.EigenQR(vectors, values, 1000000, 1e-8f);
+
+    std::string strBuf1 = "";
+    vectors.ToString(strBuf1);
+    std::cout << "Vectors:\n" << strBuf1 << std::endl;
+    strBuf1 = "";
+    values.ToString(strBuf1);
+    std::cout << "Values:\n" << strBuf1 << std::endl;
+
+    REQUIRE_THAT(vectors[0][0], Catch::Matchers::WithinRel(0.23197f, 1e-4f));
+    REQUIRE_THAT(vectors[1][0], Catch::Matchers::WithinRel(0.525322f, 1e-4f));
+    REQUIRE_THAT(vectors[2][0], Catch::Matchers::WithinRel(0.81867f, 1e-4f));
+    REQUIRE_THAT(values[0][0], Catch::Matchers::WithinRel(-1.11684f, 1e-4f));
+    REQUIRE_THAT(values[1][0], Catch::Matchers::WithinRel(0.0f, 1e-4f));
+    REQUIRE_THAT(values[2][0], Catch::Matchers::WithinRel(16.1168f, 1e-4f));
+  }
+}

unit-tests/matrix-timing-tests.cpp

@@ -0,0 +1,127 @@
+// 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 "Matrix.hpp"
+
+// any other libraries
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+// basically re-run all of the matrix tests with huge matrices and time the
+// results.
+TEST_CASE("Timing Tests", "Matrix") {
+  std::array<float, 50 * 50> arr1{};
+  for (uint16_t i{0}; i < 50 * 50; i++) {
+    arr1[i] = i;
+  }
+  std::array<float, 50 * 50> arr2{5, 6, 7, 8};
+  for (uint16_t i{50 * 50}; i < 2 * 50 * 50; i++) {
+    arr2[i] = i;
+  }
+  Matrix<50, 50> mat1{arr1};
+  Matrix<50, 50> mat2{arr2};
+  Matrix<50, 50> mat3{};
+
+  // A smaller matrix to use for really badly optimized operations
+  Matrix<4, 4> mat4{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16};
+  Matrix<4, 4> mat5{};
+
+  SECTION("Addition") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat3 = mat1 + mat2;
+    }
+  }
+
+  SECTION("Subtraction") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat3 = mat1 - mat2;
+    }
+  }
+
+  SECTION("Multiplication") {
+    for (uint32_t i{0}; i < 1000; i++) {
+      mat3 = mat1 * mat2;
+    }
+  }
+
+  SECTION("Scalar Multiplication") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat3 = mat1 * 3;
+    }
+  }
+
+  SECTION("Element Multiply") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat1.ElementMultiply(mat2, mat3);
+    }
+  }
+
+  SECTION("Element Divide") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat1.ElementDivide(mat2, mat3);
+    }
+  }
+
+  SECTION("Minor Matrix") {
+    // what about matrices of 0,0 or 1,1?
+    // minor matrix for 2x2 matrix
+    Matrix<49, 49> minorMat1{};
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat1.MinorMatrix(minorMat1, 0, 0);
+    }
+  }
+
+  SECTION("Determinant") {
+    for (uint32_t i{0}; i < 1000000; i++) {
+      float det1 = mat4.Det();
+    }
+  }
+
+  SECTION("Matrix of Minors") {
+    for (uint32_t i{0}; i < 1000000; i++) {
+      mat4.MatrixOfMinors(mat5);
+    }
+  }
+
+  SECTION("Invert") {
+    for (uint32_t i{0}; i < 1000000; i++) {
+      mat5 = mat4.Invert();
+    }
+  };
+
+  SECTION("Transpose") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat3 = mat1.Transpose();
+    }
+  }
+
+  SECTION("Normalize") {
+    for (uint32_t i{0}; i < 100000; i++) {
+      mat3 = mat1 / mat1.EuclideanNorm();
+    }
+  }
+
+  SECTION("GET ROW") {
+    Matrix<1, 50> mat1Rows{};
+    for (uint32_t i{0}; i < 100000000; i++) {
+      mat1.GetRow(0, mat1Rows);
+    }
+  }
+
+  SECTION("GET COLUMN") {
+    Matrix<50, 1> mat1Columns{};
+    for (uint32_t i{0}; i < 100000000; i++) {
+      mat1.GetColumn(0, mat1Columns);
+    }
+  }
+
+  SECTION("QR Decomposition") {
+    Matrix<50, 50> Q, R{};
+    for (uint32_t i{0}; i < 500; i++) {
+      mat1.QRDecomposition(Q, R);
+    }
+  }
+}

unit-tests/quaternion-tests.cpp

@@ -0,0 +1,103 @@
+// 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.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);
+
+        // copy initialization
+        Quaternion q4{q1};
+        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.w == 1);
+        REQUIRE(q5.v1 == 2);
+        REQUIRE(q5.v2 == 3);
+        REQUIRE(q5.v3 == 4);
+
+        // array initialization
+        Quaternion q6{std::array<float, 4>{1, 2, 3, 4}};
+        REQUIRE(q6.w == 1);
+        REQUIRE(q6.v1 == 2);
+        REQUIRE(q6.v2 == 3);
+        REQUIRE(q6.v3 == 4);
+    }
+
+    SECTION("Equals")
+    {
+        Quaternion q3{0, 0, 0, 0};
+        q3 = q1;
+        REQUIRE(q3.w == 1);
+        REQUIRE(q3.v1 == 2);
+        REQUIRE(q3.v2 == 3);
+        REQUIRE(q3.v3 == 4);
+    }
+
+    SECTION("Array access")
+    {
+        REQUIRE(q1[0] == 1);
+        REQUIRE(q1[1] == 2);
+        REQUIRE(q1[2] == 3);
+        REQUIRE(q1[3] == 4);
+    }
+
+    SECTION("Addition")
+    {
+        Quaternion q3 = q1 + q2;
+        REQUIRE(q3.w == 6);
+        REQUIRE(q3.v1 == 8);
+        REQUIRE(q3.v2 == 10);
+        REQUIRE(q3.v3 == 12);
+    }
+
+    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::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));
+    }
+}

unit-tests/run-matrix-timing-test.sh

@@ -1,7 +0,0 @@
-# be in the root folder of this project when you run this
-cd build/
-ninja matrix-tests
-echo "Running tests. This will take a while."
-./unit-tests/matrix-tests -n "Timing Tests" -d yes > ../unit-tests/matrix-test-timings-temp.txt
-cd ../unit-tests/
-python3 test-timing-post-process.py

unit-tests/timing-results/matrix-timing-tests.txt

@@ -0,0 +1,36 @@
+Running matrix-timing-tests with timing
+Randomness seeded to: 3567651885
+1.857 s: Addition
+1.857 s: Timing Tests
+1.788 s: Subtraction
+1.788 s: Timing Tests
+1.929 s: Multiplication
+1.929 s: Timing Tests
+1.268 s: Scalar Multiplication
+1.268 s: Timing Tests
+1.798 s: Element Multiply
+1.798 s: Timing Tests
+1.802 s: Element Divide
+1.803 s: Timing Tests
+1.553 s: Minor Matrix
+1.554 s: Timing Tests
+1.009 s: Determinant
+1.009 s: Timing Tests
+4.076 s: Matrix of Minors
+4.076 s: Timing Tests
+1.066 s: Invert
+1.066 s: Timing Tests
+1.246 s: Transpose
+1.246 s: Timing Tests
+2.284 s: Normalize
+2.284 s: Timing Tests
+0.606 s: GET ROW
+0.606 s: Timing Tests
+24.629 s: GET COLUMN
+24.630 s: Timing Tests
+3.064 s: QR Decomposition
+3.064 s: Timing Tests
+===============================================================================
+test cases: 1 | 1 passed
+assertions: - none -
+

unit-tests/vector-tests.cpp

@@ -0,0 +1,45 @@
+// 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 "Vector3D.hpp"
+#include "Matrix.hpp"
+
+// any other libraries
+#include <array>
+#include <cmath>
+#include <iostream>
+
+TEST_CASE("Vector Math", "Vector")
+{
+    V3D<float> v1{1, 2, 3};
+    V3D<float> v2{4, 5, 6};
+    V3D<float> v3{};
+
+    SECTION("Initialization")
+    {
+        // list initialization
+        REQUIRE(v1.x == 1);
+        REQUIRE(v1.y == 2);
+        REQUIRE(v1.z == 3);
+
+        // copy initialization
+        V3D<float> v4{v2};
+        REQUIRE(v4.x == 4);
+        REQUIRE(v4.y == 5);
+        REQUIRE(v4.z == 6);
+
+        // empty initialization
+        REQUIRE(v3.x == 0);
+        REQUIRE(v3.y == 0);
+        REQUIRE(v3.z == 0);
+
+        // matrix initialization
+        Matrix<1, 3> mat1{v1.ToArray()};
+        V3D<float> v5{mat1};
+        REQUIRE(v5.x == v1.x);
+        REQUIRE(v5.y == v1.y);
+        REQUIRE(v5.z == v1.z);
+    }
+}