| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// SPDX-License-Identifier: BSD-3-Clause |
| 2 |
|
|
/// @file linear_algebra.h |
| 3 |
|
|
/// @brief Small dense linear solves and least-squares fits. |
| 4 |
|
|
#pragma once |
| 5 |
|
|
|
| 6 |
|
|
#include <glm/glm.hpp> |
| 7 |
|
|
|
| 8 |
|
|
#include <array> |
| 9 |
|
|
#include <cmath> |
| 10 |
|
|
#include <cstddef> |
| 11 |
|
|
#include <utility> |
| 12 |
|
|
#include <vector> |
| 13 |
|
|
|
| 14 |
|
|
namespace ernie { |
| 15 |
|
|
|
| 16 |
|
|
/// @brief Solves a x = b by Gaussian elimination with partial pivoting. |
| 17 |
|
|
/// @param a The n x n matrix, row-major. |
| 18 |
|
|
/// @param b The n right-hand sides; `T` is a number or a vector solved component-wise. |
| 19 |
|
|
/// @return x, or all zeros if the matrix is singular. |
| 20 |
|
5 |
template <typename T> std::vector<T> SolveLinearSystem(std::vector<double> a, std::vector<T> b) { |
| 21 |
|
5 |
const std::size_t n = b.size(); |
| 22 |
|
103 |
const auto at = [&](std::size_t row, std::size_t column) -> double& { |
| 23 |
6/8
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 time.
✓ Branch 2 taken 6 times.
✓ Branch 3 taken 1 time.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
|
94 |
return a[row * n + column]; |
| 24 |
|
|
}; |
| 25 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 2 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
|
16 |
for (std::size_t column = 0; column < n; ++column) { |
| 26 |
|
12 |
std::size_t pivot = column; |
| 27 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 7 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
|
21 |
for (std::size_t row = column + 1; row < n; ++row) { |
| 28 |
3/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 time.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
|
9 |
if (std::abs(at(row, column)) > std::abs(at(pivot, column))) { |
| 29 |
|
4 |
pivot = row; |
| 30 |
|
|
} |
| 31 |
|
|
} |
| 32 |
3/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 time.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
|
12 |
if (std::abs(at(pivot, column)) < 1e-300) { |
| 33 |
|
1 |
return std::vector<T>(n, T{}); |
| 34 |
|
|
} |
| 35 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 6 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 5 times.
|
39 |
for (std::size_t k = 0; k < n; ++k) { |
| 36 |
|
28 |
std::swap(at(column, k), at(pivot, k)); |
| 37 |
|
|
} |
| 38 |
|
11 |
std::swap(b[column], b[pivot]); |
| 39 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 6 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 5 times.
|
20 |
for (std::size_t row = column + 1; row < n; ++row) { |
| 40 |
|
9 |
const double factor = at(row, column) / at(column, column); |
| 41 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 5 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 4 times.
|
31 |
for (std::size_t k = column; k < n; ++k) { |
| 42 |
|
22 |
at(row, k) -= factor * at(column, k); |
| 43 |
|
|
} |
| 44 |
|
9 |
b[row] -= factor * b[column]; |
| 45 |
|
|
} |
| 46 |
|
|
} |
| 47 |
|
4 |
std::vector<T> x(n, T{}); |
| 48 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 2 times.
|
14 |
for (std::size_t row = n; row-- > 0;) { |
| 49 |
|
10 |
T sum = b[row]; |
| 50 |
4/4
std::vector<double, std::allocator<double> > ernie::SolveLinearSystem<double>(std::vector<double, std::allocator<double> >, std::vector<double, std::allocator<double> >):
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > > ernie::SolveLinearSystem<glm::vec<3, double, (glm::qualifier)0> >(std::vector<double, std::allocator<double> >, std::vector<glm::vec<3, double, (glm::qualifier)0>, std::allocator<glm::vec<3, double, (glm::qualifier)0> > >):
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 5 times.
|
18 |
for (std::size_t k = row + 1; k < n; ++k) { |
| 51 |
|
8 |
sum -= at(row, k) * x[k]; |
| 52 |
|
|
} |
| 53 |
|
10 |
x[row] = sum / at(row, row); |
| 54 |
|
|
} |
| 55 |
|
4 |
return x; |
| 56 |
|
4 |
} |
| 57 |
|
|
|
| 58 |
|
|
/// @brief Solves a 3 x 3 system. |
| 59 |
|
|
/// @param a The matrix. |
| 60 |
|
|
/// @param b The right-hand side. |
| 61 |
|
|
/// @return x with a x = b, or zero if the matrix is singular. |
| 62 |
|
839 |
inline glm::dvec3 Solve(const glm::dmat3& a, const glm::dvec3& b) { |
| 63 |
2/2
✓ Branch 0 taken 1 time.
✓ Branch 1 taken 838 times.
|
839 |
if (std::abs(glm::determinant(a)) < 1e-300) { |
| 64 |
|
1 |
return glm::dvec3(0.0); |
| 65 |
|
|
} |
| 66 |
|
838 |
return glm::inverse(a) * b; |
| 67 |
|
|
} |
| 68 |
|
|
|
| 69 |
|
|
/// @brief Solves the normal equations `normal` x = `right`, Jacobi-scaled for conditioning. |
| 70 |
|
|
/// @param normal The normal matrix. |
| 71 |
|
|
/// @param right The right-hand side. |
| 72 |
|
|
/// @param n How many leading unknowns to solve for. |
| 73 |
|
|
/// @return The solution, or all zeros if the system is singular. |
| 74 |
|
|
template <std::size_t N> |
| 75 |
|
1 |
std::array<double, N> SolveNormal(const std::array<std::array<double, N>, N>& normal, |
| 76 |
|
|
const std::array<double, N>& right, std::size_t n) { |
| 77 |
|
1 |
std::array<double, N> scale{}; |
| 78 |
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 |
for (std::size_t i = 0; i < n; ++i) { |
| 79 |
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 |
scale[i] = normal[i][i] > 0.0 ? 1.0 / std::sqrt(normal[i][i]) : 0.0; |
| 80 |
|
|
} |
| 81 |
1/2
✓ Branch 2 taken 1 time.
✗ Branch 3 not taken.
|
1 |
std::vector<double> a(n * n); |
| 82 |
1/2
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
|
1 |
std::vector<double> b(n); |
| 83 |
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 |
for (std::size_t i = 0; i < n; ++i) { |
| 84 |
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
|
6 |
for (std::size_t j = 0; j < n; ++j) { |
| 85 |
|
4 |
a[i * n + j] = normal[i][j] * scale[i] * scale[j]; |
| 86 |
|
|
} |
| 87 |
|
2 |
b[i] = right[i] * scale[i]; |
| 88 |
|
|
} |
| 89 |
1/2
✓ Branch 1 taken 1 time.
✗ Branch 2 not taken.
|
2 |
const std::vector<double> scaled = SolveLinearSystem(std::move(a), std::move(b)); |
| 90 |
|
1 |
std::array<double, N> x{}; |
| 91 |
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 time.
|
3 |
for (std::size_t i = 0; i < n; ++i) { |
| 92 |
|
2 |
x[i] = scaled[i] * scale[i]; |
| 93 |
|
|
} |
| 94 |
|
1 |
return x; |
| 95 |
|
1 |
} |
| 96 |
|
|
|
| 97 |
|
|
/// @brief Accumulates a linear least-squares fit's normal equations. |
| 98 |
|
|
template <std::size_t N> struct NormalEquations { |
| 99 |
|
|
/// The sum of row row^T. |
| 100 |
|
|
std::array<std::array<double, N>, N> normal{}; |
| 101 |
|
|
/// The sum of row value. |
| 102 |
|
|
std::array<double, N> right{}; |
| 103 |
|
|
|
| 104 |
|
|
/// @brief Adds the equation `row` . x = `value`. |
| 105 |
|
|
/// @param row The coefficients. |
| 106 |
|
|
/// @param value The right-hand side. |
| 107 |
|
|
/// @param n How many leading unknowns to use. |
| 108 |
|
5 |
void Add(const std::array<double, N>& row, double value, std::size_t n = N) { |
| 109 |
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 5 times.
|
15 |
for (std::size_t i = 0; i < n; ++i) { |
| 110 |
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 10 times.
|
30 |
for (std::size_t j = 0; j < n; ++j) { |
| 111 |
|
20 |
normal[i][j] += row[i] * row[j]; |
| 112 |
|
|
} |
| 113 |
|
10 |
right[i] += row[i] * value; |
| 114 |
|
|
} |
| 115 |
|
5 |
} |
| 116 |
|
|
|
| 117 |
|
|
/// @brief Solves the accumulated system (`SolveNormal`). |
| 118 |
|
|
/// @param n How many leading unknowns to solve for. |
| 119 |
|
|
/// @return The solution, or zeros if singular. |
| 120 |
|
1 |
std::array<double, N> Solve(std::size_t n = N) const { |
| 121 |
|
1 |
return SolveNormal(normal, right, n); |
| 122 |
|
|
} |
| 123 |
|
|
}; |
| 124 |
|
|
|
| 125 |
|
|
} // namespace ernie |
| 126 |
|
|
|