HDK
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ImathMatrixAlgo.h
Go to the documentation of this file.
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
32 /// M33f identity matrix
34 /// M44f identity matrix
36 /// M22d identity matrix
38 /// M33d identity matrix
40 /// M44d identity matrix
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>
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>
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>
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>
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
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
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
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
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
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>
723 {
725 
726  T tr, s;
727  T q[4];
728  int i, j, k;
729  Quat<T> quat;
730 
731  int nxt[3] = { 1, 2, 0 };
732  tr = mat[0][0] + mat[1][1] + mat[2][2];
733 
734  // check the diagonal
735  if (tr > 0.0)
736  {
737  s = std::sqrt (tr + T (1.0));
738  quat.r = s / T (2.0);
739  s = T (0.5) / s;
740 
741  quat.v.x = (mat[1][2] - mat[2][1]) * s;
742  quat.v.y = (mat[2][0] - mat[0][2]) * s;
743  quat.v.z = (mat[0][1] - mat[1][0]) * s;
744  }
745  else
746  {
747  // diagonal is negative
748  i = 0;
749  if (mat[1][1] > mat[0][0])
750  i = 1;
751  if (mat[2][2] > mat[i][i])
752  i = 2;
753 
754  j = nxt[i];
755  k = nxt[j];
756  s = std::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + T (1.0));
757 
758  q[i] = s * T (0.5);
759  if (s != T (0.0))
760  s = T (0.5) / s;
761 
762  q[3] = (mat[j][k] - mat[k][j]) * s;
763  q[j] = (mat[i][j] + mat[j][i]) * s;
764  q[k] = (mat[i][k] + mat[k][i]) * s;
765 
766  quat.v.x = q[0];
767  quat.v.y = q[1];
768  quat.v.z = q[2];
769  quat.r = q[3];
770  }
771 
772  return quat;
773 }
774 
775 template <class T>
776 bool
778  Vec3<T>& s,
779  Vec3<T>& h,
780  Vec3<T>& r,
781  Vec3<T>& t,
782  bool exc /* = true */,
783  typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */)
784 {
786 
787  rot = mat;
788  if (!extractAndRemoveScalingAndShear (rot, s, h, exc))
789  return false;
790 
791  extractEulerXYZ (rot, r);
792 
793  t.x = mat[3][0];
794  t.y = mat[3][1];
795  t.z = mat[3][2];
796 
797  if (rOrder != Euler<T>::XYZ)
798  {
799  Euler<T> eXYZ (r, Euler<T>::XYZ);
800  Euler<T> e (eXYZ, rOrder);
801  r = e.toXYZVector();
802  }
803 
804  return true;
805 }
806 
807 template <class T>
808 bool
809 extractSHRT (const Matrix44<T>& mat, Vec3<T>& s, Vec3<T>& h, Vec3<T>& r, Vec3<T>& t, bool exc)
810 {
811  return extractSHRT (mat, s, h, r, t, exc, Euler<T>::XYZ);
812 }
813 
814 template <class T>
815 bool
817  Vec3<T>& s,
818  Vec3<T>& h,
819  Euler<T>& r,
820  Vec3<T>& t,
821  bool exc /* = true */)
822 {
823  return extractSHRT (mat, s, h, r, t, exc, r.order());
824 }
825 
826 template <class T>
827 bool
828 checkForZeroScaleInRow (const T& scl, const Vec3<T>& row, bool exc /* = true */)
829 {
830  for (int i = 0; i < 3; i++)
831  {
832  if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl)))
833  {
834  if (exc)
835  throw std::domain_error ("Cannot remove zero scaling "
836  "from matrix.");
837  else
838  return false;
839  }
840  }
841 
842  return true;
843 }
844 
845 template <class T>
847 outerProduct (const Vec4<T>& a, const Vec4<T>& b)
848 {
849  return Matrix44<T> (a.x * b.x,
850  a.x * b.y,
851  a.x * b.z,
852  a.x * b.w,
853  a.y * b.x,
854  a.y * b.y,
855  a.y * b.z,
856  a.x * b.w,
857  a.z * b.x,
858  a.z * b.y,
859  a.z * b.z,
860  a.x * b.w,
861  a.w * b.x,
862  a.w * b.y,
863  a.w * b.z,
864  a.w * b.w);
865 }
866 
867 template <class T>
869 rotationMatrix (const Vec3<T>& from, const Vec3<T>& to)
870 {
871  Quat<T> q;
872  q.setRotation (from, to);
873  return q.toMatrix44();
874 }
875 
876 template <class T>
878 rotationMatrixWithUpDir (const Vec3<T>& fromDir, const Vec3<T>& toDir, const Vec3<T>& upDir)
879 {
880  //
881  // The goal is to obtain a rotation matrix that takes
882  // "fromDir" to "toDir". We do this in two steps and
883  // compose the resulting rotation matrices;
884  // (a) rotate "fromDir" into the z-axis
885  // (b) rotate the z-axis into "toDir"
886  //
887 
888  // The from direction must be non-zero; but we allow zero to and up dirs.
889  if (fromDir.length() == 0)
890  return Matrix44<T>();
891 
892  else
893  {
894  Matrix44<T> zAxis2FromDir (UNINITIALIZED);
895  alignZAxisWithTargetDir (zAxis2FromDir, fromDir, Vec3<T> (0, 1, 0));
896 
897  Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed();
898 
899  Matrix44<T> zAxis2ToDir (UNINITIALIZED);
900  alignZAxisWithTargetDir (zAxis2ToDir, toDir, upDir);
901 
902  return fromDir2zAxis * zAxis2ToDir;
903  }
904 }
905 
906 template <class T>
907 void
909 {
910  //
911  // Ensure that the target direction is non-zero.
912  //
913 
914  if (targetDir.length() == 0)
915  targetDir = Vec3<T> (0, 0, 1);
916 
917  //
918  // Ensure that the up direction is non-zero.
919  //
920 
921  if (upDir.length() == 0)
922  upDir = Vec3<T> (0, 1, 0);
923 
924  //
925  // Check for degeneracies. If the upDir and targetDir are parallel
926  // or opposite, then compute a new, arbitrary up direction that is
927  // not parallel or opposite to the targetDir.
928  //
929 
930  if (upDir.cross (targetDir).length() == 0)
931  {
932  upDir = targetDir.cross (Vec3<T> (1, 0, 0));
933  if (upDir.length() == 0)
934  upDir = targetDir.cross (Vec3<T> (0, 0, 1));
935  }
936 
937  //
938  // Compute the x-, y-, and z-axis vectors of the new coordinate system.
939  //
940 
941  Vec3<T> targetPerpDir = upDir.cross (targetDir);
942  Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
943 
944  //
945  // Rotate the x-axis into targetPerpDir (row 0),
946  // rotate the y-axis into targetUpDir (row 1),
947  // rotate the z-axis into targetDir (row 2).
948  //
949 
950  Vec3<T> row[3];
951  row[0] = targetPerpDir.normalized();
952  row[1] = targetUpDir.normalized();
953  row[2] = targetDir.normalized();
954 
955  result.x[0][0] = row[0][0];
956  result.x[0][1] = row[0][1];
957  result.x[0][2] = row[0][2];
958  result.x[0][3] = (T) 0;
959 
960  result.x[1][0] = row[1][0];
961  result.x[1][1] = row[1][1];
962  result.x[1][2] = row[1][2];
963  result.x[1][3] = (T) 0;
964 
965  result.x[2][0] = row[2][0];
966  result.x[2][1] = row[2][1];
967  result.x[2][2] = row[2][2];
968  result.x[2][3] = (T) 0;
969 
970  result.x[3][0] = (T) 0;
971  result.x[3][1] = (T) 0;
972  result.x[3][2] = (T) 0;
973  result.x[3][3] = (T) 1;
974 }
975 
976 // Compute an orthonormal direct frame from : a position, an x axis direction and a normal to the y axis
977 // If the x axis and normal are perpendicular, then the normal will have the same direction as the z axis.
978 // Inputs are :
979 // -the position of the frame
980 // -the x axis direction of the frame
981 // -a normal to the y axis of the frame
982 // Return is the orthonormal frame
983 template <class T>
985 computeLocalFrame (const Vec3<T>& p, const Vec3<T>& xDir, const Vec3<T>& normal)
986 {
987  Vec3<T> _xDir (xDir);
988  Vec3<T> x = _xDir.normalize();
989  Vec3<T> y = (normal % x).normalize();
990  Vec3<T> z = (x % y).normalize();
991 
992  Matrix44<T> L;
993  L[0][0] = x[0];
994  L[0][1] = x[1];
995  L[0][2] = x[2];
996  L[0][3] = 0.0;
997 
998  L[1][0] = y[0];
999  L[1][1] = y[1];
1000  L[1][2] = y[2];
1001  L[1][3] = 0.0;
1002 
1003  L[2][0] = z[0];
1004  L[2][1] = z[1];
1005  L[2][2] = z[2];
1006  L[2][3] = 0.0;
1007 
1008  L[3][0] = p[0];
1009  L[3][1] = p[1];
1010  L[3][2] = p[2];
1011  L[3][3] = 1.0;
1012 
1013  return L;
1014 }
1015 
1016 /// Add a translate/rotate/scale offset to an input frame and put it
1017 /// in another frame of reference.
1018 ///
1019 /// @param inMat input frame
1020 /// @param tOffset translate offset
1021 /// @param rOffset rotate offset in degrees
1022 /// @param sOffset scale offset
1023 /// @param ref Frame of reference
1024 /// @return The offsetted frame
1025 template <class T>
1027 addOffset (const Matrix44<T>& inMat,
1028  const Vec3<T>& tOffset,
1029  const Vec3<T>& rOffset,
1030  const Vec3<T>& sOffset,
1031  const Matrix44<T>& ref)
1032 {
1033  Matrix44<T> O;
1034 
1035  Vec3<T> _rOffset (rOffset);
1036  _rOffset *= M_PI / 180.0;
1037  O.rotate (_rOffset);
1038 
1039  O[3][0] = tOffset[0];
1040  O[3][1] = tOffset[1];
1041  O[3][2] = tOffset[2];
1042 
1043  Matrix44<T> S;
1044  S.scale (sOffset);
1045 
1046  Matrix44<T> X = S * O * inMat * ref;
1047 
1048  return X;
1049 }
1050 
1051 // Compute Translate/Rotate/Scale matrix from matrix A with the Rotate/Scale of Matrix B
1052 // Inputs are :
1053 // -keepRotateA : if true keep rotate from matrix A, use B otherwise
1054 // -keepScaleA : if true keep scale from matrix A, use B otherwise
1055 // -Matrix A
1056 // -Matrix B
1057 // Return Matrix A with tweaked rotation/scale
1058 template <class T>
1060 computeRSMatrix (bool keepRotateA, bool keepScaleA, const Matrix44<T>& A, const Matrix44<T>& B)
1061 {
1062  Vec3<T> as, ah, ar, at;
1063  extractSHRT (A, as, ah, ar, at);
1064 
1065  Vec3<T> bs, bh, br, bt;
1066  extractSHRT (B, bs, bh, br, bt);
1067 
1068  if (!keepRotateA)
1069  ar = br;
1070 
1071  if (!keepScaleA)
1072  as = bs;
1073 
1074  Matrix44<T> mat;
1075  mat.makeIdentity();
1076  mat.translate (at);
1077  mat.rotate (ar);
1078  mat.scale (as);
1079 
1080  return mat;
1081 }
1082 
1083 //-----------------------------------------------------------------------------
1084 // Implementation for 3x3 Matrix
1085 //------------------------------
1086 
1087 template <class T>
1088 bool
1089 extractScaling (const Matrix33<T>& mat, Vec2<T>& scl, bool exc)
1090 {
1091  T shr;
1092  Matrix33<T> M (mat);
1093 
1094  if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1095  return false;
1096 
1097  return true;
1098 }
1099 
1100 template <class T>
1102 sansScaling (const Matrix33<T>& mat, bool exc)
1103 {
1104  Vec2<T> scl;
1105  T shr;
1106  T rot;
1107  Vec2<T> tran;
1108 
1109  if (!extractSHRT (mat, scl, shr, rot, tran, exc))
1110  return mat;
1111 
1112  Matrix33<T> M;
1113 
1114  M.translate (tran);
1115  M.rotate (rot);
1116  M.shear (shr);
1117 
1118  return M;
1119 }
1120 
1121 template <class T>
1122 bool
1123 removeScaling (Matrix33<T>& mat, bool exc)
1124 {
1125  Vec2<T> scl;
1126  T shr;
1127  T rot;
1128  Vec2<T> tran;
1129 
1130  if (!extractSHRT (mat, scl, shr, rot, tran, exc))
1131  return false;
1132 
1133  mat.makeIdentity();
1134  mat.translate (tran);
1135  mat.rotate (rot);
1136  mat.shear (shr);
1137 
1138  return true;
1139 }
1140 
1141 template <class T>
1142 bool
1143 extractScalingAndShear (const Matrix33<T>& mat, Vec2<T>& scl, T& shr, bool exc)
1144 {
1145  Matrix33<T> M (mat);
1146 
1147  if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1148  return false;
1149 
1150  return true;
1151 }
1152 
1153 template <class T>
1155 sansScalingAndShear (const Matrix33<T>& mat, bool exc)
1156 {
1157  Vec2<T> scl;
1158  T shr;
1159  Matrix33<T> M (mat);
1160 
1161  if (!extractAndRemoveScalingAndShear (M, scl, shr, exc))
1162  return mat;
1163 
1164  return M;
1165 }
1166 
1167 template <class T>
1168 bool
1170 {
1171  Vec2<T> scl;
1172  T shr;
1173 
1174  if (!extractAndRemoveScalingAndShear (mat, scl, shr, exc))
1175  return false;
1176 
1177  return true;
1178 }
1179 
1180 template <class T>
1181 bool
1183 {
1184  Vec2<T> row[2];
1185 
1186  row[0] = Vec2<T> (mat[0][0], mat[0][1]);
1187  row[1] = Vec2<T> (mat[1][0], mat[1][1]);
1188 
1189  T maxVal = 0;
1190  for (int i = 0; i < 2; i++)
1191  for (int j = 0; j < 2; j++)
1192  if (IMATH_INTERNAL_NAMESPACE::abs (row[i][j]) > maxVal)
1193  maxVal = IMATH_INTERNAL_NAMESPACE::abs (row[i][j]);
1194 
1195  //
1196  // We normalize the 2x2 matrix here.
1197  // It was noticed that this can improve numerical stability significantly,
1198  // especially when many of the upper 2x2 matrix's coefficients are very
1199  // close to zero; we correct for this step at the end by multiplying the
1200  // scaling factors by maxVal at the end (shear and rotation are not
1201  // affected by the normalization).
1202 
1203  if (maxVal != 0)
1204  {
1205  for (int i = 0; i < 2; i++)
1206  if (!checkForZeroScaleInRow (maxVal, row[i], exc))
1207  return false;
1208  else
1209  row[i] /= maxVal;
1210  }
1211 
1212  // Compute X scale factor.
1213  scl.x = row[0].length();
1214  if (!checkForZeroScaleInRow (scl.x, row[0], exc))
1215  return false;
1216 
1217  // Normalize first row.
1218  row[0] /= scl.x;
1219 
1220  // An XY shear factor will shear the X coord. as the Y coord. changes.
1221  // There are 2 combinations (XY, YX), although we only extract the XY
1222  // shear factor because we can effect the an YX shear factor by
1223  // shearing in XY combined with rotations and scales.
1224  //
1225  // shear matrix < 1, YX, 0,
1226  // XY, 1, 0,
1227  // 0, 0, 1 >
1228 
1229  // Compute XY shear factor and make 2nd row orthogonal to 1st.
1230  shr = row[0].dot (row[1]);
1231  row[1] -= shr * row[0];
1232 
1233  // Now, compute Y scale.
1234  scl.y = row[1].length();
1235  if (!checkForZeroScaleInRow (scl.y, row[1], exc))
1236  return false;
1237 
1238  // Normalize 2nd row and correct the XY shear factor for Y scaling.
1239  row[1] /= scl.y;
1240  shr /= scl.y;
1241 
1242  // At this point, the upper 2x2 matrix in mat is orthonormal.
1243  // Check for a coordinate system flip. If the determinant
1244  // is -1, then flip the rotation matrix and adjust the scale(Y)
1245  // and shear(XY) factors to compensate.
1246  if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
1247  {
1248  row[1][0] *= -1;
1249  row[1][1] *= -1;
1250  scl[1] *= -1;
1251  shr *= -1;
1252  }
1253 
1254  // Copy over the orthonormal rows into the returned matrix.
1255  // The upper 2x2 matrix in mat is now a rotation matrix.
1256  for (int i = 0; i < 2; i++)
1257  {
1258  mat[i][0] = row[i][0];
1259  mat[i][1] = row[i][1];
1260  }
1261 
1262  scl *= maxVal;
1263 
1264  return true;
1265 }
1266 
1267 template <class T>
1268 void
1269 extractEuler (const Matrix22<T>& mat, T& rot)
1270 {
1271  //
1272  // Normalize the local x and y axes to remove scaling.
1273  //
1274 
1275  Vec2<T> i (mat[0][0], mat[0][1]);
1276  Vec2<T> j (mat[1][0], mat[1][1]);
1277 
1278  i.normalize();
1279  j.normalize();
1280 
1281  //
1282  // Extract the angle, rot.
1283  //
1284 
1285  rot = -std::atan2 (j[0], i[0]);
1286 }
1287 
1288 template <class T>
1289 void
1290 extractEuler (const Matrix33<T>& mat, T& rot)
1291 {
1292  //
1293  // Normalize the local x and y axes to remove scaling.
1294  //
1295 
1296  Vec2<T> i (mat[0][0], mat[0][1]);
1297  Vec2<T> j (mat[1][0], mat[1][1]);
1298 
1299  i.normalize();
1300  j.normalize();
1301 
1302  //
1303  // Extract the angle, rot.
1304  //
1305 
1306  rot = -std::atan2 (j[0], i[0]);
1307 }
1308 
1309 template <class T>
1310 bool
1311 extractSHRT (const Matrix33<T>& mat, Vec2<T>& s, T& h, T& r, Vec2<T>& t, bool exc)
1312 {
1313  Matrix33<T> rot;
1314 
1315  rot = mat;
1316  if (!extractAndRemoveScalingAndShear (rot, s, h, exc))
1317  return false;
1318 
1319  extractEuler (rot, r);
1320 
1321  t.x = mat[2][0];
1322  t.y = mat[2][1];
1323 
1324  return true;
1325 }
1326 
1327 /// @cond Doxygen_Suppress
1328 template <class T>
1329 bool
1330 checkForZeroScaleInRow (const T& scl, const Vec2<T>& row, bool exc /* = true */)
1331 {
1332  for (int i = 0; i < 2; i++)
1333  {
1334  if ((abs (scl) < 1 && abs (row[i]) >= std::numeric_limits<T>::max() * abs (scl)))
1335  {
1336  if (exc)
1337  throw std::domain_error ("Cannot remove zero scaling from matrix.");
1338  else
1339  return false;
1340  }
1341  }
1342 
1343  return true;
1344 }
1345 /// @endcond
1346 
1347 template <class T>
1349 outerProduct (const Vec3<T>& a, const Vec3<T>& b)
1350 {
1351  return Matrix33<T> (a.x * b.x,
1352  a.x * b.y,
1353  a.x * b.z,
1354  a.y * b.x,
1355  a.y * b.y,
1356  a.y * b.z,
1357  a.z * b.x,
1358  a.z * b.y,
1359  a.z * b.z);
1360 }
1361 
1362 /// Computes the translation and rotation that brings the 'from' points
1363 /// as close as possible to the 'to' points under the Frobenius norm.
1364 /// To be more specific, let x be the matrix of 'from' points and y be
1365 /// the matrix of 'to' points, we want to find the matrix A of the form
1366 /// [ R t ]
1367 /// [ 0 1 ]
1368 /// that minimizes
1369 /// || (A*x - y)^T * W * (A*x - y) ||_F
1370 /// If doScaling is true, then a uniform scale is allowed also.
1371 /// @param A From points
1372 /// @param B To points
1373 /// @param weights Per-point weights
1374 /// @param numPoints The number of points in `A`, `B`, and `weights` (must be equal)
1375 /// @param doScaling If true, include a scaling transformation
1376 /// @return The procrustes transformation
1377 template <typename T>
1378 M44d
1380  const Vec3<T>* B,
1381  const T* weights,
1382  const size_t numPoints,
1383  const bool doScaling = false);
1384 
1385 /// Computes the translation and rotation that brings the 'from' points
1386 /// as close as possible to the 'to' points under the Frobenius norm.
1387 /// To be more specific, let x be the matrix of 'from' points and y be
1388 /// the matrix of 'to' points, we want to find the matrix A of the form
1389 /// [ R t ]
1390 /// [ 0 1 ]
1391 /// that minimizes
1392 /// || (A*x - y)^T * W * (A*x - y) ||_F
1393 /// If doScaling is true, then a uniform scale is allowed also.
1394 /// @param A From points
1395 /// @param B To points
1396 /// @param numPoints The number of points in `A` and `B` (must be equal)
1397 /// @param doScaling If true, include a scaling transformation
1398 /// @return The procrustes transformation
1399 template <typename T>
1400 M44d
1402  const Vec3<T>* B,
1403  const size_t numPoints,
1404  const bool doScaling = false);
1405 
1406 /// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method
1407 /// should be quite accurate (competitive with LAPACK) even for poorly
1408 /// conditioned matrices, and because it has been written specifically for the
1409 /// 3x3/4x4 case it is much faster than calling out to LAPACK.
1410 ///
1411 /// The SVD of a 3x3/4x4 matrix A is defined as follows:
1412 /// A = U * S * V^T
1413 /// where S is the diagonal matrix of singular values and both U and V are
1414 /// orthonormal. By convention, the entries S are all positive and sorted from
1415 /// the largest to the smallest. However, some uses of this function may
1416 /// require that the matrix U*V^T have positive determinant; in this case, we
1417 /// may make the smallest singular value negative to ensure that this is
1418 /// satisfied.
1419 ///
1420 /// Currently only available for single- and double-precision matrices.
1421 template <typename T>
1422 void jacobiSVD (const Matrix33<T>& A,
1423  Matrix33<T>& U,
1424  Vec3<T>& S,
1425  Matrix33<T>& V,
1426  const T tol = std::numeric_limits<T>::epsilon(),
1427  const bool forcePositiveDeterminant = false);
1428 
1429 /// Compute the SVD of a 3x3 matrix using Jacobi transformations. This method
1430 /// should be quite accurate (competitive with LAPACK) even for poorly
1431 /// conditioned matrices, and because it has been written specifically for the
1432 /// 3x3/4x4 case it is much faster than calling out to LAPACK.
1433 ///
1434 /// The SVD of a 3x3/4x4 matrix A is defined as follows:
1435 /// A = U * S * V^T
1436 /// where S is the diagonal matrix of singular values and both U and V are
1437 /// orthonormal. By convention, the entries S are all positive and sorted from
1438 /// the largest to the smallest. However, some uses of this function may
1439 /// require that the matrix U*V^T have positive determinant; in this case, we
1440 /// may make the smallest singular value negative to ensure that this is
1441 /// satisfied.
1442 ///
1443 /// Currently only available for single- and double-precision matrices.
1444 template <typename T>
1445 void jacobiSVD (const Matrix44<T>& A,
1446  Matrix44<T>& U,
1447  Vec4<T>& S,
1448  Matrix44<T>& V,
1449  const T tol = std::numeric_limits<T>::epsilon(),
1450  const bool forcePositiveDeterminant = false);
1451 
1452 /// Compute the eigenvalues (S) and the eigenvectors (V) of a real
1453 /// symmetric matrix using Jacobi transformation, using a given
1454 /// tolerance `tol`.
1455 ///
1456 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1457 /// A = V * S * V^T
1458 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1459 /// Input matrix A must be symmetric. A is also modified during
1460 /// the computation so that upper diagonal entries of A become zero.
1461 template <typename T>
1462 void jacobiEigenSolver (Matrix33<T>& A, Vec3<T>& S, Matrix33<T>& V, const T tol);
1463 
1464 /// Compute the eigenvalues (S) and the eigenvectors (V) of
1465 /// a real symmetric matrix using Jacobi transformation.
1466 ///
1467 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1468 /// A = V * S * V^T
1469 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1470 /// Input matrix A must be symmetric. A is also modified during
1471 /// the computation so that upper diagonal entries of A become zero.
1472 template <typename T>
1473 inline void
1475 {
1476  jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon());
1477 }
1478 
1479 /// Compute the eigenvalues (S) and the eigenvectors (V) of a real
1480 /// symmetric matrix using Jacobi transformation, using a given
1481 /// tolerance `tol`.
1482 ///
1483 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1484 /// A = V * S * V^T
1485 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1486 /// Input matrix A must be symmetric. A is also modified during
1487 /// the computation so that upper diagonal entries of A become zero.
1488 template <typename T>
1489 void jacobiEigenSolver (Matrix44<T>& A, Vec4<T>& S, Matrix44<T>& V, const T tol);
1490 
1491 /// Compute the eigenvalues (S) and the eigenvectors (V) of
1492 /// a real symmetric matrix using Jacobi transformation.
1493 ///
1494 /// Jacobi transformation of a 3x3/4x4 matrix A outputs S and V:
1495 /// A = V * S * V^T
1496 /// where V is orthonormal and S is the diagonal matrix of eigenvalues.
1497 /// Input matrix A must be symmetric. A is also modified during
1498 /// the computation so that upper diagonal entries of A become zero.
1499 template <typename T>
1500 inline void
1502 {
1503  jacobiEigenSolver (A, S, V, std::numeric_limits<T>::epsilon());
1504 }
1505 
1506 /// Compute a eigenvector corresponding to the abs max eigenvalue
1507 /// of a real symmetric matrix using Jacobi transformation.
1508 template <typename TM, typename TV> void maxEigenVector (TM& A, TV& S);
1509 
1510 /// Compute a eigenvector corresponding to the abs min eigenvalue
1511 /// of a real symmetric matrix using Jacobi transformation.
1512 template <typename TM, typename TV> void minEigenVector (TM& A, TV& S);
1513 
1514 IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
1515 
1516 #endif // INCLUDED_IMATHMATRIXALGO_H
void extractEulerXYZ(const Matrix44< T > &mat, Vec3< T > &rot)
bool extractScalingAndShear(const Matrix44< T > &mat, Vec3< T > &scl, Vec3< T > &shr, bool exc=true)
Matrix44< T > rotationMatrixWithUpDir(const Vec3< T > &fromDir, const Vec3< T > &toDir, const Vec3< T > &upDir)
bool extractScaling(const Matrix44< T > &mat, Vec3< T > &scl, bool exc=true)
SYS_API double atan2(double y, double x)
Definition: SYS_FPUMath.h:79
void maxEigenVector(TM &A, TV &S)
T z
Definition: ImathVec.h:310
bool extractSHRT(const Matrix44< T > &mat, Vec3< T > &s, Vec3< T > &h, Vec3< T > &r, Vec3< T > &t, bool exc, typename Euler< T >::Order rOrder)
Matrix44< T > computeRSMatrix(bool keepRotateA, bool keepScaleA, const Matrix44< T > &A, const Matrix44< T > &B)
IMATH_EXPORT_CONST M44d identity44d
M44d identity matrix.
Definition: ImathVec.h:32
Definition: ImathQuat.h:42
IMATH_HOSTDEVICE T length() const IMATH_NOEXCEPT
Return the Euclidean norm.
Definition: ImathVec.h:1734
#define M_PI
Definition: fmath.h:90
GA_API const UT_StringHolder rot
vfloat4 sqrt(const vfloat4 &a)
Definition: simd.h:7481
GLdouble GLdouble GLdouble z
Definition: glcorearb.h:848
Matrix44< T > addOffset(const Matrix44< T > &inMat, const Vec3< T > &tOffset, const Vec3< T > &rOffset, const Vec3< T > &sOffset, const Vec3< T > &ref)
IMATH_EXPORT_CONST M44f identity44f
M44f identity matrix.
GLboolean GLboolean GLboolean GLboolean a
Definition: glcorearb.h:1222
GLdouble s
Definition: glad.h:3009
IMATH_HOSTDEVICE Vec3< T > normalized() const IMATH_NOEXCEPT
Return a normalized vector. Does not modify *this.
Definition: ImathVec.h:1801
IMATH_HOSTDEVICE constexpr T dot(const Vec3 &v) const IMATH_NOEXCEPT
Dot product.
Definition: ImathVec.h:1542
X
Definition: ImathEuler.h:183
Vec3< T > v
The imaginary vector.
Definition: ImathQuat.h:53
GLint y
Definition: glcorearb.h:103
T x[4][4]
Matrix elements.
Definition: ImathMatrix.h:670
**But if you need a result
Definition: thread.h:613
GLdouble GLdouble GLdouble q
Definition: glad.h:2445
IMATH_EXPORT_CONST M22d identity22d
M22d identity matrix.
bool extractAndRemoveScalingAndShear(Matrix44< T > &mat, Vec3< T > &scl, Vec3< T > &shr, bool exc=true)
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix33 & shear(const S &xy) IMATH_NOEXCEPT
bool checkForZeroScaleInRow(const T &scl, const Vec3< T > &row, bool exc=true)
T r
The real part.
Definition: ImathQuat.h:50
T z
Definition: ImathVec.h:581
Matrix44< T > rotationMatrix(const Vec3< T > &fromDirection, const Vec3< T > &toDirection)
Matrix44< T > sansScaling(const Matrix44< T > &mat, bool exc=true)
T x
Definition: ImathVec.h:52
T x
Definition: ImathVec.h:310
IMATH_HOSTDEVICE constexpr T dot(const Vec2 &v) const IMATH_NOEXCEPT
Dot product.
Definition: ImathVec.h:1102
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix44 & shear(const Vec3< S > &h) IMATH_NOEXCEPT
GLint ref
Definition: glcorearb.h:124
void extractEulerZYX(const Matrix44< T > &mat, Vec3< T > &rot)
T y
Definition: ImathVec.h:52
IMATH_HOSTDEVICE Vec3< T > toXYZVector() const IMATH_NOEXCEPT
fpreal64 dot(const CE_VectorT< T > &a, const CE_VectorT< T > &b)
Definition: CE_Vector.h:140
IMATH_HOSTDEVICE constexpr Matrix44< T > toMatrix44() const IMATH_NOEXCEPT
Return a 4x4 rotation matrix.
Definition: ImathQuat.h:814
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Quat< T > & setRotation(const Vec3< T > &fromDirection, const Vec3< T > &toDirection) IMATH_NOEXCEPT
Definition: ImathQuat.h:692
#define IMATH_EXPORT_CONST
Definition: ImathExport.h:48
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix33 & translate(const Vec2< S > &t) IMATH_NOEXCEPT
IMATH_EXPORT_CONST M33d identity33d
M33d identity matrix.
Matrix44< T > outerProduct(const Vec4< T > &a, const Vec4< T > &b)
Return the 4x4 outer product two 4-vectors.
void jacobiSVD(const Matrix33< T > &A, Matrix33< T > &U, Vec3< T > &S, Matrix33< T > &V, const T tol=std::numeric_limits< T >::epsilon(), const bool forcePositiveDeterminant=false)
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 Order order() const IMATH_NOEXCEPT
Return the order.
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix44 & translate(const Vec3< S > &t) IMATH_NOEXCEPT
GLboolean GLboolean GLboolean b
Definition: glcorearb.h:1222
GLint GLenum GLint x
Definition: glcorearb.h:409
GLdouble t
Definition: glad.h:2397
bool removeScalingAndShear(Matrix44< T > &mat, bool exc=true)
Remove scaling and shear from the given 4x4 matrix in place.
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix44 & scale(const Vec3< S > &s) IMATH_NOEXCEPT
IMATH_HOSTDEVICE void makeIdentity() IMATH_NOEXCEPT
Set to the identity matrix.
Definition: ImathMatrix.h:1932
Quat< T > extractQuat(const Matrix44< T > &mat)
GLint j
Definition: glad.h:2733
GLfloat GLfloat GLfloat GLfloat h
Definition: glcorearb.h:2002
IMATH_HOSTDEVICE constexpr Matrix44 transposed() const IMATH_NOEXCEPT
Return the transpose.
Definition: ImathMatrix.h:3760
Definition: ImathVec.h:31
T x
Definition: ImathVec.h:581
void jacobiEigenSolver(Matrix33< T > &A, Vec3< T > &S, Matrix33< T > &V, const T tol)
IMATH_HOSTDEVICE const Matrix44 & rotate(const Vec3< S > &r) IMATH_NOEXCEPT
Matrix44< T > sansScalingAndShear(const Matrix44< T > &mat, bool exc=true)
ImageBuf OIIO_API max(Image_or_Const A, Image_or_Const B, ROI roi={}, int nthreads=0)
GA_API const UT_StringHolder N
Matrix44< T > computeLocalFrame(const Vec3< T > &p, const Vec3< T > &xDir, const Vec3< T > &normal)
IMATH_HOSTDEVICE IMATH_CONSTEXPR14 const Matrix33 & rotate(S r) IMATH_NOEXCEPT
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER IMATH_EXPORT_CONST M22f identity22f
M22f identity matrix.
T y
Definition: ImathVec.h:310
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER IMATH_HOSTDEVICE constexpr T abs(T a) IMATH_NOEXCEPT
Definition: ImathFun.h:26
GLenum GLenum GLsizei void * row
Definition: glad.h:5135
GLboolean r
Definition: glcorearb.h:1222
IMATH_EXPORT_CONST M33f identity33f
M33f identity matrix.
Definition: ImathVec.h:33
T w
Definition: ImathVec.h:581
void extractEuler(const Matrix22< T > &mat, T &rot)
T y
Definition: ImathVec.h:581
IMATH_HOSTDEVICE constexpr Vec3 cross(const Vec3 &v) const IMATH_NOEXCEPT
Right-handed cross product.
Definition: ImathVec.h:1556
IMATH_HOSTDEVICE const Vec3 & normalize() IMATH_NOEXCEPT
Normalize in place. If length()==0, return a null vector.
Definition: ImathVec.h:1753
void minEigenVector(TM &A, TV &S)
SIM_DerVector3 cross(const SIM_DerVector3 &lhs, const SIM_DerVector3 &rhs)
void alignZAxisWithTargetDir(Matrix44< T > &result, Vec3< T > targetDir, Vec3< T > upDir)
M44d procrustesRotationAndTranslation(const Vec3< T > *A, const Vec3< T > *B, const T *weights, const size_t numPoints, const bool doScaling=false)
constexpr T normalize(UT_FixedVector< T, D > &a) noexcept
bool removeScaling(Matrix44< T > &mat, bool exc=true)
IMATH_HOSTDEVICE void makeIdentity() IMATH_NOEXCEPT
Set to the identity matrix.
Definition: ImathMatrix.h:3274
IMATH_HOSTDEVICE T length() const IMATH_NOEXCEPT
Return the Euclidean norm.
Definition: ImathVec.h:1269
IMATH_HOSTDEVICE const Vec2 & normalize() IMATH_NOEXCEPT
Normalize in place. If length()==0, return a null vector.
Definition: ImathVec.h:1288