Update matrix-timing-tests timings [skip ci]

Timings get auto-comitted

Update matrix-timing-tests timings [skip ci]

Updated readme

Update matrix-timing-tests timings [skip ci]

Fixing auto-checkout issues

.gitea/workflows/Merge-Checker.yaml

@@ -3,9 +3,11 @@ name: Merge-Checker
 on:
   pull_request:
     branches: ["**"]
+    paths-ignore:
+        - 'unit-tests/timing-results/**'
 
 jobs:
-  build_and_test:
+  Benchmarking:
     runs-on: ubuntu-latest
 
     steps:
@@ -26,16 +28,6 @@ jobs:
       - 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
@@ -99,4 +91,69 @@ jobs:
             echo "timings_changed=true" >> $GITHUB_OUTPUT
           else
             echo "timings_changed=false" >> $GITHUB_OUTPUT
-          fi
+          fi
+
+      - name: Commit and push timing results
+        if: steps.check_diff.outputs.timings_changed == 'true' && github.event.pull_request.head.repo.full_name == github.repository
+        run: |
+          git config --global user.name "ci-bot"
+          git config --global user.email "ci-bot@local"
+
+          BRANCH_NAME="${{ github.event.pull_request.head.ref }}"
+          git stash
+          echo "Checking out source branch $BRANCH_NAME"
+          git fetch origin "$BRANCH_NAME"
+          git checkout "$BRANCH_NAME"
+          git pull
+
+          echo "Checking if last commit was a timing update"
+          LAST_COMMIT_MSG=$(git log -1 --pretty=%B)
+
+          if echo "$LAST_COMMIT_MSG" | grep -q "Update matrix-timing-tests timings"; then
+            echo "Last commit was a timing update, skipping commit."
+            exit 0
+          else
+            echo "Last commit name was: $LAST_COMMIT_MSG"
+            git stash pop
+          fi
+
+          git add unit-tests/timing-results/matrix-timing-tests.txt
+
+          if git diff --quiet --cached; then
+            echo "No changes to commit"
+          else
+            git commit -m "Update matrix-timing-tests timings [skip ci]"
+            git push origin "$BRANCH_NAME"
+          fi
+
+  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

.vscode/settings.json

@@ -75,9 +75,5 @@
     },
     "clangd.enable": true,
     "C_Cpp.dimInactiveRegions": false,
-    "editor.defaultFormatter": "xaver.clang-format",
+    "editor.defaultFormatter": "xaver.clang-format"
-    "clangd.inactiveRegions.useBackgroundHighlight": false,
-    "clangd.arguments": [
-        "--compile-commands-dir=${workspaceFolder}/build"
-    ],
 }

CMakeLists.txt

@@ -6,9 +6,7 @@ add_subdirectory(unit-tests)
 
 set(CMAKE_CXX_STANDARD 11)
 
-add_compile_options(-Wall -Wextra -Wpedantic)
+add_compile_options(-fdiagnostics-color=always -Wall -Wextra -Wpedantic)
-add_compile_options (-fdiagnostics-color=always)
-set(CMAKE_COLOR_DIAGNOSTICS ON)
 
 include(FetchContent)
  

README.md

@@ -2,11 +2,4 @@
 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
+There are still several operations that are works in progress
-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`

src/Matrix.cpp

@@ -1,10 +1,3 @@
-// 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"
@@ -12,28 +5,29 @@
 #include <algorithm>
 #include <cmath>
 #include <cstdlib>
+#include <type_traits>
 #include <cstring>
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array) {
+Matrix<rows, columns>::Matrix(float value)
+{
+  this->Fill(value);
+}
+
+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) {
+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()));
@@ -41,19 +35,22 @@ Matrix<rows, columns>::Matrix(Args... args) {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> Matrix<rows, columns>::Identity() {
+void Matrix<rows, columns>::Identity()
-  Matrix<rows, columns> identityMatrix{0};
+{
-  uint32_t minDimension = std::min(rows, columns);
+  this->Fill(0);
-  for (uint8_t idx{0}; idx < minDimension; idx++) {
+  for (uint8_t idx{0}; idx < rows; idx++)
-    identityMatrix[idx][idx] = 1;
+  {
+    this->matrix[idx * columns + idx] = 1;
   }
-  return identityMatrix;
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
+Matrix<rows, columns>::Matrix(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++) {
+  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);
     }
@@ -62,15 +59,21 @@ Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
 
 template <uint8_t rows, uint8_t columns>
 void Matrix<rows, columns>::setMatrixToArray(
-    const std::array<float, rows * columns> &array) {
+    const std::array<float, rows * columns> &array)
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
+  {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+    {
       uint16_t array_idx =
           static_cast<uint16_t>(row_idx) * static_cast<uint16_t>(columns) +
           static_cast<uint16_t>(column_idx);
-      if (array_idx < array.size()) {
+      if (array_idx < array.size())
+      {
         this->matrix[row_idx * columns + column_idx] = array[array_idx];
-      } else {
+      }
+      else
+      {
         this->matrix[row_idx * columns + column_idx] = 0;
       }
     }
@@ -80,9 +83,12 @@ void Matrix<rows, columns>::setMatrixToArray(
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
 Matrix<rows, columns>::Add(const Matrix<rows, columns> &other,
-                           Matrix<rows, columns> &result) const {
+                           Matrix<rows, columns> &result) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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) + other.Get(row_idx, column_idx);
     }
@@ -93,9 +99,12 @@ Matrix<rows, columns>::Add(const Matrix<rows, columns> &other,
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
 Matrix<rows, columns>::Sub(const Matrix<rows, columns> &other,
-                           Matrix<rows, columns> &result) const {
+                           Matrix<rows, columns> &result) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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) - other.Get(row_idx, column_idx);
     }
@@ -108,15 +117,18 @@ template <uint8_t rows, uint8_t columns>
 template <uint8_t other_columns>
 Matrix<rows, other_columns> &
 Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
-                            Matrix<rows, other_columns> &result) const {
+                            Matrix<rows, other_columns> &result) const
+{
   // allocate some buffers for all of our dot products
   Matrix<1, columns> this_row;
   Matrix<columns, 1> other_column;
 
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+  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 < other_columns; column_idx++) {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+    {
       // get the other matrix'ss column
       other.GetColumn(column_idx, other_column);
 
@@ -131,9 +143,12 @@ Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
 
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
-Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const {
+Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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) * scalar;
     }
   }
@@ -142,7 +157,9 @@ 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() 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,
@@ -152,7 +169,8 @@ Matrix<rows, columns> Matrix<rows, columns>::Invert() const {
   // unfortunately we can't calculate this at compile time so we'll just reurn
   // zeros
   float determinant{this->Det()};
-  if (determinant == 0) {
+  if (determinant == 0)
+  {
     // you can't invert a matrix with a negative determinant
     result.Fill(0);
     return result;
@@ -177,10 +195,14 @@ Matrix<rows, columns> Matrix<rows, columns>::Invert() const {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<columns, rows> Matrix<rows, columns>::Transpose() 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 column_idx{0}; column_idx < rows; column_idx++)
-    for (uint8_t row_idx{0}; row_idx < columns; row_idx++) {
+  {
+    for (uint8_t row_idx{0}; row_idx < columns; row_idx++)
+    {
       result[row_idx][column_idx] = this->Get(column_idx, row_idx);
     }
   }
@@ -192,19 +214,24 @@ Matrix<columns, rows> Matrix<rows, columns>::Transpose() const {
 // the fastest way to calculate a 2x2 matrix determinant
 // template <>
 // inline float Matrix<0, 0>::Det() const { return 1e+6; }
-template <> inline float Matrix<1, 1>::Det() const { return this->matrix[0]; }
+template <>
-template <> inline float Matrix<2, 2>::Det() const {
+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];
 }
 
 template <uint8_t rows, uint8_t columns>
-float Matrix<rows, columns>::Det() const {
+float Matrix<rows, columns>::Det() const
+{
   static_assert(rows == columns,
                 "You can't take the determinant of a non-square matrix.");
 
   Matrix<rows - 1, columns - 1> MinorMatrix{};
   float determinant{0};
-  for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+  {
     // for odd indices the sign is negative
     float sign = (column_idx % 2 == 0) ? 1 : -1;
     determinant += sign * this->matrix[column_idx] *
@@ -217,9 +244,12 @@ float Matrix<rows, columns>::Det() const {
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
 Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other,
-                                       Matrix<rows, columns> &result) const {
+                                       Matrix<rows, columns> &result) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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) * other.Get(row_idx, column_idx);
     }
@@ -231,9 +261,12 @@ Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other,
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
 Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other,
-                                     Matrix<rows, columns> &result) const {
+                                     Matrix<rows, columns> &result) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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) / other.Get(row_idx, column_idx);
     }
@@ -244,8 +277,10 @@ Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other,
 
 template <uint8_t rows, uint8_t columns>
 float Matrix<rows, columns>::Get(uint8_t row_index,
-                                 uint8_t column_index) const {
+                                 uint8_t column_index) const
-  if (row_index > rows - 1 || column_index > columns - 1) {
+{
+  if (row_index > rows - 1 || column_index > columns - 1)
+  {
     return 1e+10; // TODO: We should throw something here instead of failing
                   // quietly
   }
@@ -255,7 +290,8 @@ float Matrix<rows, columns>::Get(uint8_t row_index,
 template <uint8_t rows, uint8_t columns>
 Matrix<1, columns> &
 Matrix<rows, columns>::GetRow(uint8_t row_index,
-                              Matrix<1, columns> &row) const {
+                              Matrix<1, columns> &row) const
+{
   memcpy(&(row[0]), this->matrix.begin() + row_index * columns,
          columns * sizeof(float));
 
@@ -265,8 +301,10 @@ Matrix<rows, columns>::GetRow(uint8_t row_index,
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, 1> &
 Matrix<rows, columns>::GetColumn(uint8_t column_index,
-                                 Matrix<rows, 1> &column) const {
+                                 Matrix<rows, 1> &column) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
+  {
     column[row_idx][0] = this->Get(row_idx, column_index);
   }
 
@@ -274,13 +312,17 @@ Matrix<rows, columns>::GetColumn(uint8_t column_index,
 }
 
 template <uint8_t rows, uint8_t columns>
-void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
+void Matrix<rows, columns>::ToString(std::string &stringBuffer) const
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
+  {
     stringBuffer += "|";
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+    {
       stringBuffer +=
           std::to_string(this->matrix[row_idx * columns + column_idx]);
-      if (column_idx != columns - 1) {
+      if (column_idx != columns - 1)
+      {
         stringBuffer += "\t";
       }
     }
@@ -289,14 +331,11 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-const float *Matrix<rows, columns>::ToArray() const {
+std::array<float, columns> &Matrix<rows, columns>::
-  return this->matrix.data();
+operator[](uint8_t row_index)
-}
+{
-
+  if (row_index > rows - 1)
-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;
   }
@@ -307,8 +346,9 @@ Matrix<rows, columns>::operator[](uint8_t row_index) {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> &
+Matrix<rows, columns> &Matrix<rows, columns>::
-Matrix<rows, columns>::operator=(const Matrix<rows, columns> &other) {
+operator=(const Matrix<rows, columns> &other)
+{
   memcpy(this->matrix.begin(), other.matrix.begin(),
          rows * columns * sizeof(float));
 
@@ -317,16 +357,18 @@ Matrix<rows, columns>::operator=(const Matrix<rows, columns> &other) {
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns>
+Matrix<rows, columns> Matrix<rows, columns>::
-Matrix<rows, columns>::operator+(const Matrix<rows, columns> &other) const {
+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> Matrix<rows, columns>::
-Matrix<rows, columns>::operator-(const Matrix<rows, columns> &other) const {
+operator-(const Matrix<rows, columns> &other) const
+{
   Matrix<rows, columns> buffer{};
   this->Sub(other, buffer);
   return buffer;
@@ -334,42 +376,30 @@ Matrix<rows, columns>::operator-(const Matrix<rows, columns> &other) const {
 
 template <uint8_t rows, uint8_t columns>
 template <uint8_t other_columns>
-Matrix<rows, other_columns> Matrix<rows, columns>::operator*(
+Matrix<rows, other_columns> Matrix<rows, columns>::
-    const Matrix<columns, other_columns> &other) const {
+operator*(const Matrix<columns, other_columns> &other) const
+{
   Matrix<rows, other_columns> buffer{};
   this->Mult(other, buffer);
   return buffer;
 }
 
 template <uint8_t rows, uint8_t columns>
-Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const {
+Matrix<rows, columns> Matrix<rows, columns>::operator*(float scalar) const
+{
   Matrix<rows, columns> buffer{};
   this->Mult(scalar, buffer);
   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,
-                                        const Matrix<1, vector_size> &vec2) {
+                                        const Matrix<1, vector_size> &vec2)
+{
   float sum{0};
-  for (uint8_t i{0}; i < vector_size; i++) {
+  for (uint8_t i{0}; i < vector_size; i++)
+  {
     sum += vec1.Get(0, i) * vec2.Get(0, i);
   }
 
@@ -379,9 +409,11 @@ 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,
-                                        const Matrix<vector_size, 1> &vec2) {
+                                        const Matrix<vector_size, 1> &vec2)
+{
   float sum{0};
-  for (uint8_t i{0}; i < vector_size; i++) {
+  for (uint8_t i{0}; i < vector_size; i++)
+  {
     sum += vec1.Get(i, 0) * vec2.Get(i, 0);
   }
 
@@ -389,9 +421,12 @@ 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) {
+void Matrix<rows, columns>::Fill(float value)
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+{
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  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;
     }
   }
@@ -399,11 +434,14 @@ void Matrix<rows, columns>::Fill(float value) {
 
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
-Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const {
+Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const
+{
   Matrix<rows - 1, columns - 1> MinorMatrix{};
 
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+    {
       this->MinorMatrix(MinorMatrix, row_idx, column_idx);
       result[row_idx][column_idx] = MinorMatrix.Det();
     }
@@ -415,15 +453,20 @@ Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const {
 template <uint8_t rows, uint8_t columns>
 Matrix<rows - 1, columns - 1> &
 Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result,
-                                   uint8_t row_idx, uint8_t column_idx) const {
+                                   uint8_t row_idx, uint8_t column_idx) const
+{
   std::array<float, (rows - 1) * (columns - 1)> subArray{};
   uint16_t array_idx{0};
-  for (uint8_t row_iter{0}; row_iter < rows; row_iter++) {
+  for (uint8_t row_iter{0}; row_iter < rows; row_iter++)
-    if (row_iter == row_idx) {
+  {
+    if (row_iter == row_idx)
+    {
       continue;
     }
-    for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
+    for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
-      if (column_iter == column_idx) {
+    {
+      if (column_iter == column_idx)
+      {
         continue;
       }
       subArray[array_idx] = this->Get(row_iter, column_iter);
@@ -437,9 +480,12 @@ Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result,
 
 template <uint8_t rows, uint8_t columns>
 Matrix<rows, columns> &
-Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
+Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const
-  for (uint8_t row_iter{0}; row_iter < rows; row_iter++) {
+{
-    for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
+  for (uint8_t row_iter{0}; row_iter < rows; row_iter++)
+  {
+    for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
+    {
       float sign = ((row_iter + 1) % 2) == 0 ? -1 : 1;
       sign *= ((column_iter + 1) % 2) == 0 ? -1 : 1;
       result[column_iter][row_iter] = this->Get(row_iter, column_iter) * sign;
@@ -450,34 +496,55 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const {
 }
 
 template <uint8_t rows, uint8_t columns>
-float Matrix<rows, columns>::EuclideanNorm() const {
+Matrix<rows, columns> &
-
+Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const
+{
   float sum{0};
-  for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
+  for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
-    for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
+  {
+    for (uint8_t column_idx{0}; column_idx < columns; column_idx++)
+    {
       float val{this->Get(row_idx, column_idx)};
       sum += val * val;
     }
   }
 
-  return sqrt(sum);
+  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;
 }
 
 template <uint8_t rows, uint8_t columns>
-template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
+template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset>
-          uint8_t column_offset>
+Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const
-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(
+  static_assert(sub_columns + column_offset <= columns,
-      sub_columns + column_offset <= columns,
+                "The submatrix you're trying to get is out of bounds (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 row_idx{0}; row_idx < sub_rows; row_idx++)
-    for (uint8_t column_idx{0}; column_idx < sub_columns; column_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);
     }
@@ -486,121 +553,21 @@ Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const {
 }
 
 template <uint8_t rows, uint8_t columns>
-template <uint8_t sub_rows, uint8_t sub_columns>
+template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset>
-void Matrix<rows, columns>::SetSubMatrix(
+void Matrix<rows, columns>::SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix)
-    uint8_t rowOffset, uint8_t columnOffset,
+{
-    const Matrix<sub_rows, sub_columns> &sub_matrix) {
+  static_assert(sub_rows + row_offset <= rows,
-  int16_t adjustedSubRows = sub_rows;
+                "The submatrix you're trying to set is out of bounds (rows)");
-  int16_t adjustedSubColumns = sub_columns;
+  static_assert(sub_columns + column_offset <= columns,
-  int16_t adjustedRowOffset = rowOffset;
+                "The submatrix you're trying to set is out of bounds (columns)");
-  int16_t adjustedColumnOffset = columnOffset;
 
-  // a bunch of safety checks to make sure we don't overflow the matrix
+  for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++)
-  if (sub_rows > rows) {
+  {
-    adjustedSubRows = rows;
+    for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++)
-  }
+    {
-  if (sub_columns > columns) {
+      this->matrix[(row_idx + row_offset) * columns + column_idx + column_offset] = sub_matrix.Get(row_idx, column_idx);
-    adjustedSubColumns = columns;
-  }
-
-  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 {
-      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);
-    }
-  }
-}
-
-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");
-
-  Matrix<rows, rows> Ak = *this; // Copy original matrix
-  Matrix<rows, rows> QQ{Matrix<rows, rows>::Identity()};
-  Matrix<rows, rows> shift{0};
-
-  for (uint32_t iter = 0; iter < maxIterations; ++iter) {
-    Matrix<rows, rows> Q, R;
-
-    // // 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,4 +1,5 @@
-#pragma once
+#ifndef MATRIX_H_
+#define MATRIX_H_
 
 #include <array>
 #include <cstdint>
@@ -18,6 +19,11 @@ public:
    */
   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
    */
@@ -34,9 +40,9 @@ public:
   template <typename... Args> Matrix(Args... args);
 
   /**
-   * @brief Create an identity matrix
+   * @brief set the matrix diagonals to 1 and all other values to 0
    */
-  static Matrix<rows, columns> Identity();
+  void Identity();
 
   /**
    * @brief Set all elements in this to value
@@ -123,11 +129,10 @@ public:
   Matrix<columns, rows> Transpose() const;
 
   /**
-   * @brief Returns the euclidean magnitude of the matrix. Also known as the L2
+   * @brief reduce the matrix so the sum of its elements equal 1
-   * norm
    * @param result a buffer to store the result into
    */
-  float EuclideanNorm() const;
+  Matrix<rows, columns> &Normalize(Matrix<rows, columns> &result) const;
 
   /**
    * @brief Get a row from the matrix
@@ -154,16 +159,8 @@ 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
@@ -196,15 +193,13 @@ public:
 
   Matrix<rows, columns> operator*(float scalar) const;
 
-  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>
+  template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
-  void SetSubMatrix(uint8_t rowOffset, uint8_t columnOffset,
+            uint8_t column_offset>
-                    const Matrix<sub_rows, sub_columns> &sub_matrix);
+  void SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix);
 
   /**
    * @brief take the dot product of the two vectors
@@ -221,28 +216,6 @@ public:
     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;
 
@@ -252,6 +225,6 @@ private:
   void setMatrixToArray(const std::array<float, rows * columns> &array);
 };
 
-#ifndef MATRIX_H_
 #include "Matrix.cpp"
+
 #endif // MATRIX_H_

src/Quaternion.cpp

@@ -6,115 +6,115 @@
  * @param angle The angle to rotate by
  * @param axis The axis to rotate around
  */
-Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis) {
+Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis)
-  const float halfAngle = angle / 2;
+{
-  const float sinHalfAngle = sin(halfAngle);
+    const float halfAngle = angle / 2;
-  Matrix<1, 3> normalizedAxis = axis / axis.EuclideanNorm();
+    const float sinHalfAngle = sin(halfAngle);
-  return Quaternion{static_cast<float>(cos(halfAngle)),
+    Matrix<1, 3> normalizedAxis{};
-                    normalizedAxis.Get(0, 0) * sinHalfAngle,
+    axis.Normalize(normalizedAxis);
-                    normalizedAxis.Get(0, 1) * sinHalfAngle,
+    return Quaternion{
-                    normalizedAxis.Get(0, 2) * sinHalfAngle};
+        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 {
+float Quaternion::operator[](uint8_t index) const
-  if (index < 4) {
+{
-    return this->matrix[index];
+    if (index < 4)
-  }
+    {
+        return this->matrix[index];
+    }
 
-  // index out of bounds
+    // index out of bounds
-  return 1e+6;
+    return 1e+6;
 }
 
-void Quaternion::operator=(const Quaternion &other) {
+void Quaternion::operator=(const Quaternion &other)
-  memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
+{
+    memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
 }
 
-Quaternion Quaternion::operator*(const Quaternion &other) const {
+Quaternion Quaternion::operator*(const Quaternion &other) const
-  Quaternion result{};
+{
-  this->Q_Mult(other, result);
+    Quaternion result{};
-  return result;
+    this->Q_Mult(other, result);
+    return result;
 }
 
-Quaternion Quaternion::operator*(float scalar) const {
+Quaternion Quaternion::operator*(float scalar) const
-  return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar,
+{
-                    this->v3 * scalar};
+    return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * scalar};
 }
 
-Quaternion Quaternion::operator+(const Quaternion &other) const {
+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};
+    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 &
-                               Quaternion &buffer) const {
+Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const
+{
 
-  // eq. 6
+    // eq. 6
-  buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 -
+    buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3);
-              other.v3 * this->v3);
+    buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2);
-  buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 +
+    buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1);
-               other.v3 * this->v2);
+    buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w);
-  buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w -
+    return buffer;
-               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 &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
-  Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
+{
-  buffer.v1 = other.v1;
+    Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
-  buffer.v2 = other.v2;
+    buffer.v1 = other.v1;
-  buffer.v3 = other.v3;
+    buffer.v2 = other.v2;
-  buffer.w = 0;
+    buffer.v3 = other.v3;
+    buffer.w = 0;
 
-  Quaternion temp{};
+    Quaternion temp{};
-  this->Q_Mult(buffer, temp);
+    this->Q_Mult(buffer, temp);
-  temp.Q_Mult(prime, buffer);
+    temp.Q_Mult(prime, buffer);
-  return buffer;
+    return buffer;
 }
 
-void Quaternion::Normalize() {
+void Quaternion::Normalize()
-  float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
+{
-                         this->v3 * this->v3 + this->w * this->w);
+    float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
-  if (magnitude == 0) {
+    if (magnitude == 0)
-    return;
+    {
-  }
+        return;
-  this->v1 /= magnitude;
+    }
-  this->v2 /= magnitude;
+    this->v1 /= magnitude;
-  this->v3 /= magnitude;
+    this->v2 /= magnitude;
-  this->w /= magnitude;
+    this->v3 /= magnitude;
+    this->w /= magnitude;
 }
 
-Matrix<3, 3> Quaternion::ToRotationMatrix() const {
+Matrix<3, 3> Quaternion::ToRotationMatrix() const
-  float xx = this->v1 * this->v1;
+{
-  float yy = this->v2 * this->v2;
+    float xx = this->v1 * this->v1;
-  float zz = this->v3 * this->v3;
+    float yy = this->v2 * this->v2;
-  Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
+    float zz = this->v3 * this->v3;
-                              2 * (this->v1 * this->v2 - this->v3 * this->w),
+    Matrix<3, 3> rotationMatrix{
-                              2 * (this->v1 * this->v3 + this->v2 * this->w),
+        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),
+        2 * (this->v1 * this->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 2 * (this->v2 * this->v3 - this->v1 * this->w),
-                              1 - 2 * (xx - zz),
+        2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)};
-                              2 * (this->v2 * this->v3 - this->v1 * this->w),
+    return rotationMatrix;
-                              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 {
+Matrix<3, 1> Quaternion::ToEulerAngle() const
-  float sqv1 = this->v1 * this->v1;
+{
-  float sqv2 = this->v2 * this->v2;
+    float sqv1 = this->v1 * this->v1;
-  float sqv3 = this->v3 * this->v3;
+    float sqv2 = this->v2 * this->v2;
-  float sqw = this->w * this->w;
+    float sqv3 = this->v3 * this->v3;
+    float sqw = this->w * this->w;
 
-  Matrix<3, 1> eulerAngle;
+    Matrix<3, 1> eulerAngle;
-  {
+    {
-    atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
+        atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw));
-          (sqv1 - sqv2 - sqv3 + sqw));
+        asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw));
-    asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) /
+        atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw));
-         (sqv1 + sqv2 + sqv3 + sqw));
+    };
-    atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w),
+    return eulerAngle;
-          (-sqv1 - sqv2 + sqv3 + sqw));
-  };
-  return eulerAngle;
 }

src/Quaternion.h

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

unit-tests/matrix-tests.cpp

@@ -10,61 +10,41 @@
 #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);
@@ -86,6 +66,10 @@ 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);
@@ -135,35 +119,7 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     REQUIRE(mat3.Get(1, 0) == 43);
     REQUIRE(mat3.Get(1, 1) == 50);
 
-    // Non-square multiplication
+    // TODO: You need to add non-square multiplications to this.
-    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") {
@@ -298,6 +254,26 @@ 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);
@@ -352,289 +328,29 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
     Matrix<3, 3> mat4 = startMatrix;
 
     Matrix<2, 2> mat5{10, 11, 12, 13};
-    mat4.SetSubMatrix(0, 0, mat5);
+    mat4.SetSubMatrix<2, 2, 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);
 
     mat4 = startMatrix;
-    mat4.SetSubMatrix(1, 1, mat5);
+    mat4.SetSubMatrix<2, 2, 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);
+    mat4.SetSubMatrix<3, 1, 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);
+    mat4.SetSubMatrix<1, 3, 0, 0>(mat7);
     REQUIRE(mat4.Get(0, 0) == 10);
     REQUIRE(mat4.Get(0, 1) == 11);
     REQUIRE(mat4.Get(0, 2) == 12);
   }
-}
-
-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("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("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++;
-    }
-  }
-}
-
-// 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));
-  }
-}
-
-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));
-      }
-    }
-
-    // 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));
-      }
-    }
-
-    // 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));
-      }
-    }
-
-    // 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));
-      }
-    }
-
-    // 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("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));
-      }
-    }
-
-    // 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));
-      }
-    }
-
-    // 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

@@ -8,7 +8,6 @@
 // any other libraries
 #include <array>
 #include <cmath>
-#include <cstdint>
 
 // basically re-run all of the matrix tests with huge matrices and time the
 // results.
@@ -30,13 +29,13 @@ TEST_CASE("Timing Tests", "Matrix") {
   Matrix<4, 4> mat5{};
 
   SECTION("Addition") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat3 = mat1 + mat2;
     }
   }
 
   SECTION("Subtraction") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat3 = mat1 - mat2;
     }
   }
@@ -48,19 +47,19 @@ TEST_CASE("Timing Tests", "Matrix") {
   }
 
   SECTION("Scalar Multiplication") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat3 = mat1 * 3;
     }
   }
 
   SECTION("Element Multiply") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat1.ElementMultiply(mat2, mat3);
     }
   }
 
   SECTION("Element Divide") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat1.ElementDivide(mat2, mat3);
     }
   }
@@ -69,59 +68,52 @@ TEST_CASE("Timing Tests", "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++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat1.MinorMatrix(minorMat1, 0, 0);
     }
   }
 
   SECTION("Determinant") {
-    for (uint32_t i{0}; i < 1000000; i++) {
+    for (uint32_t i{0}; i < 100000; i++) {
       float det1 = mat4.Det();
     }
   }
 
   SECTION("Matrix of Minors") {
-    for (uint32_t i{0}; i < 1000000; i++) {
+    for (uint32_t i{0}; i < 100000; i++) {
       mat4.MatrixOfMinors(mat5);
     }
   }
 
   SECTION("Invert") {
-    for (uint32_t i{0}; i < 1000000; i++) {
+    for (uint32_t i{0}; i < 100000; i++) {
       mat5 = mat4.Invert();
     }
   };
 
   SECTION("Transpose") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
       mat3 = mat1.Transpose();
     }
   }
 
   SECTION("Normalize") {
-    for (uint32_t i{0}; i < 100000; i++) {
+    for (uint32_t i{0}; i < 10000; i++) {
-      mat3 = mat1 / mat1.EuclideanNorm();
+      mat1.Normalize(mat3);
     }
   }
 
   SECTION("GET ROW") {
     Matrix<1, 50> mat1Rows{};
-    for (uint32_t i{0}; i < 100000000; i++) {
+    for (uint32_t i{0}; i < 1000000; i++) {
       mat1.GetRow(0, mat1Rows);
     }
   }
 
   SECTION("GET COLUMN") {
     Matrix<50, 1> mat1Columns{};
-    for (uint32_t i{0}; i < 100000000; i++) {
+    for (uint32_t i{0}; i < 1000000; 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/timing-results/matrix-timing-tests.txt

@@ -1,36 +1,56 @@
-Running matrix-timing-tests with timing
+Randomness seeded to: 3576947534
-Randomness seeded to: 3567651885
+0.177 s: Addition
-1.857 s: Addition
+0.178 s: Timing Tests
-1.857 s: Timing Tests
+0.182 s: Subtraction
-1.788 s: Subtraction
+0.182 s: Timing Tests
-1.788 s: Timing Tests
+1.889 s: Multiplication
-1.929 s: Multiplication
+1.889 s: Timing Tests
-1.929 s: Timing Tests
+0.126 s: Scalar Multiplication
-1.268 s: Scalar Multiplication
+0.126 s: Timing Tests
-1.268 s: Timing Tests
+0.176 s: Element Multiply
-1.798 s: Element Multiply
+0.176 s: Timing Tests
-1.798 s: Timing Tests
+0.175 s: Element Divide
-1.802 s: Element Divide
+0.175 s: Timing Tests
-1.803 s: Timing Tests
+0.151 s: Minor Matrix
-1.553 s: Minor Matrix
+0.151 s: Timing Tests
-1.554 s: Timing Tests
+0.099 s: Determinant
-1.009 s: Determinant
+0.100 s: Timing Tests
-1.009 s: Timing Tests
+0.412 s: Matrix of Minors
-4.076 s: Matrix of Minors
+0.412 s: Timing Tests
-4.076 s: Timing Tests
+0.110 s: Invert
-1.066 s: Invert
+0.110 s: Timing Tests
-1.066 s: Timing Tests
+0.122 s: Transpose
-1.246 s: Transpose
+0.122 s: Timing Tests
-1.246 s: Timing Tests
+0.184 s: Normalize
-2.284 s: Normalize
+0.184 s: Timing Tests
-2.284 s: Timing Tests
+0.006 s: GET ROW
-0.606 s: GET ROW
+0.006 s: Timing Tests
-0.606 s: Timing Tests
+0.232 s: GET COLUMN
-24.629 s: GET COLUMN
+0.232 s: Timing Tests
-24.630 s: Timing Tests
-3.064 s: QR Decomposition
-3.064 s: Timing Tests
 ===============================================================================
 test cases: 1 | 1 passed
 assertions: - none -
 
+	Command being timed: "build/unit-tests/matrix-timing-tests -d yes"
+	User time (seconds): 4.03
+	System time (seconds): 0.00
+	Percent of CPU this job got: 99%
+	Elapsed (wall clock) time (h:mm:ss or m:ss): 0:04.04
+	Average shared text size (kbytes): 0
+	Average unshared data size (kbytes): 0
+	Average stack size (kbytes): 0
+	Average total size (kbytes): 0
+	Maximum resident set size (kbytes): 3200
+	Average resident set size (kbytes): 0
+	Major (requiring I/O) page faults: 184
+	Minor (reclaiming a frame) page faults: 174
+	Voluntary context switches: 1
+	Involuntary context switches: 53
+	Swaps: 0
+	File system inputs: 12
+	File system outputs: 1
+	Socket messages sent: 0
+	Socket messages received: 0
+	Signals delivered: 0
+	Page size (bytes): 4096
+	Exit status: 0