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

Compare commits

base: Cynopolis/Vector3D:main
Branches Tags
Cynopolis/Vector3D:main Cynopolis/Vector3D:Advanced-Matrix-Operations
..
compare: Cynopolis/Vector3D:c3da60c4120176eb81295eacb6098a20f722bae7
Branches Tags
Cynopolis/Vector3D:main Cynopolis/Vector3D:Advanced-Matrix-Operations
10 Commits
main .. c3da60c412
Author SHA1 Message Date
ci-bot c3da60c412 Update matrix-timing-tests timings [skip ci] 2025-05-29 16:01:45 +00:00
Cynopolis 35a8c39302 Fixing timing test runner
Merge-Checker / Benchmarking (pull_request) Successful in 2m31s
Details
Merge-Checker / build_and_test (pull_request) Successful in 22s
Details
2025-05-29 11:59:10 -04:00
ci-bot dbdae6c70a Update matrix-timing-tests timings [skip ci] 2025-05-29 15:20:33 +00:00
Cynopolis 46f8e87509 Added a check to see if the timing results have signifigantly changed
Merge-Checker / build_and_test (pull_request) Successful in 1m25s
Details
2025-05-29 11:19:25 -04:00
ci-bot c5af1edc4d Update matrix-timing-tests timings [skip ci] 2025-05-29 15:01:26 +00:00
Cynopolis ec913ad19c Split timing tests into its own job
Merge-Checker / build_and_test (pull_request) Successful in 27s
Details
2025-05-29 11:00:11 -04:00
ci-bot 7aa7949ce3 Update matrix-timing-tests timings [skip ci] 2025-05-21 22:40:40 +00:00
Cynopolis eb98e6a6c3 updated readme
Merge-Checker / build_and_test (pull_request) Successful in 21s
Details
2025-05-21 18:40:16 -04:00
Cynopolis 61b67052f3 Added matrix test timings 
Timings get auto-comitted

Update matrix-timing-tests timings [skip ci]

Updated readme

Update matrix-timing-tests timings [skip ci]

Fixing auto-checkout issues
 2025-05-21 18:38:36 -04:00
Cynopolis a5dbd01aa1 Added a merge checker script that has to run before you can merge to main 
Updated merge checker and seperated the matrix tests fro mthe timing tests
 2025-05-21 18:38:33 -04:00
22 changed files with 587 additions and 7278 deletions
Expand all files Collapse all files
.gitea/workflows/Merge-Checker.yaml
+69 -12
View File
@@ -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
Vendored
+1 -5
View File
@@ -75,9 +75,5 @@
},
"clangd.enable": true,
"C_Cpp.dimInactiveRegions": false,
"editor.defaultFormatter": "xaver.clang-format",
"clangd.inactiveRegions.useBackgroundHighlight": false,
"clangd.arguments": [
"--compile-commands-dir=${workspaceFolder}/build"
],
"editor.defaultFormatter": "xaver.clang-format"
}
CMakeLists.txt
+2 -4
View File
@@ -4,11 +4,9 @@ project(Vector3D)
add_subdirectory(src)
add_subdirectory(unit-tests)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD 11)
add_compile_options(-Wall -Wextra -Wpedantic)
add_compile_options (-fdiagnostics-color=always)
set(CMAKE_COLOR_DIAGNOSTICS ON)
add_compile_options(-fdiagnostics-color=always -Wall -Wextra -Wpedantic)
include(FetchContent)
README.md
+1 -8
View File
@@ -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
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`
There are still several operations that are works in progress
src/CMakeLists.txt
-36
View File
@@ -41,40 +41,6 @@ target_link_libraries(vector-3d
PRIVATE
)
# SVD
add_library(svd
STATIC
SVD.cpp
)
target_link_libraries(svd
PUBLIC
vector-3d-intf
PRIVATE
)
set_target_properties(svd
PROPERTIES
LINKER_LANGUAGE CXX
)
# QR (eigenvalues/eigenvectors via implicit shifted QR iteration)
add_library(qr
STATIC
QR.cpp
)
target_link_libraries(qr
PUBLIC
vector-3d-intf
PRIVATE
)
set_target_properties(qr
PROPERTIES
LINKER_LANGUAGE CXX
)
# Matrix
add_library(matrix
STATIC
@@ -85,8 +51,6 @@ target_link_libraries(matrix
PUBLIC
vector-3d-intf
PRIVATE
svd
qr
)
set_target_properties(matrix
src/Matrix.cpp
+211 -261
View File
@@ -1,39 +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
// Forward-declare QR::EigenQR so the Matrix::EigenQR implementation below can
// call it even when Matrix.cpp is pulled in through QR.hpp's own include chain
// (QR.cpp -> QR.hpp -> Matrix.hpp -> Matrix.cpp), where the QR namespace has
// not been declared yet at this point. If we are not already inside that
// chain, pull in the full QR library so its template definition is available.
namespace QR {
template <uint8_t N>
void EigenQR(Matrix<N, N> &matrixToDecompose, Matrix<N, N> &eigenVectors,
Matrix<N, 1> &eigenValues, uint32_t maxIterations,
float tolerance);
}
#ifndef QR_H_
#include "QR.hpp"
#endif
// Forward-declare SVD::SVD so the Matrix::SVD implementation below can call
// it even when Matrix.cpp is pulled in through SVD.hpp's own include chain
// (SVD.hpp -> Matrix.hpp -> Matrix.cpp), where the SVD namespace has not
// been declared yet at this point. If we are not already inside that chain,
// pull in the full SVD library so its template definition is available.
namespace SVD {
template <uint8_t rows, uint8_t columns>
void SVD(Matrix<rows, columns> &matrixToDecompose, Matrix<rows, columns> &U,
Matrix<columns, 1> &sigma, Matrix<columns, columns> &Vt);
}
#ifndef SVD_H_
#include "SVD.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"
@@ -41,29 +5,29 @@ void SVD(Matrix<rows, columns> &matrixToDecompose, Matrix<rows, columns> &U,
#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,
std::enable_if_t<(std::is_arithmetic_v<Args> && ...), int>>
Matrix<rows, columns>::Matrix(Args... args) {
template <typename... Args>
Matrix<rows, columns>::Matrix(Args... args)
{
constexpr uint16_t arraySize{static_cast<uint16_t>(rows) *
static_cast<uint16_t>(columns)};
std::initializer_list<float> initList{static_cast<float>(args)...};
// if there is only one value, we actually want to do a fill
if (sizeof...(args) == 1) {
this->Fill(*initList.begin());
}
static_assert(sizeof...(args) == arraySize || sizeof...(args) == 1,
"You did not provide the right amount of initializers for this "
"matrix size");
// choose whichever buffer size is smaller for the copy length
uint32_t minSize =
std::min(arraySize, static_cast<uint16_t>(initList.size()));
@@ -71,19 +35,22 @@ Matrix<rows, columns>::Matrix(Args... args) {
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> Matrix<rows, columns>::Identity() {
Matrix<rows, columns> identityMatrix{0};
uint32_t minDimension = std::min(rows, columns);
for (uint8_t idx{0}; idx < minDimension; idx++) {
identityMatrix[idx][idx] = 1;
void Matrix<rows, columns>::Identity()
{
this->Fill(0);
for (uint8_t idx{0}; idx < rows; idx++)
{
this->matrix[idx * columns + idx] = 1;
}
return identityMatrix;
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns>::Matrix(const Matrix<rows, columns> &other) {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
this->matrix[row_idx * columns + column_idx] =
other.Get(row_idx, column_idx);
}
@@ -92,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) {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
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;
}
}
@@ -110,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
result[row_idx][column_idx] =
this->Get(row_idx, column_idx) + other.Get(row_idx, column_idx);
}
@@ -123,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
result[row_idx][column_idx] =
this->Get(row_idx, column_idx) - other.Get(row_idx, column_idx);
}
@@ -138,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);
@@ -161,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
result[row_idx][column_idx] = this->Get(row_idx, column_idx) * scalar;
}
}
@@ -172,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,
@@ -182,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;
@@ -207,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 row_idx{0}; row_idx < columns; row_idx++) {
for (uint8_t column_idx{0}; column_idx < rows; column_idx++)
{
for (uint8_t row_idx{0}; row_idx < columns; row_idx++)
{
result[row_idx][column_idx] = this->Get(column_idx, row_idx);
}
}
@@ -222,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 <> inline float Matrix<2, 2>::Det() const {
template <>
inline float Matrix<1, 1>::Det() const { return this->matrix[0]; }
template <>
inline float Matrix<2, 2>::Det() const
{
return this->matrix[0] * this->matrix[3] - this->matrix[1] * this->matrix[2];
}
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] *
@@ -247,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
result[row_idx][column_idx] =
this->Get(row_idx, column_idx) * other.Get(row_idx, column_idx);
}
@@ -261,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
result[row_idx][column_idx] =
this->Get(row_idx, column_idx) / other.Get(row_idx, column_idx);
}
@@ -274,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 {
if (row_index > rows - 1 || column_index > columns - 1) {
uint8_t column_index) const
{
if (row_index > rows - 1 || column_index > columns - 1)
{
return 1e+10; // TODO: We should throw something here instead of failing
// quietly
}
@@ -285,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));
@@ -295,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
Matrix<rows, 1> &column) const
{
for (uint8_t row_idx{0}; row_idx < rows; row_idx++)
{
column[row_idx][0] = this->Get(row_idx, column_index);
}
@@ -304,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 {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
void Matrix<rows, columns>::ToString(std::string &stringBuffer) const
{
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";
}
}
@@ -319,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 {
return this->matrix.data();
}
template <uint8_t rows, uint8_t columns>
std::array<float, columns> &
Matrix<rows, columns>::operator[](uint8_t row_index) {
if (row_index > rows - 1) {
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;
}
@@ -337,8 +346,9 @@ Matrix<rows, columns>::operator[](uint8_t row_index) {
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns> &
Matrix<rows, columns>::operator=(const Matrix<rows, columns> &other) {
Matrix<rows, columns> &Matrix<rows, columns>::
operator=(const Matrix<rows, columns> &other)
{
memcpy(this->matrix.begin(), other.matrix.begin(),
rows * columns * sizeof(float));
@@ -347,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>::operator+(const Matrix<rows, columns> &other) const {
Matrix<rows, columns> Matrix<rows, columns>::
operator+(const Matrix<rows, columns> &other) const
{
Matrix<rows, columns> buffer{};
this->Add(other, buffer);
return buffer;
}
template <uint8_t rows, uint8_t columns>
Matrix<rows, columns>
Matrix<rows, columns>::operator-(const Matrix<rows, columns> &other) const {
Matrix<rows, columns> Matrix<rows, columns>::
operator-(const Matrix<rows, columns> &other) const
{
Matrix<rows, columns> buffer{};
this->Sub(other, buffer);
return buffer;
@@ -364,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*(
const Matrix<columns, other_columns> &other) const {
Matrix<rows, other_columns> Matrix<rows, columns>::
operator*(const Matrix<columns, other_columns> &other) const
{
Matrix<rows, other_columns> buffer{};
this->Mult(other, buffer);
return buffer;
}
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);
}
@@ -409,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);
}
@@ -419,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) {
for (uint8_t row_idx{0}; row_idx < rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < columns; column_idx++) {
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++)
{
this->matrix[row_idx * columns + column_idx] = value;
}
}
@@ -429,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 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->MinorMatrix(MinorMatrix, row_idx, column_idx);
result[row_idx][column_idx] = MinorMatrix.Det();
}
@@ -445,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++) {
if (row_iter == row_idx) {
for (uint8_t row_iter{0}; row_iter < rows; row_iter++)
{
if (row_iter == row_idx)
{
continue;
}
for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
if (column_iter == column_idx) {
for (uint8_t column_iter{0}; column_iter < columns; column_iter++)
{
if (column_iter == column_idx)
{
continue;
}
subArray[array_idx] = this->Get(row_iter, column_iter);
@@ -467,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 {
for (uint8_t row_iter{0}; row_iter < rows; row_iter++) {
for (uint8_t column_iter{0}; column_iter < columns; column_iter++) {
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++)
{
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;
@@ -480,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 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++)
{
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,
uint8_t column_offset>
Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const {
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset>
Matrix<sub_rows, sub_columns> Matrix<rows, columns>::SubMatrix() const
{
// static assert that sub_rows + row_offset <= rows
// static assert that sub_columns + column_offset <= columns
static_assert(sub_rows + row_offset <= rows,
"The submatrix you're trying to get is out of bounds (rows)");
static_assert(
sub_columns + column_offset <= columns,
"The submatrix you're trying to get is out of bounds (columns)");
static_assert(sub_columns + column_offset <= columns,
"The submatrix you're trying to get is out of bounds (columns)");
Matrix<sub_rows, sub_columns> buffer{};
for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++) {
for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++) {
for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++)
{
for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++)
{
buffer[row_idx][column_idx] =
this->Get(row_idx + row_offset, column_idx + column_offset);
}
@@ -516,108 +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>
void Matrix<rows, columns>::SetSubMatrix(
uint8_t rowOffset, uint8_t columnOffset,
const Matrix<sub_rows, sub_columns> &sub_matrix) {
int16_t adjustedSubRows = sub_rows;
int16_t adjustedSubColumns = sub_columns;
int16_t adjustedRowOffset = rowOffset;
int16_t adjustedColumnOffset = columnOffset;
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset, uint8_t column_offset>
void Matrix<rows, columns>::SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix)
{
static_assert(sub_rows + row_offset <= rows,
"The submatrix you're trying to set is out of bounds (rows)");
static_assert(sub_columns + column_offset <= columns,
"The submatrix you're trying to set is out of bounds (columns)");
// a bunch of safety checks to make sure we don't overflow the matrix
if (sub_rows > rows) {
adjustedSubRows = rows;
}
if (sub_columns > columns) {
adjustedSubColumns = columns;
}
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);
for (uint8_t row_idx{0}; row_idx < sub_rows; row_idx++)
{
for (uint8_t column_idx{0}; column_idx < sub_columns; column_idx++)
{
this->matrix[(row_idx + row_offset) * columns + column_idx + column_offset] = 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> 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");
// Delegate to the QR library: implicit shifted QR iteration with
// Wilkinson shift (see src/QR.hpp for the algorithm and conventions).
Matrix<rows, rows> A = *this; // QR::EigenQR does not modify its input
QR::EigenQR(A, eigenVectors, eigenValues, maxIterations, tolerance);
}
template <uint8_t rows, uint8_t columns>
void Matrix<rows, columns>::SVD(Matrix<rows, columns> &U,
Matrix<columns, 1> &sigma,
Matrix<columns, columns> &Vt) const {
// Delegate to the SVD library (see src/SVD.hpp for the algorithm and
// conventions). NB: the fully-qualified ::SVD is required here — inside
// this member the unqualified name SVD refers to this method, which
// would shadow the namespace in a qualified lookup. SVD::SVD takes its
// input by non-const reference but does not modify it; pass a copy so
// the const-ness of *this is preserved.
Matrix<rows, columns> A = *this;
::SVD::SVD<rows, columns>(A, U, sigma, Vt);
}
#endif // MATRIX_H_
src/Matrix.hpp
+19 -73
View File
@@ -1,11 +1,13 @@
#pragma once
#ifndef MATRIX_H_
#define MATRIX_H_
#include <array>
#include <cstdint>
#include <string>
#include <type_traits>
// TODO: Add a function to calculate eigenvalues/vectors
// TODO: Add a function to compute RREF
// TODO: Add a function for SVD decomposition
// TODO: Add a function for LQ decomposition
template <uint8_t rows, uint8_t columns> class Matrix {
@@ -17,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
*/
@@ -28,17 +35,14 @@ public:
Matrix(const Matrix<rows, columns> &other);
/**
* @brief Initialize a matrix directly with scalar values
* Uses SFINAE to only accept arithmetic types (int, float, double, etc.)
* @brief Initialize a matrix directly with any number of arguments
*/
template <typename... Args,
std::enable_if_t<(std::is_arithmetic_v<Args> && ...), int> = 0>
Matrix(Args... args);
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
@@ -125,11 +129,10 @@ public:
Matrix<columns, rows> Transpose() const;
/**
* @brief Returns the euclidean magnitude of the matrix. Also known as the L2
* norm
* @brief reduce the matrix so the sum of its elements equal 1
* @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
@@ -156,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
@@ -198,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>
void SetSubMatrix(uint8_t rowOffset, uint8_t columnOffset,
const Matrix<sub_rows, sub_columns> &sub_matrix);
template <uint8_t sub_rows, uint8_t sub_columns, uint8_t row_offset,
uint8_t column_offset>
void SetSubMatrix(const Matrix<sub_rows, sub_columns> &sub_matrix);
/**
* @brief take the dot product of the two vectors
@@ -223,53 +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 Calculates the eigenvectors and values of this matrix using the
* implicit shifted QR iteration (Wilkinson shift, Givens bulge chasing);
* see src/QR.hpp in the QR library for the full algorithm.
* @note For a matrix larger than 2x2 the matrix MUST be symmetric.
* A general (nonsymmetric) 2x2 is handled via the closed-form
* solution.
* @note The eigenvalues come out sorted DESCENDING (largest first); the
* eigenvector columns are swapped to match. Eigenvector signs are
* arbitrary.
* @param eigenVectors a buffer that will contain the eigenvectors of this
* matrix in its columns (column i pairs with eigenValues[i])
* @param eigenValues a buffer that will contain the eigenvalues of this
* matrix, sorted descending
* @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;
/**
* @brief Compute the Singular Value Decomposition (SVD) of this matrix.
*
* Wrapper around SVD::SVD (see SVD.hpp for the full algorithm
* description, output storage conventions, and stack-usage notes).
* Decomposes A = U · Σ · Vᵀ where U is rows×columns, Σ is the vector
* of singular values (columns×1, sorted descending), and Vᵀ is
* columns×columns. Works for any shape (wide matrices are handled
* internally by computing SVD(Aᵀ) and swapping the factors back).
* This matrix is not modified.
*
* @param U Output: left singular vectors (rows×columns)
* @param sigma Output: singular values in descending order (columns×1)
* @param Vt Output: right singular vectors, transposed (columns×columns)
*/
void SVD(Matrix<rows, columns> &U, Matrix<columns, 1> &sigma,
Matrix<columns, columns> &Vt) const;
protected:
std::array<float, rows * columns> matrix;
@@ -279,6 +225,6 @@ private:
void setMatrixToArray(const std::array<float, rows * columns> &array);
};
#ifndef MATRIX_H_
#include "Matrix.cpp"
#endif // MATRIX_H_
src/QR.cpp
-422
View File
@@ -1,422 +0,0 @@
// This #ifndef section makes clangd happy so that it can properly do type hints
// in this file
#ifndef QR_H_
#define QR_H_
#include "QR.hpp"
#endif
#ifdef QR_H_ // since the .cpp file has to be included by the .hpp file this
// will evaluate to true
#include "QR.hpp"
#include <cmath>
#include <cstdint>
namespace QR {
// ============================================================================
// QR Building Block Implementations (fully templated, heap-free)
// ============================================================================
/**
* GivensRotation: R * (a, b)^T = (r, 0)^T with R = [[c, s], [-s, c]],
* r = +hypot(a, b), c = a/r, s = b/r.
*/
// [[maybe_unused]]: this helper is only referenced from template
// (EigenQR/Tridiagonalize), so in translation units that include this file
// but never instantiate those templates, the definition is legitimately
// unused. The attribute silences -Wunused-function there without hiding
// real dead code in TUs that do use the algorithm.
[[maybe_unused]] static void GivensRotation(float a, float b, float &c,
float &s) {
float r = sqrtf(a * a + b * b);
if (r == 0.0f) {
c = 1.0f;
s = 0.0f;
return;
}
c = a / r;
s = b / r;
}
/**
* ApplyRotationBothSides: A <- G A G^T (similarity transform) with
* G = [[c, s], [-s, c]] on the (i, i+1) block, i.e. G is the ZEROING
* rotation G*(x, y)^T = (r, 0)^T (the orientation used by the implicit QR
* chase: A = Q R with Q = G^T gives the next iterate R Q = G A G^T).
* With (c, s) = GivensRotation(A[i][i], A[i+1][i]) this zeroes
* A[i+1][i] after the LEFT multiplication; the right multiplication then
* chases the bulge along the superdiagonal (tridiagonal chase).
*
* A must be symmetric on entry; the result stays symmetric, so both
* triangles are written.
*
* Block updates (with a00 = A[i][i], a01 = A[i][i+1], a11 = A[i+1][i+1]):
* A[i][i] = c^2 a00 + 2 c s a01 + s^2 a11
* A[i][i+1] = (c^2 - s^2) a01 + c s (a11 - a00)
* A[i+1][i+1] = s^2 a00 - 2 c s a01 + c^2 a11
* Off-block updates (uniform for both sides, since the left factor G and
* the right factor G^T mix each side with the pattern (a, b) -> (c a + s b,
* -s a + c b) after transposition):
* for j not in {i, i+1}:
* A[i][j] = A[j][i] = c A[i][j] + s A[i+1][j]
* A[i+1][j] = A[j][i+1] = -s A[i][j] + c A[i+1][j]
*/
template <uint8_t N>
static void ApplyRotationBothSides(Matrix<N, N> &A, uint8_t i, float c,
float s) {
float a00 = A.Get(i, i);
float a01 = A.Get(i, i + 1);
float a11 = A.Get(i + 1, i + 1);
float c2 = c * c;
float s2 = s * s;
float cs = c * s;
A[i][i] = c2 * a00 + 2.0f * cs * a01 + s2 * a11;
A[i][i + 1] = (c2 - s2) * a01 + cs * (a11 - a00);
A[i + 1][i + 1] = s2 * a00 - 2.0f * cs * a01 + c2 * a11;
A[i + 1][i] = A[i][i + 1]; // keep both triangles in sync
for (uint8_t j = 0; j < N; ++j) {
if (j == i || j == i + 1)
continue;
float x = A.Get(i, j);
float y = A.Get(i + 1, j);
A[i][j] = c * x + s * y;
A[j][i] = A[i][j];
A[i + 1][j] = -s * x + c * y;
A[j][i + 1] = A[i + 1][j];
}
}
/**
* ApplyRotationToVectors: V <- V G^T with G = [[c, s], [-s, c]] on columns
* (i, i+1), applied to every row. G^T = [[c, -s], [s, c]], so
* V[r][i] <- c V[r][i] + s V[r][i+1]
* V[r][i+1] <- -s V[r][i] + c V[r][i+1]
*
* Convention pairing: if A evolves as A <- G A G^T (ApplyRotationBothSides
* with the SAME c, s), then V accumulates V <- V G^T. With V0 = I the
* invariant A0 = V A V^T is preserved at every step, so at convergence
* A0 = V D V^T and the columns of V are the eigenvectors. (Rationale:
* each chase step is A <- R Q with R = G A the upper-triangular factor and
* Q = G^T the orthogonal factor of A = Q R, so A = G^T A' G and the
* orthogonal factors multiply as G1^T G2^T ... in application order.)
*/
template <uint8_t N>
static void ApplyRotationToVectors(Matrix<N, N> &V, uint8_t i, float c,
float s) {
for (uint8_t r = 0; r < N; ++r) {
float x = V.Get(r, i);
float y = V.Get(r, i + 1);
V[r][i] = c * x + s * y;
V[r][i + 1] = -s * x + c * y;
}
}
/**
* WilkinsonShift: eigenvalue of [[a, b], [b, d]] closest to d.
* mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2), sign(0) = +1.
*/
[[maybe_unused]] static float WilkinsonShift(float a, float b, float d) {
float delta = 0.5f * (a - d);
float spread = sqrtf(delta * delta + b * b);
return 0.5f * (a + d) - (delta >= 0.0f ? spread : -spread);
}
/**
* Solve2x2Eigen: closed-form eigen-decomposition of the 2x2 block at
* (lo, lo+1). Works for symmetric blocks and for general 2x2 blocks with
* real eigenvalues (used by the N == 2 entry point).
*
* lambdaHi/lambdaLo come from the characteristic polynomial
* lambda^2 - trace*lambda + det = 0.
* The eigenvector for lambdaHi is v = (b, lambdaHi - a) (from the first
* row of (A - lambda*I)v = 0), normalized to unit length. If b == 0 the
* block is triangular and the eigenvectors are coordinate vectors:
* e1 for the larger of {a, d}, e2 for the other.
*/
template <uint8_t N>
static void Solve2x2Eigen(const Matrix<N, N> &A, uint8_t lo, float &lambdaHi,
float &lambdaLo, float &c, float &s) {
float a = A.Get(lo, lo);
float b = A.Get(lo, lo + 1);
float e = A.Get(lo + 1, lo);
float d = A.Get(lo + 1, lo + 1);
float trace = a + d;
float det = a * d - b * e;
float disc = trace * trace - 4.0f * det;
if (disc < 0.0f)
disc = 0.0f; // round-off clamp: real 2x2 blocks have disc >= 0
float sqrtDisc = sqrtf(disc);
lambdaHi = 0.5f * (trace + sqrtDisc);
lambdaLo = 0.5f * (trace - sqrtDisc);
if (b != 0.0f) {
float v1 = lambdaHi - a;
float n = sqrtf(b * b + v1 * v1);
c = b / n;
s = v1 / n;
} else if (a >= d) {
c = 1.0f; // e1 is the eigenvector of a = lambdaHi
s = 0.0f;
} else {
c = 0.0f; // e2 is the eigenvector of d = lambdaHi
s = 1.0f;
}
}
/**
* Deflate: zero subdiagonal entries i in [lo, hi) whose magnitude is at or
* below tolerance * (|A[i][i]| + |A[i+1][i+1]|).
*/
template <uint8_t N>
static void Deflate(Matrix<N, N> &A, uint8_t lo, uint8_t hi, float tolerance) {
for (uint8_t i = lo; i < hi; ++i) {
float t = A.Get(i + 1, i);
float scale = fabsf(A.Get(i, i)) + fabsf(A.Get(i + 1, i + 1));
if (fabsf(t) <= tolerance * scale) {
A[i + 1][i] = 0.0f;
A[i][i + 1] = 0.0f;
}
}
}
// ============================================================================
// QR::EigenQR driver (implicit Wilkinson-shifted QR, bulge chasing)
// ============================================================================
/**
* Tridiagonalize: Givens tridiagonalization (Golub & Van Loan 8.3.1).
*
* For column k = 0..N-3 the entries A[k+2..N-1, k] are eliminated by
* rotations on (i, i+1) applied BOTTOM-UP, i = N-2 down to k+1, each
* formed from the CURRENT (already-updated) pair (A[i][k], A[i+1][k]).
* Bottom-up is essential: a top-down pass zeros A[i+1][k] with a rotation
* that would later be undone when the next rotation (i+1, i+2) is formed
* from an entry below, reviving A[i][k]. Each bottom-up rotation zeros the
* bottom of the remaining nonzero pair and the entries below stay zero
* (they are not mixed again, only rows i-1/i are mixed next).
*
* Already-tridiagonalized leading columns j < k are untouched: the mixed
* rows are both >= k+1 > j+1, so A[i][j] and A[i+1][j] are both zero there.
* The rotation on (i, i+1) also keeps column k+1..k+2 structure intact and
* does not destroy earlier columns, so after column k is done the leading
* (k+1)x(k+1) block is tridiagonal forever.
*
* On return: A is symmetric tridiagonal and A_orig = U A U^T (U = product
* of every rotation applied, in application order, as U <- U G^T).
*/
template <uint8_t N>
static void Tridiagonalize(Matrix<N, N> &A, Matrix<N, N> &U) {
U = Matrix<N, N>{0};
for (uint8_t i = 0; i < N; ++i) {
U[i][i] = 1.0f;
}
float c = 0.0f, s = 0.0f;
for (uint8_t k = 0; k + 2 < N; ++k) {
for (int i = (int)N - 2; i >= (int)k + 1; --i) {
GivensRotation(A.Get(i, k), A.Get(i + 1, k), c, s);
ApplyRotationBothSides(A, (uint8_t)i, c, s);
ApplyRotationToVectors(U, (uint8_t)i, c, s);
}
}
}
/**
* See QR.hpp for the full contract. Implementation sketch:
*
* Phase 0 (N >= 3): Tridiagonalize(A, U) // A_orig = U A U^T
* V = I.
* while (hi > 0):
* Deflate(A, 0, hi, tol); peel exact-zero trailing subdiagonals (hi--)
* lo = top of the trailing unreduced block (scan down, stop at first
* exact zero subdiagonal)
* if lo == hi - 1: closed-form 2x2 eigen-solve; fold Vblock into V
* else: one implicit Wilkinson-shifted QR step:
* mu = WilkinsonShift(A[hi-1][hi-1], A[hi][hi-1], A[hi][hi])
* A[lo..hi diagonal] -= mu // whole block!
* G1 = Givens(A[lo][lo], A[lo+1][lo])
* for i = lo..hi-1:
* (i > lo: Gi = Givens(A[i][i], A[i+1][i]))
* ApplyRotationBothSides(A, i, Gi) // A <- Gi A Gi^T
* ApplyRotationToVectors(V, i, Gi) // V <- V Gi^T
* A[lo..hi diagonal] += mu
* eigenvalues = diag(A), sorted descending with matching V column swaps.
* eigenvectors = U * V.
*
* Invariant maintained for N >= 3 (symmetric input): A is symmetric
* tridiagonal (up to deflated zeros and ~1e-7 float roundoff in the
* off-tridiagonal corners) at the top of every loop iteration, and
* A_orig = U A U^T = (U V) A (U V)^T throughout (V = product of every
* rotation applied so far, in application order, as V <- V Gi^T). At
* convergence A = V D V^T and therefore A_orig = (U V) D (U V)^T.
*
* Orientation note: each chase rotation Gi is the ZEROING rotation
* (Gi * (x, y)^T = (r, 0)^T). The step A <- Gi A Gi^T equals R Q with
* R = Gi A upper-triangular (on the block) and Q = Gi^T -- i.e. it IS the
* standard QR update Q(A - mu I)Q^T with Q the orthogonal QR factor. The
* eigenvector accumulator therefore collects the Q factors: V <- V Gi^T.
*/
template <uint8_t N>
void EigenQR(Matrix<N, N> &matrixToDecompose, Matrix<N, N> &eigenVectors,
Matrix<N, 1> &eigenValues, uint32_t maxIterations, float tolerance) {
static_assert(N >= 2, "QR::EigenQR requires N >= 2 (N = 1 is trivial)");
Matrix<N, N> A = matrixToDecompose; // input is not modified
Matrix<N, N> V{0};
// NB: Matrix::Identity() is a static factory that returns by value; a
// bare call would be a no-op. Set the diagonal explicitly.
for (uint8_t i = 0; i < N; ++i) {
V[i][i] = 1.0f;
}
// ------------------------------------------------------------------
// N == 2: closed-form solution (works for nonsymmetric input too)
// ------------------------------------------------------------------
if (N == 2) {
float l1 = 0.0f, l2 = 0.0f, c = 0.0f, s = 0.0f;
Solve2x2Eigen(A, 0, l1, l2, c, s);
// V = I * Vblock = [[c, -s], [s, c]]
V[0][0] = c;
V[0][1] = -s;
V[1][0] = s;
V[1][1] = c;
eigenValues[0][0] = l1;
eigenValues[1][0] = l2;
for (uint8_t r = 0; r < N; ++r)
for (uint8_t col = 0; col < N; ++col)
eigenVectors[r][col] = V.Get(r, col);
return;
}
// ------------------------------------------------------------------
// N >= 3: implicit shifted QR iteration (symmetric input required)
// ------------------------------------------------------------------
// Phase 0: general symmetric -> symmetric tridiagonal. The implicit
// QR bulge chase only preserves a tridiagonal structure, so the input
// must be reduced first: A_orig = U A U^T with A tridiagonal.
Matrix<N, N> U{};
Tridiagonalize(A, U);
uint32_t iter = 0;
uint8_t hi = N - 1;
while (hi > 0) {
Deflate(A, 0, hi, tolerance);
// Peel trailing rows whose subdiagonal is exactly zero (deflated or
// already solved). Must be re-done every iteration: a peel is only
// meaningful once the subdiagonal beneath it has converged.
while (hi > 0 && A.Get(hi, hi - 1) == 0.0f) {
--hi;
}
if (hi == 0) {
break; // fully diagonal (within tolerance)
}
// Find the top of the trailing unreduced block: scan down from hi-1
// and stop at the first exact zero subdiagonal. A[hi][hi-1] != 0 here
// (just peeled), so lo < hi.
uint8_t lo = hi;
for (int i = (int)hi - 1; i >= 0; --i) {
if (A.Get(i + 1, i) == 0.0f) {
break;
}
lo = (uint8_t)i;
}
if (lo + 1 == hi) {
// Trailing unreduced block is 2x2: solve in closed form.
float l1 = 0.0f, l2 = 0.0f, c = 0.0f, s = 0.0f;
Solve2x2Eigen(A, lo, l1, l2, c, s);
A[lo][lo] = l1;
A[lo + 1][lo + 1] = l2;
A[lo][lo + 1] = 0.0f;
A[lo + 1][lo] = 0.0f;
// Fold Vblock = [[c, -s], [s, c]] into V: V <- V * Vblock on
// columns (lo, lo+1). NOTE the sign convention differs from
// ApplyRotationToVectors (which applies [[c, s], [-s, c]]):
// here column 0 of Vblock is (c, s)^T, column 1 is (-s, c)^T.
for (uint8_t r = 0; r < N; ++r) {
float x = V.Get(r, lo);
float y = V.Get(r, lo + 1);
V[r][lo] = c * x + s * y;
V[r][lo + 1] = -s * x + c * y;
}
if (lo == 0) {
break; // block reached the top: matrix is fully solved
}
hi = (uint8_t)(lo - 1);
continue;
}
// One implicit Wilkinson-shifted QR step on block [lo, hi].
float mu = WilkinsonShift(A.Get(hi - 1, hi - 1), A.Get(hi, hi - 1),
A.Get(hi, hi));
// The shift applies to the ENTIRE active block: bulge chasing
// triangularizes (A - mu*I), and the first Givens rotation is formed
// from (A[lo][lo] - mu, A[lo+1][lo]).
for (uint8_t i = lo; i <= hi; ++i) {
A[i][i] -= mu;
}
float c = 0.0f, s = 0.0f;
for (uint8_t i = lo; i < hi; ++i) {
if (i == lo) {
GivensRotation(A.Get(lo, lo), A.Get(lo + 1, lo), c, s);
} else {
GivensRotation(A.Get(i, i), A.Get(i + 1, i), c, s);
}
ApplyRotationBothSides(A, i, c, s);
ApplyRotationToVectors(V, i, c, s);
}
for (uint8_t i = lo; i <= hi; ++i) {
A[i][i] += mu;
}
if (++iter >= maxIterations) {
// Best-effort: fall through with the partially diagonalized A.
break;
}
}
// ------------------------------------------------------------------
// Collect eigenvalues and sort DESCENDING (swap eigenvectors to match)
// ------------------------------------------------------------------
for (uint8_t i = 0; i < N; ++i) {
eigenValues[i][0] = A.Get(i, i);
}
for (uint8_t i = 0; i < N - 1; ++i) {
uint8_t k = i;
for (uint8_t j = i + 1; j < N; ++j) {
if (eigenValues.Get(j, 0) > eigenValues.Get(k, 0)) {
k = j;
}
}
if (k != i) {
float t = eigenValues[i][0];
eigenValues[i][0] = eigenValues[k][0];
eigenValues[k][0] = t;
for (uint8_t r = 0; r < N; ++r) {
float x = V.Get(r, i);
V[r][i] = V.Get(r, k);
V[r][k] = x;
}
}
}
// True eigenvectors of the original matrix: U * V. Reuse the A buffer
// (its diagonal has already been collected into eigenValues).
U.Mult(V, A);
for (uint8_t r = 0; r < N; ++r) {
for (uint8_t col = 0; col < N; ++col) {
eigenVectors[r][col] = A.Get(r, col);
}
}
}
} // namespace QR
#endif // QR_H_
src/QR.hpp
-196
View File
@@ -1,196 +0,0 @@
#pragma once
#include "Matrix.hpp"
/**
* @brief Library that uses Matrix.hpp and computes the eigenvalues and
* eigenvectors of a square matrix with the implicit shifted QR iteration
* (Wilkinson shift, Givens bulge chasing).
*
* @note Fully templated: QR::EigenQR works for ANY Matrix<N,N> with N in
* 2..255 (the uint8_t range of Matrix). There is no 5x5 limit.
*
* @note N >= 3: the input matrix MUST be symmetric (A[i][j] == A[j][i]).
* The implicit QR bulge chase maintains a symmetric tridiagonal
* structure, which only exists for symmetric input. N = 2 handles
* a general (nonsymmetric) 2x2 via the closed-form solution, so
* nonsymmetric 2x2 inputs also work.
*
* @note The input matrix is NOT modified (the iteration runs on a local
* copy), mirroring the SVD::SVD convention.
*
* @note EMBEDDED CONSTRAINT -- no heap. All working storage is stack
* allocated as templated Matrix<N,N> buffers. Peak stack usage per
* call is 3 * N^2 floats (A working copy + U and V accumulators) =
* 12 * N^2 bytes:
* N = 5 -> ~0.3 KB
* N = 10 -> ~1.2 KB
* N = 20 -> ~4.8 KB
* N = 50 -> ~30 KB
* N = 100 -> ~120 KB
* N = 255 -> ~783 KB
* Instantiate only the sizes that fit your call-stack budget.
*
* @note Conventions:
* - Eigenvalues come out sorted DESCENDING (largest first); the
* eigenvector columns are swapped to match.
* - Eigenvector signs are arbitrary (v and -v are both valid);
* tests must be sign-invariant.
* - Wilkinson shift: the eigenvalue of the trailing 2x2 block
* closest to the bottom-right corner (Trefethen & Bau 13.4.1).
*
* @note Algorithm (Trefethen & Bau 13.4, Golub & Van Loan 8.4.3):
* Phase 0 (N >= 3): Givens tridiagonalization. A general symmetric
* matrix is NOT suitable for implicit QR (the bulge chase only
* preserves the tridiagonal structure), so first reduce A with
* adjacent Givens similarities A <- G A G^T (rotations applied
* BOTTOM-UP, i = N-2 down to k+1, per column k), accumulating
* U <- U G^T, until A is symmetric tridiagonal and
* A_orig = U A U^T. (N = 2 needs no reduction.)
* Phase 1: iterate until A is diagonal:
* 1. Deflate: zero out subdiagonal entries at/under the tolerance
* (scaled by the adjacent diagonal magnitudes).
* 2. Scan for the trailing unreduced block [lo, hi].
* - block of size 1: A[hi][hi] is a converged eigenvalue, done.
* - block of size 2: solve the 2x2 eigenproblem in closed form
* and fold its eigenvector matrix into V.
* - block larger: one implicit Wilkinson-shifted QR step
* (bulge chasing with Givens rotations; the shift is applied
* to the ENTIRE active block [lo, hi], not just the trailing
* 2x2 -- the first Givens rotation must be formed from
* (A[lo][lo] - mu, A[lo+1][lo])). Every rotation is folded
* into V.
* Phase 2: eigenvalues = diag(A), sorted DESCENDING (eigenvector
* columns swapped to match), and the true eigenvectors of the
* ORIGINAL matrix are U * V.
*
* @note If maxIterations is exhausted before convergence the best-effort
* (partially diagonalized) values on the diagonal are returned.
*/
namespace QR {
/**
* @brief Compute the eigenvalues and eigenvectors of a square matrix
*
* @param matrixToDecompose The matrix to take eigenvalues of (not
* modified). MUST be symmetric for N >= 3.
* @param eigenVectors a buffer that will contain the eigenvectors in its
* COLUMNS, sorted by descending eigenvalue (column i is the
* eigenvector for eigenValues[i]).
* @param eigenValues a buffer that will contain the eigenvalues sorted
* DESCENDING (largest first).
* @param maxIterations the number of QR steps to perform before giving up
* on reaching the given tolerance
* @param tolerance the level of accuracy to obtain before stopping; a
* subdiagonal entry is deflated when |A[i+1][i]| <= tolerance *
* (|A[i][i]| + |A[i+1][i+1]|). For float32 arithmetic, values
* around 1e-6 are a sensible choice (single-precision epsilon is
* ~1.2e-7).
*/
template <uint8_t N>
void EigenQR(Matrix<N, N> &matrixToDecompose, Matrix<N, N> &eigenVectors,
Matrix<N, 1> &eigenValues, uint32_t maxIterations, float tolerance);
/**
* @brief Apply the similarity transform A <- G A G^T on rows/cols (i, i+1)
*
* G = [ c s ] on the (i, i+1) block, identity elsewhere, where G is the
* [ -s c ]
* ZEROING rotation (G * (x, y)^T = (r, 0)^T) -- the orientation used by
* the implicit QR chase: A = Q R with Q = G^T gives the next iterate
* R Q = G A G^T. With (c, s) = GivensRotation(A[i][i], A[i+1][i]) the
* (i+1, i) entry is zeroed by the left multiplication and the bulge is
* chased along the superdiagonal by the right one. The matrix must be
* symmetric on entry (guaranteed by construction in the QR iteration:
* symmetric input stays symmetric under similarity by an orthogonal
* matrix). Updates the full matrix, not just the tridiagonal structure.
*/
template <uint8_t N>
static void ApplyRotationBothSides(Matrix<N, N> &A, uint8_t i, float c,
float s);
/**
* @brief Accumulate eigenvectors: V <- V G^T on columns (i, i+1)
*
* G^T = [ c -s ] on columns (i, i+1), identity elsewhere, where G =
* [ s c ]
* [ c, s ] / [ -s, c ] is the zeroing rotation paired with
* ApplyRotationBothSides. Applied to all rows:
* V[r][i] -> c V[r][i] + s V[r][i+1]
* V[r][i+1] -> -s V[r][i] + c V[r][i+1]
*
* Every QR step's rotation is folded into V this way so that, together
* with A <- G A G^T, the invariant A_orig = V A V^T is preserved at every
* step (each step is A <- R Q with Q = G^T the orthogonal factor, and
* the orthogonal factors multiply as G1^T G2^T ... in application order).
* At convergence A_orig = V D V^T and the columns of V are the
* eigenvectors.
*/
template <uint8_t N>
static void ApplyRotationToVectors(Matrix<N, N> &V, uint8_t i, float c,
float s);
/**
* @brief Solve the 2x2 eigenproblem of block rows/cols (lo, lo+1)
*
* Solves the (possibly nonsymmetric) 2x2 block
* [ A[lo][lo] A[lo][lo+1] ]
* [ A[lo+1][lo] A[lo+1][lo+1] ]
* in closed form (characteristic polynomial + eigenvector back-substitution).
*
* @param A the matrix containing the block (not modified)
* @param lo the row/col index of the top-left corner of the block
* @param lambdaHi (out) the LARGER eigenvalue
* @param lambdaLo (out) the smaller eigenvalue
* @param c (out), s (out) eigenvector pair as an orthogonal matrix
* Vblock = [ c -s ] whose columns are the eigenvectors: column 0
* [ s c ]
* (c, s) is the unit eigenvector for lambdaHi, column 1 (-s, c) is
* the unit eigenvector for lambdaLo.
*
* Note: the caller applies Vblock to its eigenvector accumulator with
* V <- V * Vblock (i.e. V[r][lo] = c*x + s*y,
* V[r][lo+1] = -s*x + c*y). Vblock has the
* SAME [ c -s; s c ] form as the G^T factor used by
* ApplyRotationToVectors, so both folding operations follow one uniform
* convention.
*/
template <uint8_t N>
static void Solve2x2Eigen(const Matrix<N, N> &A, uint8_t lo, float &lambdaHi,
float &lambdaLo, float &c, float &s);
/**
* @brief Deflate (zero out) subdiagonal entries that are at/under tolerance
*
* For each i in [lo, hi): if |A[i+1][i]| <= tolerance *
* (|A[i][i]| + |A[i+1][i+1]|), sets A[i+1][i] = A[i][i+1] = 0, splitting
* the matrix into smaller independent blocks.
*/
template <uint8_t N>
static void Deflate(Matrix<N, N> &A, uint8_t lo, uint8_t hi, float tolerance);
/**
* @brief Reduce a symmetric matrix to symmetric tridiagonal form
*
* Chases each column's entries below the subdiagonal to zero with
* adjacent Givens similarities (Golub & Van Loan 8.3.1, Givens variant):
* for column k = 0..N-3, rotations on (N-2, N-1), (N-3, N-2), ...
* (k+1, k+2) -- BOTTOM-UP, each formed from the current (A[i][k],
* A[i+1][k]) -- zero A[k+2..N-1, k] one by one. A top-down pass would not
* work: the rotation that zeros A[i+1][k] would be undone by the later
* rotation on (i+1, i+2) forming a new nonzero at A[i][k]. Each rotation
* is applied to A as a similarity (A <- G A G^T) and accumulated into U
* (U <- U G^T), so on return:
* - A is symmetric tridiagonal (off-tridiagonal entries EXACTLY zero),
* - A_orig = U A U^T (i.e. U^T A_orig U = A).
*
* U is initialized to the identity internally (its input contents are
* ignored).
*/
template <uint8_t N>
static void Tridiagonalize(Matrix<N, N> &A, Matrix<N, N> &U);
} // namespace QR
#ifndef QR_H_
#include "QR.cpp"
#endif
src/Quaternion.cpp
+87 -87
View File
@@ -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) {
const float halfAngle = angle / 2;
const float sinHalfAngle = sin(halfAngle);
Matrix<1, 3> normalizedAxis = axis / axis.EuclideanNorm();
return Quaternion{static_cast<float>(cos(halfAngle)),
normalizedAxis.Get(0, 0) * sinHalfAngle,
normalizedAxis.Get(0, 1) * sinHalfAngle,
normalizedAxis.Get(0, 2) * sinHalfAngle};
Quaternion Quaternion::FromAngleAndAxis(float angle, const Matrix<1, 3> &axis)
{
const float halfAngle = angle / 2;
const float sinHalfAngle = sin(halfAngle);
Matrix<1, 3> normalizedAxis{};
axis.Normalize(normalizedAxis);
return Quaternion{
static_cast<float>(cos(halfAngle)),
normalizedAxis.Get(0, 0) * sinHalfAngle,
normalizedAxis.Get(0, 1) * sinHalfAngle,
normalizedAxis.Get(0, 2) * sinHalfAngle};
}
float Quaternion::operator[](uint8_t index) const {
if (index < 4) {
return this->matrix[index];
}
float Quaternion::operator[](uint8_t index) const
{
if (index < 4)
{
return this->matrix[index];
}
// index out of bounds
return 1e+6;
// index out of bounds
return 1e+6;
}
void Quaternion::operator=(const Quaternion &other) {
memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
void Quaternion::operator=(const Quaternion &other)
{
memcpy(&(this->matrix), &(other.matrix), 4 * sizeof(float));
}
Quaternion Quaternion::operator*(const Quaternion &other) const {
Quaternion result{};
this->Q_Mult(other, result);
return result;
Quaternion Quaternion::operator*(const Quaternion &other) const
{
Quaternion result{};
this->Q_Mult(other, result);
return result;
}
Quaternion Quaternion::operator*(float scalar) const {
return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar,
this->v3 * scalar};
Quaternion Quaternion::operator*(float scalar) const
{
return Quaternion{this->w * scalar, this->v1 * scalar, this->v2 * scalar, this->v3 * scalar};
}
Quaternion Quaternion::operator+(const Quaternion &other) const {
return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2,
this->v3 + other.v3};
Quaternion Quaternion::operator+(const Quaternion &other) const
{
return Quaternion{this->w + other.w, this->v1 + other.v1, this->v2 + other.v2, this->v3 + other.v3};
}
Quaternion &Quaternion::Q_Mult(const Quaternion &other,
Quaternion &buffer) const {
Quaternion &
Quaternion::Q_Mult(const Quaternion &other, Quaternion &buffer) const
{
// eq. 6
buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 -
other.v3 * this->v3);
buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 +
other.v3 * this->v2);
buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w -
other.v3 * this->v1);
buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 +
other.v3 * this->w);
return buffer;
// eq. 6
buffer.w = (other.w * this->w - other.v1 * this->v1 - other.v2 * this->v2 - other.v3 * this->v3);
buffer.v1 = (other.w * this->v1 + other.v1 * this->w - other.v2 * this->v3 + other.v3 * this->v2);
buffer.v2 = (other.w * this->v2 + other.v1 * this->v3 + other.v2 * this->w - other.v3 * this->v1);
buffer.v3 = (other.w * this->v3 - other.v1 * this->v2 + other.v2 * this->v1 + other.v3 * this->w);
return buffer;
}
Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const {
Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
buffer.v1 = other.v1;
buffer.v2 = other.v2;
buffer.v3 = other.v3;
buffer.w = 0;
Quaternion &Quaternion::Rotate(Quaternion &other, Quaternion &buffer) const
{
Quaternion prime{this->w, -this->v1, -this->v2, -this->v3};
buffer.v1 = other.v1;
buffer.v2 = other.v2;
buffer.v3 = other.v3;
buffer.w = 0;
Quaternion temp{};
this->Q_Mult(buffer, temp);
temp.Q_Mult(prime, buffer);
return buffer;
Quaternion temp{};
this->Q_Mult(buffer, temp);
temp.Q_Mult(prime, buffer);
return buffer;
}
void Quaternion::Normalize() {
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 +
this->v3 * this->v3 + this->w * this->w);
if (magnitude == 0) {
return;
}
this->v1 /= magnitude;
this->v2 /= magnitude;
this->v3 /= magnitude;
this->w /= magnitude;
void Quaternion::Normalize()
{
float magnitude = sqrt(this->v1 * this->v1 + this->v2 * this->v2 + this->v3 * this->v3 + this->w * this->w);
if (magnitude == 0)
{
return;
}
this->v1 /= magnitude;
this->v2 /= magnitude;
this->v3 /= magnitude;
this->w /= magnitude;
}
Matrix<3, 3> Quaternion::ToRotationMatrix() const {
float xx = this->v1 * this->v1;
float yy = this->v2 * this->v2;
float zz = this->v3 * this->v3;
Matrix<3, 3> rotationMatrix{1 - 2 * (yy - zz),
2 * (this->v1 * this->v2 - this->v3 * this->w),
2 * (this->v1 * this->v3 + this->v2 * this->w),
2 * (this->v1 * this->v2 + this->v3 * this->w),
1 - 2 * (xx - zz),
2 * (this->v2 * this->v3 - this->v1 * this->w),
2 * (this->v1 * this->v3 - this->v2 * this->w),
2 * (this->v2 * this->v3 + this->v1 * this->w),
1 - 2 * (xx - yy)};
return rotationMatrix;
Matrix<3, 3> Quaternion::ToRotationMatrix() const
{
float xx = this->v1 * this->v1;
float yy = this->v2 * this->v2;
float zz = this->v3 * this->v3;
Matrix<3, 3> rotationMatrix{
1 - 2 * (yy - zz), 2 * (this->v1 * this->v2 - this->v3 * this->w), 2 * (this->v1 * this->v3 + this->v2 * this->w),
2 * (this->v1 * this->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 2 * (this->v2 * this->v3 - this->v1 * this->w),
2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)};
return rotationMatrix;
};
Matrix<3, 1> Quaternion::ToEulerAngle() const {
float sqv1 = this->v1 * this->v1;
float sqv2 = this->v2 * this->v2;
float sqv3 = this->v3 * this->v3;
float sqw = this->w * this->w;
Matrix<3, 1> Quaternion::ToEulerAngle() const
{
float sqv1 = this->v1 * this->v1;
float sqv2 = this->v2 * this->v2;
float sqv3 = this->v3 * this->v3;
float sqw = this->w * this->w;
Matrix<3, 1> eulerAngle;
{
atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w),
(sqv1 - sqv2 - sqv3 + sqw));
asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) /
(sqv1 + sqv2 + sqv3 + sqw));
atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w),
(-sqv1 - sqv2 + sqv3 + sqw));
};
return eulerAngle;
Matrix<3, 1> eulerAngle;
{
atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw));
asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw));
atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw));
};
return eulerAngle;
}
src/Quaternion.h
+71 -70
View File
@@ -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(float w, float v1, float v2, float v3)
: Matrix<1, 4>(w, v1, v2, v3) {}
Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {}
Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {}
Quaternion(const std::array<float, 4> &array) : Matrix<1, 4>(array) {}
Quaternion() : Matrix<1, 4>() {}
Quaternion(float fillValue) : Matrix<1, 4>(fillValue) {}
Quaternion(float w, float v1, float v2, float v3) : Matrix<1, 4>(w, v1, v2, v3) {}
Quaternion(const Quaternion &q) : Matrix<1, 4>(q.w, q.v1, q.v2, q.v3) {}
Quaternion(const Matrix<1, 4> &matrix) : Matrix<1, 4>(matrix) {}
Quaternion(const std::array<float, 4> &array) : Matrix<1, 4>(array) {}
/**
* @brief Create a quaternion from an angle and axis
* @param angle The angle to rotate by
* @param axis The axis to rotate around
*/
static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis);
/**
* @brief Create a quaternion from an angle and axis
* @param angle The angle to rotate by
* @param axis The axis to rotate around
*/
static Quaternion FromAngleAndAxis(float angle, const Matrix<1, 3> &axis);
/**
* @brief Access the elements of the quaternion
* @param index The index of the element to access
* @return The value of the element at the index
*/
float operator[](uint8_t index) const;
/**
* @brief Access the elements of the quaternion
* @param index The index of the element to access
* @return The value of the element at the index
*/
float operator[](uint8_t index) const;
/**
* @brief Assign one quaternion to another
*/
void operator=(const Quaternion &other);
/**
* @brief Assign one quaternion to another
*/
void operator=(const Quaternion &other);
/**
* @brief Do quaternion multiplication
*/
Quaternion operator*(const Quaternion &other) const;
/**
* @brief Do quaternion multiplication
*/
Quaternion operator*(const Quaternion &other) const;
/**
* @brief Multiply the quaternion by a scalar
*/
Quaternion operator*(float scalar) const;
/**
* @brief Multiply the quaternion by a scalar
*/
Quaternion operator*(float scalar) const;
/**
* @brief Add two quaternions together
* @param other The quaternion to add to this one
* @return The net quaternion
*/
Quaternion operator+(const Quaternion &other) const;
/**
* @brief Add two quaternions together
* @param other The quaternion to add to this one
* @return The net quaternion
*/
Quaternion operator+(const Quaternion &other) const;
/**
* @brief Q_Mult a quaternion by another quaternion
* @param other The quaternion to rotate by
* @param buffer The buffer to store the result in
* @return A reference to the buffer
*/
Quaternion &Q_Mult(const Quaternion &other, Quaternion &buffer) const;
/**
* @brief Q_Mult a quaternion by another quaternion
* @param other The quaternion to rotate by
* @param buffer The buffer to store the result in
* @return A reference to the buffer
*/
Quaternion &Q_Mult(const Quaternion &other, Quaternion &buffer) const;
/**
* @brief Rotate a quaternion by this quaternion
* @param other The quaternion to rotate
* @param buffer The buffer to store the result in
*
*/
Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
/**
* @brief Rotate a quaternion by this quaternion
* @param other The quaternion to rotate
* @param buffer The buffer to store the result in
*
*/
Quaternion &Rotate(Quaternion &other, Quaternion &buffer) const;
/**
* @brief Normalize the quaternion to a magnitude of 1
*/
void Normalize();
/**
* @brief Normalize the quaternion to a magnitude of 1
*/
void Normalize();
/**
* @brief Convert the quaternion to a rotation matrix
* @return The rotation matrix
*/
Matrix<3, 3> ToRotationMatrix() const;
/**
* @brief Convert the quaternion to a rotation matrix
* @return The rotation matrix
*/
Matrix<3, 3> ToRotationMatrix() const;
/**
* @brief Convert the quaternion to an Euler angle representation
* @return The Euler angle representation of the quaternion
*/
Matrix<3, 1> ToEulerAngle() const;
/**
* @brief Convert the quaternion to an Euler angle representation
* @return The Euler angle representation of the quaternion
*/
Matrix<3, 1> ToEulerAngle() const;
// Give people an easy way to access the elements
float &w{matrix[0]};
float &v1{matrix[1]};
float &v2{matrix[2]};
float &v3{matrix[3]};
// Give people an easy way to access the elements
float &w{matrix[0]};
float &v1{matrix[1]};
float &v2{matrix[2]};
float &v3{matrix[3]};
};
#endif // QUATERNION_H_
src/SVD.cpp
-1004
View File
File diff suppressed because it is too large Load Diff
src/SVD.hpp
-411
View File
@@ -1,411 +0,0 @@
#pragma once
#include "Matrix.hpp"
/**
* @brief library that uses Matrix.hpp and performs SVD on a matrix
*
* @note Fully templated: SVD works for ANY Matrix<R, C> with R, C in
* 1..255 (the uint8_t range of Matrix). There is no 5×5 limit.
*
* @note EMBEDDED CONSTRAINT — no heap. All working storage is stack
* allocated as templated Matrix<N,N> buffers where
* N = max(R, C). Peak stack usage per SVD call is
* ≈ 11·N² floats (≈ 44·N² bytes):
* N = 5 → ~1.1 KB
* N = 10 → ~4.4 KB
* N = 20 → ~18 KB
* N = 50 → ~110 KB
* N = 100 → ~440 KB
* N = 255 → ~2.9 MB
* Instantiate only the sizes that fit your call-stack budget.
*/
namespace SVD {
/**
* @brief Compute the Singular Value Decomposition (SVD) of this matrix.
*
* Decomposes A into U × Σ × Vᵀ where:
* - U is an m×k orthogonal matrix (left singular vectors)
* - Σ is a k×k diagonal matrix with non-negative singular values
* (stored as a k×1 column vector)
* - Vᵀ is a k×n orthogonal matrix (right singular vectors, transposed)
* - k = min(m, n)
*
* The decomposition satisfies: A ≈ U × diag(Σ) × Vᵀ
* Singular values are returned in descending order.
*
* Output storage conventions:
* - U: Matrix<rows, columns> — first k columns are meaningful
* (rows k..columns−1 are zero in the wide case)
* - sigma: Matrix<columns, 1> — first k entries are the singular
* values; entries beyond k (wide matrices only) are zero
* - Vt: Matrix<columns, columns> — first k rows are meaningful
* (zero-padded in the tall case)
*
* For wide matrices (rows < columns) the SVD is computed on Aᵀ and the
* factors are swapped back.
*
* @tparam rows Number of rows in A (1..255)
* @tparam columns Number of columns in A (1..255)
* @param matrixToDecompose Input: the matrix A
* @param U Output: left singular vectors (rows×columns matrix)
* @param sigma Output: singular values (columns×1 vector, sorted descending)
* @param Vt Output: right singular vectors transposed (columns×columns)
*
* @note This implementation uses Householder bidiagonalization followed
* by block reduction: 2×2 blocks via closed form, larger blocks
* via cyclic Jacobi eigen-decomposition of BᵀB with residual
* singular values σᵢ = ‖B·vᵢ‖ (see docs/svd-refactor.md).
*/
template <uint8_t rows, uint8_t columns>
void SVD(Matrix<rows, columns> &matrixToDecompose, Matrix<rows, columns> &U,
Matrix<columns, 1> &sigma, Matrix<columns, columns> &Vt);
// ========================================================================
// SVD Building Block Functions (for unit testing)
//
// Templated on the working-buffer size N. All block operations work on
// N×N matrices with runtime bounds (m, n, p, blockSize, ...) — the
// regions beyond the bounds are zero-padded working space.
//
// N is deduced from the Matrix arguments at the call site, e.g.
// Matrix<8, 8> W, QL, QR;
// SVD::Bidiagonalize(W, 6, 8, 6, QL, QR); // N = 8 deduced
// ========================================================================
/**
* @brief Compute a Householder reflector vector.
*
* Given input vector x, computes normalized v and scalar alpha such that:
* (I - 2·v·vᵀ) · x = [alpha, 0, 0, ...]ᵀ
*
* @param x Input vector (up to len elements)
* @param len Number of valid elements in x
* @param v Output: normalized Householder vector (length ≥ len)
* @param alpha Output: the resulting first element after reflection
* @return The norm of the input vector x
*/
static float ComputeHouseholder(const float *x, uint8_t len, float *v,
float &alpha);
/**
* @brief Apply a Householder reflection from the left.
*
* Transforms W = (I - 2·v·vᵀ) · W where v operates on rows [startRow..endRow]
* and is applied across all N columns (zero-padded columns are a no-op).
*
* @tparam N Working buffer size
* @param W Input/output: matrix to transform
* @param v Householder vector (length = endRow - startRow + 1)
* @param startRow First row index
* @param endRow Last row index
*/
template <uint8_t N>
static void ApplyHouseholderLeft(Matrix<N, N> &W, const float *v,
uint8_t startRow, uint8_t endRow);
/**
* @brief Apply a Householder reflection from the right.
*
* Transforms W = W · (I - 2·v·vᵀ) where v operates on columns
* [startCol..endCol] and is applied across all N rows (zero-padded rows
* are a no-op).
*
* @tparam N Working buffer size
* @param W Input/output: matrix to transform
* @param v Householder vector (length = endCol - startCol + 1)
* @param startCol First column index
* @param endCol Last column index
*/
template <uint8_t N>
static void ApplyHouseholderRight(Matrix<N, N> &W, const float *v,
uint8_t startCol, uint8_t endCol);
/**
* @brief Reduce a matrix to upper bidiagonal form using Householder reflections.
*
* Applies a sequence of Householder reflections to reduce the input matrix
* W (m×q, where q ≥ p, stored in N×N working space) to upper bidiagonal
* form B (p×q), accumulating the left and right transformation matrices
* in QL and QR respectively.
*
* Algorithm (Golub-Kahan bidiagonalization):
* For k = 0 to p-1:
* 1. Left HH on column k, rows k..m-1: zero out subdiagonal below B[k+1][k]
* 2. Right HH on row k, cols k+2..q-1: zero out superdiagonal above B[k][k+1]
*
* The accumulated transformations satisfy:
* QLᵀ · W_original · QR = B (upper bidiagonal)
*
* @tparam N Working buffer size (≥ m and ≥ q)
* @param W Input/output: matrix to bidiagonalize (first m×q used)
* @param m Number of rows in the working matrix
* @param q Number of columns in the working matrix (q ≥ p)
* @param p Rank = min(m, original_columns) — number of bidiagonalization steps
* @param QL Input/output: left Householder accumulation (initialized to identity)
* @param QR Input/output: right Householder accumulation (initialized to identity)
*/
template <uint8_t N>
static void Bidiagonalize(Matrix<N, N> &W, uint8_t m, uint8_t q, uint8_t p,
Matrix<N, N> &QL, Matrix<N, N> &QR);
/**
* @brief Deflate a bidiagonal matrix by zeroing negligible superdiagonals.
*
* Scans the p×p upper-bidiagonal matrix stored in W and zeros out any
* superdiagonal element W[i][i+1] whose magnitude is negligible relative
* to the local diagonal scale (|W[i][i]| + |W[i+1][i+1]|). Deflating
* splits the matrix into independent unreduced blocks that can each be
* solved separately.
*
* @tparam N Working buffer size
* @param W Input/output: bidiagonal matrix (first p×p used)
* @param p Size of the bidiagonal matrix (min(rows, columns))
* @param tol Relative deflation tolerance (e.g. 1e-8f)
*/
template <uint8_t N>
static void DeflateBidiagonal(Matrix<N, N> &W, uint8_t p, float tol);
/**
* @brief Check whether a bidiagonal matrix has fully reduced to diagonal.
*
* Returns true when every superdiagonal element of the p×p bidiagonal
* matrix in W is (numerically) zero, i.e. the diagonal entries are the
* (unsorted) singular values and no unreduced blocks remain.
*
* @tparam N Working buffer size
* @param W Input: bidiagonal matrix (first p×p used)
* @param p Size of the bidiagonal matrix (min(rows, columns))
* @param tol Numerical zero threshold multiplier
* @return true when all superdiagonal elements are ~0
*/
template <uint8_t N>
static bool BidiagonalIsDiagonal(const Matrix<N, N> &W, uint8_t p, float tol);
/**
* @brief Compute the full SVD of a 2×2 upper-bidiagonal block (pure).
*
* Decomposes B = [[a, b], [0, d]] as:
* B = Ublock · diag(sigma[0], sigma[1]) · Vblockᵀ
*
* Guarantees:
* - sigma[0] ≥ sigma[1] ≥ 0 (singular values, from eigenvalues of BᵀB)
* - Ublock and Vblock are orthogonal (columns are the left/right
* singular vectors respectively; Vblock = scipy's Vᵀᵀ)
* - Ublock · diag(sigma) · Vblockᵀ == B (within float tolerance)
*
* Math: eigenvectors of BᵀB = [[a², ab], [ab, b²+d²]] give the right
* singular vectors (v1 = normalize(ab, σ1²−a²) with a safe fallback when
* that vector is ~0; v2 = (−v1y, v1x)); left singular vectors are
* uᵢ = B·vᵢ/σᵢ with a rank-deficiency guard: when σᵢ ≈ 0 (i.e. ~1e-30),
* that U column is filled with the signed orthogonal complement of the
* other U column instead of dividing by ~0.
*
* @param a B[0][0] (first diagonal element)
* @param b B[0][1] (superdiagonal element)
* @param d B[1][1] (second diagonal element)
* @param Ublock Output: 2×2 left singular vectors (columns)
* @param Vblock Output: 2×2 right singular vectors (columns)
* @param sigma Output: singular values, sigma[0] ≥ sigma[1] ≥ 0
*/
static void SolveBidiagonalBlock2x2(float a, float b, float d,
float Ublock[2][2], float Vblock[2][2],
float sigma[2]);
/**
* @brief Cyclic Jacobi eigenvalue algorithm for a symmetric matrix (pure).
*
* Reduces symmetric n×n matrix T to (near-)diagonal form IN PLACE using
* cyclic Jacobi rotations, accumulating the eigenvectors in V.
*
* On return:
* - T's diagonal entries are the eigenvalues (off-diagonals ~0)
* - evals[i] = T[i][i], UNSORTED, SIGNED (this is a general symmetric
* eigen solver, not just for PSD matrices like T = BᵀB)
* - columns of V are the corresponding eigenvectors (T·V = V·Λ)
*
* Convergence: relative off-diagonal tolerance 1e-10, hard-capped at
* 100 sweeps.
*
* @tparam N Working buffer size (≥ n)
* @param T Input/output: symmetric matrix (first n×n used, destroyed in place)
* @param n Matrix size
* @param evals Output: eigenvalues, unsorted, length ≥ n
* @param V Output: eigenvector matrix (first n×n used), columns are eigenvectors
*/
template <uint8_t N>
static void JacobiEigenSymmetric(Matrix<N, N> &T, uint8_t n, float *evals,
Matrix<N, N> &V);
/**
* @brief Fold a block SVD's factors into the QL/QR accumulators.
*
* Given the block SVD of a bidiagonal block, B = Ublock·Σ·Vblockᵀ, the
* accumulated Householder matrices must absorb the block factors:
* QL[:, blockStart..blockStart+blockSize−1] ← QL[:, ...] · Ublock
* (rows 0..rowsQL−1)
* QR[:, blockStart..blockStart+blockSize−1] ← QR[:, ...] · Vblock
* (rows 0..rowsQR−1)
*
* rowsQL / rowsQR are the meaningful row extents of the accumulators
* (e.g. for a wide matrix W = Aᵀ, QL carries n = rows(W) meaningful
* rows while QR is read back over its first m rows).
*
* In-place update is done through temporary buffers (updating QL's block
* columns while still reading them corrupts the result).
*
* @tparam N Working buffer size
* @param blockStart First column/row index of the block in W
* @param blockSize Size of the block (2, or > 2 for the Jacobi path)
* @param Ublock Left singular-vector factor of the block (first blockSize×blockSize used)
* @param Vblock Right singular-vector factor of the block (first blockSize×blockSize used)
* @param rowsQL Number of meaningful rows of QL
* @param rowsQR Number of meaningful rows of QR
* @param QL Input/output: left transformation accumulator
* @param QR Input/output: right transformation accumulator
*/
template <uint8_t N>
static void ApplyBlockFactorsToAccumulators(uint8_t blockStart,
uint8_t blockSize,
const Matrix<N, N> &Ublock,
const Matrix<N, N> &Vblock,
uint8_t rowsQL, uint8_t rowsQR,
Matrix<N, N> &QL,
Matrix<N, N> &QR);
/**
* @brief Solve a bidiagonal block larger than 2×2 via Jacobi eigen of BᵀB.
*
* Computes the full SVD of the unreduced upper-bidiagonal block
* W[blockStart..blockStart+blockSize−1] via eigendecomposition of the
* tridiagonal T = BᵀB:
* 1. Snapshot the ORIGINAL block diagonal/superdiagonal from W
* 2. Form T = BᵀB (tridiagonal symmetric)
* 3. JacobiEigenSymmetric on T → eigenvalues (unsorted) + V
* 4. Sort eigenvalues descending, reordering V columns
* 5. Compute RESIDUAL singular values: σᵢ = ‖B_orig · vᵢ‖
* (NOT sqrt(eigenvalue) — forming BᵀB squares the condition number,
* causing float noise to swamp true tiny eigenvalues for
* rank-deficient blocks)
* 6. Re-sort σ descending, keeping V and B·v consistent
* 7. Build Ublock: uᵢ = B_orig · vᵢ / σᵢ (unit norm); for σᵢ ≈ 0,
* use Gram-Schmidt orthogonal completion against prior U columns
* 8. Fold Ublock/Vblock into QL/QR via ApplyBlockFactorsToAccumulators
* 9. Write residual norms into W's diagonal and zero the block's
* superdiagonals
*
* @tparam N Working buffer size (≥ blockSize)
* @param W Input/output: bidiagonal matrix; the block's diagonal holds
* the singular values and its superdiagonals are zeroed on return
* @param blockStart First column/row index of the block
* @param blockSize Size of the block (> 2)
* @param rowsQL Number of meaningful rows of QL
* @param rowsQR Number of meaningful rows of QR
* @param QL Input/output: left transformation accumulator
* @param QR Input/output: right transformation accumulator
*/
template <uint8_t N>
static void SolveBidiagonalBlockJacobi(Matrix<N, N> &W, uint8_t blockStart,
uint8_t blockSize, uint8_t rowsQL,
uint8_t rowsQR, Matrix<N, N> &QL,
Matrix<N, N> &QR);
/**
* @brief Extract singular values from bidiagonal matrix diagonal and sort.
*
* Extracts absolute values of diagonal elements of W as singular values,
* then sorts them in descending order while reordering columns of QL
* and QR to maintain consistency. A negative diagonal element flips the
* sign of the corresponding QL column to keep A = U·Σ·Vᵀ.
*
* @tparam N Working buffer size
* @param W Input: bidiagonal matrix (first p×p used)
* @param sigma Output: sorted singular values (N×1 column vector, only first p used)
* @param p Number of singular values (min(rows, columns))
* @param QL Input/output: left transformation matrix (modified during sort)
* @param QR Input/output: right transformation matrix (modified during sort)
*/
template <uint8_t N>
static void ExtractAndSortSingularValues(Matrix<N, N> &W, Matrix<N, 1> &sigma,
uint8_t p, Matrix<N, N> &QL,
Matrix<N, N> &QR);
/**
* @brief Assemble final U and Vt matrices from QL/QR.
*
* Computes the final left singular vectors (U) and right singular vectors
* transposed (Vt) from the accumulated Householder transformations.
*
* For non-transpose case: U = QL[:,0:p], Vt = QR[:,0:p]ᵀ
* For transpose case: U = QR[:,0:p], Vt = full QLᵀ (all n rows)
*
* @tparam N Working buffer size (≥ m and ≥ n)
* @param m Number of rows in original matrix
* @param n Number of columns in original matrix
* @param p Rank = min(m, n)
* @param transposeNeeded True if we computed SVD(Aᵀ) instead of SVD(A)
* @param QL Left Householder accumulation (N×N)
* @param QR Right Householder accumulation (N×N)
* @param U Output: left singular vectors (N×N, first m×p used)
* @param Vt Output: right singular vectors transposed (N×N, first p×n used)
*/
template <uint8_t N>
static void AssembleUAndVt(uint8_t m, uint8_t n, uint8_t p,
bool transposeNeeded, const Matrix<N, N> &QL,
const Matrix<N, N> &QR, Matrix<N, N> &U,
Matrix<N, N> &Vt);
/**
* @brief Compute a Givens rotation that zeros out y.
*
* Computes c, s such that:
* [c s] [x] = [r]
* [-s c] [y] [0]
* where r = sqrt(x² + y²).
*
* @param x First element
* @param y Second element (to be zeroed)
* @param c Output: cosine of rotation angle
* @param s Output: sine of rotation angle
*/
static void ComputeGivens(float x, float y, float &c, float &s);
/**
* @brief Apply a Givens rotation from the left to rows i and j.
*
* Applies [c s; -s c] to rows i, j of W (columns startCol..endCol).
*
* @tparam N Working buffer size
* @param W Input/output: matrix to transform
* @param i First row index
* @param j Second row index
* @param c Cosine of rotation angle
* @param s Sine of rotation angle
* @param startCol First column to transform
* @param endCol Last column to transform
*/
template <uint8_t N>
static void ApplyGivensLeft(Matrix<N, N> &W, uint8_t i, uint8_t j, float c,
float s, uint8_t startCol, uint8_t endCol);
/**
* @brief Apply a Givens rotation from the right to columns i and j.
*
* Applies [c -s; s c]ᵀ to columns i, j of W (rows startRow..endRow).
*
* @tparam N Working buffer size
* @param W Input/output: matrix to transform
* @param i First column index
* @param j Second column index
* @param c Cosine of rotation angle
* @param s Sine of rotation angle
* @param startRow First row to transform
* @param endRow Last row to transform
*/
template <uint8_t N>
static void ApplyGivensRight(Matrix<N, N> &W, uint8_t i, uint8_t j, float c,
float s, uint8_t startRow, uint8_t endRow);
} // namespace SVD
#ifndef SVD_H_
#include "SVD.cpp"
#endif
unit-tests/CMakeLists.txt
-31
View File
@@ -13,7 +13,6 @@ add_executable(matrix-tests matrix-tests.cpp)
target_link_libraries(matrix-tests
PRIVATE
matrix
qr
Catch2::Catch2WithMain
)
@@ -33,34 +32,4 @@ target_link_libraries(vector-3d-tests
PRIVATE
vector-3d
Catch2::Catch2WithMain
)
# SVD building block tests
add_executable(svd-build-blocks-tests svd-build-blocks-tests.cpp)
target_link_libraries(svd-build-blocks-tests
PRIVATE
matrix
svd
Catch2::Catch2WithMain
)
# SVD integration tests
add_executable(svd-integration-test svd-integration-test.cpp)
target_link_libraries(svd-integration-test
PRIVATE
matrix
svd
Catch2::Catch2WithMain
)
# QR building block tests
add_executable(qr-build-blocks-tests qr-build-blocks-tests.cpp)
target_link_libraries(qr-build-blocks-tests
PRIVATE
matrix
qr
Catch2::Catch2WithMain
)
unit-tests/matrix-tests.cpp
+59 -1252
View File
File diff suppressed because it is too large Load Diff
unit-tests/matrix-timing-tests.cpp
+15 -24
View File
@@ -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,60 +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++) {
float det = mat4.Det();
(void)det;
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++) {
mat3 = mat1 / mat1.EuclideanNorm();
for (uint32_t i{0}; i < 10000; i++) {
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/qr-build-blocks-tests.cpp
-581
View File
@@ -1,581 +0,0 @@
// include the unit test framework first
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
// include the module you're going to test next
#include "Matrix.hpp"
#include "QR.hpp"
// any other libraries
#include <array>
#include <cmath>
#include <iostream>
// ============================================================================
// Helpers
// ============================================================================
/**
* @brief Frobenius norm of an N x N matrix.
*/
template <uint8_t N>
static float frob(const Matrix<N, N> &M) {
float sum = 0.0f;
for (uint8_t i = 0; i < N; i++)
for (uint8_t j = 0; j < N; j++) {
float v = M.Get(i, j);
sum += v * v;
}
return sqrtf(sum);
}
/**
* @brief Check M is orthogonal (M^T M ~ I).
*/
template <uint8_t N>
static bool isOrthogonal(const Matrix<N, N> &M, float tol = 1e-5f) {
Matrix<N, N> Mt = M.Transpose();
Matrix<N, N> MtM{};
Mt.Mult(M, MtM);
for (uint8_t i = 0; i < N; i++)
for (uint8_t j = 0; j < N; j++) {
float expected = (i == j) ? 1.0f : 0.0f;
if (fabsf(MtM.Get(i, j) - expected) > tol)
return false;
}
return true;
}
/**
* @brief 3x3 trace.
*/
static float trace3(const Matrix<3, 3> &A) {
return A.Get(0, 0) + A.Get(1, 1) + A.Get(2, 2);
}
/**
* @brief 3x3 sum of principal 2x2 minors (2nd elementary invariant).
*/
static float e2_3x3(const Matrix<3, 3> &A) {
return A.Get(0, 0) * A.Get(1, 1) - A.Get(0, 1) * A.Get(0, 1) +
A.Get(0, 0) * A.Get(2, 2) - A.Get(0, 2) * A.Get(0, 2) +
A.Get(1, 1) * A.Get(2, 2) - A.Get(1, 2) * A.Get(1, 2);
}
/**
* @brief 3x3 determinant.
*/
static float det3(const Matrix<3, 3> &A) {
return A.Get(0, 0) *
(A.Get(1, 1) * A.Get(2, 2) - A.Get(1, 2) * A.Get(2, 1)) -
A.Get(0, 1) *
(A.Get(1, 0) * A.Get(2, 2) - A.Get(1, 2) * A.Get(2, 0)) +
A.Get(0, 2) *
(A.Get(1, 0) * A.Get(2, 1) - A.Get(1, 1) * A.Get(2, 0));
}
/**
* @brief Sign-invariant comparison of |actual| against refAbs.
*/
static bool matchesAbs(float actual, float refAbs, float relTol = 1e-5f,
float absTol = 1e-6f) {
float a = fabsf(actual);
if (refAbs < 1e-3f)
return a < absTol + relTol;
return fabsf(a - refAbs) <= relTol * refAbs;
}
// ============================================================================
// TEST 1: GivensRotation
// ============================================================================
TEST_CASE("QR Building Block: GivensRotation", "[Matrix][QR]") {
// R = [[c, s], [-s, c]] must satisfy R * (a, b)^T = (r, 0)^T.
{
// Reference: hypot(2, 1) = sqrt(5) = 2.236067977
float c = 0, s = 0;
QR::GivensRotation(2.0f, 1.0f, c, s);
REQUIRE_THAT(c, Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
REQUIRE_THAT(c * 2.0f + s * 1.0f,
Catch::Matchers::WithinRel(2.236067977f, 1e-6f));
REQUIRE_THAT(-s * 2.0f + c * 1.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}
{
// Reference: hypot(3, 4) = 5 exactly
float c = 0, s = 0;
QR::GivensRotation(3.0f, 4.0f, c, s);
REQUIRE_THAT(c, Catch::Matchers::WithinRel(0.6f, 1e-6f));
REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.8f, 1e-6f));
REQUIRE_THAT(c * 3.0f + s * 4.0f, Catch::Matchers::WithinRel(5.0f, 1e-6f));
REQUIRE_THAT(-s * 3.0f + c * 4.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}
{
// Pure second component: c = 0, s = 1
float c = 1, s = 1;
QR::GivensRotation(0.0f, 5.0f, c, s);
REQUIRE_THAT(c, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(s, Catch::Matchers::WithinRel(1.0f, 1e-6f));
}
{
// Zero vector: identity rotation
float c = 0, s = 0;
QR::GivensRotation(0.0f, 0.0f, c, s);
REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-7f));
REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
}
{
// Negative first component preserves the sign of c
float c = 0, s = 0;
QR::GivensRotation(-2.0f, 1.0f, c, s);
REQUIRE_THAT(c, Catch::Matchers::WithinRel(-0.894427191f, 1e-6f));
REQUIRE_THAT(s, Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
REQUIRE_THAT(-s * -2.0f + c * 1.0f, Catch::Matchers::WithinAbs(0.0f, 1e-6f));
}
}
// ============================================================================
// TEST 2: ApplyRotationBothSides (similarity A <- G A G^T)
// ============================================================================
TEST_CASE("QR Building Block: ApplyRotationBothSides", "[Matrix][QR]") {
// Reference (numpy, float64): A = [[2,1,0],[1,3,1],[0,1,4]], i = 0,
// Givens(2,1) -> G A G^T =
// [[ 3.0, 1.0, 0.447213595],
// [ 1.0, 2.0, 0.894427191],
// [ 0.447213595, 0.894427191, 4.0]]
// (Note: G A G^T with G zeroing (2,1) sends the A[0][1] coupling into the
// (0,2) corner, NOT into the subdiagonal -- the subdiagonal-zeroing happens
// in the QR chase context where the bulge column has the right shape.)
{
Matrix<3, 3> A{2, 1, 0, 1, 3, 1, 0, 1, 4};
float c = 0.894427191f, s = 0.447213595f;
QR::ApplyRotationBothSides(A, 0, c, s);
REQUIRE_THAT(A.Get(0, 0), Catch::Matchers::WithinRel(3.0f, 1e-5f));
REQUIRE_THAT(A.Get(0, 1), Catch::Matchers::WithinRel(1.0f, 1e-5f));
REQUIRE_THAT(A.Get(0, 2),
Catch::Matchers::WithinRel(0.447213595f, 1e-5f));
REQUIRE_THAT(A.Get(1, 1), Catch::Matchers::WithinRel(2.0f, 1e-5f));
REQUIRE_THAT(A.Get(1, 2),
Catch::Matchers::WithinRel(0.894427191f, 1e-5f));
REQUIRE_THAT(A.Get(2, 2), Catch::Matchers::WithinRel(4.0f, 1e-5f));
// Symmetry must be preserved exactly in both triangles
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++)
REQUIRE(A.Get(i, j) == A.Get(j, i));
}
// Same check at i = 1.
// Reference (numpy, float64): B = [[5,0,1],[0,6,2],[1,2,7]], i = 1,
// Givens(6,2) -> G B G^T =
// [[ 5.0, 0.316227766, 0.948683298],
// [ 0.316227766, 7.3, 1.9],
// [ 0.948683298, 1.9, 5.7]]
{
Matrix<3, 3> B{5, 0, 1, 0, 6, 2, 1, 2, 7};
float c = 0.948683298f, s = 0.316227766f;
QR::ApplyRotationBothSides(B, 1, c, s);
REQUIRE_THAT(B.Get(0, 0), Catch::Matchers::WithinRel(5.0f, 1e-5f));
REQUIRE_THAT(B.Get(0, 1),
Catch::Matchers::WithinRel(0.316227766f, 1e-5f));
REQUIRE_THAT(B.Get(0, 2),
Catch::Matchers::WithinRel(0.948683298f, 1e-5f));
REQUIRE_THAT(B.Get(1, 1), Catch::Matchers::WithinRel(7.3f, 1e-5f));
REQUIRE_THAT(B.Get(1, 2), Catch::Matchers::WithinRel(1.9f, 1e-5f));
REQUIRE_THAT(B.Get(2, 2), Catch::Matchers::WithinRel(5.7f, 1e-5f));
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++)
REQUIRE(B.Get(i, j) == B.Get(j, i));
}
// Identity rotation leaves the matrix unchanged
{
Matrix<3, 3> C{1, 2, 3, 2, 4, 5, 3, 5, 6};
QR::ApplyRotationBothSides(C, 1, 1.0f, 0.0f);
REQUIRE(C.Get(0, 0) == 1.0f);
REQUIRE(C.Get(0, 1) == 2.0f);
REQUIRE(C.Get(0, 2) == 3.0f);
REQUIRE(C.Get(1, 1) == 4.0f);
REQUIRE(C.Get(1, 2) == 5.0f);
REQUIRE(C.Get(2, 2) == 6.0f);
}
// Spectrum invariants (trace, Frobenius norm) are preserved. (c, s)
// must be a unit vector for G A G^T to be a similarity transform.
{
Matrix<3, 3> D{1, 2, 3, 2, 5, 8, 3, 8, 9};
float tr = trace3(D);
float fn = frob(D);
float c = 0.6f, s = 0.8f;
QR::ApplyRotationBothSides(D, 0, c, s);
REQUIRE_THAT(trace3(D), Catch::Matchers::WithinRel(tr, 1e-5f));
REQUIRE_THAT(frob(D), Catch::Matchers::WithinRel(fn, 1e-5f));
}
}
// ============================================================================
// TEST 3: ApplyRotationToVectors (V <- V G^T)
// ============================================================================
TEST_CASE("QR Building Block: ApplyRotationToVectors", "[Matrix][QR]") {
// V = I, i = 0, Givens(2,1): V <- I * G^T with G^T = [[c, -s], [s, c]] =
// [[ c, -s, 0],
// [ s, c, 0],
// [ 0, 0, 1]]
{
Matrix<3, 3> V{0};
V[0][0] = 1;
V[1][1] = 1;
V[2][2] = 1;
float c = 0.894427191f, s = 0.447213595f;
QR::ApplyRotationToVectors(V, 0, c, s);
REQUIRE_THAT(V.Get(0, 0), Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
REQUIRE_THAT(V.Get(0, 1), Catch::Matchers::WithinRel(-0.447213595f, 1e-6f));
REQUIRE_THAT(V.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(V.Get(1, 0), Catch::Matchers::WithinRel(0.447213595f, 1e-6f));
REQUIRE_THAT(V.Get(1, 1), Catch::Matchers::WithinRel(0.894427191f, 1e-6f));
REQUIRE_THAT(V.Get(1, 2), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(V.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(V.Get(2, 1), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(V.Get(2, 2), Catch::Matchers::WithinRel(1.0f, 1e-7f));
// Product of rotations must stay orthogonal
REQUIRE(isOrthogonal(V));
}
// Two successive rotations accumulate (V <- V G1^T G2^T)
// Reference (numpy, float64):
// [[ 0.894427191, -0.424264069, 0.141421356],
// [ 0.447213595, 0.848528137, -0.282842712],
// [ 0.0, 0.316227766, 0.948683298]]
{
Matrix<3, 3> V{0};
V[0][0] = 1;
V[1][1] = 1;
V[2][2] = 1;
QR::ApplyRotationToVectors(V, 0, 0.894427191f, 0.447213595f);
QR::ApplyRotationToVectors(V, 1, 0.948683298f, 0.316227766f);
REQUIRE(isOrthogonal(V));
// Column 0 was only touched by the first rotation
REQUIRE_THAT(V.Get(0, 0), Catch::Matchers::WithinRel(0.894427191f, 1e-5f));
REQUIRE_THAT(V.Get(1, 0), Catch::Matchers::WithinRel(0.447213595f, 1e-5f));
REQUIRE_THAT(V.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(V.Get(0, 1), Catch::Matchers::WithinRel(-0.424264069f, 1e-5f));
REQUIRE_THAT(V.Get(0, 2), Catch::Matchers::WithinRel(0.141421356f, 1e-5f));
REQUIRE_THAT(V.Get(1, 2), Catch::Matchers::WithinRel(-0.282842712f, 1e-5f));
REQUIRE_THAT(V.Get(2, 1), Catch::Matchers::WithinRel(0.316227766f, 1e-5f));
REQUIRE_THAT(V.Get(2, 2), Catch::Matchers::WithinRel(0.948683298f, 1e-5f));
}
}
// ============================================================================
// TEST 4: WilkinsonShift
// ============================================================================
TEST_CASE("QR Building Block: WilkinsonShift", "[Matrix][QR]") {
// mu = (a+d)/2 - sign(a-d) * sqrt(((a-d)/2)^2 + b^2)
// Reference: eigenvalues of [[2,1],[1,4]] are 1.5858, 4.4142; closest
// to d = 4 is 4.414213562.
REQUIRE_THAT(QR::WilkinsonShift(2.0f, 1.0f, 4.0f),
Catch::Matchers::WithinRel(4.414213562f, 1e-6f));
// [[5,2],[2,1]]: eigenvalues 0.1716, 5.8284; closest to d = 1 is 0.171572875
REQUIRE_THAT(QR::WilkinsonShift(5.0f, 2.0f, 1.0f),
Catch::Matchers::WithinRel(0.171572875f, 1e-5f));
// Zero off-diagonal: returns d itself (sign(0) = +1 picks d, not a)
REQUIRE_THAT(QR::WilkinsonShift(3.0f, 0.0f, 7.0f),
Catch::Matchers::WithinRel(7.0f, 1e-7f));
REQUIRE_THAT(QR::WilkinsonShift(7.0f, 0.0f, 3.0f),
Catch::Matchers::WithinRel(3.0f, 1e-7f));
// a == d: shift is the larger-magnitude off-diagonal combination
// [[1,3],[3,1]]: eigenvalues -2, 4; closest to d = 1 is -2
REQUIRE_THAT(QR::WilkinsonShift(1.0f, 3.0f, 1.0f),
Catch::Matchers::WithinRel(-2.0f, 1e-6f));
}
// ============================================================================
// TEST 5: Solve2x2Eigen
// ============================================================================
TEST_CASE("QR Building Block: Solve2x2Eigen", "[Matrix][QR]") {
// Symmetric block [[2,1],[1,3]]:
// eigenvalues 1.381966011, 3.618033989;
// eigenvector of 3.618033989 is +/- (0.525731112, 0.850650808)
{
Matrix<2, 2> A{2, 1, 1, 3};
float lHi = 0, lLo = 0, c = 0, s = 0;
QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(3.618033989f, 1e-6f));
REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(1.381966011f, 1e-6f));
REQUIRE(matchesAbs(c, 0.525731112f));
REQUIRE(matchesAbs(s, 0.850650808f));
// Residual: A * vHi = lHi * vHi with vHi = (c, s)
REQUIRE_THAT(c * 2.0f + s * 1.0f,
Catch::Matchers::WithinRel(lHi * c, 1e-5f));
REQUIRE_THAT(c * 1.0f + s * 3.0f,
Catch::Matchers::WithinRel(lHi * s, 1e-5f));
// Second eigenvector vLo = (-s, c)
REQUIRE_THAT(-s * 2.0f + c * 1.0f,
Catch::Matchers::WithinRel(lLo * -s, 1e-5f));
REQUIRE_THAT(-s * 1.0f + c * 3.0f,
Catch::Matchers::WithinRel(lLo * c, 1e-5f));
}
// Nonsymmetric block [[1,2],[3,4]] (used by the N == 2 entry point):
// eigenvalues 5.372281323, -0.372281323;
// eigenvector of 5.372281323 is +/- (0.415973558, 0.909376709)
{
Matrix<2, 2> A{1, 2, 3, 4};
float lHi = 0, lLo = 0, c = 0, s = 0;
QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.372281323f, 1e-6f));
REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(-0.372281323f, 1e-6f));
REQUIRE(matchesAbs(c, 0.415973558f));
REQUIRE(matchesAbs(s, 0.909376709f));
// Both-row residual with vHi = (c, s): A v = l v
REQUIRE_THAT(c * 1.0f + s * 2.0f,
Catch::Matchers::WithinRel(lHi * c, 1e-5f));
REQUIRE_THAT(c * 3.0f + s * 4.0f,
Catch::Matchers::WithinRel(lHi * s, 1e-5f));
}
// Diagonal blocks: eigenvectors are coordinate vectors
{
Matrix<2, 2> A{5, 0, 0, 2};
float lHi = 0, lLo = 0, c = 0, s = 0;
QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.0f, 1e-7f));
REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(2.0f, 1e-7f));
REQUIRE_THAT(c, Catch::Matchers::WithinRel(1.0f, 1e-7f));
REQUIRE_THAT(s, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
A = Matrix<2, 2>{2, 0, 0, 5};
QR::Solve2x2Eigen(A, 0, lHi, lLo, c, s);
REQUIRE_THAT(lHi, Catch::Matchers::WithinRel(5.0f, 1e-7f));
REQUIRE_THAT(lLo, Catch::Matchers::WithinRel(2.0f, 1e-7f));
REQUIRE_THAT(c, Catch::Matchers::WithinAbs(0.0f, 1e-7f));
REQUIRE_THAT(s, Catch::Matchers::WithinRel(1.0f, 1e-7f));
}
}
// ============================================================================
// TEST 6: Deflate
// ============================================================================
TEST_CASE("QR Building Block: Deflate", "[Matrix][QR]") {
// subdiag[0] = 1e-9 <= 1e-6 * (|2| + |3|) = 5e-6 -> deflated
// subdiag[1] = 0.5 > 1e-6 * (|3| + |4|) = 7e-6 -> kept
{
Matrix<3, 3> A{2, 1e-9f, 0, 1e-9f, 3, 0.5f, 0, 0.5f, 4};
QR::Deflate(A, 0, 2, 1e-6f);
REQUIRE(A.Get(1, 0) == 0.0f);
REQUIRE(A.Get(0, 1) == 0.0f);
REQUIRE_THAT(A.Get(2, 1), Catch::Matchers::WithinRel(0.5f, 1e-7f));
REQUIRE_THAT(A.Get(1, 2), Catch::Matchers::WithinRel(0.5f, 1e-7f));
// Diagonals untouched
REQUIRE_THAT(A.Get(0, 0), Catch::Matchers::WithinRel(2.0f, 1e-7f));
REQUIRE_THAT(A.Get(1, 1), Catch::Matchers::WithinRel(3.0f, 1e-7f));
REQUIRE_THAT(A.Get(2, 2), Catch::Matchers::WithinRel(4.0f, 1e-7f));
}
// Nothing deflated when all subdiagonals are well above tolerance
{
Matrix<3, 3> A{2, 0.1f, 0, 0.1f, 3, 0.2f, 0, 0.2f, 4};
QR::Deflate(A, 0, 2, 1e-6f);
REQUIRE_THAT(A.Get(1, 0), Catch::Matchers::WithinRel(0.1f, 1e-7f));
REQUIRE_THAT(A.Get(2, 1), Catch::Matchers::WithinRel(0.2f, 1e-7f));
}
}
// ============================================================================
// TEST 7: Tridiagonalize
// ============================================================================
TEST_CASE("QR Building Block: Tridiagonalize", "[Matrix][QR]") {
// 4x4 symmetric with a full (0,3) corner coupling
{
Matrix<4, 4> A{2, 1, 0, 1, 1, 3, 1, 0, 0, 1, 4, 1, 1, 0, 1, 5};
Matrix<4, 4> Aorig = A;
Matrix<4, 4> U{0};
QR::Tridiagonalize(A, U);
// Off-tridiagonal entries must be zero up to float32 roundoff (the
// Givens zeroing cancels only in exact arithmetic; residuals are
// ~1e-7 for O(1) entries).
REQUIRE_THAT(A.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(A.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(A.Get(0, 3), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(A.Get(3, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(A.Get(1, 3), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(A.Get(3, 1), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
// Symmetry preserved exactly
for (uint8_t i = 0; i < 4; i++)
for (uint8_t j = 0; j < 4; j++)
REQUIRE(A.Get(i, j) == A.Get(j, i));
// U must be orthogonal
REQUIRE(isOrthogonal(U));
// Reconstruction: U * A_tri * U^T == Aorig (absolute check for
// originally-zero entries: WithinRel has no absolute fallback there)
Matrix<4, 4> UAt{};
U.Mult(A, UAt);
Matrix<4, 4> UAtU{};
UAt.Mult(U.Transpose(), UAtU);
for (uint8_t i = 0; i < 4; i++)
for (uint8_t j = 0; j < 4; j++) {
float actual = UAtU.Get(i, j);
float expected = Aorig.Get(i, j);
if (fabsf(expected) < 1e-3f)
REQUIRE_THAT(actual, Catch::Matchers::WithinAbs(0.0f, 1e-5f));
else
REQUIRE_THAT(actual,
Catch::Matchers::WithinRel(expected, 1e-5f));
}
// Spectrum invariants match the original
{
float tr0 = Aorig.Get(0, 0) + Aorig.Get(1, 1) + Aorig.Get(2, 2) +
Aorig.Get(3, 3);
float tr1 = A.Get(0, 0) + A.Get(1, 1) + A.Get(2, 2) + A.Get(3, 3);
REQUIRE_THAT(tr1, Catch::Matchers::WithinRel(tr0, 1e-6f));
REQUIRE_THAT(frob(A), Catch::Matchers::WithinRel(frob(Aorig), 1e-6f));
}
// Eigenvalues of the tridiagonal match the original (scipy reference):
// 6.0, 4.0, 3.0, 1.0
{
Matrix<4, 1> vals{};
Matrix<4, 4> vecs{};
QR::EigenQR(A, vecs, vals, 10000, 1e-6f);
REQUIRE_THAT(vals[0][0], Catch::Matchers::WithinRel(6.0f, 1e-4f));
REQUIRE_THAT(vals[1][0], Catch::Matchers::WithinRel(4.0f, 1e-4f));
REQUIRE_THAT(vals[2][0], Catch::Matchers::WithinRel(3.0f, 1e-4f));
REQUIRE_THAT(vals[3][0], Catch::Matchers::WithinRel(1.0f, 1e-4f));
}
}
// 5x5 symmetric
{
Matrix<5, 5> A{3, 1, 0, 0, 1, 1, 4, 1, 0, 0, 0, 1, 5, 1, 0, 0, 0, 1, 6, 1,
1, 0, 0, 1, 7};
Matrix<5, 5> Aorig = A;
Matrix<5, 5> U{0};
QR::Tridiagonalize(A, U);
// All |i - j| >= 2 entries zero up to float32 roundoff
for (uint8_t i = 0; i < 5; i++)
for (uint8_t j = 0; j < 5; j++)
if (i > j + 1 || j > i + 1)
REQUIRE_THAT(A.Get(i, j), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE(isOrthogonal(U));
Matrix<5, 5> UAt{};
U.Mult(A, UAt);
Matrix<5, 5> UAtU{};
UAt.Mult(U.Transpose(), UAtU);
for (uint8_t i = 0; i < 5; i++)
for (uint8_t j = 0; j < 5; j++) {
float actual = UAtU.Get(i, j);
float expected = Aorig.Get(i, j);
if (fabsf(expected) < 1e-3f)
REQUIRE_THAT(actual, Catch::Matchers::WithinAbs(0.0f, 1e-5f));
else
REQUIRE_THAT(actual,
Catch::Matchers::WithinRel(expected, 1e-5f));
}
}
// Already tridiagonal: U must come out as the identity
{
Matrix<3, 3> A{1, 2, 0, 2, 5, 2, 0, 2, 9};
Matrix<3, 3> U{0};
QR::Tridiagonalize(A, U);
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++) {
float expected = (i == j) ? 1.0f : 0.0f;
REQUIRE_THAT(U.Get(i, j), Catch::Matchers::WithinAbs(expected, 1e-7f));
}
}
}
// ============================================================================
// TEST 8: One full shifted QR step (integration of the blocks)
// ============================================================================
TEST_CASE("QR Building Block: Full Shifted QR Step", "[Matrix][QR]") {
// One Wilkinson-shifted QR step on the whole 3x3 block is a similarity
// transform, so all spectrum invariants (trace, sum of principal 2x2
// minors, determinant) must be preserved.
//
// A = [[1,2,3],[2,5,8],[3,8,9]]: tr = 15, e2 = -18, det = -4
{
Matrix<3, 3> A{1, 2, 3, 2, 5, 8, 3, 8, 9};
float tr0 = trace3(A); // 15
float e20 = e2_3x3(A); // -18
float det0 = det3(A); // -4
// mu from the trailing 2x2 [[5,8],[8,9]]: eigenvalues
// -1.246211251, 15.246211251; closest to d = 9 is 15.246211251 (Wilkinson)
float mu = QR::WilkinsonShift(A.Get(1, 1), A.Get(2, 1), A.Get(2, 2));
REQUIRE_THAT(mu, Catch::Matchers::WithinRel(15.246211251f, 1e-5f));
for (uint8_t i = 0; i < 3; i++)
A[i][i] -= mu;
// Bulge chase: rotations on (0,1) then (1,2)
float c = 0, s = 0;
QR::GivensRotation(A.Get(0, 0), A.Get(1, 0), c, s);
QR::ApplyRotationBothSides(A, 0, c, s);
QR::GivensRotation(A.Get(1, 1), A.Get(2, 1), c, s);
QR::ApplyRotationBothSides(A, 1, c, s);
for (uint8_t i = 0; i < 3; i++)
A[i][i] += mu;
// Symmetry preserved
for (uint8_t i = 0; i < 3; i++)
for (uint8_t j = 0; j < 3; j++)
REQUIRE(A.Get(i, j) == A.Get(j, i));
// Spectrum invariants preserved
REQUIRE_THAT(trace3(A), Catch::Matchers::WithinRel(tr0, 1e-5f));
REQUIRE_THAT(e2_3x3(A), Catch::Matchers::WithinRel(e20, 1e-5f));
REQUIRE_THAT(det3(A), Catch::Matchers::WithinRel(det0, 1e-5f));
}
// For TRIDIAGONAL input a single step keeps the tridiagonal structure
{
Matrix<3, 3> T{1, 2, 0, 2, 5, 2, 0, 2, 9};
float mu = QR::WilkinsonShift(T.Get(1, 1), T.Get(2, 1), T.Get(2, 2));
for (uint8_t i = 0; i < 3; i++)
T[i][i] -= mu;
float c = 0, s = 0;
QR::GivensRotation(T.Get(0, 0), T.Get(1, 0), c, s);
QR::ApplyRotationBothSides(T, 0, c, s);
QR::GivensRotation(T.Get(1, 1), T.Get(2, 1), c, s);
QR::ApplyRotationBothSides(T, 1, c, s);
for (uint8_t i = 0; i < 3; i++)
T[i][i] += mu;
// Corners must vanish up to float32 roundoff: tridiagonal form
// maintained. The cancellation is exact in exact arithmetic (the
// corner is s1*a - c1*b times a factor, and Givens gives s1*a = c1*b),
// so the residual is pure rounding, ~1e-6 for O(1) entries.
REQUIRE_THAT(T.Get(0, 2), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
REQUIRE_THAT(T.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-5f));
}
}
unit-tests/qr-reference-values.py
-246
View File
@@ -1,246 +0,0 @@
#!/usr/bin/env python3
"""
Reference values for the QR eigen-decomposition building block tests
(unit-tests/qr-build-blocks-tests.cpp). Run this to verify/implement the
C++ implementation in src/QR.hpp / src/QR.cpp against numpy/scipy.
Conventions (match the C++ exactly):
* Givens zeroing rotation: G = [[c, s], [-s, c]], c = x/r, s = y/r,
r = hypot(x, y). G * (x, y)^T = (r, 0)^T.
* Similarity transform: A <- G A G^T (ApplyRotationBothSides).
* Eigenvector accumulation: V <- V G^T (ApplyRotationToVectors).
Vblock in the 2x2 closed form is [[c, -s], [s, c]] (same shape as G^T).
* Tridiagonalization: bottom-up Givens (i = N-2 down to k+1 per column k).
* Shifted QR loop: Wilkinson shift mu from the trailing 2x2, chase on the
trailing unreduced block [lo, hi], deflate by relative tolerance, peel
exact-zero subdiagonals, 2x2 closed-form termination.
* Pipeline: M0 = U * Mtri * U^T and Mtri = V * D * V^T =>
eigenvectors of M0 = U * V (columns), eigenvalues = diag(D).
Usage: python3 qr-reference-values.py
"""
import numpy as np
import scipy.linalg as sla
np.set_printoptions(precision=9, linewidth=120)
def givens(x, y):
"""c = x/r, s = y/r with r = hypot(x, y)."""
r = np.hypot(x, y)
if r == 0.0:
return 1.0, 0.0
return x / r, y / r
def rot(n, i, c, s):
"""G = I with [[c, s], [-s, c]] embedded at (i, i+1)."""
G = np.eye(n)
G[i:i + 2, i:i + 2] = np.array([[c, s], [-s, c]])
return G
def tridiagonalize(M0):
"""Bottom-up Givens tridiagonalization. Returns (Mtri, U) with
M0 = U Mtri U^T."""
n = len(M0)
M = M0.copy()
U = np.eye(n)
for k in range(n - 2):
for i in range(n - 2, k, -1):
c, s = givens(M[i, k], M[i + 1, k])
G = rot(n, i, c, s)
M = G @ M @ G.T
U = U @ G.T
return M, U
def wilkinson(a, b, d):
"""Eigenvalue of [[a, b], [b, d]] closest to d."""
delta = 0.5 * (a - d)
spread = np.sqrt(delta * delta + b * b)
return 0.5 * (a + d) - (spread if delta >= 0 else -spread)
def solve2x2(A, lo):
"""Closed form for the block at (lo, lo+1): (lHi, lLo, c, s) with
vHi = (c, s), vLo = (-s, c)."""
a = A[lo, lo]
b = A[lo, lo + 1]
e = A[lo + 1, lo]
d = A[lo + 1, lo + 1]
tr = a + d
det = a * d - b * e
disc = max(0.0, tr * tr - 4 * det)
lhi = 0.5 * (tr + np.sqrt(disc))
llo = 0.5 * (tr - np.sqrt(disc))
if b != 0.0:
v1 = lhi - a
nn = np.hypot(b, v1)
c, s = b / nn, v1 / nn
elif a >= d:
c, s = 1.0, 0.0
else:
c, s = 0.0, 1.0
return lhi, llo, c, s
def eigenqr(M0, tol=1e-12, max_iter=100000):
"""Full pipeline mirroring QR::EigenQR. Returns (eigs, W) where W has
the eigenvectors of M0 as columns."""
n = len(M0)
if n == 2:
l1, l2, c, s = solve2x2(M0, 0)
return np.array([l1, l2]), np.array([[c, -s], [s, c]])
M, U = tridiagonalize(M0)
V = np.eye(n)
hi = n - 1
for _ in range(max_iter):
# deflate: zero tiny subdiagonals (relative test)
for i in range(hi):
t = M[i + 1, i]
scale = abs(M[i, i]) + abs(M[i + 1, i + 1])
if abs(t) <= tol * scale:
M[i + 1, i] = M[i, i + 1] = 0.0
# peel exact-zero trailing subdiagonals
while hi > 0 and M[hi, hi - 1] == 0.0:
hi -= 1
if hi == 0:
break
# find start of trailing unreduced block
lo = hi
for i in range(hi - 1, -1, -1):
if M[i + 1, i] == 0.0:
break
lo = i
if lo + 1 == hi:
# closed-form 2x2 termination: set diagonal, fold Vblock in
l1, l2, c, s = solve2x2(M, lo)
Vb = np.eye(n)
Vb[lo:lo + 2, lo:lo + 2] = np.array([[c, -s], [s, c]])
V = V @ Vb
M[lo, lo] = l1
M[lo + 1, lo + 1] = l2
M[lo + 1, lo] = M[lo, lo + 1] = 0.0
if lo == 0:
break
hi = lo - 1
continue
# full shifted step on [lo, hi] (shift applies to the active block)
mu = wilkinson(M[hi - 1, hi - 1], M[hi, hi - 1], M[hi, hi])
diag = M.diagonal().copy()
diag[lo:hi + 1] -= mu
np.fill_diagonal(M, diag)
c, s = givens(M[lo, lo], M[lo + 1, lo])
G = rot(n, lo, c, s)
M = G @ M @ G.T
V = V @ G.T
for i in range(lo + 1, hi):
c, s = givens(M[i, i], M[i + 1, i])
G = rot(n, i, c, s)
M = G @ M @ G.T
V = V @ G.T
diag = M.diagonal().copy()
diag[lo:hi + 1] += mu
np.fill_diagonal(M, diag)
eigs = np.diag(M).astype(float)
order = np.argsort(eigs)[::-1] # descending, like the C++ test harness
eigs = eigs[order]
W = U @ V
W = W[:, order]
return eigs, W
def report(name, val, ref=None, tol=1e-6):
ok = "OK " if ref is None or np.allclose(val, ref, rtol=tol, atol=tol) else "FAIL"
print(f"[{ok}] {name} = {val}")
if ref is not None:
print(f" scipy/numpy ref = {ref}")
def main():
print("=== TEST 1: GivensRotation ===")
c, s = givens(2.0, 1.0)
print(f" c = {c} s = {s}")
# G * (x, y)^T = (r, 0)^T: G = [[c, s], [-s, c]]
assert abs(c * 2 + s * 1 - np.sqrt(5)) < 1e-15
assert abs(-s * 2 + c * 1) < 1e-15
print("\n=== TEST 2: ApplyRotationBothSides A <- G A G^T ===")
A = np.array([[3.0, 4.0, 5.0], [6.0, 7.0, 8.0], [9.0, 10.0, 11.0]])
G = rot(3, 0, 0.6, 0.8)
B = G @ A @ G.T
print(B)
A = np.array([[5.0, 0.0, 1.0], [0.0, 6.0, 2.0], [1.0, 2.0, 7.0]])
c, s = givens(6.0, 2.0)
G = rot(3, 1, c, s)
B = G @ A @ G.T
print(B)
print("\n=== TEST 3: V accumulation V <- V G^T ===")
V = np.eye(3)
G = rot(3, 0, 0.894427191, 0.447213595)
V = V @ G.T
print(V)
V2 = V @ rot(3, 1, 0.848874681, 0.528748047).T
print(V2)
print("\n=== TEST 4: Solve2x2Eigen ===")
for A in (np.array([[5.0, 8.0], [8.0, 9.0]]), np.array([[1.0, 2.0], [3.0, 4.0]])):
l1, l2, c, s = solve2x2(A, 0)
ref = np.linalg.eigvalsh(A) if np.allclose(A, A.T) else np.linalg.eigvals(A)
print(f" A={A.ravel()} lHi={l1} lLo={l2} c={c} s={s} ref={np.sort(ref)[::-1]}")
print("\n=== TEST 8: WilkinsonShift ===")
print(f" W(5, 8, 9) = {wilkinson(5, 8, 9)}")
print(f" W(4, 2, 7) = {wilkinson(4, 2, 7)}")
print(f" W(9, 2, 5) = {wilkinson(9, 2, 5)}")
print("\n=== TEST 8b: one full shifted chase step on tridiagonal 3x3 ===")
T = np.array([[1.0, 2.0, 0.0], [2.0, 5.0, 2.0], [0.0, 2.0, 9.0]])
mu = wilkinson(5, 2, 9)
M = T - mu * np.eye(3)
c, s = givens(M[0, 0], M[1, 0])
M = rot(3, 0, c, s) @ M @ rot(3, 0, c, s).T
c, s = givens(M[1, 1], M[2, 1])
M = rot(3, 1, c, s) @ M @ rot(3, 1, c, s).T
M = M + mu * np.eye(3)
print(f" mu = {mu}")
print(M)
print(f" corners: {M[0, 2]}, {M[2, 0]} (exact-arithmetic zeros)")
print(f" trace {M.trace():.15f} (was {T.trace()})")
print("\n=== TEST 7: Tridiagonalize ===")
M4 = np.array([[2.0, 1, 0, 1], [1, 3, 1, 0], [0, 1, 4, 1], [1, 0, 1, 5]])
M, U = tridiagonalize(M4)
print(" M4 tridiagonalized:\n", M)
print(f" reconstruction U M U^T == M4: {np.allclose(U @ M @ U.T, M4, atol=1e-9)}")
M5 = np.array([[3.0, 1, 0, 0, 1], [1, 4, 1, 0, 0], [0, 1, 5, 1, 0],
[0, 0, 1, 6, 1], [1, 0, 0, 1, 7]])
M, U = tridiagonalize(M5)
print(" M5 tridiagonalized:\n", M)
print(f" reconstruction: {np.allclose(U @ M @ U.T, M5, atol=1e-9)}")
print("\n=== End-to-end: random symmetric vs scipy.linalg.eigh ===")
rng = np.random.default_rng(12345)
worst = 0.0
for n in range(3, 9):
M0 = rng.normal(size=(n, n))
M0 = (M0 + M0.T) / 2
eigs, W = eigenqr(M0.astype(float))
ref = sla.eigh(M0)
e_err = np.max(np.abs(np.sort(eigs) - ref[0]))
resid = np.linalg.norm(W @ np.diag(eigs) @ W.T - M0)
ortho = np.linalg.norm(W.T @ W - np.eye(n))
print(f" n={n}: eigs_err={e_err:.2e} resid={resid:.2e} ortho={ortho:.2e}")
worst = max(worst, e_err, resid, ortho)
print(f"\nworst over all n: {worst:.2e}")
assert worst < 1e-10, "end-to-end reference FAILED"
print("ALL REFERENCES OK")
if __name__ == "__main__":
main()
unit-tests/svd-build-blocks-tests.cpp
-1647
View File
File diff suppressed because it is too large Load Diff
unit-tests/svd-integration-test.cpp
-363
View File
@@ -1,363 +0,0 @@
#include "Matrix.hpp"
#include "SVD.hpp"
#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include <iostream>
// Generic helper functions for any matrix size
template <uint8_t rows, uint8_t columns>
static float frobeniusNorm(const Matrix<rows, columns> &M) {
float sum = 0.0f;
for (int i = 0; i < rows; i++)
for (int j = 0; j < columns; j++) {
float v = M.Get(i, j);
sum += v * v;
}
return sqrtf(sum);
}
template <uint8_t n>
static bool isOrthogonal(const Matrix<n, n> &M, float tol = 1e-4f) {
Matrix<n, n> Mt = M.Transpose();
Matrix<n, n> MtM{0};
Mt.Mult(M, MtM);
for (int i = 0; i < n; i++)
for (int j = 0; j < n; j++) {
float expected = (i == j) ? 1.0f : 0.0f;
if (fabsf(MtM.Get(i, j) - expected) > tol)
return false;
}
return true;
}
TEST_CASE("SVD Integration: 2x2 [[1,2],[3,4]]", "[Matrix][SVD][Integration]") {
Matrix<2, 2> A{1, 2, 3, 4};
Matrix<2, 2> U{0};
Matrix<2, 1> sigma{0};
Matrix<2, 2> Vt{0};
SVD::SVD(A, U, sigma, Vt);
// Reference singular values from scipy: [5.464985704219, 0.365966190626]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(5.4649857f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(0.3659662f, 1e-3f));
// Check orthogonality of U and Vt (first 2x2 blocks)
REQUIRE(isOrthogonal<2>(U));
REQUIRE(isOrthogonal<2>(Vt));
// Check reconstruction: A ≈ U · diag(sigma) · Vt
Matrix<2, 2> recon{0};
Matrix<2, 2> Usig{0};
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
Usig.Mult(Vt, recon);
float err = 0.0f;
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++) {
float diff = recon.Get(i, j) - A.Get(i, j);
err += diff * diff;
}
err = sqrtf(err);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
std::cout << "SVD 2x2 [[1,2],[3,4]]:\n";
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)
<< "]\n";
}
TEST_CASE("SVD Integration: 3x3 diagonal [10,5,2]",
"[Matrix][SVD][Integration]") {
Matrix<3, 3> A{10, 0, 0, 0, 5, 0, 0, 0, 2};
Matrix<3, 3> U{0};
Matrix<3, 1> sigma{0};
Matrix<3, 3> Vt{0};
SVD::SVD(A, U, sigma, Vt);
// Singular values should be [10, 5, 2] (already diagonal)
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(10.0f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(5.0f, 1e-3f));
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(2.0f, 1e-3f));
// U and Vt should be identity (or close) for diagonal matrix
float uErr = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
float vtErr = frobeniusNorm(Vt - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
REQUIRE_THAT(uErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
REQUIRE_THAT(vtErr, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
}
TEST_CASE("SVD Integration: 3x3 rank-deficient [[1,2,3],[4,5,6],[7,8,9]]",
"[Matrix][SVD][Integration]") {
Matrix<3, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9};
Matrix<3, 3> U{0};
Matrix<3, 1> sigma{0};
Matrix<3, 3> Vt{0};
SVD::SVD(A, U, sigma, Vt);
// Reference: [16.848103352614, 1.068369514555, 0.0]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(16.8481f, 1e-2f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1.06837f, 1e-2f));
// Third singular value should be ~0 (rank-deficient)
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-2f));
// Check reconstruction
Matrix<3, 3> recon{0};
Matrix<3, 3> Usig{0};
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
Usig.Mult(Vt, recon);
float err = 0.0f;
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++) {
float diff = recon.Get(i, j) - A.Get(i, j);
err += diff * diff;
}
err = sqrtf(err);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
std::cout << "SVD 3x3 rank-deficient:\n";
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
<< sigma.Get(2, 0) << "]\n";
}
TEST_CASE("SVD Integration: tall 4x3 matrix", "[Matrix][SVD][Integration]") {
Matrix<4, 3> A{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
Matrix<4, 3> U{0};
Matrix<3, 1> sigma{0};
Matrix<3, 3> Vt{0};
SVD::SVD(A, U, sigma, Vt);
// Reference: [25.462407436036, 1.290661675761, 0.0]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(25.4624f, 1e-2f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1.29066f, 1e-2f));
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-2f));
// Check reconstruction
Matrix<4, 3> recon{0};
Matrix<4, 3> Usig{0};
for (int i = 0; i < 4; i++)
for (int j = 0; j < 3; j++)
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
Usig.Mult(Vt, recon);
float err = 0.0f;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 3; j++) {
float diff = recon.Get(i, j) - A.Get(i, j);
err += diff * diff;
}
err = sqrtf(err);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
std::cout << "SVD tall 4x3:\n";
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
<< sigma.Get(2, 0) << "]\n";
}
TEST_CASE("SVD Integration: wide 3x5 matrix", "[Matrix][SVD][Integration]") {
Matrix<3, 5> A{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
Matrix<3, 5> U{0};
Matrix<5, 1> sigma{0}; // sigma is columns x 1 = 5x1 for wide matrix
Matrix<5, 5> Vt{0}; // Vt is columns x columns = 5x5
SVD::SVD(A, U, sigma, Vt);
// Reference: [35.127223333575, 2.465396696917, 0.0]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(35.1272f, 1e-2f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(2.46540f, 1e-2f));
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-2f));
// Check reconstruction: A (3x5) = U * Sigma * Vt, where U (3x5) has
// its meaningful part in the first 3 columns, sigma (5x1) in the
// first 3 entries, and Vt (5x5) in its first 3 rows (right
// singular vectors as rows). So:
// A[i][j] = sum_k U[i][k] * sigma[k] * Vt[k][j]
float err2 = 0.0f;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 5; j++) {
float recon_val = 0.0f;
for (int k = 0; k < 3; k++) {
recon_val += U.Get(i, k) * sigma.Get(k, 0) * Vt.Get(k, j);
}
float diff = recon_val - A.Get(i, j);
err2 += diff * diff;
}
}
err2 = sqrtf(err2);
REQUIRE_THAT(err2, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
std::cout << "SVD wide 3x5:\n";
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0) << ", "
<< sigma.Get(2, 0) << "]\n";
}
TEST_CASE("SVD Integration: identity 3x3", "[Matrix][SVD][Integration]") {
Matrix<3, 3> A{1, 0, 0, 0, 1, 0, 0, 0, 1};
Matrix<3, 3> U{0};
Matrix<3, 1> sigma{0};
Matrix<3, 3> Vt{0};
SVD::SVD(A, U, sigma, Vt);
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(1.0f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(1.0f, 1e-3f));
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinRel(1.0f, 1e-3f));
float err = frobeniusNorm(U - Matrix<3, 3>{1, 0, 0, 0, 1, 0, 0, 0, 1});
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-2f));
}
TEST_CASE("SVD Integration: symmetric positive definite 2x2 [[5,3],[3,5]]",
"[Matrix][SVD][Integration]") {
Matrix<2, 2> A{5, 3, 3, 5};
Matrix<2, 2> U{0};
Matrix<2, 1> sigma{0};
Matrix<2, 2> Vt{0};
SVD::SVD(A, U, sigma, Vt);
// For SPD matrix, singular values = eigenvalues: [8, 2]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(8.0f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(2.0f, 1e-3f));
// Check reconstruction
Matrix<2, 2> recon{0};
Matrix<2, 2> Usig{0};
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
Usig.Mult(Vt, recon);
float err = 0.0f;
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++) {
float diff = recon.Get(i, j) - A.Get(i, j);
err += diff * diff;
}
err = sqrtf(err);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
std::cout << "SVD SPD 2x2 [[5,3],[3,5]]:\n";
std::cout << "Sigma: [" << sigma.Get(0, 0) << ", " << sigma.Get(1, 0)
<< "]\n";
}
// ----------------------------------------------------------------------------
// Matrix::SVD member wrapper (delegates to SVD::SVD)
// ----------------------------------------------------------------------------
/**
* Reconstruction error ‖U·diag(sigma)·Vᵀ − A‖_F. Zero-padded entries of
* U/sigma/Vt (wide/tall cases) are zero by the output conventions, so the
* full product equals U[:, :k]·diag(sigma[:k])·Vt[:k, :].
*/
template <uint8_t rows, uint8_t columns>
static float svdReconstructionError(const Matrix<rows, columns> &A,
const Matrix<rows, columns> &U,
const Matrix<columns, 1> &sigma,
const Matrix<columns, columns> &Vt) {
Matrix<rows, columns> recon{0};
Matrix<rows, columns> Usig{0};
for (int i = 0; i < rows; i++)
for (int j = 0; j < columns; j++)
Usig[i][j] = U.Get(i, j) * sigma.Get(j, 0);
Usig.Mult(Vt, recon);
float err = 0.0f;
for (int i = 0; i < rows; i++)
for (int j = 0; j < columns; j++) {
float diff = recon.Get(i, j) - A.Get(i, j);
err += diff * diff;
}
return sqrtf(err);
}
/**
* Orthonormality of the first k columns of M: the k×k leading block of
* MᵀM must equal I_k. (For a tall SVD, U has k = min(rows, cols)
* meaningful columns and this is the full UᵀU.)
*/
template <uint8_t r, uint8_t c>
static bool leadingColumnsOrthonormal(const Matrix<r, c> &M, uint8_t k,
float tol = 1e-4f) {
Matrix<c, r> Mt = M.Transpose();
Matrix<c, c> MtM{0};
Mt.Mult(M, MtM);
for (int i = 0; i < k; i++)
for (int j = 0; j < k; j++) {
float expected = (i == j) ? 1.0f : 0.0f;
if (fabsf(MtM.Get(i, j) - expected) > tol)
return false;
}
return true;
}
/**
* Orthonormality of the first k rows of M: the k×k leading block of
* M·Mᵀ must equal I_k. (Vᵀ may have zero-padded trailing rows in the
* wide case, so check only the meaningful leading block.)
*/
template <uint8_t r, uint8_t c>
static bool leadingRowsOrthonormal(const Matrix<r, c> &M, uint8_t k,
float tol = 1e-4f) {
Matrix<c, r> Mt = M.Transpose();
Matrix<r, r> MMt{0};
M.Mult(Mt, MMt);
for (int i = 0; i < k; i++)
for (int j = 0; j < k; j++) {
float expected = (i == j) ? 1.0f : 0.0f;
if (fabsf(MMt.Get(i, j) - expected) > tol)
return false;
}
return true;
}
TEST_CASE("Matrix::SVD wrapper: 3x2 tall [[1,2],[3,4],[5,6]]",
"[Matrix][SVD][Wrapper]") {
Matrix<3, 2> A{1, 2, 3, 4, 5, 6};
Matrix<3, 2> U{0};
Matrix<2, 1> sigma{0};
Matrix<2, 2> Vt{0};
A.SVD(U, sigma, Vt);
// Reference singular values from numpy: [9.52552, 0.514301]
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(9.52552f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(0.514301f, 1e-3f));
REQUIRE(leadingColumnsOrthonormal(U, 2));
REQUIRE(leadingRowsOrthonormal(Vt, 2));
float err = svdReconstructionError(A, U, sigma, Vt);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}
TEST_CASE("Matrix::SVD wrapper: 2x3 wide [[1,2,3],[4,5,6]]",
"[Matrix][SVD][Wrapper]") {
Matrix<2, 3> A{1, 2, 3, 4, 5, 6};
Matrix<2, 3> U{0};
Matrix<3, 1> sigma{0};
Matrix<3, 3> Vt{0};
A.SVD(U, sigma, Vt);
// Reference singular values from numpy: [9.50803, 0.77287]; the third
// entry (wide-matrix padding) must be zero.
REQUIRE_THAT(sigma.Get(0, 0), Catch::Matchers::WithinRel(9.50803f, 1e-3f));
REQUIRE_THAT(sigma.Get(1, 0), Catch::Matchers::WithinRel(0.77287f, 1e-3f));
REQUIRE_THAT(sigma.Get(2, 0), Catch::Matchers::WithinAbs(0.0f, 1e-6f));
REQUIRE(leadingColumnsOrthonormal(U, 2));
REQUIRE(leadingRowsOrthonormal(Vt, 2));
float err = svdReconstructionError(A, U, sigma, Vt);
REQUIRE_THAT(err, Catch::Matchers::WithinAbs(0.0f, 1e-3f));
}
unit-tests/svd-reference-values.py
-513
View File
@@ -1,513 +0,0 @@
#!/usr/bin/env python3
"""
Generate reference values for SVD building block unit tests.
Run this to verify/implement the C++ SVD implementation against scipy/numpy.
Usage: python3 svd-reference-values.py
"""
import numpy as np
from scipy.linalg import svd, qr as scipy_qr
import json
def compute_householder(x):
"""Compute Householder reflector: H*x = [alpha, 0, 0, ...]^T.
Returns (v_normalized, alpha) where v is the normalized Householder vector.
H = I - 2*v*v^T / (v^T*v)
"""
x = np.array(x, dtype=np.float64)
norm_x = np.linalg.norm(x)
if norm_x < 1e-30:
return x.copy(), 0.0
alpha = -np.sign(x[0]) * norm_x if x[0] != 0 else -norm_x
v = x.copy()
v[0] -= alpha
v_norm = np.linalg.norm(v)
if v_norm < 1e-30:
return np.zeros_like(x), alpha
v /= v_norm
return v, alpha
def apply_householder_left(A, v, start_row):
"""Apply Householder reflection from the left: A = (I - 2vv^T) @ A.
v is the normalized Householder vector operating on rows [start_row:].
The length of v must match the number of rows affected.
"""
A = A.copy()
k = len(v)
for col in range(A.shape[1]):
dot = np.dot(v, A[start_row:start_row+k, col])
A[start_row:start_row+k, col] -= 2.0 * dot * v
return A
def apply_householder_right(A, v, start_col):
"""Apply Householder reflection from the right: A = A @ (I - 2vv^T).
v is the normalized Householder vector operating on columns [start_col:].
The length of v must match the number of columns affected.
"""
A = A.copy()
k = len(v)
for row in range(A.shape[0]):
dot = np.dot(A[row, start_col:start_col+k], v)
A[row, start_col:start_col+k] -= 2.0 * dot * v
return A
def compute_givens(x, y):
"""Compute Givens rotation that zeros out y.
Returns (c, s) such that [c s; -s c] @ [x; y] = [r; 0].
"""
r = np.sqrt(x*x + y*y)
if r < 1e-30:
return 1.0, 0.0
c = x / r
s = y / r
return c, s
def apply_givens_left(A, i, j, c, s):
"""Apply Givens rotation from the left to rows i and j of A.
[c s] [row_i]
[-s c] @ [row_j] = [new_row_i]
[new_row_j]
"""
A = A.copy()
new_i = c * A[i] + s * A[j]
new_j = -s * A[i] + c * A[j]
A[i] = new_i
A[j] = new_j
return A
def apply_givens_right(A, i, j, c, s):
"""Apply Givens rotation from the right to columns i and j of A.
[col_i col_j] @ [c -s] = [new_col_i new_col_j]
[s c]
"""
A = A.copy()
new_i = c * A[:, i] + s * A[:, j]
new_j = -s * A[:, i] + c * A[:, j]
A[:, i] = new_i
A[:, j] = new_j
return A
def householder_bidiagonalization(A):
"""Full Householder bidiagonalization: A = Q_L @ B @ Q_R^T.
Returns (B, Q_L, Q_R) where B is upper bidiagonal.
"""
m, n = A.shape
p = min(m, n)
QL = np.eye(m, dtype=np.float64)
QR = np.eye(n, dtype=np.float64)
W = A.copy()
for k in range(p):
# Left HH: zero out W[k+1:, k]
if k < m - 1:
x = W[k+1:, k].copy()
v, alpha = compute_householder(x)
if np.linalg.norm(v) > 1e-30:
W = apply_householder_left(W, v, k + 1)
QL = apply_householder_right(QL, v, k + 1)
# Right HH: zero out W[k, k+2:] (superdiagonal)
if k < p - 1 and k + 2 <= n:
x = W[k, k+2:].copy()
v, alpha = compute_householder(x)
if np.linalg.norm(v) > 1e-30:
W = apply_householder_right(W, v, k + 2)
QR = apply_householder_right(QR, v, k + 2)
return W, QL, QR
def implicit_qr_iteration(B, QR_acc):
"""Implicit QR iteration on a bidiagonal matrix.
Returns (Sigma, QR_acc) where Sigma is diagonal with singular values
and QR_acc contains the accumulated right transformations.
"""
m, n = B.shape
p = min(m, n)
W = B.copy()
max_iter = 1000
tol = 1e-10
for iteration in range(max_iter):
# Deflate negligible subdiagonal elements
for i in range(p - 1, 0, -1):
if abs(W[i, i-1]) < tol * (abs(W[i-1, i-1]) + abs(W[i, i])):
W[i, i-1] = 0.0
# Find smallest unreduced block [start, end]
start = 0
for i in range(p - 1):
if abs(W[i+1, i]) >= tol * (abs(W[i, i]) + abs(W[i+1, i+1])):
start = i + 1
end = p - 1
for i in range(p - 2, -1, -1):
if abs(W[i+1, i]) >= tol * (abs(W[i, i]) + abs(W[i+1, i+1])):
end = i
break
if start >= end:
continue
# Wilkinson shift from bottom 2x2 corner
a, b = W[end-1, end-1], W[end-1, end]
c_val, d = W[end, end-1], W[end, end]
trace = a + d
det = a * d - b * c_val
disc = trace**2 - 4 * det
if disc >= 0:
sqrt_disc = np.sqrt(disc)
e1, e2 = (trace + sqrt_disc) / 2, (trace - sqrt_disc) / 2
shift = e1 if abs(e1 - d) < abs(e2 - d) else e2
else:
shift = d
# Implicit QR step using Givens rotations
# Process from top to bottom within the block
x = W[start, start] - shift
y = W[start + 1, start]
for i in range(start, end):
r = np.sqrt(x*x + y*y)
if r < 1e-30:
x = W[i + 1, i]
y = W[i + 1, i + 1] if i + 2 <= end else 0.0
continue
c_rot = x / r
s_rot = y / r
# Apply from left to rows i, i+1 (columns i..n-1)
for j in range(i, n):
t1, t2 = W[i, j], W[i + 1, j]
W[i, j] = c_rot * t1 + s_rot * t2
W[i + 1, j] = -s_rot * t1 + c_rot * t2
# Apply from right to columns i, i+1 (rows 0..i)
if i > start:
for j in range(i + 1):
t1, t2 = W[j, i], W[j, i + 1]
W[j, i] = c_rot * t1 + s_rot * t2
W[j, i + 1] = -s_rot * t1 + c_rot * t2
# Accumulate into QR_acc
for j in range(QR_acc.shape[0]):
t1, t2 = QR_acc[j, i], QR_acc[j, i + 1]
QR_acc[j, i] = c_rot * t1 + s_rot * t2
QR_acc[j, i + 1] = -s_rot * t1 + c_rot * t2
# Prepare for next rotation
x = W[i + 1, i]
y = W[i + 1, i + 1] if i + 2 <= end else 0.0
return W, QR_acc
def main():
print("=" * 70)
print("SVB BUILDING BLOCK REFERENCE VALUES")
print("Generated with scipy/numpy for C++ unit test verification")
print("=" * 70)
# ------------------------------------------------------------------
# Test 1: Householder Vector Computation
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 1: computeHouseholderVector")
print("=" * 70)
test_vectors = [
("2D [1,3]", [1.0, 3.0]),
("2D [3,4] (norm=5)", [3.0, 4.0]),
("3D [1,2,3]", [1.0, 2.0, 3.0]),
("3D [0,0,1]", [0.0, 0.0, 1.0]),
("4D [5,-3,2,1]", [5.0, -3.0, 2.0, 1.0]),
]
for name, vec in test_vectors:
v, alpha = compute_householder(vec)
x = np.array(vec)
Hx = x - 2 * np.dot(v, x) * v
print(f"\n{name}:")
print(f" Input: {list(x)}")
print(f" ||x||: {np.linalg.norm(x):.15f}")
print(f" alpha: {alpha:.15f}")
print(f" v (normalized): {[round(float(vi), 12) for vi in v]}")
print(f" H*x = [alpha,0..]: {[round(float(xi), 12) for xi in Hx]}")
print(f" Off-diagonal ~0: {np.allclose(Hx[1:], 0, atol=1e-12)}")
# ------------------------------------------------------------------
# Test 2: Householder Apply Left
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 2: applyHouseholderLeft")
print("=" * 70)
A_test = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]], dtype=np.float64)
x_col = A_test[1:, 0].copy()
v_left, _ = compute_householder(x_col)
print(f"\nInput matrix:\n{A_test}")
print(f"Householder vector (rows 1:3): {[round(float(vi), 12) for vi in v_left]}")
A_result = apply_householder_left(A_test, v_left, 1)
print(f"\nAfter applyHouseholderLeft:\n{A_result}")
print(f" A[1,0] = {A_result[1,0]:.2e}, A[2,0] = {A_result[2,0]:.2e} (should be ~0)")
# ------------------------------------------------------------------
# Test 3: Householder Apply Right
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 3: applyHouseholderRight")
print("=" * 70)
A_test = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]], dtype=np.float64)
x_row = A_test[0, 1:].copy()
v_right, _ = compute_householder(x_row)
print(f"\nInput matrix:\n{A_test}")
print(f"Householder vector (cols 1:3): {[round(float(vi), 12) for vi in v_right]}")
A_result = apply_householder_right(A_test, v_right, 1)
print(f"\nAfter applyHouseholderRight:\n{A_result}")
print(f" A[0,1] = {A_result[0,1]:.2e}, A[0,2] = {A_result[0,2]:.2e} (should be ~0)")
# ------------------------------------------------------------------
# Test 4: Givens Rotation Computation
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 4: computeGivens")
print("=" * 70)
givens_tests = [
("3-4-5 triangle", 3.0, 4.0),
("y already zero", 1.0, 0.0),
("x is zero", 0.0, 5.0),
("Both negative", -3.0, -4.0),
("45 degree case", 1.0, -1.0),
]
for name, x, y in givens_tests:
c, s = compute_givens(x, y)
result_x = c * x + s * y
result_y = -s * x + c * y
print(f"\n{name}: x={x}, y={y}")
print(f" r = {np.sqrt(x*x+y*y):.12f}")
print(f" c = {c:.12f}, s = {s:.12f}")
print(f" [c s; -s c] @ [x;y] = [{result_x:.2e}, {result_y:.2e}]")
# ------------------------------------------------------------------
# Test 5: Apply Givens Left/Right
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 5: applyGivensLeft / applyGivensRight")
print("=" * 70)
A_test = np.array([[3.0, 4.0], [1.0, 2.0]], dtype=np.float64)
c, s = compute_givens(3.0, 1.0)
print(f"\nInput matrix:\n{A_test}")
print(f"Givens rotation (rows 0,1): c={c:.12f}, s={s:.12f}")
A_left = apply_givens_left(A_test, 0, 1, c, s)
print(f"\nAfter applyGivensLeft:\n{A_left}")
print(f" A[1,0] = {A_left[1,0]:.2e} (should be ~0)")
A_test = np.array([[3.0, 1.0], [4.0, 2.0]], dtype=np.float64)
c, s = compute_givens(3.0, 4.0)
print(f"\nInput matrix:\n{A_test}")
print(f"Givens rotation (cols 0,1): c={c:.12f}, s={s:.12f}")
A_right = apply_givens_right(A_test, 0, 1, c, s)
print(f"\nAfter applyGivensRight:\n{A_right}")
print(f" A[0,1] = {A_right[0,1]:.2e} (should be ~0)")
# ------------------------------------------------------------------
# Test 6: Full Bidiagonalization
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 6: householderBidiagonalization")
print("=" * 70)
bidiag_tests = [
("2x2 [[1,2],[3,4]]", np.array([[1.0, 2.0], [3.0, 4.0]])),
("3x3 SPD [[5,3],[3,5]]", np.array([[5.0, 3.0], [3.0, 5.0]])),
("3x3 diag [[10,0,0],[0,5,0],[0,0,2]]",
np.array([[10.0, 0, 0], [0, 5.0, 0], [0, 0, 2.0]])),
("3x3 full [[1,2,3],[4,5,6],[7,8,10]]",
np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 10.0]])),
("Tall 4x3", np.array([[1,2,3],[4,5,6],[7,8,9],[10,11,12]], dtype=np.float64)),
]
for name, A in bidiag_tests:
B, QL, QR = householder_bidiagonalization(A)
m, n = A.shape
p = min(m, n)
print(f"\n{name}:")
print(f" Original:\n{A}")
print(f"\n Bidiagonal B:\n{B}")
print(f" Diagonal: {[round(float(B[i,i]), 10) for i in range(p)]}")
print(f" Superdiag: {[round(float(B[i,i+1]), 10) for i in range(min(p-1, n-1))]}")
recon = QL @ B @ QR.T
err = np.linalg.norm(recon - A, 'fro')
print(f" ||QL @ B @ QR^T - A||_F = {err:.2e}")
# ------------------------------------------------------------------
# Test 7: Full SVD Reference Values
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 7: Full SVD Reference Values (scipy.linalg.svd)")
print("=" * 70)
test_matrices = [
("Simple 2x2", np.array([[1,2],[3,4]], dtype=np.float64)),
("SPD 2x2", np.array([[5,3],[3,5]], dtype=np.float64)),
("Full-rank 3x3", np.array([[1,2,3],[4,5,6],[7,8,10]], dtype=np.float64)),
("Rank-deficient 3x3", np.array([[1,2,3],[4,5,6],[7,8,9]], dtype=np.float64)),
("Diagonal 3x3", np.array([[10,0,0],[0,5,0],[0,0,2]], dtype=np.float64)),
("Tall 4x3", np.array([[1,2,3],[4,5,6],[7,8,9],[10,11,12]], dtype=np.float64)),
("Wide 3x5", np.array([[1,2,3,4,5],[6,7,8,9,10],[11,12,13,14,15]], dtype=np.float64)),
("Symmetric tri 5x5", np.array([[2,-1,0,0,0],[-1,2,-1,0,0],[0,-1,2,-1,0],[0,0,-1,2,-1],[0,0,0,-1,2]], dtype=np.float64)),
("Neg values 2x3", np.array([[0.5,-0.3,0.8],[-0.2,0.7,0.1]], dtype=np.float64)),
("Near-singular 2x2", np.array([[1,0],[0,1e-6]], dtype=np.float64)),
("Orthogonal 3x3", np.array([[np.cos(np.pi/4), -np.sin(np.pi/4), 0],
[np.sin(np.pi/4), np.cos(np.pi/4), 0],
[0, 0, 1]], dtype=np.float64)),
("Identity 3x3", np.eye(3)),
("Zero 3x3", np.zeros((3,3))),
("Col vector 2x1", np.array([[3],[4]], dtype=np.float64)),
("Row vector 1x2", np.array([[3,4]], dtype=np.float64)),
# Large-size instantiation cases (N > 5). Literals MUST match the
# C++ test matrices in unit-tests/matrix-tests.cpp exactly, and the
# C++ references use float32 inputs: cast to float32 before svd().
("Tall 7x5", np.array([
[-0.7528, 2.7043, 1.392, 0.592, -2.0639],
[-2.064, -2.6515, 2.1971, 0.6067, 1.2484],
[-2.8765, 2.8195, 1.9947, -1.726, -1.9091],
[-1.8996, -1.1745, 0.1485, -0.4083, -1.2526],
[0.6711, -2.163, -1.2471, -0.8018, -0.2636],
[1.7111, -1.802, 0.0854, 0.5545, -2.7213],
[0.6453, -1.9769, -2.6097, 2.6933, 2.7938]], dtype=np.float32)),
("Square 6x6", np.array([
[1.2336, -0.7815, -1.6093, 0.7369, -0.2394, -1.5118],
[-0.0193, -1.8624, 1.6373, -0.9649, 0.6501, -0.7532],
[0.0803, 0.1868, -1.2606, 1.8783, 1.1005, 1.758],
[1.5793, 0.3916, 1.6875, -1.646, -1.2161, -1.8191],
[-0.6987, -0.4453, -0.9146, 1.315, -0.573, -0.8763],
[0.1708, -1.4363, 1.2088, -1.7018, 1.089, 1.9475]], dtype=np.float32)),
("Wide 5x8", np.array([
[-1.5064, -2.4724, 1.5773, 1.0343, 1.145, 1.3564, -2.1298, -0.7077],
[-1.9207, 1.8155, 0.6165, -0.8455, -2.1822, -0.9451, -0.8741, 1.148],
[0.6878, 1.9361, -0.1389, -1.902, 1.0662, 1.3039, 0.3064, 1.3548],
[-0.031, 0.1137, -0.3623, -2.3729, -1.9605, -2.3429, 0.6821, -0.9282],
[0.0429, 2.0378, -1.2535, -0.4481, 1.2778, -1.356, -2.1151, -1.0512]], dtype=np.float32)),
("Tall 6x4 rank-def", np.array([
[-0.086904, 1.410225, 1.308323, 2.234762],
[0.022123, 0.896751, 0.324176, 0.773607],
[-0.473015, 1.555111, 0.290059, 1.157726],
[-0.78371, 1.398884, -1.930606, -1.548717],
[0.201518, -0.626835, 0.976596, 0.875294],
[-1.24206, 1.60595, -3.078089, -2.73695]], dtype=np.float32)),
]
for name, A in test_matrices:
U, s, Vt = svd(A, full_matrices=False)
print(f"\n{name}: shape={A.shape}")
print(f" Singular values: {[round(float(x), 12) for x in s]}")
print(f" U:\n{np.array2string(U, precision=6, floatmode='maxprec_equal')}")
print(f" Vt:\n{np.array2string(Vt, precision=6, floatmode='maxprec_equal')}")
recon_err = np.linalg.norm(A - U @ np.diag(s) @ Vt, 'fro')
print(f" Reconstruction error: {recon_err:.2e}")
# ------------------------------------------------------------------
# Test 8: Implicit QR Iteration on Bidiagonal
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("TEST 8: implicitQRIteration")
print("=" * 70)
qr_tests = [
("2x2 [[1,2],[3,4]]", np.array([[1.0, 2.0], [3.0, 4.0]])),
("3x3 diag", np.array([[10.0, 0, 0], [0, 5.0, 0], [0, 0, 2.0]])),
]
for name, A in qr_tests:
B, QL, QR = householder_bidiagonalization(A)
Sigma, QR_final = implicit_qr_iteration(B.copy(), QR.copy())
print(f"\n{name}:")
print(f" Bidiagonal B:\n{B}")
print(f" After QR iteration (Sigma):\n{Sigma}")
print(f" Diagonal entries: {[round(float(Sigma[i,i]), 10) for i in range(min(Sigma.shape))]}")
# Verify: QL @ Sigma @ QR_final^T ≈ A
recon = QL @ Sigma @ QR_final.T
err = np.linalg.norm(recon - A, 'fro')
print(f" ||QL @ Sigma @ QR^T - A||_F = {err:.2e}")
# ------------------------------------------------------------------
# JSON output for easy import into C++ tests
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("JSON OUTPUT (for easy C++ integration)")
print("=" * 70)
json_data = {}
# Householder test vectors
hh_tests = {}
for name, vec in test_vectors:
v, alpha = compute_householder(vec)
x = np.array(vec)
Hx = x - 2 * np.dot(v, x) * v
hh_tests[name] = {
"input": [float(xi) for xi in x],
"norm": float(np.linalg.norm(x)),
"alpha": float(alpha),
"v_normalized": [round(float(vi), 12) for vi in v],
"Hx": [round(float(xi), 12) for xi in Hx],
}
json_data["householder_vectors"] = hh_tests
# Full SVD reference values
svd_tests = {}
for name, A in test_matrices:
U, s, Vt = svd(A, full_matrices=False)
svd_tests[name] = {
"shape": list(A.shape),
"singular_values": [round(float(x), 12) for x in s],
"U": [[round(float(U[i,j]), 8) for j in range(U.shape[1])] for i in range(U.shape[0])],
"Vt": [[round(float(Vt[i,j]), 8) for j in range(Vt.shape[1])] for i in range(Vt.shape[0])],
}
json_data["svd_reference"] = svd_tests
print(json.dumps(json_data, indent=2))
if __name__ == "__main__":
main()
unit-tests/timing-results/matrix-timing-tests.txt
+52 -32
View File
@@ -1,36 +1,56 @@
Running matrix-timing-tests with timing
Randomness seeded to: 3567651885
1.857 s: Addition
1.857 s: Timing Tests
1.788 s: Subtraction
1.788 s: Timing Tests
1.929 s: Multiplication
1.929 s: Timing Tests
1.268 s: Scalar Multiplication
1.268 s: Timing Tests
1.798 s: Element Multiply
1.798 s: Timing Tests
1.802 s: Element Divide
1.803 s: Timing Tests
1.553 s: Minor Matrix
1.554 s: Timing Tests
1.009 s: Determinant
1.009 s: Timing Tests
4.076 s: Matrix of Minors
4.076 s: Timing Tests
1.066 s: Invert
1.066 s: Timing Tests
1.246 s: Transpose
1.246 s: Timing Tests
2.284 s: Normalize
2.284 s: Timing Tests
0.606 s: GET ROW
0.606 s: Timing Tests
24.629 s: GET COLUMN
24.630 s: Timing Tests
3.064 s: QR Decomposition
3.064 s: Timing Tests
Randomness seeded to: 3576947534
0.177 s: Addition
0.178 s: Timing Tests
0.182 s: Subtraction
0.182 s: Timing Tests
1.889 s: Multiplication
1.889 s: Timing Tests
0.126 s: Scalar Multiplication
0.126 s: Timing Tests
0.176 s: Element Multiply
0.176 s: Timing Tests
0.175 s: Element Divide
0.175 s: Timing Tests
0.151 s: Minor Matrix
0.151 s: Timing Tests
0.099 s: Determinant
0.100 s: Timing Tests
0.412 s: Matrix of Minors
0.412 s: Timing Tests
0.110 s: Invert
0.110 s: Timing Tests
0.122 s: Transpose
0.122 s: Timing Tests
0.184 s: Normalize
0.184 s: Timing Tests
0.006 s: GET ROW
0.006 s: Timing Tests
0.232 s: GET COLUMN
0.232 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