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