source: NonGTP/OpenEXR/include/Imath/ImathMatrixAlgo.h @ 855

Revision 855, 27.4 KB checked in by igarcia, 19 years ago (diff)
RevLine 
[855]1///////////////////////////////////////////////////////////////////////////
2//
3// Copyright (c) 2002, 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 <ImathMatrix.h>
52#include <ImathQuat.h>
53#include <ImathEuler.h>
54#include <ImathExc.h>
55#include <ImathVec.h>
56#include <math.h>
57
58
59#ifdef OPENEXR_DLL
60    #ifdef IMATH_EXPORTS
61        #define IMATH_EXPORT_CONST extern __declspec(dllexport)
62    #else
63        #define IMATH_EXPORT_CONST extern __declspec(dllimport)
64    #endif
65#else
66    #define IMATH_EXPORT_CONST extern const
67#endif
68
69
70namespace Imath {
71
72//------------------
73// Identity matrices
74//------------------
75
76IMATH_EXPORT_CONST M33f identity33f;
77IMATH_EXPORT_CONST M44f identity44f;
78IMATH_EXPORT_CONST M33d identity33d;
79IMATH_EXPORT_CONST M44d identity44d;
80
81//----------------------------------------------------------------------
82// Extract scale, shear, rotation, and translation values from a matrix:
83//
84// Notes:
85//
86// This implementation follows the technique described in the paper by
87// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
88// Matrix into Simple Transformations", p. 320.
89//
90// - Some of the functions below have an optional exc parameter
91//   that determines the functions' behavior when the matrix'
92//   scaling is very close to zero:
93//
94//   If exc is true, the functions throw an Imath::ZeroScale exception.
95//
96//   If exc is false:
97//
98//      extractScaling (m, s)            returns false, s is invalid
99//      sansScaling (m)                  returns m
100//      removeScaling (m)                returns false, m is unchanged
101//      sansScalingAndShear (m)          returns m
102//      removeScalingAndShear (m)        returns false, m is unchanged
103//      extractAndRemoveScalingAndShear (m, s, h) 
104//                                       returns false, m is unchanged,
105//                                                      (sh) are invalid
106//      checkForZeroScaleInRow ()        returns false
107//      extractSHRT (m, s, h, r, t)      returns false, (shrt) are invalid
108//
109// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX()
110//   assume that the matrix does not include shear or non-uniform scaling,
111//   but they do not examine the matrix to verify this assumption. 
112//   Matrices with shear or non-uniform scaling are likely to produce
113//   meaningless results.  Therefore, you should use the
114//   removeScalingAndShear() routine, if necessary, prior to calling
115//   extractEuler...() .
116//
117// - All functions assume that the matrix does not include perspective
118//   transformation(s), but they do not examine the matrix to verify
119//   this assumption.  Matrices with perspective transformations are
120//   likely to produce meaningless results.
121//
122//----------------------------------------------------------------------
123
124
125//
126// Declarations for 4x4 matrix.
127//
128
129template <class T>  bool        extractScaling
130                                            (const Matrix44<T> &mat,
131                                             Vec3<T> &scl,
132                                             bool exc = true);
133 
134template <class T>  Matrix44<T> sansScaling (const Matrix44<T> &mat,
135                                             bool exc = true);
136
137template <class T>  bool        removeScaling
138                                            (Matrix44<T> &mat,
139                                             bool exc = true);
140
141template <class T>  bool        extractScalingAndShear
142                                            (const Matrix44<T> &mat,
143                                             Vec3<T> &scl,
144                                             Vec3<T> &shr,
145                                             bool exc = true);
146 
147template <class T>  Matrix44<T> sansScalingAndShear
148                                            (const Matrix44<T> &mat,
149                                             bool exc = true);
150
151template <class T>  bool        removeScalingAndShear
152                                            (Matrix44<T> &mat,
153                                             bool exc = true);
154
155template <class T>  bool        extractAndRemoveScalingAndShear
156                                            (Matrix44<T> &mat,
157                                             Vec3<T>     &scl,
158                                             Vec3<T>     &shr,
159                                             bool exc = true);
160
161template <class T>  void        extractEulerXYZ
162                                            (const Matrix44<T> &mat,
163                                             Vec3<T> &rot);
164
165template <class T>  void        extractEulerZYX
166                                            (const Matrix44<T> &mat,
167                                             Vec3<T> &rot);
168
169template <class T>  Quat<T>     extractQuat (const Matrix44<T> &mat);
170
171template <class T>  bool        extractSHRT
172                                    (const Matrix44<T> &mat,
173                                     Vec3<T> &s,
174                                     Vec3<T> &h,
175                                     Vec3<T> &r,
176                                     Vec3<T> &t,
177                                     bool exc /*= true*/,
178                                     typename Euler<T>::Order rOrder);
179
180template <class T>  bool        extractSHRT
181                                    (const Matrix44<T> &mat,
182                                     Vec3<T> &s,
183                                     Vec3<T> &h,
184                                     Vec3<T> &r,
185                                     Vec3<T> &t,
186                                     bool exc = true);
187
188template <class T>  bool        extractSHRT
189                                    (const Matrix44<T> &mat,
190                                     Vec3<T> &s,
191                                     Vec3<T> &h,
192                                     Euler<T> &r,
193                                     Vec3<T> &t,
194                                     bool exc = true);
195
196//
197// Internal utility function.
198//
199
200template <class T>  bool        checkForZeroScaleInRow
201                                            (const T       &scl,
202                                             const Vec3<T> &row,
203                                             bool exc = true);
204
205//
206// Returns a matrix that rotates "fromDirection" vector to "toDirection"
207// vector.
208//
209
210template <class T> Matrix44<T>  rotationMatrix (const Vec3<T> &fromDirection,
211                                                const Vec3<T> &toDirection);
212
213
214
215//
216// Returns a matrix that rotates the "fromDir" vector
217// so that it points towards "toDir".  You may also
218// specify that you want the up vector to be pointing
219// in a certain direction "upDir".
220//
221
222template <class T> Matrix44<T>  rotationMatrixWithUpDir
223                                            (const Vec3<T> &fromDir,
224                                             const Vec3<T> &toDir,
225                                             const Vec3<T> &upDir);
226
227
228//
229// Returns a matrix that rotates the z-axis so that it
230// points towards "targetDir".  You must also specify
231// that you want the up vector to be pointing in a
232// certain direction "upDir".
233//
234// Notes: The following degenerate cases are handled:
235//        (a) when the directions given by "toDir" and "upDir"
236//            are parallel or opposite;
237//            (the direction vectors must have a non-zero cross product)
238//        (b) when any of the given direction vectors have zero length
239//
240
241template <class T> Matrix44<T>  alignZAxisWithTargetDir
242                                            (Vec3<T> targetDir,
243                                             Vec3<T> upDir);
244
245
246//----------------------------------------------------------------------
247
248
249//
250// Declarations for 3x3 matrix.
251//
252
253 
254template <class T>  bool        extractScaling
255                                            (const Matrix33<T> &mat,
256                                             Vec2<T> &scl,
257                                             bool exc = true);
258 
259template <class T>  Matrix33<T> sansScaling (const Matrix33<T> &mat,
260                                             bool exc = true);
261
262template <class T>  bool        removeScaling
263                                            (Matrix33<T> &mat,
264                                             bool exc = true);
265
266template <class T>  bool        extractScalingAndShear
267                                            (const Matrix33<T> &mat,
268                                             Vec2<T> &scl,
269                                             T &h,
270                                             bool exc = true);
271 
272template <class T>  Matrix33<T> sansScalingAndShear
273                                            (const Matrix33<T> &mat,
274                                             bool exc = true);
275
276template <class T>  bool        removeScalingAndShear
277                                            (Matrix33<T> &mat,
278                                             bool exc = true);
279
280template <class T>  bool        extractAndRemoveScalingAndShear
281                                            (Matrix33<T> &mat,
282                                             Vec2<T>     &scl,
283                                             T           &shr,
284                                             bool exc = true);
285
286template <class T>  void        extractEuler
287                                            (const Matrix33<T> &mat,
288                                             T       &rot);
289
290template <class T>  bool        extractSHRT (const Matrix33<T> &mat,
291                                             Vec2<T> &s,
292                                             T       &h,
293                                             T       &r,
294                                             Vec2<T> &t,
295                                             bool exc = true);
296
297template <class T>  bool        checkForZeroScaleInRow
298                                            (const T       &scl,
299                                             const Vec2<T> &row,
300                                             bool exc = true);
301
302
303
304
305//-----------------------------------------------------------------------------
306// Implementation for 4x4 Matrix
307//------------------------------
308
309
310template <class T>
311bool
312extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
313{
314    Vec3<T> shr;
315    Matrix44<T> M (mat);
316
317    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
318        return false;
319   
320    return true;
321}
322
323
324template <class T>
325Matrix44<T>
326sansScaling (const Matrix44<T> &mat, bool exc)
327{
328    Vec3<T> scl;
329    Vec3<T> shr;
330    Vec3<T> rot;
331    Vec3<T> tran;
332
333    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
334        return mat;
335
336    Matrix44<T> M;
337   
338    M.translate (tran);
339    M.rotate (rot);
340    M.shear (shr);
341
342    return M;
343}
344
345
346template <class T>
347bool
348removeScaling (Matrix44<T> &mat, bool exc)
349{
350    Vec3<T> scl;
351    Vec3<T> shr;
352    Vec3<T> rot;
353    Vec3<T> tran;
354
355    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
356        return false;
357
358    mat.makeIdentity ();
359    mat.translate (tran);
360    mat.rotate (rot);
361    mat.shear (shr);
362
363    return true;
364}
365
366
367template <class T>
368bool
369extractScalingAndShear (const Matrix44<T> &mat,
370                        Vec3<T> &scl, Vec3<T> &shr, bool exc)
371{
372    Matrix44<T> M (mat);
373
374    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
375        return false;
376   
377    return true;
378}
379
380
381template <class T>
382Matrix44<T>
383sansScalingAndShear (const Matrix44<T> &mat, bool exc)
384{
385    Vec3<T> scl;
386    Vec3<T> shr;
387    Matrix44<T> M (mat);
388
389    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
390        return mat;
391   
392    return M;
393}
394
395
396template <class T>
397bool
398removeScalingAndShear (Matrix44<T> &mat, bool exc)
399{
400    Vec3<T> scl;
401    Vec3<T> shr;
402
403    if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
404        return false;
405   
406    return true;
407}
408
409
410template <class T>
411bool
412extractAndRemoveScalingAndShear (Matrix44<T> &mat,
413                                 Vec3<T> &scl, Vec3<T> &shr, bool exc)
414{
415    //
416    // This implementation follows the technique described in the paper by
417    // Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
418    // Matrix into Simple Transformations", p. 320.
419    //
420
421    Vec3<T> row[3];
422
423    row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
424    row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
425    row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
426   
427    T maxVal = 0;
428    for (int i=0; i < 3; i++)
429        for (int j=0; j < 3; j++)
430            if (Imath::abs (row[i][j]) > maxVal)
431                maxVal = Imath::abs (row[i][j]);
432
433    //
434    // We normalize the 3x3 matrix here.
435    // It was noticed that this can improve numerical stability significantly,
436    // especially when many of the upper 3x3 matrix's coefficients are very
437    // close to zero; we correct for this step at the end by multiplying the
438    // scaling factors by maxVal at the end (shear and rotation are not
439    // affected by the normalization).
440
441    if (maxVal != 0)
442    {
443        for (int i=0; i < 3; i++)
444            if (! checkForZeroScaleInRow (maxVal, row[i], exc))
445                return false;
446            else
447                row[i] /= maxVal;
448    }
449
450    // Compute X scale factor.
451    scl.x = row[0].length ();
452    if (! checkForZeroScaleInRow (scl.x, row[0], exc))
453        return false;
454
455    // Normalize first row.
456    row[0] /= scl.x;
457
458    // An XY shear factor will shear the X coord. as the Y coord. changes.
459    // There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
460    // extract the first 3 because we can effect the last 3 by shearing in
461    // XY, XZ, YZ combined rotations and scales.
462    //
463    // shear matrix <   1,  YX,  ZX,  0,
464    //                 XY,   1,  ZY,  0,
465    //                 XZ,  YZ,   1,  0,
466    //                  0,   0,   0,  1 >
467
468    // Compute XY shear factor and make 2nd row orthogonal to 1st.
469    shr[0]  = row[0].dot (row[1]);
470    row[1] -= shr[0] * row[0];
471
472    // Now, compute Y scale.
473    scl.y = row[1].length ();
474    if (! checkForZeroScaleInRow (scl.y, row[1], exc))
475        return false;
476
477    // Normalize 2nd row and correct the XY shear factor for Y scaling.
478    row[1] /= scl.y;
479    shr[0] /= scl.y;
480
481    // Compute XZ and YZ shears, orthogonalize 3rd row.
482    shr[1]  = row[0].dot (row[2]);
483    row[2] -= shr[1] * row[0];
484    shr[2]  = row[1].dot (row[2]);
485    row[2] -= shr[2] * row[1];
486
487    // Next, get Z scale.
488    scl.z = row[2].length ();
489    if (! checkForZeroScaleInRow (scl.z, row[2], exc))
490        return false;
491
492    // Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
493    row[2] /= scl.z;
494    shr[1] /= scl.z;
495    shr[2] /= scl.z;
496
497    // At this point, the upper 3x3 matrix in mat is orthonormal.
498    // Check for a coordinate system flip. If the determinant
499    // is less than zero, then negate the matrix and the scaling factors.
500    if (row[0].dot (row[1].cross (row[2])) < 0)
501        for (int  i=0; i < 3; i++)
502        {
503            scl[i] *= -1;
504            row[i] *= -1;
505        }
506
507    // Copy over the orthonormal rows into the returned matrix.
508    // The upper 3x3 matrix in mat is now a rotation matrix.
509    for (int i=0; i < 3; i++)
510    {
511        mat[i][0] = row[i][0];
512        mat[i][1] = row[i][1];
513        mat[i][2] = row[i][2];
514    }
515
516    // Correct the scaling factors for the normalization step that we
517    // performed above; shear and rotation are not affected by the
518    // normalization.
519    scl *= maxVal;
520
521    return true;
522}
523
524
525template <class T>
526void
527extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
528{
529    //
530    // Normalize the local x, y and z axes to remove scaling.
531    //
532
533    Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
534    Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
535    Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
536
537    i.normalize();
538    j.normalize();
539    k.normalize();
540
541    Matrix44<T> M (i[0], i[1], i[2], 0,
542                   j[0], j[1], j[2], 0,
543                   k[0], k[1], k[2], 0,
544                   0,    0,    0,    1);
545
546    //
547    // Extract the first angle, rot.x.
548    //
549
550    rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
551
552    //
553    // Remove the rot.x rotation from M, so that the remaining
554    // rotation, N, is only around two axes, and gimbal lock
555    // cannot occur.
556    //
557
558    Matrix44<T> N;
559    N.rotate (Vec3<T> (-rot.x, 0, 0));
560    N = N * M;
561
562    //
563    // Extract the other two angles, rot.y and rot.z, from N.
564    //
565
566    T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]);
567    rot.y = Math<T>::atan2 (-N[0][2], cy);
568    rot.z = Math<T>::atan2 (-N[1][0], N[1][1]);
569}
570
571
572template <class T>
573void
574extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
575{
576    //
577    // Normalize the local x, y and z axes to remove scaling.
578    //
579
580    Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
581    Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
582    Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
583
584    i.normalize();
585    j.normalize();
586    k.normalize();
587
588    Matrix44<T> M (i[0], i[1], i[2], 0,
589                   j[0], j[1], j[2], 0,
590                   k[0], k[1], k[2], 0,
591                   0,    0,    0,    1);
592
593    //
594    // Extract the first angle, rot.x.
595    //
596
597    rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
598
599    //
600    // Remove the x rotation from M, so that the remaining
601    // rotation, N, is only around two axes, and gimbal lock
602    // cannot occur.
603    //
604
605    Matrix44<T> N;
606    N.rotate (Vec3<T> (0, 0, -rot.x));
607    N = N * M;
608
609    //
610    // Extract the other two angles, rot.y and rot.z, from N.
611    //
612
613    T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]);
614    rot.y = -Math<T>::atan2 (-N[2][0], cy);
615    rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]);
616}
617
618
619template <class T>
620Quat<T>
621extractQuat (const Matrix44<T> &mat)
622{
623  Matrix44<T> rot;
624
625  T        tr, s;
626  T         q[4];
627  int    i, j, k;
628  Quat<T>   quat;
629
630  int nxt[3] = {1, 2, 0};
631  tr = mat[0][0] + mat[1][1] + mat[2][2];
632
633  // check the diagonal
634  if (tr > 0.0) {
635     s = Math<T>::sqrt (tr + 1.0);
636     quat.r = s / 2.0;
637     s = 0.5 / s;
638
639     quat.v.x = (mat[1][2] - mat[2][1]) * s;
640     quat.v.y = (mat[2][0] - mat[0][2]) * s;
641     quat.v.z = (mat[0][1] - mat[1][0]) * s;
642  }
643  else {     
644     // diagonal is negative
645     i = 0;
646     if (mat[1][1] > mat[0][0])
647        i=1;
648     if (mat[2][2] > mat[i][i])
649        i=2;
650   
651     j = nxt[i];
652     k = nxt[j];
653     s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
654     
655     q[i] = s * 0.5;
656     if (s != 0.0)
657        s = 0.5 / s;
658
659     q[3] = (mat[j][k] - mat[k][j]) * s;
660     q[j] = (mat[i][j] + mat[j][i]) * s;
661     q[k] = (mat[i][k] + mat[k][i]) * s;
662
663     quat.v.x = q[0];
664     quat.v.y = q[1];
665     quat.v.z = q[2];
666     quat.r = q[3];
667 }
668
669  return quat;
670}
671
672template <class T>
673bool
674extractSHRT (const Matrix44<T> &mat,
675             Vec3<T> &s,
676             Vec3<T> &h,
677             Vec3<T> &r,
678             Vec3<T> &t,
679             bool exc /* = true */ ,
680             typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
681{
682    Matrix44<T> rot;
683
684    rot = mat;
685    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
686        return false;
687
688    extractEulerXYZ (rot, r);
689
690    t.x = mat[3][0];
691    t.y = mat[3][1];
692    t.z = mat[3][2];
693
694    if (rOrder != Euler<T>::XYZ)
695    {
696        Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
697        Imath::Euler<T> e (eXYZ, rOrder);
698        r = e.toXYZVector ();
699    }
700
701    return true;
702}
703
704template <class T>
705bool
706extractSHRT (const Matrix44<T> &mat,
707             Vec3<T> &s,
708             Vec3<T> &h,
709             Vec3<T> &r,
710             Vec3<T> &t,
711             bool exc)
712{
713    return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
714}
715
716template <class T>
717bool
718extractSHRT (const Matrix44<T> &mat,
719             Vec3<T> &s,
720             Vec3<T> &h,
721             Euler<T> &r,
722             Vec3<T> &t,
723             bool exc /* = true */)
724{
725    return extractSHRT (mat, s, h, r, t, exc, r.order ());
726}
727
728
729template <class T>
730bool           
731checkForZeroScaleInRow (const T& scl,
732                        const Vec3<T> &row,
733                        bool exc /* = true */ )
734{
735    for (int i = 0; i < 3; i++)
736    {
737        if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
738        {
739            if (exc)
740                throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
741                                           "from matrix.");
742            else
743                return false;
744        }
745    }
746
747    return true;
748}
749
750
751template <class T>
752Matrix44<T>
753rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
754{
755    Quat<T> q;
756    q.setRotation(from, to);
757    return q.toMatrix44();
758}
759
760
761template <class T>
762Matrix44<T>     
763rotationMatrixWithUpDir (const Vec3<T> &fromDir,
764                         const Vec3<T> &toDir,
765                         const Vec3<T> &upDir)
766{
767    //
768    // The goal is to obtain a rotation matrix that takes
769    // "fromDir" to "toDir".  We do this in two steps and
770    // compose the resulting rotation matrices;
771    //    (a) rotate "fromDir" into the z-axis
772    //    (b) rotate the z-axis into "toDir"
773    //
774
775    // The from direction must be non-zero; but we allow zero to and up dirs.
776    if (fromDir.length () == 0)
777        return Matrix44<T> ();
778
779    else
780    {
781        Matrix44<T> zAxis2FromDir  = alignZAxisWithTargetDir
782                                         (fromDir, Vec3<T> (0, 1, 0));
783
784        Matrix44<T> fromDir2zAxis  = zAxis2FromDir.transposed ();
785       
786        Matrix44<T> zAxis2ToDir    = alignZAxisWithTargetDir (toDir, upDir);
787
788        return fromDir2zAxis * zAxis2ToDir;
789    }
790}
791
792
793template <class T>
794Matrix44<T>
795alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
796{
797    //
798    // Ensure that the target direction is non-zero.
799    //
800
801    if ( targetDir.length () == 0 )
802        targetDir = Vec3<T> (0, 0, 1);
803
804    //
805    // Ensure that the up direction is non-zero.
806    //
807
808    if ( upDir.length () == 0 )
809        upDir = Vec3<T> (0, 1, 0);
810
811    //
812    // Check for degeneracies.  If the upDir and targetDir are parallel
813    // or opposite, then compute a new, arbitrary up direction that is
814    // not parallel or opposite to the targetDir.
815    //
816
817    if (upDir.cross (targetDir).length () == 0)
818    {
819        upDir = targetDir.cross (Vec3<T> (1, 0, 0));
820        if (upDir.length() == 0)
821            upDir = targetDir.cross(Vec3<T> (0, 0, 1));
822    }
823
824    //
825    // Compute the x-, y-, and z-axis vectors of the new coordinate system.
826    //
827
828    Vec3<T> targetPerpDir = upDir.cross (targetDir);   
829    Vec3<T> targetUpDir   = targetDir.cross (targetPerpDir);
830   
831    //
832    // Rotate the x-axis into targetPerpDir (row 0),
833    // rotate the y-axis into targetUpDir   (row 1),
834    // rotate the z-axis into targetDir     (row 2).
835    //
836   
837    Vec3<T> row[3];
838    row[0] = targetPerpDir.normalized ();
839    row[1] = targetUpDir  .normalized ();
840    row[2] = targetDir    .normalized ();
841   
842    Matrix44<T> mat ( row[0][0],  row[0][1],  row[0][2],  0,
843                      row[1][0],  row[1][1],  row[1][2],  0,
844                      row[2][0],  row[2][1],  row[2][2],  0,
845                      0,          0,          0,          1 );
846   
847    return mat;
848}
849
850
851
852//-----------------------------------------------------------------------------
853// Implementation for 3x3 Matrix
854//------------------------------
855
856
857template <class T>
858bool
859extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
860{
861    T shr;
862    Matrix33<T> M (mat);
863
864    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
865        return false;
866
867    return true;
868}
869
870
871template <class T>
872Matrix33<T>
873sansScaling (const Matrix33<T> &mat, bool exc)
874{
875    Vec2<T> scl;
876    T shr;
877    T rot;
878    Vec2<T> tran;
879
880    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
881        return mat;
882
883    Matrix33<T> M;
884   
885    M.translate (tran);
886    M.rotate (rot);
887    M.shear (shr);
888
889    return M;
890}
891
892
893template <class T>
894bool
895removeScaling (Matrix33<T> &mat, bool exc)
896{
897    Vec2<T> scl;
898    T shr;
899    T rot;
900    Vec2<T> tran;
901
902    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
903        return false;
904
905    mat.makeIdentity ();
906    mat.translate (tran);
907    mat.rotate (rot);
908    mat.shear (shr);
909
910    return true;
911}
912
913
914template <class T>
915bool
916extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
917{
918    Matrix33<T> M (mat);
919
920    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
921        return false;
922
923    return true;
924}
925
926
927template <class T>
928Matrix33<T>
929sansScalingAndShear (const Matrix33<T> &mat, bool exc)
930{
931    Vec2<T> scl;
932    T shr;
933    Matrix33<T> M (mat);
934
935    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
936        return mat;
937   
938    return M;
939}
940
941
942template <class T>
943bool
944removeScalingAndShear (Matrix33<T> &mat, bool exc)
945{
946    Vec2<T> scl;
947    T shr;
948
949    if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
950        return false;
951   
952    return true;
953}
954
955template <class T>
956bool
957extractAndRemoveScalingAndShear (Matrix33<T> &mat,
958                                 Vec2<T> &scl, T &shr, bool exc)
959{
960    Vec2<T> row[2];
961
962    row[0] = Vec2<T> (mat[0][0], mat[0][1]);
963    row[1] = Vec2<T> (mat[1][0], mat[1][1]);
964   
965    T maxVal = 0;
966    for (int i=0; i < 2; i++)
967        for (int j=0; j < 2; j++)
968            if (Imath::abs (row[i][j]) > maxVal)
969                maxVal = Imath::abs (row[i][j]);
970
971    //
972    // We normalize the 2x2 matrix here.
973    // It was noticed that this can improve numerical stability significantly,
974    // especially when many of the upper 2x2 matrix's coefficients are very
975    // close to zero; we correct for this step at the end by multiplying the
976    // scaling factors by maxVal at the end (shear and rotation are not
977    // affected by the normalization).
978
979    if (maxVal != 0)
980    {
981        for (int i=0; i < 2; i++)
982            if (! checkForZeroScaleInRow (maxVal, row[i], exc))
983                return false;
984            else
985                row[i] /= maxVal;
986    }
987
988    // Compute X scale factor.
989    scl.x = row[0].length ();
990    if (! checkForZeroScaleInRow (scl.x, row[0], exc))
991        return false;
992
993    // Normalize first row.
994    row[0] /= scl.x;
995
996    // An XY shear factor will shear the X coord. as the Y coord. changes.
997    // There are 2 combinations (XY, YX), although we only extract the XY
998    // shear factor because we can effect the an YX shear factor by
999    // shearing in XY combined with rotations and scales.
1000    //
1001    // shear matrix <   1,  YX,  0,
1002    //                 XY,   1,  0,
1003    //                  0,   0,  1 >
1004
1005    // Compute XY shear factor and make 2nd row orthogonal to 1st.
1006    shr     = row[0].dot (row[1]);
1007    row[1] -= shr * row[0];
1008
1009    // Now, compute Y scale.
1010    scl.y = row[1].length ();
1011    if (! checkForZeroScaleInRow (scl.y, row[1], exc))
1012        return false;
1013
1014    // Normalize 2nd row and correct the XY shear factor for Y scaling.
1015    row[1] /= scl.y;
1016    shr    /= scl.y;
1017
1018    // At this point, the upper 2x2 matrix in mat is orthonormal.
1019    // Check for a coordinate system flip. If the determinant
1020    // is -1, then flip the rotation matrix and adjust the scale(Y)
1021    // and shear(XY) factors to compensate.
1022    if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
1023    {
1024        row[1][0] *= -1;
1025        row[1][1] *= -1;
1026        scl[1] *= -1;
1027        shr *= -1;
1028    }
1029
1030    // Copy over the orthonormal rows into the returned matrix.
1031    // The upper 2x2 matrix in mat is now a rotation matrix.
1032    for (int i=0; i < 2; i++)
1033    {
1034        mat[i][0] = row[i][0];
1035        mat[i][1] = row[i][1];
1036    }
1037
1038    scl *= maxVal;
1039
1040    return true;
1041}
1042
1043
1044template <class T>
1045void
1046extractEuler (const Matrix33<T> &mat, T &rot)
1047{
1048    //
1049    // Normalize the local x and y axes to remove scaling.
1050    //
1051
1052    Vec2<T> i (mat[0][0], mat[0][1]);
1053    Vec2<T> j (mat[1][0], mat[1][1]);
1054
1055    i.normalize();
1056    j.normalize();
1057
1058    //
1059    // Extract the angle, rot.
1060    //
1061
1062    rot = - Math<T>::atan2 (j[0], i[0]);
1063}
1064
1065
1066template <class T>
1067bool
1068extractSHRT (const Matrix33<T> &mat,
1069             Vec2<T> &s,
1070             T       &h,
1071             T       &r,
1072             Vec2<T> &t,
1073             bool    exc)
1074{
1075    Matrix33<T> rot;
1076
1077    rot = mat;
1078    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
1079        return false;
1080
1081    extractEuler (rot, r);
1082
1083    t.x = mat[2][0];
1084    t.y = mat[2][1];
1085
1086    return true;
1087}
1088
1089
1090template <class T>
1091bool           
1092checkForZeroScaleInRow (const T& scl,
1093                        const Vec2<T> &row,
1094                        bool exc /* = true */ )
1095{
1096    for (int i = 0; i < 2; i++)
1097    {
1098        if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
1099        {
1100            if (exc)
1101                throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
1102                                           "from matrix.");
1103            else
1104                return false;
1105        }
1106    }
1107
1108    return true;
1109}
1110
1111
1112} // namespace Imath
1113
1114#endif
Note: See TracBrowser for help on using the repository browser.