Debian Codegraph @

src/Imath/ImathMatrixAlgo.cpp

raw
1 //
2 // SPDX-License-Identifier: BSD-3-Clause
3 // Copyright Contributors to the OpenEXR Project.
4 //
5
6 ///
7 /// @file ImathMatrixAlgo.cpp
8 ///
9 /// @brief Functions operating on Matrix22, Matrix33, and Matrix44 types.
10 ///
11
12 // clang-format off
13 /// @cond Doxygen_Suppress
14
15 #include "ImathMatrixAlgo.h"
16 #include <algorithm>
17 #include <cmath>
18
19 #if defined(IMATH_DLL)
20 # define EXPORT_CONST __declspec(dllexport)
21 #else
22 # define EXPORT_CONST const
23 #endif
24
25 IMATH_INTERNAL_NAMESPACE_SOURCE_ENTER
26
27 EXPORT_CONST M22f identity22f ( 1, 0,
28 0, 1);
29 EXPORT_CONST M22d identity22d ( 1, 0,
30 0, 1);
31 EXPORT_CONST M33f identity33f ( 1, 0, 0,
32 0, 1, 0,
33 0, 0, 1);
34 EXPORT_CONST M33d identity33d ( 1, 0, 0,
35 0, 1, 0,
36 0, 0, 1);
37 EXPORT_CONST M44f identity44f ( 1, 0, 0, 0,
38 0, 1, 0, 0,
39 0, 0, 1, 0,
40 0, 0, 0, 1);
41 EXPORT_CONST M44d identity44d ( 1, 0, 0, 0,
42 0, 1, 0, 0,
43 0, 0, 1, 0,
44 0, 0, 0, 1);
45
46 // clang-format on
47
48 namespace
49 {
50
51 class KahanSum
52 {
53 public:
54 KahanSum() : _total (0), _correction (0) {}
55
56 void operator+= (const double val)
57 {
58 const double y = val - _correction;
59 const double t = _total + y;
60 _correction = (t - _total) - y;
61 _total = t;
62 }
63
64 double get() const { return _total; }
65
66 private:
67 double _total;
68 double _correction;
69 };
70
71 } // namespace
72
73 template <typename T>
74 M44d
75 procrustesRotationAndTranslation (const Vec3<T>* A,
76 const Vec3<T>* B,
77 const T* weights,
78 const size_t numPoints,
79 const bool doScale)
80 {
81 if (numPoints == 0)
82 return M44d();
83
84 // Always do the accumulation in double precision:
85 V3d Acenter (0.0);
86 V3d Bcenter (0.0);
87 double weightsSum = 0.0;
88
89 if (weights == 0)
90 {
91 for (size_t i = 0; i < numPoints; ++i)
92 {
93 Acenter += (V3d) A[i];
94 Bcenter += (V3d) B[i];
95 }
96 weightsSum = (double) numPoints;
97 }
98 else
99 {
100 for (size_t i = 0; i < numPoints; ++i)
101 {
102 const double w = weights[i];
103 weightsSum += w;
104
105 Acenter += w * (V3d) A[i];
106 Bcenter += w * (V3d) B[i];
107 }
108 }
109
110 if (weightsSum == 0)
111 return M44d();
112
113 Acenter /= weightsSum;
114 Bcenter /= weightsSum;
115
116 //
117 // Find Q such that |Q*A - B| (actually A-Acenter and B-Bcenter, weighted)
118 // is minimized in the least squares sense.
119 // From Golub/Van Loan, p.601
120 //
121 // A,B are 3xn
122 // Let C = B A^T (where A is 3xn and B^T is nx3, so C is 3x3)
123 // Compute the SVD: C = U D V^T (U,V rotations, D diagonal).
124 // Throw away the D part, and return Q = U V^T
125 M33d C (0.0);
126 if (weights == 0)
127 {
128 for (size_t i = 0; i < numPoints; ++i)
129 C += outerProduct ((V3d) B[i] - Bcenter, (V3d) A[i] - Acenter);
130 }
131 else
132 {
133 for (size_t i = 0; i < numPoints; ++i)
134 {
135 const double w = weights[i];
136 C += outerProduct (w * ((V3d) B[i] - Bcenter), (V3d) A[i] - Acenter);
137 }
138 }
139
140 M33d U, V;
141 V3d S;
142 jacobiSVD (C, U, S, V, std::numeric_limits<double>::epsilon(), true);
143
144 // We want Q.transposed() here since we are going to be using it in the
145 // Imath style (multiplying vectors on the right, v' = v*A^T):
146 const M33d Qt = V * U.transposed();
147
148 double s = 1.0;
149 if (doScale && numPoints > 1)
150 {
151 // Finding a uniform scale: let us assume the Q is completely fixed
152 // at this point (solving for both simultaneously seems much harder).
153 // We are trying to compute (again, per Golub and van Loan)
154 // min || s*A*Q - B ||_F
155 // Notice that we've jammed a uniform scale in front of the Q.
156 // Now, the Frobenius norm (the least squares norm over matrices)
157 // has the neat property that it is equivalent to minimizing the trace
158 // of M^T*M (see your friendly neighborhood linear algebra text for a
159 // derivation). Thus, we can expand this out as
160 // min tr (s*A*Q - B)^T*(s*A*Q - B)
161 // = min tr(Q^T*A^T*s*s*A*Q) + tr(B^T*B) - 2*tr(Q^T*A^T*s*B) by linearity of the trace
162 // = min s^2 tr(A^T*A) + tr(B^T*B) - 2*s*tr(Q^T*A^T*B) using the fact that the trace is invariant
163 // under similarity transforms Q*M*Q^T
164 // If we differentiate w.r.t. s and set this to 0, we get
165 // 0 = 2*s*tr(A^T*A) - 2*tr(Q^T*A^T*B)
166 // so
167 // 2*s*tr(A^T*A) = 2*s*tr(Q^T*A^T*B)
168 // s = tr(Q^T*A^T*B) / tr(A^T*A)
169
170 KahanSum traceATA;
171 if (weights == 0)
172 {
173 for (size_t i = 0; i < numPoints; ++i)
174 traceATA += ((V3d) A[i] - Acenter).length2();
175 }
176 else
177 {
178 for (size_t i = 0; i < numPoints; ++i)
179 traceATA += ((double) weights[i]) * ((V3d) A[i] - Acenter).length2();
180 }
181
182 KahanSum traceBATQ;
183 for (int i = 0; i < 3; ++i)
184 for (int j = 0; j < 3; ++j)
185 traceBATQ += Qt[j][i] * C[i][j];
186
187 s = traceBATQ.get() / traceATA.get();
188 }
189
190 // Q is the rotation part of what we want to return.
191 // The entire transform is:
192 // (translate origin to Bcenter) * Q * (translate Acenter to origin)
193 // last first
194 // The effect of this on a point is:
195 // (translate origin to Bcenter) * Q * (translate Acenter to origin) * point
196 // = (translate origin to Bcenter) * Q * (-Acenter + point)
197 // = (translate origin to Bcenter) * (-Q*Acenter + Q*point)
198 // = (translate origin to Bcenter) * (translate Q*Acenter to origin) * Q*point
199 // = (translate Q*Acenter to Bcenter) * Q*point
200 // So what we want to return is:
201 // (translate Q*Acenter to Bcenter) * Q
202 //
203 // In block form, this is:
204 // [ 1 0 0 | ] [ 0 ] [ 1 0 0 | ] [ 1 0 0 | ] [ | ] [ ]
205 // [ 0 1 0 tb ] [ s*Q 0 ] [ 0 1 0 -ta ] = [ 0 1 0 tb ] [ s*Q -s*Q*ta ] = [ Q tb-s*Q*ta ]
206 // [ 0 0 1 | ] [ 0 ] [ 0 0 1 | ] [ 0 0 1 | ] [ | ] [ ]
207 // [ 0 0 0 1 ] [ 0 0 0 1 ] [ 0 0 0 1 ] [ 0 0 0 1 ] [ 0 0 0 1 ] [ 0 0 0 1 ]
208 // (ofc the whole thing is transposed for Imath).
209 const V3d translate = Bcenter - s * Acenter * Qt;
210
211 return M44d (s * Qt.x[0][0],
212 s * Qt.x[0][1],
213 s * Qt.x[0][2],
214 T (0),
215 s * Qt.x[1][0],
216 s * Qt.x[1][1],
217 s * Qt.x[1][2],
218 T (0),
219 s * Qt.x[2][0],
220 s * Qt.x[2][1],
221 s * Qt.x[2][2],
222 T (0),
223 translate.x,
224 translate.y,
225 translate.z,
226 T (1));
227 } // procrustesRotationAndTranslation
228
229 ///
230 /// Return the procrustes transformation of a set of points: the
231 /// rotation, translation, and optionally the scale that comes closest
232 /// in a least squares sense to transforming the `A` points into
233 /// `B`.
234 ///
235
236 template <typename T>
237 M44d
238 procrustesRotationAndTranslation (const Vec3<T>* A,
239 const Vec3<T>* B,
240 const size_t numPoints,
241 const bool doScale)
242 {
243 return procrustesRotationAndTranslation (A, B, (const T*) 0, numPoints, doScale);
244 } // procrustesRotationAndTranslation
245
246 /// TODO
247 template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3d* from,
248 const V3d* to,
249 const size_t numPoints,
250 const bool doScale);
251 /// TODO
252 template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3f* from,
253 const V3f* to,
254 const size_t numPoints,
255 const bool doScale);
256 /// TODO
257 template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3d* from,
258 const V3d* to,
259 const double* weights,
260 const size_t numPoints,
261 const bool doScale);
262 /// TODO
263 template IMATH_EXPORT M44d procrustesRotationAndTranslation (const V3f* from,
264 const V3f* to,
265 const float* weights,
266 const size_t numPoints,
267 const bool doScale);
268
269 namespace
270 {
271
272 // Applies the 2x2 Jacobi rotation
273 // [ c s 0 ] [ 1 0 0 ] [ c 0 s ]
274 // [ -s c 0 ] or [ 0 c s ] or [ 0 1 0 ]
275 // [ 0 0 1 ] [ 0 -s c ] [ -s 0 c ]
276 // from the right; that is, computes
277 // J * A
278 // for the Jacobi rotation J and the matrix A. This is efficient because we
279 // only need to touch exactly the 2 columns that are affected, so we never
280 // need to explicitly construct the J matrix.
281 template <typename T, int j, int k>
282 void
283 jacobiRotateRight (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A, const T c, const T s)
284 {
285 for (int i = 0; i < 3; ++i)
286 {
287 const T tau1 = A[i][j];
288 const T tau2 = A[i][k];
289 A[i][j] = c * tau1 - s * tau2;
290 A[i][k] = s * tau1 + c * tau2;
291 }
292 }
293
294 template <typename T>
295 void
296 jacobiRotateRight (IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
297 const int j,
298 const int k,
299 const T c,
300 const T s)
301 {
302 for (int i = 0; i < 4; ++i)
303 {
304 const T tau1 = A[i][j];
305 const T tau2 = A[i][k];
306 A[i][j] = c * tau1 - s * tau2;
307 A[i][k] = s * tau1 + c * tau2;
308 }
309 }
310
311 // This routine solves the 2x2 SVD:
312 // [ c1 s1 ] [ w x ] [ c2 s2 ] [ d1 0 ]
313 // [ ] [ ] [ ] = [ ]
314 // [ -s1 c1 ] [ y z ] [ -s2 c2 ] [ 0 d2 ]
315 // where
316 // [ w x ]
317 // A = [ ]
318 // [ y z ]
319 // is the subset of A consisting of the [j,k] entries, A([j k], [j k]) in
320 // Matlab parlance. The method is the 'USVD' algorithm described in the
321 // following paper:
322 // 'Computation of the Singular Value Decomposition using Mesh-Connected Processors'
323 // by Richard P. Brent, Franklin T. Luk, and Charles Van Loan
324 // It breaks the computation into two steps: the first symmetrizes the matrix,
325 // and the second diagonalizes the symmetric matrix.
326 template <typename T, int j, int k, int l>
327 bool
328 twoSidedJacobiRotation (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
329 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
330 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
331 const T tol)
332 {
333 // Load everything into local variables to make things easier on the
334 // optimizer:
335 const T w = A[j][j];
336 const T x = A[j][k];
337 const T y = A[k][j];
338 const T z = A[k][k];
339
340 // We will keep track of whether we're actually performing any rotations,
341 // since if the matrix is already diagonal we'll end up with the identity
342 // as our Jacobi rotation and we can short-circuit.
343 bool changed = false;
344
345 // The first step is to symmetrize the 2x2 matrix,
346 // [ c s ]^T [ w x ] = [ p q ]
347 // [ -s c ] [ y z ] [ q r ]
348 T mu_1 = w + z;
349 T mu_2 = x - y;
350
351 T c, s;
352 if (std::abs (mu_2) <= tol * std::abs (mu_1)) // Already symmetric (to tolerance)
353 { // Note that the <= is important here
354 c = T (1); // because we want to bypass the computation
355 s = T (0); // of rho if mu_1 = mu_2 = 0.
356
357 const T p = w;
358 const T r = z;
359 mu_1 = r - p;
360 mu_2 = x + y;
361 }
362 else
363 {
364 // TODO is there a native inverse square root function?
365 const T rho = mu_1 / mu_2;
366
367 s = T (1) / std::sqrt (T (1) + rho * rho);
368 if (rho < 0)
369 s = -s;
370 c = s * rho;
371
372 mu_1 = s * (x + y) + c * (z - w); // = r - p
373 mu_2 = T (2) * (c * x - s * z); // = 2*q
374
375 changed = true;
376 }
377
378 // The second stage diagonalizes,
379 // [ c2 s2 ]^T [ p q ] [ c2 s2 ] = [ d1 0 ]
380 // [ -s2 c2 ] [ q r ] [ -s2 c2 ] [ 0 d2 ]
381 T c_2, s_2;
382 if (std::abs (mu_2) <= tol * std::abs (mu_1))
383 {
384 c_2 = T (1);
385 s_2 = T (0);
386 }
387 else
388 {
389 const T rho_2 = mu_1 / mu_2;
390 T t_2 = T (1) / (std::abs (rho_2) + std::sqrt (1 + rho_2 * rho_2));
391 if (rho_2 < 0)
392 t_2 = -t_2;
393 c_2 = T (1) / std::sqrt (T (1) + t_2 * t_2);
394 s_2 = c_2 * t_2;
395
396 changed = true;
397 }
398
399 const T c_1 = c_2 * c - s_2 * s;
400 const T s_1 = s_2 * c + c_2 * s;
401
402 if (!changed)
403 {
404 // We've decided that the off-diagonal entries are already small
405 // enough, so we'll set them to zero. This actually appears to result
406 // in smaller errors than leaving them be, possibly because it prevents
407 // us from trying to do extra rotations later that we don't need.
408 A[k][j] = 0;
409 A[j][k] = 0;
410 return false;
411 }
412
413 const T d_1 = c_1 * (w * c_2 - x * s_2) - s_1 * (y * c_2 - z * s_2);
414 const T d_2 = s_1 * (w * s_2 + x * c_2) + c_1 * (y * s_2 + z * c_2);
415
416 // For the entries we just zeroed out, we'll just set them to 0, since
417 // they should be 0 up to machine precision.
418 A[j][j] = d_1;
419 A[k][k] = d_2;
420 A[k][j] = 0;
421 A[j][k] = 0;
422
423 // Rotate the entries that _weren't_ involved in the 2x2 SVD:
424 {
425 // Rotate on the left by
426 // [ c1 s1 0 ]^T [ c1 0 s1 ]^T [ 1 0 0 ]^T
427 // [ -s1 c1 0 ] or [ 0 1 0 ] or [ 0 c1 s1 ]
428 // [ 0 0 1 ] [ -s1 0 c1 ] [ 0 -s1 c1 ]
429 // This has the effect of adding the (weighted) ith and jth _rows_ to
430 // each other.
431 const T tau1 = A[j][l];
432 const T tau2 = A[k][l];
433 A[j][l] = c_1 * tau1 - s_1 * tau2;
434 A[k][l] = s_1 * tau1 + c_1 * tau2;
435 }
436
437 {
438 // Rotate on the right by
439 // [ c2 s2 0 ] [ c2 0 s2 ] [ 1 0 0 ]
440 // [ -s2 c2 0 ] or [ 0 1 0 ] or [ 0 c2 s2 ]
441 // [ 0 0 1 ] [ -s2 0 c2 ] [ 0 -s2 c2 ]
442 // This has the effect of adding the (weighted) ith and jth _columns_ to
443 // each other.
444 const T tau1 = A[l][j];
445 const T tau2 = A[l][k];
446 A[l][j] = c_2 * tau1 - s_2 * tau2;
447 A[l][k] = s_2 * tau1 + c_2 * tau2;
448 }
449
450 // Now apply the rotations to U and V:
451 // Remember that we have
452 // R1^T * A * R2 = D
453 // This is in the 2x2 case, but after doing a bunch of these
454 // we will get something like this for the 3x3 case:
455 // ... R1b^T * R1a^T * A * R2a * R2b * ... = D
456 // ----------------- ---------------
457 // = U^T = V
458 // So,
459 // U = R1a * R1b * ...
460 // V = R2a * R2b * ...
461 jacobiRotateRight<T, j, k> (U, c_1, s_1);
462 jacobiRotateRight<T, j, k> (V, c_2, s_2);
463
464 return true;
465 }
466
467 template <typename T>
468 bool
469 twoSidedJacobiRotation (IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
470 int j,
471 int k,
472 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
473 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
474 const T tol)
475 {
476 // Load everything into local variables to make things easier on the
477 // optimizer:
478 const T w = A[j][j];
479 const T x = A[j][k];
480 const T y = A[k][j];
481 const T z = A[k][k];
482
483 // We will keep track of whether we're actually performing any rotations,
484 // since if the matrix is already diagonal we'll end up with the identity
485 // as our Jacobi rotation and we can short-circuit.
486 bool changed = false;
487
488 // The first step is to symmetrize the 2x2 matrix,
489 // [ c s ]^T [ w x ] = [ p q ]
490 // [ -s c ] [ y z ] [ q r ]
491 T mu_1 = w + z;
492 T mu_2 = x - y;
493
494 T c, s;
495 if (std::abs (mu_2) <= tol * std::abs (mu_1)) // Already symmetric (to tolerance)
496 { // Note that the <= is important here
497 c = T (1); // because we want to bypass the computation
498 s = T (0); // of rho if mu_1 = mu_2 = 0.
499
500 const T p = w;
501 const T r = z;
502 mu_1 = r - p;
503 mu_2 = x + y;
504 }
505 else
506 {
507 // TODO is there a native inverse square root function?
508 const T rho = mu_1 / mu_2;
509
510 s = T (1) / std::sqrt (T (1) + rho * rho);
511 if (rho < 0)
512 s = -s;
513 c = s * rho;
514
515 mu_1 = s * (x + y) + c * (z - w); // = r - p
516 mu_2 = T (2) * (c * x - s * z); // = 2*q
517
518 changed = true;
519 }
520
521 // The second stage diagonalizes,
522 // [ c2 s2 ]^T [ p q ] [ c2 s2 ] = [ d1 0 ]
523 // [ -s2 c2 ] [ q r ] [ -s2 c2 ] [ 0 d2 ]
524 T c_2, s_2;
525 if (std::abs (mu_2) <= tol * std::abs (mu_1))
526 {
527 c_2 = T (1);
528 s_2 = T (0);
529 }
530 else
531 {
532 const T rho_2 = mu_1 / mu_2;
533 T t_2 = T (1) / (std::abs (rho_2) + std::sqrt (1 + rho_2 * rho_2));
534 if (rho_2 < 0)
535 t_2 = -t_2;
536 c_2 = T (1) / std::sqrt (T (1) + t_2 * t_2);
537 s_2 = c_2 * t_2;
538
539 changed = true;
540 }
541
542 const T c_1 = c_2 * c - s_2 * s;
543 const T s_1 = s_2 * c + c_2 * s;
544
545 if (!changed)
546 {
547 // We've decided that the off-diagonal entries are already small
548 // enough, so we'll set them to zero. This actually appears to result
549 // in smaller errors than leaving them be, possibly because it prevents
550 // us from trying to do extra rotations later that we don't need.
551 A[k][j] = 0;
552 A[j][k] = 0;
553 return false;
554 }
555
556 const T d_1 = c_1 * (w * c_2 - x * s_2) - s_1 * (y * c_2 - z * s_2);
557 const T d_2 = s_1 * (w * s_2 + x * c_2) + c_1 * (y * s_2 + z * c_2);
558
559 // For the entries we just zeroed out, we'll just set them to 0, since
560 // they should be 0 up to machine precision.
561 A[j][j] = d_1;
562 A[k][k] = d_2;
563 A[k][j] = 0;
564 A[j][k] = 0;
565
566 // Rotate the entries that _weren't_ involved in the 2x2 SVD:
567 for (int l = 0; l < 4; ++l)
568 {
569 if (l == j || l == k)
570 continue;
571
572 // Rotate on the left by
573 // [ 1 ]
574 // [ . ]
575 // [ c2 s2 ] j
576 // [ 1 ]
577 // [ -s2 c2 ] k
578 // [ . ]
579 // [ 1 ]
580 // j k
581 //
582 // This has the effect of adding the (weighted) ith and jth _rows_ to
583 // each other.
584 const T tau1 = A[j][l];
585 const T tau2 = A[k][l];
586 A[j][l] = c_1 * tau1 - s_1 * tau2;
587 A[k][l] = s_1 * tau1 + c_1 * tau2;
588 }
589
590 for (int l = 0; l < 4; ++l)
591 {
592 // We set the A[j/k][j/k] entries already
593 if (l == j || l == k)
594 continue;
595
596 // Rotate on the right by
597 // [ 1 ]
598 // [ . ]
599 // [ c2 s2 ] j
600 // [ 1 ]
601 // [ -s2 c2 ] k
602 // [ . ]
603 // [ 1 ]
604 // j k
605 //
606 // This has the effect of adding the (weighted) ith and jth _columns_ to
607 // each other.
608 const T tau1 = A[l][j];
609 const T tau2 = A[l][k];
610 A[l][j] = c_2 * tau1 - s_2 * tau2;
611 A[l][k] = s_2 * tau1 + c_2 * tau2;
612 }
613
614 // Now apply the rotations to U and V:
615 // Remember that we have
616 // R1^T * A * R2 = D
617 // This is in the 2x2 case, but after doing a bunch of these
618 // we will get something like this for the 3x3 case:
619 // ... R1b^T * R1a^T * A * R2a * R2b * ... = D
620 // ----------------- ---------------
621 // = U^T = V
622 // So,
623 // U = R1a * R1b * ...
624 // V = R2a * R2b * ...
625 jacobiRotateRight (U, j, k, c_1, s_1);
626 jacobiRotateRight (V, j, k, c_2, s_2);
627
628 return true;
629 }
630
631 template <typename T>
632 void
633 swapColumns (IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A, int j, int k)
634 {
635 for (int i = 0; i < 3; ++i)
636 std::swap (A[i][j], A[i][k]);
637 }
638
639 template <typename T>
640 IMATH_CONSTEXPR14 T
641 maxOffDiag (const IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A)
642 {
643 T result = 0;
644 result = std::max (result, std::abs (A[0][1]));
645 result = std::max (result, std::abs (A[0][2]));
646 result = std::max (result, std::abs (A[1][0]));
647 result = std::max (result, std::abs (A[1][2]));
648 result = std::max (result, std::abs (A[2][0]));
649 result = std::max (result, std::abs (A[2][1]));
650 return result;
651 }
652
653 template <typename T>
654 IMATH_CONSTEXPR14 T
655 maxOffDiag (const IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A)
656 {
657 T result = 0;
658 for (int i = 0; i < 4; ++i)
659 {
660 for (int j = 0; j < 4; ++j)
661 {
662 if (i != j)
663 result = std::max (result, std::abs (A[i][j]));
664 }
665 }
666
667 return result;
668 }
669
670 template <typename T>
671 void
672 twoSidedJacobiSVD (IMATH_INTERNAL_NAMESPACE::Matrix33<T> A,
673 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
674 IMATH_INTERNAL_NAMESPACE::Vec3<T>& S,
675 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
676 const T tol,
677 const bool forcePositiveDeterminant)
678 {
679 // The two-sided Jacobi SVD works by repeatedly zeroing out
680 // off-diagonal entries of the matrix, 2 at a time. Basically,
681 // we can take our 3x3 matrix,
682 // [* * *]
683 // [* * *]
684 // [* * *]
685 // and use a pair of orthogonal transforms to zero out, say, the
686 // pair of entries (0, 1) and (1, 0):
687 // [ c1 s1 ] [* * *] [ c2 s2 ] [* *]
688 // [-s1 c1 ] [* * *] [-s2 c2 ] = [ * *]
689 // [ 1] [* * *] [ 1] [* * *]
690 // When we go to zero out the next pair of entries (say, (0, 2) and (2, 0))
691 // then we don't expect those entries to stay 0:
692 // [ c1 s1 ] [* *] [ c2 s2 ] [* * ]
693 // [-s1 c1 ] [ * *] [-s2 c2 ] = [* * *]
694 // [ 1] [* * *] [ 1] [ * *]
695 // However, if we keep doing this, we'll find that the off-diagonal entries
696 // converge to 0 fairly quickly (convergence should be roughly cubic). The
697 // result is a diagonal A matrix and a bunch of orthogonal transforms:
698 // [* * *] [* ]
699 // L1 L2 ... Ln [* * *] Rn ... R2 R1 = [ * ]
700 // [* * *] [ *]
701 // ------------ ------- ------------ -------
702 // U^T A V S
703 // This turns out to be highly accurate because (1) orthogonal transforms
704 // are extremely stable to compute and apply (this is why QR factorization
705 // works so well, FWIW) and because (2) by applying everything to the original
706 // matrix A instead of computing (A^T * A) we avoid any precision loss that
707 // would result from that.
708 U.makeIdentity();
709 V.makeIdentity();
710
711 const int maxIter = 20; // In case we get really unlucky, prevents infinite loops
712 const T absTol = tol * maxOffDiag (A); // Tolerance is in terms of the maximum
713 if (absTol != 0) // _off-diagonal_ entry.
714 {
715 int numIter = 0;
716 do
717 {
718 ++numIter;
719 bool changed = twoSidedJacobiRotation<T, 0, 1, 2> (A, U, V, tol);
720 changed = twoSidedJacobiRotation<T, 0, 2, 1> (A, U, V, tol) || changed;
721 changed = twoSidedJacobiRotation<T, 1, 2, 0> (A, U, V, tol) || changed;
722 if (!changed)
723 break;
724 } while (maxOffDiag (A) > absTol && numIter < maxIter);
725 }
726
727 // The off-diagonal entries are (effectively) 0, so whatever's left on the
728 // diagonal are the singular values:
729 S.x = A[0][0];
730 S.y = A[1][1];
731 S.z = A[2][2];
732
733 // Nothing thus far has guaranteed that the singular values are positive,
734 // so let's go back through and flip them if not (since by contract we are
735 // supposed to return all positive SVs):
736 for (int i = 0; i < 3; ++i)
737 {
738 if (S[i] < 0)
739 {
740 // If we flip S[i], we need to flip the corresponding column of U
741 // (we could also pick V if we wanted; it doesn't really matter):
742 S[i] = -S[i];
743 for (int j = 0; j < 3; ++j)
744 U[j][i] = -U[j][i];
745 }
746 }
747
748 // Order the singular values from largest to smallest; this requires
749 // exactly two passes through the data using bubble sort:
750 for (int i = 0; i < 2; ++i)
751 {
752 for (int j = 0; j < (2 - i); ++j)
753 {
754 // No absolute values necessary since we already ensured that
755 // they're positive:
756 if (S[j] < S[j + 1])
757 {
758 // If we swap singular values we also have to swap
759 // corresponding columns in U and V:
760 std::swap (S[j], S[j + 1]);
761 swapColumns (U, j, j + 1);
762 swapColumns (V, j, j + 1);
763 }
764 }
765 }
766
767 if (forcePositiveDeterminant)
768 {
769 // We want to guarantee that the returned matrices always have positive
770 // determinant. We can do this by adding the appropriate number of
771 // matrices of the form:
772 // [ 1 ]
773 // L = [ 1 ]
774 // [ -1 ]
775 // Note that L' = L and L*L = Identity. Thus we can add:
776 // U*L*L*S*V = (U*L)*(L*S)*V
777 // if U has a negative determinant, and
778 // U*S*L*L*V = U*(S*L)*(L*V)
779 // if V has a neg. determinant.
780 if (U.determinant() < 0)
781 {
782 for (int i = 0; i < 3; ++i)
783 U[i][2] = -U[i][2];
784 S.z = -S.z;
785 }
786
787 if (V.determinant() < 0)
788 {
789 for (int i = 0; i < 3; ++i)
790 V[i][2] = -V[i][2];
791 S.z = -S.z;
792 }
793 }
794 }
795
796 template <typename T>
797 void
798 twoSidedJacobiSVD (IMATH_INTERNAL_NAMESPACE::Matrix44<T> A,
799 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
800 IMATH_INTERNAL_NAMESPACE::Vec4<T>& S,
801 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
802 const T tol,
803 const bool forcePositiveDeterminant)
804 {
805 // Please see the Matrix33 version for a detailed description of the algorithm.
806 U.makeIdentity();
807 V.makeIdentity();
808
809 const int maxIter = 20; // In case we get really unlucky, prevents infinite loops
810 const T absTol = tol * maxOffDiag (A); // Tolerance is in terms of the maximum
811 if (absTol != 0) // _off-diagonal_ entry.
812 {
813 int numIter = 0;
814 do
815 {
816 ++numIter;
817 bool changed = twoSidedJacobiRotation (A, 0, 1, U, V, tol);
818 changed = twoSidedJacobiRotation (A, 0, 2, U, V, tol) || changed;
819 changed = twoSidedJacobiRotation (A, 0, 3, U, V, tol) || changed;
820 changed = twoSidedJacobiRotation (A, 1, 2, U, V, tol) || changed;
821 changed = twoSidedJacobiRotation (A, 1, 3, U, V, tol) || changed;
822 changed = twoSidedJacobiRotation (A, 2, 3, U, V, tol) || changed;
823 if (!changed)
824 break;
825 } while (maxOffDiag (A) > absTol && numIter < maxIter);
826 }
827
828 // The off-diagonal entries are (effectively) 0, so whatever's left on the
829 // diagonal are the singular values:
830 S[0] = A[0][0];
831 S[1] = A[1][1];
832 S[2] = A[2][2];
833 S[3] = A[3][3];
834
835 // Nothing thus far has guaranteed that the singular values are positive,
836 // so let's go back through and flip them if not (since by contract we are
837 // supposed to return all positive SVs):
838 for (int i = 0; i < 4; ++i)
839 {
840 if (S[i] < 0)
841 {
842 // If we flip S[i], we need to flip the corresponding column of U
843 // (we could also pick V if we wanted; it doesn't really matter):
844 S[i] = -S[i];
845 for (int j = 0; j < 4; ++j)
846 U[j][i] = -U[j][i];
847 }
848 }
849
850 // Order the singular values from largest to smallest using insertion sort:
851 for (int i = 1; i < 4; ++i)
852 {
853 const IMATH_INTERNAL_NAMESPACE::Vec4<T> uCol (U[0][i], U[1][i], U[2][i], U[3][i]);
854 const IMATH_INTERNAL_NAMESPACE::Vec4<T> vCol (V[0][i], V[1][i], V[2][i], V[3][i]);
855 const T sVal = S[i];
856
857 int j = i - 1;
858 while (std::abs (S[j]) < std::abs (sVal))
859 {
860 for (int k = 0; k < 4; ++k)
861 U[k][j + 1] = U[k][j];
862 for (int k = 0; k < 4; ++k)
863 V[k][j + 1] = V[k][j];
864 S[j + 1] = S[j];
865
866 --j;
867 if (j < 0)
868 break;
869 }
870
871 for (int k = 0; k < 4; ++k)
872 U[k][j + 1] = uCol[k];
873 for (int k = 0; k < 4; ++k)
874 V[k][j + 1] = vCol[k];
875 S[j + 1] = sVal;
876 }
877
878 if (forcePositiveDeterminant)
879 {
880 // We want to guarantee that the returned matrices always have positive
881 // determinant. We can do this by adding the appropriate number of
882 // matrices of the form:
883 // [ 1 ]
884 // L = [ 1 ]
885 // [ 1 ]
886 // [ -1 ]
887 // Note that L' = L and L*L = Identity. Thus we can add:
888 // U*L*L*S*V = (U*L)*(L*S)*V
889 // if U has a negative determinant, and
890 // U*S*L*L*V = U*(S*L)*(L*V)
891 // if V has a neg. determinant.
892 if (U.determinant() < 0)
893 {
894 for (int i = 0; i < 4; ++i)
895 U[i][3] = -U[i][3];
896 S[3] = -S[3];
897 }
898
899 if (V.determinant() < 0)
900 {
901 for (int i = 0; i < 4; ++i)
902 V[i][3] = -V[i][3];
903 S[3] = -S[3];
904 }
905 }
906 }
907
908 } // namespace
909
910 /// TODO
911 template <typename T>
912 void
913 jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<T>& A,
914 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& U,
915 IMATH_INTERNAL_NAMESPACE::Vec3<T>& S,
916 IMATH_INTERNAL_NAMESPACE::Matrix33<T>& V,
917 const T tol,
918 const bool forcePositiveDeterminant)
919 {
920 twoSidedJacobiSVD (A, U, S, V, tol, forcePositiveDeterminant);
921 }
922
923 /// TODO
924 template <typename T>
925 void
926 jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<T>& A,
927 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& U,
928 IMATH_INTERNAL_NAMESPACE::Vec4<T>& S,
929 IMATH_INTERNAL_NAMESPACE::Matrix44<T>& V,
930 const T tol,
931 const bool forcePositiveDeterminant)
932 {
933 twoSidedJacobiSVD (A, U, S, V, tol, forcePositiveDeterminant);
934 }
935
936 /// TODO
937 template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<float>& A,
938 IMATH_INTERNAL_NAMESPACE::Matrix33<float>& U,
939 IMATH_INTERNAL_NAMESPACE::Vec3<float>& S,
940 IMATH_INTERNAL_NAMESPACE::Matrix33<float>& V,
941 const float tol,
942 const bool forcePositiveDeterminant);
943 /// TODO
944 template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix33<double>& A,
945 IMATH_INTERNAL_NAMESPACE::Matrix33<double>& U,
946 IMATH_INTERNAL_NAMESPACE::Vec3<double>& S,
947 IMATH_INTERNAL_NAMESPACE::Matrix33<double>& V,
948 const double tol,
949 const bool forcePositiveDeterminant);
950 /// TODO
951 template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<float>& A,
952 IMATH_INTERNAL_NAMESPACE::Matrix44<float>& U,
953 IMATH_INTERNAL_NAMESPACE::Vec4<float>& S,
954 IMATH_INTERNAL_NAMESPACE::Matrix44<float>& V,
955 const float tol,
956 const bool forcePositiveDeterminant);
957 /// TODO
958 template IMATH_EXPORT void jacobiSVD (const IMATH_INTERNAL_NAMESPACE::Matrix44<double>& A,
959 IMATH_INTERNAL_NAMESPACE::Matrix44<double>& U,
960 IMATH_INTERNAL_NAMESPACE::Vec4<double>& S,
961 IMATH_INTERNAL_NAMESPACE::Matrix44<double>& V,
962 const double tol,
963 const bool forcePositiveDeterminant);
964
965 namespace
966 {
967
968 template <int j, int k, typename TM>
969 inline void
970 jacobiRotateRight (TM& A, const typename TM::BaseType s, const typename TM::BaseType tau)
971 {
972 typedef typename TM::BaseType T;
973
974 for (unsigned int i = 0; i < TM::dimensions(); ++i)
975 {
976 const T nu1 = A[i][j];
977 const T nu2 = A[i][k];
978 A[i][j] -= s * (nu2 + tau * nu1);
979 A[i][k] += s * (nu1 - tau * nu2);
980 }
981 }
982
983 template <int j, int k, int l, typename T>
984 bool
985 jacobiRotation (Matrix33<T>& A, Matrix33<T>& V, Vec3<T>& Z, const T tol)
986 {
987 // Load everything into local variables to make things easier on the
988 // optimizer:
989 const T x = A[j][j];
990 const T y = A[j][k];
991 const T z = A[k][k];
992
993 // The first stage diagonalizes,
994 // [ c s ]^T [ x y ] [ c -s ] = [ d1 0 ]
995 // [ -s c ] [ y z ] [ s c ] [ 0 d2 ]
996 const T mu1 = z - x;
997 const T mu2 = 2 * y;
998
999 if (std::abs (mu2) <= tol * std::abs (mu1))
1000 {
1001 // We've decided that the off-diagonal entries are already small
1002 // enough, so we'll set them to zero. This actually appears to result
1003 // in smaller errors than leaving them be, possibly because it prevents
1004 // us from trying to do extra rotations later that we don't need.
1005 A[j][k] = 0;
1006 return false;
1007 }
1008 const T rho = mu1 / mu2;
1009 const T t = (rho < 0 ? T (-1) : T (1)) / (std::abs (rho) + std::sqrt (1 + rho * rho));
1010 const T c = T (1) / std::sqrt (T (1) + t * t);
1011 const T s = t * c;
1012 const T tau = s / (T (1) + c);
1013 const T h = t * y;
1014
1015 // Update diagonal elements.
1016 Z[j] -= h;
1017 Z[k] += h;
1018 A[j][j] -= h;
1019 A[k][k] += h;
1020
1021 // For the entries we just zeroed out, we'll just set them to 0, since
1022 // they should be 0 up to machine precision.
1023 A[j][k] = 0;
1024
1025 // We only update upper triagnular elements of A, since
1026 // A is supposed to be symmetric.
1027 T& offd1 = l < j ? A[l][j] : A[j][l];
1028 T& offd2 = l < k ? A[l][k] : A[k][l];
1029 const T nu1 = offd1;
1030 const T nu2 = offd2;
1031 offd1 = nu1 - s * (nu2 + tau * nu1);
1032 offd2 = nu2 + s * (nu1 - tau * nu2);
1033
1034 // Apply rotation to V
1035 jacobiRotateRight<j, k> (V, s, tau);
1036
1037 return true;
1038 }
1039
1040 template <int j, int k, int l1, int l2, typename T>
1041 bool
1042 jacobiRotation (Matrix44<T>& A, Matrix44<T>& V, Vec4<T>& Z, const T tol)
1043 {
1044 const T x = A[j][j];
1045 const T y = A[j][k];
1046 const T z = A[k][k];
1047
1048 const T mu1 = z - x;
1049 const T mu2 = T (2) * y;
1050
1051 // Let's see if rho^(-1) = mu2 / mu1 is less than tol
1052 // This test also checks if rho^2 will overflow
1053 // when tol^(-1) < sqrt(std::numeric_limits<T>::max()).
1054 if (std::abs (mu2) <= tol * std::abs (mu1))
1055 {
1056 A[j][k] = 0;
1057 return true;
1058 }
1059
1060 const T rho = mu1 / mu2;
1061 const T t = (rho < 0 ? T (-1) : T (1)) / (std::abs (rho) + std::sqrt (1 + rho * rho));
1062 const T c = T (1) / std::sqrt (T (1) + t * t);
1063 const T s = c * t;
1064 const T tau = s / (T (1) + c);
1065 const T h = t * y;
1066
1067 Z[j] -= h;
1068 Z[k] += h;
1069 A[j][j] -= h;
1070 A[k][k] += h;
1071 A[j][k] = 0;
1072
1073 {
1074 T& offd1 = l1 < j ? A[l1][j] : A[j][l1];
1075 T& offd2 = l1 < k ? A[l1][k] : A[k][l1];
1076 const T nu1 = offd1;
1077 const T nu2 = offd2;
1078 offd1 -= s * (nu2 + tau * nu1);
1079 offd2 += s * (nu1 - tau * nu2);
1080 }
1081
1082 {
1083 T& offd1 = l2 < j ? A[l2][j] : A[j][l2];
1084 T& offd2 = l2 < k ? A[l2][k] : A[k][l2];
1085 const T nu1 = offd1;
1086 const T nu2 = offd2;
1087 offd1 -= s * (nu2 + tau * nu1);
1088 offd2 += s * (nu1 - tau * nu2);
1089 }
1090
1091 jacobiRotateRight<j, k> (V, s, tau);
1092
1093 return true;
1094 }
1095
1096 template <typename TM>
1097 IMATH_CONSTEXPR14 inline typename TM::BaseType
1098 maxOffDiagSymm (const TM& A)
1099 {
1100 typedef typename TM::BaseType T;
1101 T result = 0;
1102 for (unsigned int i = 0; i < TM::dimensions(); ++i)
1103 for (unsigned int j = i + 1; j < TM::dimensions(); ++j)
1104 result = std::max (result, std::abs (A[i][j]));
1105
1106 return result;
1107 }
1108
1109 } // namespace
1110
1111 template <typename T>
1112 void
1113 jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V, const T tol)
1114 {
1115 V.makeIdentity();
1116 for (int i = 0; i < 3; ++i)
1117 {
1118 S[i] = A[i][i];
1119 }
1120
1121 const int maxIter = 20; // In case we get really unlucky, prevents infinite loops
1122 const T absTol = tol * maxOffDiagSymm (A); // Tolerance is in terms of the maximum
1123 if (absTol != 0) // _off-diagonal_ entry.
1124 {
1125 int numIter = 0;
1126 do
1127 {
1128 // Z is for accumulating small changes (h) to diagonal entries
1129 // of A for one sweep. Adding h's directly to A might cause
1130 // a cancellation effect when h is relatively very small to
1131 // the corresponding diagonal entry of A and
1132 // this will increase numerical errors
1133 Vec3<T> Z (0, 0, 0);
1134 ++numIter;
1135 bool changed = jacobiRotation<0, 1, 2> (A, V, Z, tol);
1136 changed = jacobiRotation<0, 2, 1> (A, V, Z, tol) || changed;
1137 changed = jacobiRotation<1, 2, 0> (A, V, Z, tol) || changed;
1138 // One sweep passed. Add accumulated changes (Z) to singular values (S)
1139 // Update diagonal elements of A for better accuracy as well.
1140 for (int i = 0; i < 3; ++i)
1141 {
1142 A[i][i] = S[i] += Z[i];
1143 }
1144 if (!changed)
1145 break;
1146 } while (maxOffDiagSymm (A) > absTol && numIter < maxIter);
1147 }
1148 }
1149
1150 template <typename T>
1151 void
1152 jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V, const T tol)
1153 {
1154 V.makeIdentity();
1155
1156 for (int i = 0; i < 4; ++i)
1157 {
1158 S[i] = A[i][i];
1159 }
1160
1161 const int maxIter = 20; // In case we get really unlucky, prevents infinite loops
1162 const T absTol = tol * maxOffDiagSymm (A); // Tolerance is in terms of the maximum
1163 if (absTol != 0) // _off-diagonal_ entry.
1164 {
1165 int numIter = 0;
1166 do
1167 {
1168 ++numIter;
1169 Vec4<T> Z (0, 0, 0, 0);
1170 bool changed = jacobiRotation<0, 1, 2, 3> (A, V, Z, tol);
1171 changed = jacobiRotation<0, 2, 1, 3> (A, V, Z, tol) || changed;
1172 changed = jacobiRotation<0, 3, 1, 2> (A, V, Z, tol) || changed;
1173 changed = jacobiRotation<1, 2, 0, 3> (A, V, Z, tol) || changed;
1174 changed = jacobiRotation<1, 3, 0, 2> (A, V, Z, tol) || changed;
1175 changed = jacobiRotation<2, 3, 0, 1> (A, V, Z, tol) || changed;
1176 for (int i = 0; i < 4; ++i)
1177 {
1178 A[i][i] = S[i] += Z[i];
1179 }
1180 if (!changed)
1181 break;
1182 } while (maxOffDiagSymm (A) > absTol && numIter < maxIter);
1183 }
1184 }
1185
1186 template <typename TM, typename TV>
1187 void
1188 maxEigenVector (TM& A, TV& V)
1189 {
1190 TV S;
1191 TM MV;
1192 jacobiEigenSolver (A, S, MV);
1193
1194 int maxIdx (0);
1195 for (unsigned int i = 1; i < TV::dimensions(); ++i)
1196 {
1197 if (std::abs (S[i]) > std::abs (S[maxIdx]))
1198 maxIdx = i;
1199 }
1200
1201 for (unsigned int i = 0; i < TV::dimensions(); ++i)
1202 V[i] = MV[i][maxIdx];
1203 }
1204
1205 template <typename TM, typename TV>
1206 void
1207 minEigenVector (TM& A, TV& V)
1208 {
1209 TV S;
1210 TM MV;
1211 jacobiEigenSolver (A, S, MV);
1212
1213 int minIdx (0);
1214 for (unsigned int i = 1; i < TV::dimensions(); ++i)
1215 {
1216 if (std::abs (S[i]) < std::abs (S[minIdx]))
1217 minIdx = i;
1218 }
1219
1220 for (unsigned int i = 0; i < TV::dimensions(); ++i)
1221 V[i] = MV[i][minIdx];
1222 }
1223
1224 template IMATH_EXPORT void
1225 jacobiEigenSolver (Matrix33<float>& A, Vec3<float>& S, Matrix33<float>& V, const float tol);
1226 template IMATH_EXPORT void
1227 jacobiEigenSolver (Matrix33<double>& A, Vec3<double>& S, Matrix33<double>& V, const double tol);
1228 template IMATH_EXPORT void
1229 jacobiEigenSolver (Matrix44<float>& A, Vec4<float>& S, Matrix44<float>& V, const float tol);
1230 template IMATH_EXPORT void
1231 jacobiEigenSolver (Matrix44<double>& A, Vec4<double>& S, Matrix44<double>& V, const double tol);
1232
1233 template IMATH_EXPORT void maxEigenVector (Matrix33<float>& A, Vec3<float>& S);
1234 template IMATH_EXPORT void maxEigenVector (Matrix44<float>& A, Vec4<float>& S);
1235 template IMATH_EXPORT void maxEigenVector (Matrix33<double>& A, Vec3<double>& S);
1236 template IMATH_EXPORT void maxEigenVector (Matrix44<double>& A, Vec4<double>& S);
1237
1238 template IMATH_EXPORT void minEigenVector (Matrix33<float>& A, Vec3<float>& S);
1239 template IMATH_EXPORT void minEigenVector (Matrix44<float>& A, Vec4<float>& S);
1240 template IMATH_EXPORT void minEigenVector (Matrix33<double>& A, Vec3<double>& S);
1241 template IMATH_EXPORT void minEigenVector (Matrix44<double>& A, Vec4<double>& S);
1242
1243
1244 IMATH_INTERNAL_NAMESPACE_SOURCE_EXIT
1245
1246 /// @endcond