Cynopolis/Vector3D
1
0
Fork 0
Code Issues 3 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge pull request 'Working on adding efficient eigenvector and value calculations' (#2) from eigenvector-and-values into main

Browse Source 
Reviewed-on: #2
This commit was merged in pull request #2.
This commit is contained in:
Quinn
2025-06-06 22:32:18 +00:00
parent 1715d2b46c c099dfe760
commit 2a7eb93ebe
9 changed files with 797 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
.vscode/settings.json vendored
View File

@@ -75,5 +75,9 @@
}, },
"clangd.enable": true, "clangd.enable": true,
"C_Cpp.dimInactiveRegions": false, "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"
],
} }

4
CMakeLists.txt
View File

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

8
README.md
View File

@@ -3,7 +3,7 @@ This matrix math library is focused on embedded development and avoids any heap
It uses templates to pre-allocate matrices on the stack. It uses templates to pre-allocate matrices on the stack.
There are still several operations that are works in progress such as: There are still several operations that are works in progress such as:
TODO: Add a function to calculate eigenvalues/vectors - Add a function to calculate eigenvalues/vectors
TODO: Add a function to compute RREF - Add a function to compute RREF
TODO: Add a function for SVD decomposition - Add a function for SVD decomposition
TODO: Add a function for LQ decomposition - Add a function for LQ decomposition

451
src/Matrix.cpp
View File

@@ -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 #ifdef MATRIX_H_ // since the .cpp file has to be included by the .hpp file this
// will evaluate to true // will evaluate to true
#include "Matrix.hpp" #include "Matrix.hpp"
@@ -5,29 +12,28 @@
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
#include <cstdlib> #include <cstdlib>
#include <type_traits>
#include <cstring> #include <cstring>
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns>::Matrix(float value) Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array) {
{
this->Fill(value);
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns>::Matrix(const std::array<float, rows * columns> &array)
{
this->setMatrixToArray(array); this->setMatrixToArray(array);
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
template <typename... Args> template <typename... Args>
Matrix<rows, columns>::Matrix(Args... args) Matrix<rows, columns>::Matrix(Args... args) {
{
constexpr uint16_t arraySize{static_cast<uint16_t>(rows) * constexpr uint16_t arraySize{static_cast<uint16_t>(rows) *
static_cast<uint16_t>(columns)}; static_cast<uint16_t>(columns)};
std::initializer_list<float> initList{static_cast<float>(args)...}; 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 // choose whichever buffer size is smaller for the copy length
uint32_t minSize = uint32_t minSize =
std::min(arraySize, static_cast<uint16_t>(initList.size())); std::min(arraySize, static_cast<uint16_t>(initList.size()));
@@ -35,22 +41,19 @@ Matrix<rows, columns>::Matrix(Args... args)
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
void Matrix<rows, columns>::Identity() Matrix<rows, columns> Matrix<rows, columns>::Identity() {
{ Matrix<rows, columns> identityMatrix{0};
this->Fill(0); uint32_t minDimension = std::min(rows, columns);
for (uint8_t idx{0}; idx < rows; idx++) for (uint8_t idx{0}; idx < minDimension; idx++) {
{ identityMatrix[idx][idx] = 1;
this->matrix[idx * columns + idx] = 1;
} }
return identityMatrix;
} }
template <uint8_t rows, uint8_t columns> 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 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->matrix[row_idx * columns + column_idx] = this->matrix[row_idx * columns + column_idx] =
other.Get(row_idx, column_idx); other.Get(row_idx, column_idx);
} }
@@ -59,21 +62,15 @@ Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other)
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
void Matrix<rows, columns>::setMatrixToArray( 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 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++)
{
uint16_t array_idx = uint16_t array_idx =
static_cast<uint16_t>(row_idx) * static_cast<uint16_t>(columns) + static_cast<uint16_t>(row_idx) * static_cast<uint16_t>(columns) +
static_cast<uint16_t>(column_idx); 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]; this->matrix[row_idx * columns + column_idx] = array[array_idx];
} } else {
else
{
this->matrix[row_idx * columns + column_idx] = 0; this->matrix[row_idx * columns + column_idx] = 0;
} }
} }
@@ -83,12 +80,9 @@ void Matrix<rows, columns>::setMatrixToArray(
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> & Matrix<rows, columns> &
Matrix<rows, columns>::Add(const Matrix<rows, columns> &other, 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 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++)
{
result[row_idx][column_idx] = result[row_idx][column_idx] =
this->Get(row_idx, column_idx) + other.Get(row_idx, column_idx); this->Get(row_idx, column_idx) + other.Get(row_idx, column_idx);
} }
@@ -99,12 +93,9 @@ Matrix<rows, columns>::Add(const Matrix<rows, columns> &other,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> & Matrix<rows, columns> &
Matrix<rows, columns>::Sub(const Matrix<rows, columns> &other, 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 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++)
{
result[row_idx][column_idx] = result[row_idx][column_idx] =
this->Get(row_idx, column_idx) - other.Get(row_idx, column_idx); this->Get(row_idx, column_idx) - other.Get(row_idx, column_idx);
} }
@@ -117,18 +108,15 @@ template <uint8_t rows, uint8_t columns>
template <uint8_t other_columns> template <uint8_t other_columns>
Matrix<rows, other_columns> & Matrix<rows, other_columns> &
Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other, 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 // allocate some buffers for all of our dot products
Matrix<1, columns> this_row; Matrix<1, columns> this_row;
Matrix<columns, 1> other_column; 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 // get our row
this->GetRow(row_idx, this_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 // get the other matrix'ss column
other.GetColumn(column_idx, other_column); other.GetColumn(column_idx, other_column);
@@ -143,12 +131,9 @@ Matrix<rows, columns>::Mult(const Matrix<columns, other_columns> &other,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, 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 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++)
{
result[row_idx][column_idx] = this->Get(row_idx, column_idx) * scalar; result[row_idx][column_idx] = this->Get(row_idx, column_idx) * scalar;
} }
} }
@@ -157,9 +142,7 @@ Matrix<rows, columns>::Mult(float scalar, Matrix<rows, columns> &result) const
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> Matrix<rows, columns> Matrix<rows, columns>::Invert() const {
Matrix<rows, columns>::Invert() const
{
// since all matrix sizes have to be statically specified at compile time we // since all matrix sizes have to be statically specified at compile time we
// can do this // can do this
static_assert(rows == columns, static_assert(rows == columns,
@@ -169,8 +152,7 @@ Matrix<rows, columns>::Invert() const
// unfortunately we can't calculate this at compile time so we'll just reurn // unfortunately we can't calculate this at compile time so we'll just reurn
// zeros // zeros
float determinant{this->Det()}; float determinant{this->Det()};
if (determinant == 0) if (determinant == 0) {
{
// you can't invert a matrix with a negative determinant // you can't invert a matrix with a negative determinant
result.Fill(0); result.Fill(0);
return result; return result;
@@ -195,14 +177,10 @@ Matrix<rows, columns>::Invert() const
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<columns, rows> Matrix<columns, rows> Matrix<rows, columns>::Transpose() const {
Matrix<rows, columns>::Transpose() const
{
Matrix<columns, rows> result{}; 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); result[row_idx][column_idx] = this->Get(column_idx, row_idx);
} }
} }
@@ -214,24 +192,19 @@ Matrix<rows, columns>::Transpose() const
// the fastest way to calculate a 2x2 matrix determinant // the fastest way to calculate a 2x2 matrix determinant
// template <> // template <>
// inline float Matrix<0, 0>::Det() const { return 1e+6; } // inline float Matrix<0, 0>::Det() const { return 1e+6; }
template <> template <> inline float Matrix<1, 1>::Det() const { return this->matrix[0]; }
inline float Matrix<1, 1>::Det() const { return this->matrix[0]; } template <> inline float Matrix<2, 2>::Det() const {
template <>
inline float Matrix<2, 2>::Det() const
{
return this->matrix[0] * this->matrix[3] - this->matrix[1] * this->matrix[2]; return this->matrix[0] * this->matrix[3] - this->matrix[1] * this->matrix[2];
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
float Matrix<rows, columns>::Det() const float Matrix<rows, columns>::Det() const {
{
static_assert(rows == columns, static_assert(rows == columns,
"You can't take the determinant of a non-square matrix."); "You can't take the determinant of a non-square matrix.");
Matrix<rows - 1, columns - 1> MinorMatrix{}; Matrix<rows - 1, columns - 1> MinorMatrix{};
float determinant{0}; 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 // for odd indices the sign is negative
float sign = (column_idx % 2 == 0) ? 1 : -1; float sign = (column_idx % 2 == 0) ? 1 : -1;
determinant += sign * this->matrix[column_idx] * determinant += sign * this->matrix[column_idx] *
@@ -244,12 +217,9 @@ float Matrix<rows, columns>::Det() const
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> & Matrix<rows, columns> &
Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other, 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 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++)
{
result[row_idx][column_idx] = result[row_idx][column_idx] =
this->Get(row_idx, column_idx) * other.Get(row_idx, column_idx); this->Get(row_idx, column_idx) * other.Get(row_idx, column_idx);
} }
@@ -261,12 +231,9 @@ Matrix<rows, columns>::ElementMultiply(const Matrix<rows, columns> &other,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> & Matrix<rows, columns> &
Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other, 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 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++)
{
result[row_idx][column_idx] = result[row_idx][column_idx] =
this->Get(row_idx, column_idx) / other.Get(row_idx, column_idx); this->Get(row_idx, column_idx) / other.Get(row_idx, column_idx);
} }
@@ -277,10 +244,8 @@ Matrix<rows, columns>::ElementDivide(const Matrix<rows, columns> &other,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
float Matrix<rows, columns>::Get(uint8_t row_index, 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 return 1e+10; // TODO: We should throw something here instead of failing
// quietly // quietly
} }
@@ -290,8 +255,7 @@ float Matrix<rows, columns>::Get(uint8_t row_index,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<1, columns> & Matrix<1, columns> &
Matrix<rows, columns>::GetRow(uint8_t row_index, 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, memcpy(&(row[0]), this->matrix.begin() + row_index * columns,
columns * sizeof(float)); columns * sizeof(float));
@@ -301,10 +265,8 @@ Matrix<rows, columns>::GetRow(uint8_t row_index,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, 1> & Matrix<rows, 1> &
Matrix<rows, columns>::GetColumn(uint8_t column_index, 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); column[row_idx][0] = this->Get(row_idx, column_index);
} }
@@ -312,17 +274,13 @@ Matrix<rows, columns>::GetColumn(uint8_t column_index,
} }
template <uint8_t rows, uint8_t columns> 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 += "|"; stringBuffer += "|";
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
{
stringBuffer += stringBuffer +=
std::to_string(this->matrix[row_idx * columns + column_idx]); std::to_string(this->matrix[row_idx * columns + column_idx]);
if (column_idx != columns - 1) if (column_idx != columns - 1) {
{
stringBuffer += "\t"; stringBuffer += "\t";
} }
} }
@@ -331,11 +289,14 @@ void Matrix<rows, columns>::ToString(std::string &stringBuffer) const
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
std::array<float, columns> &Matrix<rows, columns>:: const float *Matrix<rows, columns>::ToArray() const {
operator[](uint8_t row_index) return this->matrix.data();
{ }
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. // TODO: We should throw something here instead of failing quietly.
row_index = 0; row_index = 0;
} }
@@ -346,9 +307,8 @@ operator[](uint8_t row_index)
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> &Matrix<rows, columns>:: Matrix<rows, columns> &
operator=(const Matrix<rows, columns> &other) Matrix<rows, columns>::operator=(const Matrix<rows, columns> &other) {
{
memcpy(this->matrix.begin(), other.matrix.begin(), memcpy(this->matrix.begin(), other.matrix.begin(),
rows * columns * sizeof(float)); rows * columns * sizeof(float));
@@ -357,18 +317,16 @@ operator=(const Matrix<rows, columns> &other)
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> Matrix<rows, columns>:: Matrix<rows, columns>
operator+(const Matrix<rows, columns> &other) const Matrix<rows, columns>::operator+(const Matrix<rows, columns> &other) const {
{
Matrix<rows, columns> buffer{}; Matrix<rows, columns> buffer{};
this->Add(other, buffer); this->Add(other, buffer);
return buffer; return buffer;
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> Matrix<rows, columns>:: Matrix<rows, columns>
operator-(const Matrix<rows, columns> &other) const Matrix<rows, columns>::operator-(const Matrix<rows, columns> &other) const {
{
Matrix<rows, columns> buffer{}; Matrix<rows, columns> buffer{};
this->Sub(other, buffer); this->Sub(other, buffer);
return buffer; return buffer;
@@ -376,30 +334,42 @@ operator-(const Matrix<rows, columns> &other) const
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
template <uint8_t other_columns> template <uint8_t other_columns>
Matrix<rows, other_columns> Matrix<rows, columns>:: Matrix<rows, other_columns> Matrix<rows, columns>::operator*(
operator*(const Matrix<columns, other_columns> &other) const const Matrix<columns, other_columns> &other) const {
{
Matrix<rows, other_columns> buffer{}; Matrix<rows, other_columns> buffer{};
this->Mult(other, buffer); this->Mult(other, buffer);
return buffer; return buffer;
} }
template <uint8_t rows, uint8_t columns> 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{}; Matrix<rows, columns> buffer{};
this->Mult(scalar, buffer); this->Mult(scalar, buffer);
return 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 rows, uint8_t columns>
template <uint8_t vector_size> 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) const Matrix<1, vector_size> &vec2) {
{
float sum{0}; 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); sum += vec1.Get(0, i) * vec2.Get(0, i);
} }
@@ -409,11 +379,9 @@ float Matrix<rows, columns>::DotProduct(const Matrix<1, vector_size> &vec1,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
template <uint8_t vector_size> 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) const Matrix<vector_size, 1> &vec2) {
{
float sum{0}; 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); sum += vec1.Get(i, 0) * vec2.Get(i, 0);
} }
@@ -421,12 +389,9 @@ float Matrix<rows, columns>::DotProduct(const Matrix<vector_size, 1> &vec1,
} }
template <uint8_t rows, uint8_t columns> 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 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->matrix[row_idx * columns + column_idx] = value; this->matrix[row_idx * columns + column_idx] = value;
} }
} }
@@ -434,14 +399,11 @@ void Matrix<rows, columns>::Fill(float value)
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, 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{}; 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); this->MinorMatrix(MinorMatrix, row_idx, column_idx);
result[row_idx][column_idx] = MinorMatrix.Det(); result[row_idx][column_idx] = MinorMatrix.Det();
} }
@@ -453,20 +415,15 @@ Matrix<rows, columns>::MatrixOfMinors(Matrix<rows, columns> &result) const
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows - 1, columns - 1> & Matrix<rows - 1, columns - 1> &
Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result, 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{}; std::array<float, (rows - 1) * (columns - 1)> subArray{};
uint16_t array_idx{0}; 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; 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; continue;
} }
subArray[array_idx] = this->Get(row_iter, column_iter); subArray[array_idx] = this->Get(row_iter, column_iter);
@@ -480,12 +437,9 @@ Matrix<rows, columns>::MinorMatrix(Matrix<rows - 1, columns - 1> &result,
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, 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 row_iter{0}; row_iter < rows; row_iter++) for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
{
for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
{
float sign = ((row_iter + 1) % 2) == 0 ? -1 : 1; float sign = ((row_iter + 1) % 2) == 0 ? -1 : 1;
sign *= ((column_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; result[column_iter][row_iter] = this->Get(row_iter, column_iter) * sign;
@@ -496,55 +450,34 @@ Matrix<rows, columns>::adjugate(Matrix<rows, columns> &result) const
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> & float Matrix<rows, columns>::EuclideanNorm() const {
Matrix<rows, columns>::Normalize(Matrix<rows, columns> &result) const
{
float sum{0}; 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)}; float val{this->Get(row_idx, column_idx)};
sum += val * val; sum += val * val;
} }
} }
if (sum == 0) return sqrt(sum);
{
// 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 rows, uint8_t columns>
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset> template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const 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_rows + row_offset <= rows
// static assert that sub_columns + column_offset <= columns // static assert that sub_columns + column_offset <= columns
static_assert(sub_rows + row_offset <= rows, static_assert(sub_rows + row_offset <= rows,
"The submatrix you're trying to get is out of bounds (rows)"); "The submatrix you're trying to get is out of bounds (rows)");
static_assert(sub_columns + column_offset <= columns, static_assert(
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{}; 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] = buffer[row_idx][column_idx] =
this->Get(row_idx + row_offset, column_idx + column_offset); this->Get(row_idx + row_offset, column_idx + column_offset);
} }
@@ -553,21 +486,121 @@ Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const
} }
template <uint8_t rows, uint8_t columns> template <uint8_t rows, uint8_t columns>
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset> template <uint8_t sub_rows, uint8_t sub_columns>
void Matrix<rows, columns>::SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix) void Matrix<rows, columns>::SetSubMatrix(
{ uint8_t rowOffset, uint8_t columnOffset,
static_assert(sub_rows + row_offset <= rows, const Matrix<sub_rows, sub_columns> &sub_matrix) {
"The submatrix you're trying to set is out of bounds (rows)"); int16_t adjustedSubRows = sub_rows;
static_assert(sub_columns + column_offset <= columns, int16_t adjustedSubColumns = sub_columns;
"The submatrix you're trying to set is out of bounds (columns)"); int16_t adjustedRowOffset = rowOffset;
int16_t adjustedColumnOffset = columnOffset;
for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++) // a bunch of safety checks to make sure we don't overflow the matrix
{ if (sub_rows > rows) {
for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++) adjustedSubRows = rows;
{ }
this->matrix[(row_idx + row_offset) * columns + column_idx + column_offset] = sub_matrix.Get(row_idx, column_idx); if (sub_columns > columns) {
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_ #endif // MATRIX_H_

57
src/Matrix.hpp
View File

@@ -1,5 +1,4 @@
#ifndef MATRIX_H_ #pragma once
#define MATRIX_H_
#include <array> #include <array>
#include <cstdint> #include <cstdint>
@@ -19,11 +18,6 @@ public:
*/ */
Matrix() = default; 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 * @brief Initialize a matrix with an array
*/ */
@@ -40,9 +34,9 @@ public:
template <typename... Args> Matrix(Args... args); template <typename... Args> Matrix(Args... args);
/** /**
* @brief set the matrix diagonals to 1 and all other values to 0 * @brief Create an identity matrix
*/ */
void Identity(); static Matrix<rows, columns> Identity();
/** /**
* @brief Set all elements in this to value * @brief Set all elements in this to value
@@ -129,10 +123,11 @@ public:
Matrix<columns, rows> Transpose() 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 * @param result a buffer to store the result into
*/ */
Matrix<rows, columns> &Normalize(Matrix<rows, columns> &result) const; float EuclideanNorm() const;
/** /**
* @brief Get a row from the matrix * @brief Get a row from the matrix
@@ -159,8 +154,16 @@ public:
*/ */
constexpr uint8_t GetColumnSize() { return columns; } constexpr uint8_t GetColumnSize() { return columns; }
/**
* @brief Write a string representation of the matrix into the buffer
*/
void ToString(std::string &stringBuffer) const; 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 * @brief Get an element from the matrix
* @param row the row index of the element * @param row the row index of the element
@@ -193,13 +196,15 @@ public:
Matrix<rows, columns> operator*(float scalar) const; 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, 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> SubMatrix() const; Matrix<sub_rows, sub_columns> SubMatrix() const;
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, template <uint8_t sub_rows, uint8_t sub_columns>
uint8_t column_offset> void SetSubMatrix(uint8_t rowOffset, uint8_t columnOffset,
void SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix); const Matrix<sub_rows, sub_columns> &sub_matrix);
/** /**
* @brief take the dot product of the two vectors * @brief take the dot product of the two vectors
@@ -216,6 +221,28 @@ public:
return vec1.Get(0, 0) * vec2.Get(0, 0); 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: protected:
std::array<float, rows * columns> matrix; std::array<float, rows * columns> matrix;
@@ -225,6 +252,6 @@ private:
void setMatrixToArray(const std::array<float, rows * columns> &array); void setMatrixToArray(const std::array<float, rows * columns> &array);
}; };
#ifndef MATRIX_H_
#include "Matrix.cpp" #include "Matrix.cpp"
#endif // MATRIX_H_ #endif // MATRIX_H_

90
src/Quaternion.cpp
View File

@@ -6,23 +6,18 @@
* @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
*/ */
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 halfAngle = angle / 2;
const float sinHalfAngle = sin(halfAngle); const float sinHalfAngle = sin(halfAngle);
Matrix<1, 3> normalizedAxis{}; Matrix<1, 3> normalizedAxis = axis / axis.EuclideanNorm();
axis.Normalize(normalizedAxis); return Quaternion{static_cast<float>(cos(halfAngle)),
return Quaternion{
static_cast<float>(cos(halfAngle)),
normalizedAxis.Get(0, 0) * sinHalfAngle, normalizedAxis.Get(0, 0) * sinHalfAngle,
normalizedAxis.Get(0, 1) * sinHalfAngle, normalizedAxis.Get(0, 1) * sinHalfAngle,
normalizedAxis.Get(0, 2) * sinHalfAngle}; normalizedAxis.Get(0, 2) * sinHalfAngle};
} }
float Quaternion::operator[](uint8_t index) const float Quaternion::operator[](uint8_t index) const {
{ if (index < 4) {
if (index < 4)
{
return this->matrix[index]; return this->matrix[index];
} }
@@ -30,42 +25,42 @@ float Quaternion::operator[](uint8_t index) const
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{}; Quaternion result{};
this->Q_Mult(other, result); this->Q_Mult(other, result);
return 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,
return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * 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,
return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, this->v3 + other.v3}; this->v3 + other.v3};
} }
Quaternion & Quaternion &Quaternion::Q_Mult(const Quaternion &other,
Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const Quaternion &buffer) const {
{
// eq. 6 // eq. 6
buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3); buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 -
buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2); other.v3 * this->v3);
buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1); buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 +
buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w); 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; 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}; Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
buffer.v1 = other.v1; buffer.v1 = other.v1;
buffer.v2 = other.v2; buffer.v2 = other.v2;
@@ -78,11 +73,10 @@ Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
return buffer; return buffer;
} }
void Quaternion::Normalize() void Quaternion::Normalize() {
{ float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w); this->v3 * this->v3 + this->w * this->w);
if (magnitude == 0) if (magnitude == 0) {
{
return; return;
} }
this->v1 /= magnitude; this->v1 /= magnitude;
@@ -91,20 +85,23 @@ void Quaternion::Normalize()
this->w /= magnitude; this->w /= magnitude;
} }
Matrix<3, 3> Quaternion::ToRotationMatrix() const Matrix<3, 3> Quaternion::ToRotationMatrix() const {
{
float xx = this->v1 * this->v1; float xx = this->v1 * this->v1;
float yy = this->v2 * this->v2; float yy = this->v2 * this->v2;
float zz = this->v3 * this->v3; float zz = this->v3 * this->v3;
Matrix<3, 3> rotationMatrix{ Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
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), 2 * (this->v1 * this->v3 + this->v2 * this->w),
2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)}; 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; return rotationMatrix;
}; };
Matrix<3, 1> Quaternion::ToEulerAngle() const Matrix<3, 1> Quaternion::ToEulerAngle() const {
{
float sqv1 = this->v1 * this->v1; float sqv1 = this->v1 * this->v1;
float sqv2 = this->v2 * this->v2; float sqv2 = this->v2 * this->v2;
float sqv3 = this->v3 * this->v3; float sqv3 = this->v3 * this->v3;
@@ -112,9 +109,12 @@ Matrix<3, 1> Quaternion::ToEulerAngle() const
Matrix<3, 1> eulerAngle; Matrix<3, 1> eulerAngle;
{ {
atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw)); atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw)); (sqv1 - sqv2 - sqv3 + sqw));
atan2(2.0 * (this->v2 * this->v3 + this->v1 * 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; return eulerAngle;
} }

7
src/Quaternion.h
View File

@@ -2,12 +2,11 @@
#define QUATERNION_H_ #define QUATERNION_H_
#include "Matrix.hpp" #include "Matrix.hpp"
class Quaternion : public Matrix<1, 4> class Quaternion : public Matrix<1, 4> {
{
public: public:
Quaternion() : Matrix<1, 4>() {} Quaternion() : Matrix<1, 4>() {}
Quaternion(float fillValue) : Matrix<1, 4>(fillValue) {} Quaternion(float w, float v1, float v2, float v3)
Quaternion(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, 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) {}

400
unit-tests/matrix-tests.cpp
View File

@@ -10,41 +10,61 @@
#include <cmath> #include <cmath>
#include <iostream> #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") { TEST_CASE("Elementary Matrix Operations", "Matrix") {
std::array<float, 4> arr2{5, 6, 7, 8}; std::array<float, 4> arr2{5, 6, 7, 8};
Matrix<2, 2> mat1{1, 2, 3, 4}; Matrix<2, 2> mat1{1, 2, 3, 4};
Matrix<2, 2> mat2{arr2}; Matrix<2, 2> mat2{arr2};
Matrix<2, 2> mat3{}; 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") { SECTION("Fill") {
mat1.Fill(0); mat1.Fill(0);
REQUIRE(mat1.Get(0, 0) == 0); REQUIRE(mat1.Get(0, 0) == 0);
@@ -66,10 +86,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
} }
SECTION("Addition") { SECTION("Addition") {
std::string strBuf1 = "";
mat1.ToString(strBuf1);
std::cout << "Matrix 1:\n" << strBuf1 << std::endl;
mat1.Add(mat2, mat3); mat1.Add(mat2, mat3);
REQUIRE(mat3.Get(0, 0) == 6); REQUIRE(mat3.Get(0, 0) == 6);
@@ -119,7 +135,35 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
REQUIRE(mat3.Get(1, 0) == 43); REQUIRE(mat3.Get(1, 0) == 43);
REQUIRE(mat3.Get(1, 1) == 50); REQUIRE(mat3.Get(1, 1) == 50);
// TODO: You need to add non-square multiplications to this. // 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") { SECTION("Scalar Multiplication") {
@@ -254,26 +298,6 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
REQUIRE(mat5.Get(2, 1) == 6); 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") { SECTION("GET ROW") {
Matrix<1, 2> mat1Rows{}; Matrix<1, 2> mat1Rows{};
mat1.GetRow(0, mat1Rows); mat1.GetRow(0, mat1Rows);
@@ -328,29 +352,289 @@ TEST_CASE("Elementary Matrix Operations", "Matrix") {
Matrix<3, 3> mat4 = startMatrix; Matrix<3, 3> mat4 = startMatrix;
Matrix<2, 2> mat5{10, 11, 12, 13}; Matrix<2, 2> mat5{10, 11, 12, 13};
mat4.SetSubMatrix<2, 2, 0, 0>(mat5); mat4.SetSubMatrix(0, 0, mat5);
REQUIRE(mat4.Get(0, 0) == 10); REQUIRE(mat4.Get(0, 0) == 10);
REQUIRE(mat4.Get(0, 1) == 11); REQUIRE(mat4.Get(0, 1) == 11);
REQUIRE(mat4.Get(1, 0) == 12); REQUIRE(mat4.Get(1, 0) == 12);
REQUIRE(mat4.Get(1, 1) == 13); REQUIRE(mat4.Get(1, 1) == 13);
mat4 = startMatrix; mat4 = startMatrix;
mat4.SetSubMatrix<2, 2, 1, 1>(mat5); mat4.SetSubMatrix(1, 1, mat5);
REQUIRE(mat4.Get(1, 1) == 10); REQUIRE(mat4.Get(1, 1) == 10);
REQUIRE(mat4.Get(1, 2) == 11); REQUIRE(mat4.Get(1, 2) == 11);
REQUIRE(mat4.Get(2, 1) == 12); REQUIRE(mat4.Get(2, 1) == 12);
REQUIRE(mat4.Get(2, 2) == 13); REQUIRE(mat4.Get(2, 2) == 13);
Matrix<3, 1> mat6{10, 11, 12}; Matrix<3, 1> mat6{10, 11, 12};
mat4.SetSubMatrix<3, 1, 0, 0>(mat6); mat4.SetSubMatrix(0, 0, mat6);
REQUIRE(mat4.Get(0, 0) == 10); REQUIRE(mat4.Get(0, 0) == 10);
REQUIRE(mat4.Get(1, 0) == 11); REQUIRE(mat4.Get(1, 0) == 11);
REQUIRE(mat4.Get(2, 0) == 12); REQUIRE(mat4.Get(2, 0) == 12);
Matrix<1, 3> mat7{10, 11, 12}; Matrix<1, 3> mat7{10, 11, 12};
mat4.SetSubMatrix<1, 3, 0, 0>(mat7); mat4.SetSubMatrix(0, 0, mat7);
REQUIRE(mat4.Get(0, 0) == 10); REQUIRE(mat4.Get(0, 0) == 10);
REQUIRE(mat4.Get(0, 1) == 11); REQUIRE(mat4.Get(0, 1) == 11);
REQUIRE(mat4.Get(0, 2) == 12); 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));
}
}

2
unit-tests/matrix-timing-tests.cpp
View File

@@ -99,7 +99,7 @@ TEST_CASE("Timing Tests", "Matrix") {
SECTION("Normalize") { SECTION("Normalize") {
for (uint32_t i{0}; i < 10000; i++) { for (uint32_t i{0}; i < 10000; i++) {
mat1.Normalize(mat3); mat3 = mat1 / mat1.EuclideanNorm();
} }
} }