Debian Codegraph @

src/Imath/ImathMatrixAlgo.h

raw
1 //
2 // SPDX-License-Identifier: BSD-3-Clause
3 // Copyright Contributors to the OpenEXR Project.
4 //
5
6 //
7 //
8 // Functions operating on Matrix22, Matrix33, and Matrix44 types
9 //
10 // This file also defines a few predefined constant matrices.
11 //
12
13 #ifndef INCLUDED_IMATHMATRIXALGO_H
14 #define INCLUDED_IMATHMATRIXALGO_H
15
16 #include "ImathEuler.h"
17 #include "ImathExport.h"
18 #include "ImathMatrix.h"
19 #include "ImathNamespace.h"
20 #include "ImathQuat.h"
21 #include "ImathVec.h"
22 #include <math.h>
23
24 IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
25
26 //------------------
27 // Identity matrices
28 //------------------
29
30 /// M22f identity matrix
31 IMATH_EXPORT_CONST M22f identity22f;
32 /// M33f identity matrix
33 IMATH_EXPORT_CONST M33f identity33f;
34 /// M44f identity matrix
35 IMATH_EXPORT_CONST M44f identity44f;
36 /// M22d identity matrix
37 IMATH_EXPORT_CONST M22d identity22d;
38 /// M33d identity matrix
39 IMATH_EXPORT_CONST M33d identity33d;
40 /// M44d identity matrix
41 IMATH_EXPORT_CONST M44d identity44d;
42
43 //----------------------------------------------------------------------
44 // Extract scale, shear, rotation, and translation values from a matrix:
45 //
46 // Notes:
47 //
48 // This implementation follows the technique described in the paper by
49 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
50 // Matrix into Simple Transformations", p. 320.
51 //
52 // - Some of the functions below have an optional exc parameter
53 // that determines the functions' behavior when the matrix'
54 // scaling is very close to zero:
55 //
56 // If exc is true, the functions throw a std::domain_error exception.
57 //
58 // If exc is false:
59 //
60 // extractScaling (m, s) returns false, s is invalid
61 // sansScaling (m) returns m
62 // removeScaling (m) returns false, m is unchanged
63 // sansScalingAndShear (m) returns m
64 // removeScalingAndShear (m) returns false, m is unchanged
65 // extractAndRemoveScalingAndShear (m, s, h)
66 // returns false, m is unchanged,
67 // (sh) are invalid
68 // checkForZeroScaleInRow () returns false
69 // extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid
70 //
71 // - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX()
72 // assume that the matrix does not include shear or non-uniform scaling,
73 // but they do not examine the matrix to verify this assumption.
74 // Matrices with shear or non-uniform scaling are likely to produce
75 // meaningless results. Therefore, you should use the
76 // removeScalingAndShear() routine, if necessary, prior to calling
77 // extractEuler...() .
78 //
79 // - All functions assume that the matrix does not include perspective
80 // transformation(s), but they do not examine the matrix to verify
81 // this assumption. Matrices with perspective transformations are
82 // likely to produce meaningless results.
83 //
84 //----------------------------------------------------------------------
85
86 //
87 // Declarations for 4x4 matrix.
88 //
89
90 /// Extract the scaling component of the given 4x4 matrix.
91 ///
92 /// @param[in] mat The input matrix
93 /// @param[out] scl The extracted scale, i.e. the output value
94 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
95 /// @return True if the scale could be extracted, false if the matrix is degenerate.
96 template <class T> bool extractScaling (const Matrix44<T>& mat, Vec3<T>& scl, bool exc = true);
97
98 /// Return the given 4x4 matrix with scaling removed.
99 ///
100 /// @param[in] mat The input matrix
101 /// @param[in] exc If true, throw an exception if the scaling in `mat`
102 template <class T> Matrix44<T> sansScaling (const Matrix44<T>& mat, bool exc = true);
103
104 /// Remove scaling from the given 4x4 matrix in place. Return true if the
105 /// scale could be successfully extracted, false if the matrix is
106 /// degenerate.
107 //
108 /// @param[in] mat The matrix to operate on
109 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
110 /// @return True if the scale could be extracted, false if the matrix is degenerate.
111 template <class T> bool removeScaling (Matrix44<T>& mat, bool exc = true);
112
113 /// Extract the scaling and shear components of the given 4x4 matrix.
114 /// Return true if the scale could be successfully extracted, false if
115 /// the matrix is degenerate.
116 ///
117 /// @param[in] mat The input matrix
118 /// @param[out] scl The extracted scale
119 /// @param[out] shr The extracted shear
120 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
121 /// @return True if the scale could be extracted, false if the matrix is degenerate.
122 template <class T> bool extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc = true);
123
124 /// Return the given 4x4 matrix with scaling and shear removed.
125 ///
126 /// @param[in] mat The input matrix
127 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
128 template <class T> Matrix44<T> sansScalingAndShear (const Matrix44<T>& mat, bool exc = true);
129
130 /// Extract scaling and shear from the given 4x4 matrix in-place.
131 ///
132 /// @param[in,out] result The output matrix
133 /// @param[in] mat The return value if `result` is degenerate
134 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
135 template <class T>
136 void sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& mat, bool exc = true);
137
138 /// Remove scaling and shear from the given 4x4 matrix in place.
139 //
140 /// @param[in,out] mat The matrix to operate on
141 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
142 /// @return True if the scale could be extracted, false if the matrix is degenerate.
143 template <class T> bool removeScalingAndShear (Matrix44<T>& mat, bool exc = true);
144
145 /// Remove scaling and shear from the given 4x4 matrix in place, returning
146 /// the extracted values.
147 //
148 /// @param[in,out] mat The matrix to operate on
149 /// @param[out] scl The extracted scale
150 /// @param[out] shr The extracted shear
151 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
152 /// @return True if the scale could be extracted, false if the matrix is degenerate.
153 template <class T>
154 bool
155 extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc = true);
156
157 /// Extract the rotation from the given 4x4 matrix in the form of XYZ
158 /// euler angles.
159 ///
160 /// @param[in] mat The input matrix
161 /// @param[out] rot The extracted XYZ euler angle vector
162 template <class T> void extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& rot);
163
164 /// Extract the rotation from the given 4x4 matrix in the form of ZYX
165 /// euler angles.
166 ///
167 /// @param[in] mat The input matrix
168 /// @param[out] rot The extracted ZYX euler angle vector
169 template <class T> void extractEulerZYX (const Matrix44<T>& mat, Vec3<T>& rot);
170
171 /// Extract the rotation from the given 4x4 matrix in the form of a quaternion.
172 ///
173 /// @param[in] mat The input matrix
174 /// @return The extracted quaternion
175 template <class T> Quat<T> extractQuat (const Matrix44<T>& mat);
176
177 /// Extract the scaling, shear, rotation, and translation components
178 /// of the given 4x4 matrix. The values are such that:
179 ///
180 /// M = S * H * R * T
181 ///
182 /// @param[in] mat The input matrix
183 /// @param[out] s The extracted scale
184 /// @param[out] h The extracted shear
185 /// @param[out] r The extracted rotation
186 /// @param[out] t The extracted translation
187 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
188 /// @param[in] rOrder The order with which to extract the rotation
189 /// @return True if the values could be extracted, false if the matrix is degenerate.
190 template <class T>
191 bool extractSHRT (const Matrix44<T>& mat,
192 Vec3<T>& s,
193 Vec3<T>& h,
194 Vec3<T>& r,
195 Vec3<T>& t,
196 bool exc /*= true*/,
197 typename Euler<T>::Order rOrder);
198
199 /// Extract the scaling, shear, rotation, and translation components
200 /// of the given 4x4 matrix.
201 ///
202 /// @param[in] mat The input matrix
203 /// @param[out] s The extracted scale
204 /// @param[out] h The extracted shear
205 /// @param[out] r The extracted rotation, in XYZ euler angles
206 /// @param[out] t The extracted translation
207 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
208 /// @return True if the values could be extracted, false if the matrix is degenerate.
209 template <class T>
210 bool extractSHRT (const Matrix44<T>& mat,
211 Vec3<T>& s,
212 Vec3<T>& h,
213 Vec3<T>& r,
214 Vec3<T>& t,
215 bool exc = true);
216
217 /// Extract the scaling, shear, rotation, and translation components
218 /// of the given 4x4 matrix.
219 ///
220 /// @param[in] mat The input matrix
221 /// @param[out] s The extracted scale
222 /// @param[out] h The extracted shear
223 /// @param[out] r The extracted rotation, in Euler angles
224 /// @param[out] t The extracted translation
225 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
226 /// @return True if the values could be extracted, false if the matrix is degenerate.
227 template <class T>
228 bool extractSHRT (const Matrix44<T>& mat,
229 Vec3<T>& s,
230 Vec3<T>& h,
231 Euler<T>& r,
232 Vec3<T>& t,
233 bool exc = true);
234
235 /// Return true if the given scale can be removed from the given row
236 /// matrix, false if `scl` is small enough that the operation would
237 /// overflow. If `exc` is true, throw an exception on overflow.
238 template <class T> bool checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc = true);
239
240 /// Return the 4x4 outer product two 4-vectors
241 template <class T> Matrix44<T> outerProduct (const Vec4<T>& a, const Vec4<T>& b);
242
243 ///
244 /// Return a 4x4 matrix that rotates the vector `fromDirection` to `toDirection`
245 ///
246 template <class T>
247 Matrix44<T> rotationMatrix (const Vec3<T>& fromDirection, const Vec3<T>& toDirection);
248
249 ///
250 /// Return a 4x4 matrix that rotates the `fromDir` vector
251 /// so that it points towards `toDir1. You may also
252 /// specify that you want the up vector to be pointing
253 /// in a certain direction 1upDir`.
254 template <class T>
255 Matrix44<T>
256 rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir);
257
258 ///
259 /// Construct a 4x4 matrix that rotates the z-axis so that it points
260 /// towards `targetDir`. You must also specify that you want the up
261 /// vector to be pointing in a certain direction `upDir`.
262 ///
263 /// Notes: The following degenerate cases are handled:
264 /// (a) when the directions given by `toDir` and `upDir`
265 /// are parallel or opposite (the direction vectors must have a non-zero cross product);
266 /// (b) when any of the given direction vectors have zero length
267 ///
268 /// @param[out] result The output matrix
269 /// @param[in] targetDir The target direction vector
270 /// @param[in] upDir The up direction vector
271 template <class T>
272 void alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> upDir);
273
274 /// Compute an orthonormal direct 4x4 frame from a position, an x axis
275 /// direction and a normal to the y axis. If the x axis and normal are
276 /// perpendicular, then the normal will have the same direction as the
277 /// z axis.
278 ///
279 /// @param[in] p The position of the frame
280 /// @param[in] xDir The x axis direction of the frame
281 /// @param[in] normal A normal to the y axis of the frame
282 /// @return The orthonormal frame
283 template <class T>
284 Matrix44<T> computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal);
285
286 /// Add a translate/rotate/scale offset to a 4x4 input frame
287 /// and put it in another frame of reference
288 ///
289 /// @param[in] inMat Input frame
290 /// @param[in] tOffset Translation offset
291 /// @param[in] rOffset Rotation offset in degrees
292 /// @param[in] sOffset Scale offset
293 /// @param[in] ref Frame of reference
294 /// @return The offsetted frame
295 template <class T>
296 Matrix44<T> addOffset (const Matrix44<T>& inMat,
297 const Vec3<T>& tOffset,
298 const Vec3<T>& rOffset,
299 const Vec3<T>& sOffset,
300 const Vec3<T>& ref);
301
302 /// Compute 4x4 translate/rotate/scale matrix from `A` with the
303 /// rotate/scale of `B`.
304 ///
305 /// @param[in] keepRotateA If true, keep rotate from matrix `A`, use `B` otherwise
306 /// @param[in] keepScaleA If true, keep scale from matrix `A`, use `B` otherwise
307 /// @param[in] A Matrix A
308 /// @param[in] B Matrix B
309 /// @return Matrix `A` with tweaked rotation/scale
310 template <class T>
311 Matrix44<T>
312 computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& B);
313
314 //
315 // Declarations for 3x3 matrix.
316 //
317
318 /// Extract the scaling component of the given 3x3 matrix.
319 ///
320 /// @param[in] mat The input matrix
321 /// @param[out] scl The extracted scale, i.e. the output value
322 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
323 /// @return True if the scale could be extracted, false if the matrix is degenerate.
324 template <class T> bool extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc = true);
325
326 /// Return the given 3x3 matrix with scaling removed.
327 ///
328 /// @param[in] mat The input matrix
329 /// @param[in] exc If true, throw an exception if the scaling in `mat`
330 template <class T> Matrix33<T> sansScaling (const Matrix33<T>& mat, bool exc = true);
331
332 /// Remove scaling from the given 3x3 matrix in place. Return true if the
333 /// scale could be successfully extracted, false if the matrix is
334 /// degenerate.
335 //
336 /// @param[in] mat The matrix to operate on
337 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
338 /// @return True if the scale could be extracted, false if the matrix is degenerate.
339 template <class T> bool removeScaling (Matrix33<T>& mat, bool exc = true);
340
341 /// Extract the scaling and shear components of the given 3x3 matrix.
342 /// Return true if the scale could be successfully extracted, false if
343 /// the matrix is degenerate.
344 ///
345 /// @param[in] mat The input matrix
346 /// @param[out] scl The extracted scale
347 /// @param[out] shr The extracted shear
348 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
349 /// @return True if the scale could be extracted, false if the matrix is degenerate.
350 template <class T>
351 bool extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc = true);
352
353 /// Return the given 3x3 matrix with scaling and shear removed.
354 ///
355 /// @param[in] mat The input matrix
356 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
357 template <class T> Matrix33<T> sansScalingAndShear (const Matrix33<T>& mat, bool exc = true);
358
359
360 /// Remove scaling and shear from the given 3x3e matrix in place.
361 //
362 /// @param[in,out] mat The matrix to operate on
363 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
364 /// @return True if the scale could be extracted, false if the matrix is degenerate.
365 template <class T> bool removeScalingAndShear (Matrix33<T>& mat, bool exc = true);
366
367 /// Remove scaling and shear from the given 3x3 matrix in place, returning
368 /// the extracted values.
369 //
370 /// @param[in,out] mat The matrix to operate on
371 /// @param[out] scl The extracted scale
372 /// @param[out] shr The extracted shear
373 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
374 /// @return True if the scale could be extracted, false if the matrix is degenerate.
375 template <class T>
376 bool extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc = true);
377
378 /// Extract the rotation from the given 2x2 matrix
379 ///
380 /// @param[in] mat The input matrix
381 /// @param[out] rot The extracted rotation value
382 template <class T> void extractEuler (const Matrix22<T>& mat, T& rot);
383
384 /// Extract the rotation from the given 3x3 matrix
385 ///
386 /// @param[in] mat The input matrix
387 /// @param[out] rot The extracted rotation value
388 template <class T> void extractEuler (const Matrix33<T>& mat, T& rot);
389
390 /// Extract the scaling, shear, rotation, and translation components
391 /// of the given 3x3 matrix. The values are such that:
392 ///
393 /// M = S * H * R * T
394 ///
395 /// @param[in] mat The input matrix
396 /// @param[out] s The extracted scale
397 /// @param[out] h The extracted shear
398 /// @param[out] r The extracted rotation
399 /// @param[out] t The extracted translation
400 /// @param[in] exc If true, throw an exception if the scaling in `mat` is very close to zero.
401 /// @return True if the values could be extracted, false if the matrix is degenerate.
402 template <class T>
403 bool extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& t, bool exc = true);
404
405 /// Return true if the given scale can be removed from the given row
406 /// matrix, false if `scl` is small enough that the operation would
407 /// overflow. If `exc` is true, throw an exception on overflow.
408 template <class T> bool checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc = true);
409
410 /// Return the 3xe outer product two 3-vectors
411 template <class T> Matrix33<T> outerProduct (const Vec3<T>& a, const Vec3<T>& b);
412
413 //------------------------------
414 // Implementation for 4x4 Matrix
415 //------------------------------
416
417 template <class T>
418 bool
419 extractScaling (const Matrix44<T>& mat, Vec3<T>& scl, bool exc)
420 {
421 Vec3<T> shr;
422 Matrix44<T> M (mat);
423
424 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
425 return false;
426
427 return true;
428 }
429
430 template <class T>
431 Matrix44<T>
432 sansScaling (const Matrix44<T>& mat, bool exc)
433 {
434 Vec3<T> scl;
435 Vec3<T> shr;
436 Vec3<T> rot;
437 Vec3<T> tran;
438
439 if (!extractSHRT (mat, scl, shr, rot, tran, exc))
440 return mat;
441
442 Matrix44<T> M;
443
444 M.translate (tran);
445 M.rotate (rot);
446 M.shear (shr);
447
448 return M;
449 }
450
451 template <class T>
452 bool
453 removeScaling (Matrix44<T>& mat, bool exc)
454 {
455 Vec3<T> scl;
456 Vec3<T> shr;
457 Vec3<T> rot;
458 Vec3<T> tran;
459
460 if (!extractSHRT (mat, scl, shr, rot, tran, exc))
461 return false;
462
463 mat.makeIdentity();
464 mat.translate (tran);
465 mat.rotate (rot);
466 mat.shear (shr);
467
468 return true;
469 }
470
471 template <class T>
472 bool
473 extractScalingAndShear (const Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc)
474 {
475 Matrix44<T> M (mat);
476
477 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
478 return false;
479
480 return true;
481 }
482
483 template <class T>
484 Matrix44<T>
485 sansScalingAndShear (const Matrix44<T>& mat, bool exc)
486 {
487 Vec3<T> scl;
488 Vec3<T> shr;
489 Matrix44<T> M (mat);
490
491 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
492 return mat;
493
494 return M;
495 }
496
497 template <class T>
498 void
499 sansScalingAndShear (Matrix44<T>& result, const Matrix44<T>& mat, bool exc)
500 {
501 Vec3<T> scl;
502 Vec3<T> shr;
503
504 if (!extractAndRemoveScalingAndShear (result, scl, shr, exc))
505 result = mat;
506 }
507
508 template <class T>
509 bool
510 removeScalingAndShear (Matrix44<T>& mat, bool exc)
511 {
512 Vec3<T> scl;
513 Vec3<T> shr;
514
515 if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc))
516 return false;
517
518 return true;
519 }
520
521 template <class T>
522 bool
523 extractAndRemoveScalingAndShear (Matrix44<T>& mat, Vec3<T>& scl, Vec3<T>& shr, bool exc)
524 {
525 //
526 // This implementation follows the technique described in the paper by
527 // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
528 // Matrix into Simple Transformations", p. 320.
529 //
530
531 Vec3<T> row[3];
532
533 row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
534 row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
535 row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
536
537 T maxVal = 0;
538 for (int i = 0; i < 3; i++)
539 for (int j = 0; j < 3; j++)
540 if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
541 maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
542
543 //
544 // We normalize the 3x3 matrix here.
545 // It was noticed that this can improve numerical stability significantly,
546 // especially when many of the upper 3x3 matrix's coefficients are very
547 // close to zero; we correct for this step at the end by multiplying the
548 // scaling factors by maxVal at the end (shear and rotation are not
549 // affected by the normalization).
550
551 if (maxVal != 0)
552 {
553 for (int i = 0; i < 3; i++)
554 if (!checkForZeroScaleInRow (maxVal, row[i], exc))
555 return false;
556 else
557 row[i] /= maxVal;
558 }
559
560 // Compute X scale factor.
561 scl.x = row[0].length();
562 if (!checkForZeroScaleInRow (scl.x, row[0], exc))
563 return false;
564
565 // Normalize first row.
566 row[0] /= scl.x;
567
568 // An XY shear factor will shear the X coord. as the Y coord. changes.
569 // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
570 // extract the first 3 because we can effect the last 3 by shearing in
571 // XY, XZ, YZ combined rotations and scales.
572 //
573 // shear matrix < 1, YX, ZX, 0,
574 // XY, 1, ZY, 0,
575 // XZ, YZ, 1, 0,
576 // 0, 0, 0, 1 >
577
578 // Compute XY shear factor and make 2nd row orthogonal to 1st.
579 shr[0] = row[0].dot (row[1]);
580 row[1] -= shr[0] * row[0];
581
582 // Now, compute Y scale.
583 scl.y = row[1].length();
584 if (!checkForZeroScaleInRow (scl.y, row[1], exc))
585 return false;
586
587 // Normalize 2nd row and correct the XY shear factor for Y scaling.
588 row[1] /= scl.y;
589 shr[0] /= scl.y;
590
591 // Compute XZ and YZ shears, orthogonalize 3rd row.
592 shr[1] = row[0].dot (row[2]);
593 row[2] -= shr[1] * row[0];
594 shr[2] = row[1].dot (row[2]);
595 row[2] -= shr[2] * row[1];
596
597 // Next, get Z scale.
598 scl.z = row[2].length();
599 if (!checkForZeroScaleInRow (scl.z, row[2], exc))
600 return false;
601
602 // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
603 row[2] /= scl.z;
604 shr[1] /= scl.z;
605 shr[2] /= scl.z;
606
607 // At this point, the upper 3x3 matrix in mat is orthonormal.
608 // Check for a coordinate system flip. If the determinant
609 // is less than zero, then negate the matrix and the scaling factors.
610 if (row[0].dot (row[1].cross (row[2])) < 0)
611 for (int i = 0; i < 3; i++)
612 {
613 scl[i] *= -1;
614 row[i] *= -1;
615 }
616
617 // Copy over the orthonormal rows into the returned matrix.
618 // The upper 3x3 matrix in mat is now a rotation matrix.
619 for (int i = 0; i < 3; i++)
620 {
621 mat[i][0] = row[i][0];
622 mat[i][1] = row[i][1];
623 mat[i][2] = row[i][2];
624 }
625
626 // Correct the scaling factors for the normalization step that we
627 // performed above; shear and rotation are not affected by the
628 // normalization.
629 scl *= maxVal;
630
631 return true;
632 }
633
634 template <class T>
635 void
636 extractEulerXYZ (const Matrix44<T>& mat, Vec3<T>& rot)
637 {
638 //
639 // Normalize the local x, y and z axes to remove scaling.
640 //
641
642 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
643 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
644 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
645
646 i.normalize();
647 j.normalize();
648 k.normalize();
649
650 Matrix44<T> M (i[0], i[1], i[2], 0, j[0], j[1], j[2], 0, k[0], k[1], k[2], 0, 0, 0, 0, 1);
651
652 //
653 // Extract the first angle, rot.x.
654 //
655
656 rot.x = std::atan2 (M[1][2], M[2][2]);
657
658 //
659 // Remove the rot.x rotation from M, so that the remaining
660 // rotation, N, is only around two axes, and gimbal lock
661 // cannot occur.
662 //
663
664 Matrix44<T> N;
665 N.rotate (Vec3<T> (-rot.x, 0, 0));
666 N = N * M;
667
668 //
669 // Extract the other two angles, rot.y and rot.z, from N.
670 //
671
672 T cy = std::sqrt (N[0][0] * N[0][0] + N[0][1] * N[0][1]);
673 rot.y = std::atan2 (-N[0][2], cy);
674 rot.z = std::atan2 (-N[1][0], N[1][1]);
675 }
676
677 template <class T>
678 void
679 extractEulerZYX (const Matrix44<T>& mat, Vec3<T>& rot)
680 {
681 //
682 // Normalize the local x, y and z axes to remove scaling.
683 //
684
685 Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
686 Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
687 Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
688
689 i.normalize();
690 j.normalize();
691 k.normalize();
692
693 Matrix44<T> M (i[0], i[1], i[2], 0, j[0], j[1], j[2], 0, k[0], k[1], k[2], 0, 0, 0, 0, 1);
694
695 //
696 // Extract the first angle, rot.x.
697 //
698
699 rot.x = -std::atan2 (M[1][0], M[0][0]);
700
701 //
702 // Remove the x rotation from M, so that the remaining
703 // rotation, N, is only around two axes, and gimbal lock
704 // cannot occur.
705 //
706
707 Matrix44<T> N;
708 N.rotate (Vec3<T> (0, 0, -rot.x));
709 N = N * M;
710
711 //
712 // Extract the other two angles, rot.y and rot.z, from N.
713 //
714
715 T cy = std::sqrt (N[2][2] * N[2][2] + N[2][1] * N[2][1]);
716 rot.y = -std::atan2 (-N[2][0], cy);
717 rot.z = -std::atan2 (-N[1][2], N[1][1]);
718 }
719
720 template <class T>
721 Quat<T>
722 extractQuat (const Matrix44<T>& mat)
723 {
724 T tr, s;
725 T q[4];
726 int i, j, k;
727
728 Quat<T> quat;
729
730 int nxt[3] = { 1, 2, 0 };
731 tr = mat[0][0] + mat[1][1] + mat[2][2];
732
733 // check the diagonal
734 if (tr > 0.0)
735 {
736 s = std::sqrt (tr + T (1.0));
737 quat.r = s / T (2.0);
738 s = T (0.5) / s;
739
740 quat.v.x = (mat[1][2] - mat[2][1]) * s;
741 quat.v.y = (mat[2][0] - mat[0][2]) * s;
742 quat.v.z = (mat[0][1] - mat[1][0]) * s;
743 }
744 else
745 {
746 // diagonal is negative
747 i = 0;
748 if (mat[1][1] > mat[0][0])
749 i = 1;
750 if (mat[2][2] > mat[i][i])
751 i = 2;
752
753 j = nxt[i];
754 k = nxt[j];
755 s = std::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + T (1.0));
756
757 q[i] = s * T (0.5);
758 if (s != T (0.0))
759 s = T (0.5) / s;
760
761 q[3] = (mat[j][k] - mat[k][j]) * s;
762 q[j] = (mat[i][j] + mat[j][i]) * s;
763 q[k] = (mat[i][k] + mat[k][i]) * s;
764
765 quat.v.x = q[0];
766 quat.v.y = q[1];
767 quat.v.z = q[2];
768 quat.r = q[3];
769 }
770
771 return quat;
772 }
773
774 template <class T>
775 bool
776 extractSHRT (const Matrix44<T>& mat,
777 Vec3<T>& s,
778 Vec3<T>& h,
779 Vec3<T>& r,
780 Vec3<T>& t,
781 bool exc /* = true */,
782 typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */)
783 {
784 Matrix44<T> rot;
785
786 rot = mat;
787 if (!extractAndRemoveScalingAndShear (rot, s, h, exc))
788 return false;
789
790 extractEulerXYZ (rot, r);
791
792 t.x = mat[3][0];
793 t.y = mat[3][1];
794 t.z = mat[3][2];
795
796 if (rOrder != Euler<T>::XYZ)
797 {
798 Euler<T> eXYZ (r, Euler<T>::XYZ);
799 Euler<T> e (eXYZ, rOrder);
800 r = e.toXYZVector();
801 }
802
803 return true;
804 }
805
806 template <class T>
807 bool
808 extractSHRT (const Matrix44<T>& mat, Vec3<T>& s, Vec3<T>& h, Vec3<T>& r, Vec3<T>& t, bool exc)
809 {
810 return extractSHRT (mat, s, h, r, t, exc, Euler<T>::XYZ);
811 }
812
813 template <class T>
814 bool
815 extractSHRT (const Matrix44<T>& mat,
816 Vec3<T>& s,
817 Vec3<T>& h,
818 Euler<T>& r,
819 Vec3<T>& t,
820 bool exc /* = true */)
821 {
822 return extractSHRT (mat, s, h, r, t, exc, r.order());
823 }
824
825 template <class T>
826 bool
827 checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc /* = true */)
828 {
829 for (int i = 0; i < 3; i++)
830 {
831 if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl)))
832 {
833 if (exc)
834 throw std::domain_error ("Cannot remove zero scaling "
835 "from matrix.");
836 else
837 return false;
838 }
839 }
840
841 return true;
842 }
843
844 template <class T>
845 Matrix44<T>
846 outerProduct (const Vec4<T>& a, const Vec4<T>& b)
847 {
848 return Matrix44<T> (a.x * b.x,
849 a.x * b.y,
850 a.x * b.z,
851 a.x * b.w,
852 a.y * b.x,
853 a.y * b.y,
854 a.y * b.z,
855 a.x * b.w,
856 a.z * b.x,
857 a.z * b.y,
858 a.z * b.z,
859 a.x * b.w,
860 a.w * b.x,
861 a.w * b.y,
862 a.w * b.z,
863 a.w * b.w);
864 }
865
866 template <class T>
867 Matrix44<T>
868 rotationMatrix (const Vec3<T>& from, const Vec3<T>& to)
869 {
870 Quat<T> q;
871 q.setRotation (from, to);
872 return q.toMatrix44();
873 }
874
875 template <class T>
876 Matrix44<T>
877 rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir)
878 {
879 //
880 // The goal is to obtain a rotation matrix that takes
881 // "fromDir" to "toDir". We do this in two steps and
882 // compose the resulting rotation matrices;
883 // (a) rotate "fromDir" into the z-axis
884 // (b) rotate the z-axis into "toDir"
885 //
886
887 // The from direction must be non-zero; but we allow zero to and up dirs.
888 if (fromDir.length() == 0)
889 return Matrix44<T>();
890
891 else
892 {
893 Matrix44<T> zAxis2FromDir (UNINITIALIZED);
894 alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0));
895
896 Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed();
897
898 Matrix44<T> zAxis2ToDir (UNINITIALIZED);
899 alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir);
900
901 return fromDir2zAxis * zAxis2ToDir;
902 }
903 }
904
905 template <class T>
906 void
907 alignZAxisWithTargetDir (Matrix44<T>& result, Vec3<T> targetDir, Vec3<T> upDir)
908 {
909 //
910 // Ensure that the target direction is non-zero.
911 //
912
913 if (targetDir.length() == 0)
914 targetDir = Vec3<T> (0, 0, 1);
915
916 //
917 // Ensure that the up direction is non-zero.
918 //
919
920 if (upDir.length() == 0)
921 upDir = Vec3<T> (0, 1, 0);
922
923 //
924 // Check for degeneracies. If the upDir and targetDir are parallel
925 // or opposite, then compute a new, arbitrary up direction that is
926 // not parallel or opposite to the targetDir.
927 //
928
929 if (upDir.cross (targetDir).length() == 0)
930 {
931 upDir = targetDir.cross (Vec3<T> (1, 0, 0));
932 if (upDir.length() == 0)
933 upDir = targetDir.cross (Vec3<T> (0, 0, 1));
934 }
935
936 //
937 // Compute the x-, y-, and z-axis vectors of the new coordinate system.
938 //
939
940 Vec3<T> targetPerpDir = upDir.cross (targetDir);
941 Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
942
943 //
944 // Rotate the x-axis into targetPerpDir (row 0),
945 // rotate the y-axis into targetUpDir (row 1),
946 // rotate the z-axis into targetDir (row 2).
947 //
948
949 Vec3<T> row[3];
950 row[0] = targetPerpDir.normalized();
951 row[1] = targetUpDir.normalized();
952 row[2] = targetDir.normalized();
953
954 result.x[0][0] = row[0][0];
955 result.x[0][1] = row[0][1];
956 result.x[0][2] = row[0][2];
957 result.x[0][3] = (T) 0;
958
959 result.x[1][0] = row[1][0];
960 result.x[1][1] = row[1][1];
961 result.x[1][2] = row[1][2];
962 result.x[1][3] = (T) 0;
963
964 result.x[2][0] = row[2][0];
965 result.x[2][1] = row[2][1];
966 result.x[2][2] = row[2][2];
967 result.x[2][3] = (T) 0;
968
969 result.x[3][0] = (T) 0;
970 result.x[3][1] = (T) 0;
971 result.x[3][2] = (T) 0;
972 result.x[3][3] = (T) 1;
973 }
974
975 // Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis
976 // If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis.
977 // Inputs are :
978 // -the position of the frame
979 // -the x axis direction of the frame
980 // -a normal to the y axis of the frame
981 // Return is the orthonormal frame
982 template <class T>
983 Matrix44<T>
984 computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal)
985 {
986 Vec3<T> _xDir (xDir);
987 Vec3<T> x = _xDir.normalize();
988 Vec3<T> y = (normal % x).normalize();
989 Vec3<T> z = (x % y).normalize();
990
991 Matrix44<T> L;
992 L[0][0] = x[0];
993 L[0][1] = x[1];
994 L[0][2] = x[2];
995 L[0][3] = 0.0;
996
997 L[1][0] = y[0];
998 L[1][1] = y[1];
999 L[1][2] = y[2];
1000 L[1][3] = 0.0;
1001
1002 L[2][0] = z[0];
1003 L[2][1] = z[1];
1004 L[2][2] = z[2];
1005 L[2][3] = 0.0;
1006
1007 L[3][0] = p[0];
1008 L[3][1] = p[1];
1009 L[3][2] = p[2];
1010 L[3][3] = 1.0;
1011
1012 return L;
1013 }
1014
1015 /// Add a translate/rotate/scale offset to an input frame and put it
1016 /// in another frame of reference.
1017 ///
1018 /// @param inMat input frame
1019 /// @param tOffset translate offset
1020 /// @param rOffset rotate offset in degrees
1021 /// @param sOffset scale offset
1022 /// @param ref Frame of reference
1023 /// @return The offsetted frame
1024 template <class T>
1025 Matrix44<T>
1026 addOffset (const Matrix44<T>& inMat,
1027 const Vec3<T>& tOffset,
1028 const Vec3<T>& rOffset,
1029 const Vec3<T>& sOffset,
1030 const Matrix44<T>& ref)
1031 {
1032 Matrix44<T> O;
1033
1034 Vec3<T> _rOffset (rOffset);
1035 _rOffset *= T(M_PI / 180.0);
1036 O.rotate (_rOffset);
1037
1038 O[3][0] = tOffset[0];
1039 O[3][1] = tOffset[1];
1040 O[3][2] = tOffset[2];
1041
1042 Matrix44<T> S;
1043 S.scale (sOffset);
1044
1045 Matrix44<T> X = S * O * inMat * ref;
1046
1047 return X;
1048 }
1049
1050 // Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B
1051 // Inputs are :
1052 // -keepRotateA : if true keep rotate from matrix A, use B otherwise
1053 // -keepScaleA : if true keep scale from matrix A, use B otherwise
1054 // -Matrix A
1055 // -Matrix B
1056 // Return Matrix A with tweaked rotation/scale
1057 template <class T>
1058 Matrix44<T>
1059 computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& B)
1060 {
1061 Vec3<T> as, ah, ar, at;
1062 if (!extractSHRT (A, as, ah, ar, at))
1063 throw std::domain_error ("degenerate A matrix in computeRSMatrix");
1064
1065 Vec3<T> bs, bh, br, bt;
1066 if (!extractSHRT (B, bs, bh, br, bt))
1067 throw std::domain_error ("degenerate B matrix in computeRSMatrix");
1068
1069 if (!keepRotateA)
1070 ar = br;
1071
1072 if (!keepScaleA)
1073 as = bs;
1074
1075 Matrix44<T> mat;
1076 mat.makeIdentity();
1077 mat.translate (at);
1078 mat.rotate (ar);
1079 mat.scale (as);
1080
1081 return mat;
1082 }
1083
1084 //-----------------------------------------------------------------------------
1085 // Implementation for 3x3 Matrix
1086 //------------------------------
1087
1088 template <class T>
1089 bool
1090 extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc)
1091 {
1092 T shr;
1093 Matrix33<T> M (mat);
1094
1095 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1096 return false;
1097
1098 return true;
1099 }
1100
1101 template <class T>
1102 Matrix33<T>
1103 sansScaling (const Matrix33<T>& mat, bool exc)
1104 {
1105 Vec2<T> scl;
1106 T shr;
1107 T rot;
1108 Vec2<T> tran;
1109
1110 if (!extractSHRT (mat, scl, shr, rot, tran, exc))
1111 return mat;
1112
1113 Matrix33<T> M;
1114
1115 M.translate (tran);
1116 M.rotate (rot);
1117 M.shear (shr);
1118
1119 return M;
1120 }
1121
1122 template <class T>
1123 bool
1124 removeScaling (Matrix33<T>& mat, bool exc)
1125 {
1126 Vec2<T> scl;
1127 T shr;
1128 T rot;
1129 Vec2<T> tran;
1130
1131 if (!extractSHRT (mat, scl, shr, rot, tran, exc))
1132 return false;
1133
1134 mat.makeIdentity();
1135 mat.translate (tran);
1136 mat.rotate (rot);
1137 mat.shear (shr);
1138
1139 return true;
1140 }
1141
1142 template <class T>
1143 bool
1144 extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc)
1145 {
1146 Matrix33<T> M (mat);
1147
1148 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1149 return false;
1150
1151 return true;
1152 }
1153
1154 template <class T>
1155 Matrix33<T>
1156 sansScalingAndShear (const Matrix33<T>& mat, bool exc)
1157 {
1158 Vec2<T> scl;
1159 T shr;
1160 Matrix33<T> M (mat);
1161
1162 if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1163 return mat;
1164
1165 return M;
1166 }
1167
1168 template <class T>
1169 bool
1170 removeScalingAndShear (Matrix33<T>& mat, bool exc)
1171 {
1172 Vec2<T> scl;
1173 T shr;
1174
1175 if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc))
1176 return false;
1177
1178 return true;
1179 }
1180
1181 template <class T>
1182 bool
1183 extractAndRemoveScalingAndShear (Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc)
1184 {
1185 Vec2<T> row[2];
1186
1187 row[0] = Vec2<T> (mat[0][0], mat[0][1]);
1188 row[1] = Vec2<T> (mat[1][0], mat[1][1]);
1189
1190 T maxVal = 0;
1191 for (int i = 0; i < 2; i++)
1192 for (int j = 0; j < 2; j++)
1193 if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
1194 maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
1195
1196 //
1197 // We normalize the 2x2 matrix here.
1198 // It was noticed that this can improve numerical stability significantly,
1199 // especially when many of the upper 2x2 matrix's coefficients are very
1200 // close to zero; we correct for this step at the end by multiplying the
1201 // scaling factors by maxVal at the end (shear and rotation are not
1202 // affected by the normalization).
1203
1204 if (maxVal != 0)
1205 {
1206 for (int i = 0; i < 2; i++)
1207 if (!checkForZeroScaleInRow (maxVal, row[i], exc))
1208 return false;
1209 else
1210 row[i] /= maxVal;
1211 }
1212
1213 // Compute X scale factor.
1214 scl.x = row[0].length();
1215 if (!checkForZeroScaleInRow (scl.x, row[0], exc))
1216 return false;
1217
1218 // Normalize first row.
1219 row[0] /= scl.x;
1220
1221 // An XY shear factor will shear the X coord. as the Y coord. changes.
1222 // There are 2 combinations (XY, YX), although we only extract the XY
1223 // shear factor because we can effect the an YX shear factor by
1224 // shearing in XY combined with rotations and scales.
1225 //
1226 // shear matrix < 1, YX, 0,
1227 // XY, 1, 0,
1228 // 0, 0, 1 >
1229
1230 // Compute XY shear factor and make 2nd row orthogonal to 1st.
1231 shr = row[0].dot (row[1]);
1232 row[1] -= shr * row[0];
1233
1234 // Now, compute Y scale.
1235 scl.y = row[1].length();
1236 if (!checkForZeroScaleInRow (scl.y, row[1], exc))
1237 return false;
1238
1239 // Normalize 2nd row and correct the XY shear factor for Y scaling.
1240 row[1] /= scl.y;
1241 shr /= scl.y;
1242
1243 // At this point, the upper 2x2 matrix in mat is orthonormal.
1244 // Check for a coordinate system flip. If the determinant
1245 // is -1, then flip the rotation matrix and adjust the scale(Y)
1246 // and shear(XY) factors to compensate.
1247 if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
1248 {
1249 row[1][0] *= -1;
1250 row[1][1] *= -1;
1251 scl[1] *= -1;
1252 shr *= -1;
1253 }
1254
1255 // Copy over the orthonormal rows into the returned matrix.
1256 // The upper 2x2 matrix in mat is now a rotation matrix.
1257 for (int i = 0; i < 2; i++)
1258 {
1259 mat[i][0] = row[i][0];
1260 mat[i][1] = row[i][1];
1261 }
1262
1263 scl *= maxVal;
1264
1265 return true;
1266 }
1267
1268 template <class T>
1269 void
1270 extractEuler (const Matrix22<T>& mat, T& rot)
1271 {
1272 //
1273 // Normalize the local x and y axes to remove scaling.
1274 //
1275
1276 Vec2<T> i (mat[0][0], mat[0][1]);
1277 Vec2<T> j (mat[1][0], mat[1][1]);
1278
1279 i.normalize();
1280 j.normalize();
1281
1282 //
1283 // Extract the angle, rot.
1284 //
1285
1286 rot = -std::atan2 (j[0], i[0]);
1287 }
1288
1289 template <class T>
1290 void
1291 extractEuler (const Matrix33<T>& mat, T& rot)
1292 {
1293 //
1294 // Normalize the local x and y axes to remove scaling.
1295 //
1296
1297 Vec2<T> i (mat[0][0], mat[0][1]);
1298 Vec2<T> j (mat[1][0], mat[1][1]);
1299
1300 i.normalize();
1301 j.normalize();
1302
1303 //
1304 // Extract the angle, rot.
1305 //
1306
1307 rot = -std::atan2 (j[0], i[0]);
1308 }
1309
1310 template <class T>
1311 bool
1312 extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& t, bool exc)
1313 {
1314 Matrix33<T> rot;
1315
1316 rot = mat;
1317 if (!extractAndRemoveScalingAndShear (rot, s, h, exc))
1318 return false;
1319
1320 extractEuler (rot, r);
1321
1322 t.x = mat[2][0];
1323 t.y = mat[2][1];
1324
1325 return true;
1326 }
1327
1328 /// @cond Doxygen_Suppress
1329 template <class T>
1330 bool
1331 checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc /* = true */)
1332 {
1333 for (int i = 0; i < 2; i++)
1334 {
1335 if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl)))
1336 {
1337 if (exc)
1338 throw std::domain_error ("Cannot remove zero scaling from matrix.");
1339 else
1340 return false;
1341 }
1342 }
1343
1344 return true;
1345 }
1346 /// @endcond
1347
1348 template <class T>
1349 Matrix33<T>
1350 outerProduct (const Vec3<T>& a, const Vec3<T>& b)
1351 {
1352 return Matrix33<T> (a.x * b.x,
1353 a.x * b.y,
1354 a.x * b.z,
1355 a.y * b.x,
1356 a.y * b.y,
1357 a.y * b.z,
1358 a.z * b.x,
1359 a.z * b.y,
1360 a.z * b.z);
1361 }
1362
1363 /// Computes the translation and rotation that brings the 'from' points
1364 /// as close as possible to the 'to' points under the Frobenius norm.
1365 /// To be more specific, let x be the matrix of 'from' points and y be
1366 /// the matrix of 'to' points, we want to find the matrix A of the form
1367 /// [ R t ]
1368 /// [ 0 1 ]
1369 /// that minimizes
1370 /// || (A*x - y)^T * W * (A*x - y) ||_F
1371 /// If doScaling is true, then a uniform scale is allowed also.
1372 /// @param A From points
1373 /// @param B To points
1374 /// @param weights Per-point weights
1375 /// @param numPoints The number of points in `A`, `B`, and `weights` (must be equal)
1376 /// @param doScaling If true, include a scaling transformation
1377 /// @return The procrustes transformation
1378 template <typename T>
1379 M44d
1380 procrustesRotationAndTranslation (const Vec3<T>* A,
1381 const Vec3<T>* B,
1382 const T* weights,
1383 const size_t numPoints,
1384 const bool doScaling = false);
1385
1386 /// Computes the translation and rotation that brings the 'from' points
1387 /// as close as possible to the 'to' points under the Frobenius norm.
1388 /// To be more specific, let x be the matrix of 'from' points and y be
1389 /// the matrix of 'to' points, we want to find the matrix A of the form
1390 /// [ R t ]
1391 /// [ 0 1 ]
1392 /// that minimizes
1393 /// || (A*x - y)^T * W * (A*x - y) ||_F
1394 /// If doScaling is true, then a uniform scale is allowed also.
1395 /// @param A From points
1396 /// @param B To points
1397 /// @param numPoints The number of points in `A` and `B` (must be equal)
1398 /// @param doScaling If true, include a scaling transformation
1399 /// @return The procrustes transformation
1400 template <typename T>
1401 M44d
1402 procrustesRotationAndTranslation (const Vec3<T>* A,
1403 const Vec3<T>* B,
1404 const size_t numPoints,
1405 const bool doScaling = false);
1406
1407 /// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method
1408 /// should be quite accurate (competitive with LAPACK) even for poorly
1409 /// conditioned matrices, and because it has been written specifically for the
1410 /// 3x3/4x4 case it is much faster than calling out to LAPACK.
1411 ///
1412 /// The SVD of a 3x3/4x4 matrix A is defined as follows:
1413 /// A = U * S * V^T
1414 /// where S is the diagonal matrix of singular values and both U and V are
1415 /// orthonormal. By convention, the entries S are all positive and sorted from
1416 /// the largest to the smallest. However, some uses of this function may
1417 /// require that the matrix U*V^T have positive determinant; in this case, we
1418 /// may make the smallest singular value negative to ensure that this is
1419 /// satisfied.
1420 ///
1421 /// Currently only available for single- and double-precision matrices.
1422 template <typename T>
1423 void jacobiSVD (const Matrix33<T>& A,
1424 Matrix33<T>& U,
1425 Vec3<T>& S,
1426 Matrix33<T>& V,
1427 const T tol = std::numeric_limits<T>::epsilon(),
1428 const bool forcePositiveDeterminant = false);
1429
1430 /// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method
1431 /// should be quite accurate (competitive with LAPACK) even for poorly
1432 /// conditioned matrices, and because it has been written specifically for the
1433 /// 3x3/4x4 case it is much faster than calling out to LAPACK.
1434 ///
1435 /// The SVD of a 3x3/4x4 matrix A is defined as follows:
1436 /// A = U * S * V^T
1437 /// where S is the diagonal matrix of singular values and both U and V are
1438 /// orthonormal. By convention, the entries S are all positive and sorted from
1439 /// the largest to the smallest. However, some uses of this function may
1440 /// require that the matrix U*V^T have positive determinant; in this case, we
1441 /// may make the smallest singular value negative to ensure that this is
1442 /// satisfied.
1443 ///
1444 /// Currently only available for single- and double-precision matrices.
1445 template <typename T>
1446 void jacobiSVD (const Matrix44<T>& A,
1447 Matrix44<T>& U,
1448 Vec4<T>& S,
1449 Matrix44<T>& V,
1450 const T tol = std::numeric_limits<T>::epsilon(),
1451 const bool forcePositiveDeterminant = false);
1452
1453 /// Compute the eigenvalues (S) and the eigenvectors (V) of a real
1454 /// symmetric matrix using Jacobi transformation, using a given
1455 /// tolerance `tol`.
1456 ///
1457 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1458 /// A = V * S * V^T
1459 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1460 /// Input matrix A must be symmetric. A is also modified during
1461 /// the computation so that upper diagonal entries of A become zero.
1462 template <typename T>
1463 void jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V, const T tol);
1464
1465 /// Compute the eigenvalues (S) and the eigenvectors (V) of
1466 /// a real symmetric matrix using Jacobi transformation.
1467 ///
1468 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1469 /// A = V * S * V^T
1470 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1471 /// Input matrix A must be symmetric. A is also modified during
1472 /// the computation so that upper diagonal entries of A become zero.
1473 template <typename T>
1474 inline void
1475 jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V)
1476 {
1477 jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon());
1478 }
1479
1480 /// Compute the eigenvalues (S) and the eigenvectors (V) of a real
1481 /// symmetric matrix using Jacobi transformation, using a given
1482 /// tolerance `tol`.
1483 ///
1484 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1485 /// A = V * S * V^T
1486 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1487 /// Input matrix A must be symmetric. A is also modified during
1488 /// the computation so that upper diagonal entries of A become zero.
1489 template <typename T>
1490 void jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V, const T tol);
1491
1492 /// Compute the eigenvalues (S) and the eigenvectors (V) of
1493 /// a real symmetric matrix using Jacobi transformation.
1494 ///
1495 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1496 /// A = V * S * V^T
1497 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1498 /// Input matrix A must be symmetric. A is also modified during
1499 /// the computation so that upper diagonal entries of A become zero.
1500 template <typename T>
1501 inline void
1502 jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V)
1503 {
1504 jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon());
1505 }
1506
1507 /// Compute a eigenvector corresponding to the abs max eigenvalue
1508 /// of a real symmetric matrix using Jacobi transformation.
1509 template <typename TM, typename TV> void maxEigenVector (TM& A, TV& S);
1510
1511 /// Compute a eigenvector corresponding to the abs min eigenvalue
1512 /// of a real symmetric matrix using Jacobi transformation.
1513 template <typename TM, typename TV> void minEigenVector (TM& A, TV& S);
1514
1515 IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
1516
1517 #endif // INCLUDED_IMATHMATRIXALGO_H