numerical-toolbox-cpp/numerical/math/TriangularSolve.hpp at main · embedded-pro/numerical-toolbox-cpp · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#pragma once

#include "numerical/math/CompilerOptimizations.hpp"
#include "numerical/math/Math.hpp"
#include "numerical/math/Matrix.hpp"
#include "numerical/math/QNumber.hpp"
#include <cstddef>

namespace math
{
    template<typename T, std::size_t N>
    [[nodiscard]] OPTIMIZE_FOR_SPEED std::size_t FindPartialPivotRow(const Matrix<T, N, N>& matrix, std::size_t col)
    {
        std::size_t pivotRow = col;
        float maxVal = math::Abs(ToFloat(matrix.at(col, col)));

        for (std::size_t row = col + 1; row < N; ++row)
        {
            float absVal = math::Abs(ToFloat(matrix.at(row, col)));
            if (absVal > maxVal)
            {
                maxVal = absVal;
                pivotRow = row;
            }
        }

        return pivotRow;
    }

    namespace detail
    {
        template<bool Unit, typename T, std::size_t N>
        [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Vector<T, N> ForwardSubstitute(const Matrix<T, N, N>& l, const Vector<T, N>& c)
        {
            Vector<T, N> x{};

            for (std::size_t i = 0; i < N; ++i)
            {
                T sum = c.at(i, 0);
                for (std::size_t j = 0; j < i; ++j)
                    sum = sum - l.at(i, j) * x.at(j, 0);

                if constexpr (Unit)
                    x.at(i, 0) = sum;
                else
                    x.at(i, 0) = sum / l.at(i, i);
            }

            return x;
        }
    }

    template<typename T, std::size_t N>
    [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Vector<T, N> SolveUnitLowerTriangular(const Matrix<T, N, N>& l, const Vector<T, N>& c)
    {
        return detail::ForwardSubstitute<true>(l, c);
    }

    template<typename T, std::size_t N>
    [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Vector<T, N> SolveLowerTriangular(const Matrix<T, N, N>& l, const Vector<T, N>& c)
    {
        return detail::ForwardSubstitute<false>(l, c);
    }

    template<typename T, std::size_t N>
    [[nodiscard]] OPTIMIZE_FOR_SPEED constexpr Vector<T, N> SolveUpperTriangular(const Matrix<T, N, N>& r, const Vector<T, N>& c)
    {
        Vector<T, N> x{};

        for (std::size_t i = N; i > 0; --i)
        {
            std::size_t row = i - 1;
            T sum = c.at(row, 0);
            for (std::size_t j = row + 1; j < N; ++j)
                sum = sum - r.at(row, j) * x.at(j, 0);

            x.at(row, 0) = sum / r.at(row, row);
        }

        return x;
    }
}