tinyopt 700
Tinyopt, a lightweight, header only optimization library
Loading...
Searching...
No Matches
small_linear_solver.h
1// Copyright 2026 Julien Michot.
2// SPDX-License-Identifier: Apache-2.0
3
4#pragma once
5
6#include <algorithm>
7#include <optional>
8
9#include <tinyopt/types.h>
10
11namespace tinyopt {
12
13template <typename Derived, typename Derived2>
14std::optional<Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>> SolveSmallDenseSystem(
15 const MatrixBase<Derived> &A, const MatrixBase<Derived2> &b) {
16 using Scalar = typename Derived::Scalar;
17 using Result = Vector<Scalar, Derived::RowsAtCompileTime>;
18 constexpr int Dims = Derived::RowsAtCompileTime;
19 const Scalar epsilon = Eigen::NumTraits<Scalar>::epsilon();
20
21 if constexpr (Dims == 1) {
22 const Scalar value = A(0, 0);
23 if (!(value > epsilon) || !Eigen::numext::isfinite(value)) return std::nullopt;
24 Result solution = b / value;
25 return solution;
26 } else if constexpr (Dims == 2) {
27 const Scalar a00 = A(0, 0);
28 const Scalar a01 = A(0, 1);
29 const Scalar a11 = A(1, 1);
30 const Scalar trace = a00 + a11;
31 const Scalar scale = std::max(Eigen::numext::abs(a00),
32 std::max(Eigen::numext::abs(a01), Eigen::numext::abs(a11)));
33 if (!(trace > epsilon * scale) || !Eigen::numext::isfinite(trace) ||
34 !Eigen::numext::isfinite(scale))
35 return std::nullopt;
36 const Scalar n00 = a00 / trace;
37 const Scalar n01 = a01 / trace;
38 const Scalar n11 = a11 / trace;
39 const Scalar determinant = n00 * n11 - n01 * n01;
40 if (!(Eigen::numext::abs(determinant) > epsilon) || !Eigen::numext::isfinite(determinant))
41 return std::nullopt;
42 const Scalar denominator = trace * determinant;
43 if (denominator == Scalar(0) || !Eigen::numext::isfinite(denominator)) return std::nullopt;
44 Result solution;
45 solution(0) = (n11 * b(0) - n01 * b(1)) / denominator;
46 solution(1) = (n00 * b(1) - n01 * b(0)) / denominator;
47 return solution;
48 } else if constexpr (Dims == 3) {
49 const Scalar a00 = A(0, 0);
50 const Scalar a01 = A(0, 1);
51 const Scalar a02 = A(0, 2);
52 const Scalar a11 = A(1, 1);
53 const Scalar a12 = A(1, 2);
54 const Scalar a22 = A(2, 2);
55 const Scalar trace = a00 + a11 + a22;
56 const Scalar scale =
57 std::max(std::max(Eigen::numext::abs(a00), Eigen::numext::abs(a01)),
58 std::max(std::max(Eigen::numext::abs(a02), Eigen::numext::abs(a11)),
59 std::max(Eigen::numext::abs(a12), Eigen::numext::abs(a22))));
60 if (!(Eigen::numext::abs(trace) > epsilon * scale) || !Eigen::numext::isfinite(trace) ||
61 !Eigen::numext::isfinite(scale))
62 return std::nullopt;
63 const Scalar n00 = a00 / trace;
64 const Scalar n01 = a01 / trace;
65 const Scalar n02 = a02 / trace;
66 const Scalar n11 = a11 / trace;
67 const Scalar n12 = a12 / trace;
68 const Scalar n22 = a22 / trace;
69 const Scalar determinant = n00 * (n11 * n22 - n12 * n12) - n01 * (n01 * n22 - n12 * n02) +
70 n02 * (n01 * n12 - n11 * n02);
71 if (!(Eigen::numext::abs(determinant) > epsilon) || !Eigen::numext::isfinite(determinant))
72 return std::nullopt;
73 const Scalar denominator = trace * determinant;
74 if (denominator == Scalar(0) || !Eigen::numext::isfinite(denominator)) return std::nullopt;
75 Result solution;
76 solution(0) = ((n11 * n22 - n12 * n12) * b(0) + (n02 * n12 - n01 * n22) * b(1) +
77 (n01 * n12 - n02 * n11) * b(2)) /
78 denominator;
79 solution(1) = ((n02 * n12 - n01 * n22) * b(0) + (n00 * n22 - n02 * n02) * b(1) +
80 (n01 * n02 - n00 * n12) * b(2)) /
81 denominator;
82 solution(2) = ((n01 * n12 - n02 * n11) * b(0) + (n01 * n02 - n00 * n12) * b(1) +
83 (n00 * n11 - n01 * n01) * b(2)) /
84 denominator;
85 return solution;
86 } else {
87 static_assert(Dims == 4);
88 const Scalar a00 = A(0, 0);
89 const Scalar a01 = A(0, 1);
90 const Scalar a02 = A(0, 2);
91 const Scalar a03 = A(0, 3);
92 const Scalar a11 = A(1, 1);
93 const Scalar a12 = A(1, 2);
94 const Scalar a13 = A(1, 3);
95 const Scalar a22 = A(2, 2);
96 const Scalar a23 = A(2, 3);
97 const Scalar a33 = A(3, 3);
98 const Scalar trace = a00 + a11 + a22 + a33;
99 const Scalar scale =
100 std::max(std::max(std::max(Eigen::numext::abs(a00), Eigen::numext::abs(a01)),
101 std::max(Eigen::numext::abs(a02), Eigen::numext::abs(a03))),
102 std::max(std::max(Eigen::numext::abs(a11), Eigen::numext::abs(a12)),
103 std::max(std::max(Eigen::numext::abs(a13), Eigen::numext::abs(a22)),
104 std::max(Eigen::numext::abs(a23), Eigen::numext::abs(a33)))));
105 if (!(Eigen::numext::abs(trace) > epsilon * scale) || !Eigen::numext::isfinite(trace) ||
106 !Eigen::numext::isfinite(scale))
107 return std::nullopt;
108
109 const Scalar n00 = a00 / trace;
110 const Scalar n01 = a01 / trace;
111 const Scalar n02 = a02 / trace;
112 const Scalar n03 = a03 / trace;
113 const Scalar n11 = a11 / trace;
114 const Scalar n12 = a12 / trace;
115 const Scalar n13 = a13 / trace;
116 const Scalar n22 = a22 / trace;
117 const Scalar n23 = a23 / trace;
118 const Scalar n33 = a33 / trace;
119 const auto det3 = [](Scalar m00, Scalar m01, Scalar m02, Scalar m10, Scalar m11, Scalar m12,
120 Scalar m20, Scalar m21, Scalar m22) {
121 return m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) +
122 m02 * (m10 * m21 - m11 * m20);
123 };
124 const Scalar c00 = det3(n11, n12, n13, n12, n22, n23, n13, n23, n33);
125 const Scalar c01 = -det3(n01, n12, n13, n02, n22, n23, n03, n23, n33);
126 const Scalar c02 = det3(n01, n11, n13, n02, n12, n23, n03, n13, n33);
127 const Scalar c03 = -det3(n01, n11, n12, n02, n12, n22, n03, n13, n23);
128 const Scalar determinant = n00 * c00 + n01 * c01 + n02 * c02 + n03 * c03;
129 if (!(Eigen::numext::abs(determinant) > epsilon * epsilon) ||
130 !Eigen::numext::isfinite(determinant))
131 return std::nullopt;
132 const Scalar denominator = trace * determinant;
133 if (denominator == Scalar(0) || !Eigen::numext::isfinite(denominator)) return std::nullopt;
134 const Scalar c11 = det3(n00, n02, n03, n02, n22, n23, n03, n23, n33);
135 const Scalar c12 = -det3(n00, n01, n03, n02, n12, n23, n03, n13, n33);
136 const Scalar c13 = det3(n00, n01, n02, n02, n12, n22, n03, n13, n23);
137 const Scalar c22 = det3(n00, n01, n03, n01, n11, n13, n03, n13, n33);
138 const Scalar c23 = -det3(n00, n01, n02, n01, n11, n12, n03, n13, n23);
139 const Scalar c33 = det3(n00, n01, n02, n01, n11, n12, n02, n12, n22);
140 Result solution;
141 solution(0) = (c00 * b(0) + c01 * b(1) + c02 * b(2) + c03 * b(3)) / denominator;
142 solution(1) = (c01 * b(0) + c11 * b(1) + c12 * b(2) + c13 * b(3)) / denominator;
143 solution(2) = (c02 * b(0) + c12 * b(1) + c22 * b(2) + c23 * b(3)) / denominator;
144 solution(3) = (c03 * b(0) + c13 * b(1) + c23 * b(2) + c33 * b(3)) / denominator;
145 return solution;
146 }
147}
148
149} // namespace tinyopt