Blender V5.3
math_solvers.cc
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2015 Blender Authors
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
8
9#include "BLI_math_base_c.hh"
10#include "BLI_math_matrix_c.hh"
11#include "BLI_math_solvers.hh"
12#include "BLI_math_vector_c.hh"
13#include "MEM_guardedalloc.h"
14
15#include "BLI_utildefines.hh"
16
17#include "eigen_capi.h"
18
19#include <cstring>
20
21#include "BLI_strict_flags.hh" /* IWYU pragma: keep. Keep last. */
22
23namespace blender {
24
25/* -------------------------------------------------------------------- */
28
29bool BLI_eigen_solve_selfadjoint_m3(const float m3[3][3],
30 float r_eigen_values[3],
31 float r_eigen_vectors[3][3])
32{
33#ifndef NDEBUG
34 /* We must assert given matrix is self-adjoint (i.e. symmetric) */
35 if ((m3[0][1] != m3[1][0]) || (m3[0][2] != m3[2][0]) || (m3[1][2] != m3[2][1])) {
36 BLI_assert(0);
37 }
38#endif
39
41 reinterpret_cast<const float *>(m3),
42 r_eigen_values,
43 reinterpret_cast<float *>(r_eigen_vectors));
44}
45
46void BLI_svd_m3(const float m3[3][3], float r_U[3][3], float r_S[3], float r_V[3][3])
47{
49 reinterpret_cast<const float *>(m3),
50 reinterpret_cast<float *>(r_U),
51 r_S,
52 reinterpret_cast<float *>(r_V));
53}
54
56
57/* -------------------------------------------------------------------- */
60
62 const float *a, const float *b, const float *c, const float *d, float *r_x, const int count)
63{
64 if (count < 1) {
65 return false;
66 }
67
68 double *c1 = MEM_new_array_uninitialized<double>(size_t(count) * 2, "tridiagonal_c1d1");
69 if (!c1) {
70 return false;
71 }
72 double *d1 = c1 + count;
73
74 int i;
75 double c_prev, d_prev, x_prev;
76
77 /* forward pass */
78
79 c1[0] = c_prev = double(c[0]) / b[0];
80 d1[0] = d_prev = double(d[0]) / b[0];
81
82 for (i = 1; i < count; i++) {
83 double denum = b[i] - a[i] * c_prev;
84
85 c1[i] = c_prev = c[i] / denum;
86 d1[i] = d_prev = (d[i] - a[i] * d_prev) / denum;
87 }
88
89 /* back pass */
90
91 x_prev = d_prev;
92 r_x[--i] = float(x_prev);
93
94 while (--i >= 0) {
95 x_prev = d1[i] - c1[i] * x_prev;
96 r_x[i] = float(x_prev);
97 }
98
99 MEM_delete(c1);
100
101 return isfinite(x_prev);
102}
103
105 const float *a, const float *b, const float *c, const float *d, float *r_x, const int count)
106{
107 if (count < 1) {
108 return false;
109 }
110
111 /* Degenerate case not handled correctly by the generic formula. */
112 if (count == 1) {
113 r_x[0] = d[0] / (a[0] + b[0] + c[0]);
114
115 return isfinite(r_x[0]);
116 }
117
118 /* Degenerate case that works but can be simplified. */
119 if (count == 2) {
120 const float a2[2] = {0, a[1] + c[1]};
121 const float c2[2] = {a[0] + c[0], 0};
122
123 return BLI_tridiagonal_solve(a2, b, c2, d, r_x, count);
124 }
125
126 /* If not really cyclic, fall back to the simple solver. */
127 float a0 = a[0], cN = c[count - 1];
128
129 if (a0 == 0.0f && cN == 0.0f) {
130 return BLI_tridiagonal_solve(a, b, c, d, r_x, count);
131 }
132
133 size_t bytes = sizeof(float) * uint(count);
134 float *tmp = MEM_new_array_uninitialized<float>(size_t(count) * 2, "tridiagonal_ex");
135 if (!tmp) {
136 return false;
137 }
138 float *b2 = tmp + count;
139
140 /* Prepare the non-cyclic system; relies on tridiagonal_solve ignoring values. */
141 memcpy(b2, b, bytes);
142 b2[0] -= a0;
143 b2[count - 1] -= cN;
144
145 memset(tmp, 0, bytes);
146 tmp[0] = a0;
147 tmp[count - 1] = cN;
148
149 /* solve for partial solution and adjustment vector */
150 bool success = BLI_tridiagonal_solve(a, b2, c, tmp, tmp, count) &&
151 BLI_tridiagonal_solve(a, b2, c, d, r_x, count);
152
153 /* apply adjustment */
154 if (success) {
155 float coeff = (r_x[0] + r_x[count - 1]) / (1.0f + tmp[0] + tmp[count - 1]);
156
157 for (int i = 0; i < count; i++) {
158 r_x[i] -= coeff * tmp[i];
159 }
160 }
161
162 MEM_delete(tmp);
163
164 return success;
165}
166
168 Newton3D_JacobianFunc func_jacobian,
169 Newton3D_CorrectionFunc func_correction,
170 void *userdata,
171 float epsilon,
172 int max_iterations,
173 bool trace,
174 const float x_init[3],
175 float result[3])
176{
177 float fdelta[3], fdeltav, next_fdeltav;
178 float jacobian[3][3], step[3], x[3], x_next[3];
179
180 epsilon *= epsilon;
181
182 copy_v3_v3(x, x_init);
183
184 func_delta(userdata, x, fdelta);
185 fdeltav = len_squared_v3(fdelta);
186
187 if (trace) {
188 printf("START (%g, %g, %g) %g %g\n", x[0], x[1], x[2], fdeltav, epsilon);
189 }
190
191 for (int i = 0; i == 0 || (i < max_iterations && fdeltav > epsilon); i++) {
192 /* Newton's method step. */
193 func_jacobian(userdata, x, jacobian);
194
195 if (!invert_m3(jacobian)) {
196 return false;
197 }
198
199 mul_v3_m3v3(step, jacobian, fdelta);
200 sub_v3_v3v3(x_next, x, step);
201
202 /* Custom out-of-bounds value correction. */
203 if (func_correction) {
204 if (trace) {
205 printf("%3d * (%g, %g, %g)\n", i, x_next[0], x_next[1], x_next[2]);
206 }
207
208 if (!func_correction(userdata, x, step, x_next)) {
209 return false;
210 }
211 }
212
213 func_delta(userdata, x_next, fdelta);
214 next_fdeltav = len_squared_v3(fdelta);
215
216 if (trace) {
217 printf("%3d ? (%g, %g, %g) %g\n", i, x_next[0], x_next[1], x_next[2], next_fdeltav);
218 }
219
220 /* Line search correction. */
221 while (next_fdeltav > fdeltav && next_fdeltav > epsilon) {
222 float g0 = sqrtf(fdeltav), g1 = sqrtf(next_fdeltav);
223 float g01 = -g0 / len_v3(step);
224 float det = 2.0f * (g1 - g0 - g01);
225 float l = (det == 0.0f) ? 0.1f : -g01 / det;
226 CLAMP_MIN(l, 0.1f);
227
228 mul_v3_fl(step, l);
229 sub_v3_v3v3(x_next, x, step);
230
231 func_delta(userdata, x_next, fdelta);
232 next_fdeltav = len_squared_v3(fdelta);
233
234 if (trace) {
235 printf("%3d . (%g, %g, %g) %g\n", i, x_next[0], x_next[1], x_next[2], next_fdeltav);
236 }
237 }
238
239 copy_v3_v3(x, x_next);
240 fdeltav = next_fdeltav;
241 }
242
243 bool success = (fdeltav <= epsilon);
244
245 if (trace) {
246 printf("%s (%g, %g, %g) %g\n", success ? "OK " : "FAIL", x[0], x[1], x[2], fdeltav);
247 }
248
250 return success;
251}
252
254
255} // namespace blender
Read Blender memory allocation and freeing.
#define CLAMP_MIN(a, b)
bool EIG_self_adjoint_eigen_solve(const int size, const float *matrix, float *r_eigen_values, float *r_eigen_vectors)
VecBase< float, D > step(VecOp< float, D >, VecOp< float, D >) RET
BMesh Iterators.
bool BLI_tridiagonal_solve_cyclic(const float *a, const float *b, const float *c, const float *d, float *r_x, int count)
Solve a possibly cyclic tridiagonal system using the Sherman-Morrison formula.
BLI_assert(slot->slot_subtype.map==BMO_OP_SLOT_SUBTYPE_MAP_FLT)
void(*) Newton3D_DeltaFunc(void *userdata, const float x[3], float r_delta[3])
MINLINE void copy_v3_v3(float r[3], const float a[3])
unsigned int uint
Definition BKE_text.h:55
void * MEM_new_array_uninitialized(size_t len, size_t size, const char *str) ATTR_WARN_UNUSED_RESULT ATTR_ALLOC_SIZE(1
ATTR_WARN_UNUSED_RESULT const BMLoop * l
bool BLI_tridiagonal_solve(const float *a, const float *b, const float *c, const float *d, float *r_x, int count)
Solve a tridiagonal system of equations:
void BLI_svd_m3(const float m3[3][3], float r_U[3][3], float r_S[3], float r_V[3][3])
Compute the SVD (Singular Values Decomposition) of given 3D matrix (m3 = USV*).
MINLINE void mul_v3_fl(float r[3], float f)
MINLINE float len_v3(const float a[3]) ATTR_WARN_UNUSED_RESULT
bool BLI_newton3d_solve(Newton3D_DeltaFunc func_delta, Newton3D_JacobianFunc func_jacobian, Newton3D_CorrectionFunc func_correction, void *userdata, float epsilon, int max_iterations, bool trace, const float x_init[3], float result[3])
Solve a generic f(x) = 0 equation using Newton's method.
bool BLI_eigen_solve_selfadjoint_m3(const float m3[3][3], float r_eigen_values[3], float r_eigen_vectors[3][3])
Compute the eigen values and/or vectors of given 3D symmetric (aka adjoint) matrix.
void(*) Newton3D_JacobianFunc(void *userdata, const float x[3], float r_jacobian[3][3])
bool(*) Newton3D_CorrectionFunc(void *userdata, const float x[3], float step[3], float x_next[3])
bool invert_m3(float mat[3][3])
MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
MINLINE float len_squared_v3(const float v[3]) ATTR_WARN_UNUSED_RESULT
void mul_v3_m3v3(float r[3], const float M[3][3], const float a[3])
#define sqrtf
KERNEL_FEATURE_LIGHT_PASSES KERNEL_FEATURE_PATH_GUIDING float
void EIG_svd_square_matrix(const int size, const float *matrix, float *r_U, float *r_S, float *r_V)
Definition svd.cc:40