1
///////////////////////////////////////////////////////////////////////////
3
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
6
// All rights reserved.
8
// Redistribution and use in source and binary forms, with or without
9
// modification, are permitted provided that the following conditions are
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
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.
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.
33
///////////////////////////////////////////////////////////////////////////
36
#ifndef INCLUDED_IMATHMATRIXALGO_H
37
#define INCLUDED_IMATHMATRIXALGO_H
39
//-------------------------------------------------------------------------
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.
47
// This file also defines a few predefined constant matrices.
49
//-------------------------------------------------------------------------
51
#include "ImathMatrix.h"
52
#include "ImathQuat.h"
53
#include "ImathEuler.h"
61
#define IMATH_EXPORT_CONST extern __declspec(dllexport)
63
#define IMATH_EXPORT_CONST extern __declspec(dllimport)
66
#define IMATH_EXPORT_CONST extern const
76
IMATH_EXPORT_CONST M33f identity33f;
77
IMATH_EXPORT_CONST M44f identity44f;
78
IMATH_EXPORT_CONST M33d identity33d;
79
IMATH_EXPORT_CONST M44d identity44d;
81
//----------------------------------------------------------------------
82
// Extract scale, shear, rotation, and translation values from a matrix:
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.
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:
94
// If exc is true, the functions throw an Imath::ZeroScale exception.
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,
106
// checkForZeroScaleInRow () returns false
107
// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid
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...() .
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.
122
//----------------------------------------------------------------------
126
// Declarations for 4x4 matrix.
129
template <class T> bool extractScaling
130
(const Matrix44<T> &mat,
134
template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat,
137
template <class T> bool removeScaling
141
template <class T> bool extractScalingAndShear
142
(const Matrix44<T> &mat,
147
template <class T> Matrix44<T> sansScalingAndShear
148
(const Matrix44<T> &mat,
151
template <class T> bool removeScalingAndShear
155
template <class T> bool extractAndRemoveScalingAndShear
161
template <class T> void extractEulerXYZ
162
(const Matrix44<T> &mat,
165
template <class T> void extractEulerZYX
166
(const Matrix44<T> &mat,
169
template <class T> Quat<T> extractQuat (const Matrix44<T> &mat);
171
template <class T> bool extractSHRT
172
(const Matrix44<T> &mat,
178
typename Euler<T>::Order rOrder);
180
template <class T> bool extractSHRT
181
(const Matrix44<T> &mat,
188
template <class T> bool extractSHRT
189
(const Matrix44<T> &mat,
197
// Internal utility function.
200
template <class T> bool checkForZeroScaleInRow
206
// Returns a matrix that rotates "fromDirection" vector to "toDirection"
210
template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection,
211
const Vec3<T> &toDirection);
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".
222
template <class T> Matrix44<T> rotationMatrixWithUpDir
223
(const Vec3<T> &fromDir,
224
const Vec3<T> &toDir,
225
const Vec3<T> &upDir);
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".
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
241
template <class T> Matrix44<T> alignZAxisWithTargetDir
246
//----------------------------------------------------------------------
250
// Declarations for 3x3 matrix.
254
template <class T> bool extractScaling
255
(const Matrix33<T> &mat,
259
template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat,
262
template <class T> bool removeScaling
266
template <class T> bool extractScalingAndShear
267
(const Matrix33<T> &mat,
272
template <class T> Matrix33<T> sansScalingAndShear
273
(const Matrix33<T> &mat,
276
template <class T> bool removeScalingAndShear
280
template <class T> bool extractAndRemoveScalingAndShear
286
template <class T> void extractEuler
287
(const Matrix33<T> &mat,
290
template <class T> bool extractSHRT (const Matrix33<T> &mat,
297
template <class T> bool checkForZeroScaleInRow
305
//-----------------------------------------------------------------------------
306
// Implementation for 4x4 Matrix
307
//------------------------------
312
extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
317
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
326
sansScaling (const Matrix44<T> &mat, bool exc)
333
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
348
removeScaling (Matrix44<T> &mat, bool exc)
355
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
359
mat.translate (tran);
369
extractScalingAndShear (const Matrix44<T> &mat,
370
Vec3<T> &scl, Vec3<T> &shr, bool exc)
374
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
383
sansScalingAndShear (const Matrix44<T> &mat, bool exc)
389
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
398
removeScalingAndShear (Matrix44<T> &mat, bool exc)
403
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
412
extractAndRemoveScalingAndShear (Matrix44<T> &mat,
413
Vec3<T> &scl, Vec3<T> &shr, bool exc)
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.
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]);
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]);
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).
443
for (int i=0; i < 3; i++)
444
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
450
// Compute X scale factor.
451
scl.x = row[0].length ();
452
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
455
// Normalize first row.
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.
463
// shear matrix < 1, YX, ZX, 0,
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];
472
// Now, compute Y scale.
473
scl.y = row[1].length ();
474
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
477
// Normalize 2nd row and correct the XY shear factor for Y scaling.
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];
487
// Next, get Z scale.
488
scl.z = row[2].length ();
489
if (! checkForZeroScaleInRow (scl.z, row[2], exc))
492
// Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
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++)
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++)
511
mat[i][0] = row[i][0];
512
mat[i][1] = row[i][1];
513
mat[i][2] = row[i][2];
516
// Correct the scaling factors for the normalization step that we
517
// performed above; shear and rotation are not affected by the
527
extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
530
// Normalize the local x, y and z axes to remove scaling.
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]);
541
Matrix44<T> M (i[0], i[1], i[2], 0,
547
// Extract the first angle, rot.x.
550
rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
553
// Remove the rot.x rotation from M, so that the remaining
554
// rotation, N, is only around two axes, and gimbal lock
559
N.rotate (Vec3<T> (-rot.x, 0, 0));
563
// Extract the other two angles, rot.y and rot.z, from N.
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]);
574
extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
577
// Normalize the local x, y and z axes to remove scaling.
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]);
588
Matrix44<T> M (i[0], i[1], i[2], 0,
594
// Extract the first angle, rot.x.
597
rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
600
// Remove the x rotation from M, so that the remaining
601
// rotation, N, is only around two axes, and gimbal lock
606
N.rotate (Vec3<T> (0, 0, -rot.x));
610
// Extract the other two angles, rot.y and rot.z, from N.
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]);
621
extractQuat (const Matrix44<T> &mat)
630
int nxt[3] = {1, 2, 0};
631
tr = mat[0][0] + mat[1][1] + mat[2][2];
633
// check the diagonal
635
s = Math<T>::sqrt (tr + 1.0);
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;
644
// diagonal is negative
646
if (mat[1][1] > mat[0][0])
648
if (mat[2][2] > mat[i][i])
653
s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
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;
674
extractSHRT (const Matrix44<T> &mat,
679
bool exc /* = true */ ,
680
typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
685
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
688
extractEulerXYZ (rot, r);
694
if (rOrder != Euler<T>::XYZ)
696
Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
697
Imath::Euler<T> e (eXYZ, rOrder);
698
r = e.toXYZVector ();
706
extractSHRT (const Matrix44<T> &mat,
713
return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
718
extractSHRT (const Matrix44<T> &mat,
723
bool exc /* = true */)
725
return extractSHRT (mat, s, h, r, t, exc, r.order ());
731
checkForZeroScaleInRow (const T& scl,
733
bool exc /* = true */ )
735
for (int i = 0; i < 3; i++)
737
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
740
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
753
rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
756
q.setRotation(from, to);
757
return q.toMatrix44();
763
rotationMatrixWithUpDir (const Vec3<T> &fromDir,
764
const Vec3<T> &toDir,
765
const Vec3<T> &upDir)
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"
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> ();
781
Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir
782
(fromDir, Vec3<T> (0, 1, 0));
784
Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed ();
786
Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir);
788
return fromDir2zAxis * zAxis2ToDir;
795
alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
798
// Ensure that the target direction is non-zero.
801
if ( targetDir.length () == 0 )
802
targetDir = Vec3<T> (0, 0, 1);
805
// Ensure that the up direction is non-zero.
808
if ( upDir.length () == 0 )
809
upDir = Vec3<T> (0, 1, 0);
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.
817
if (upDir.cross (targetDir).length () == 0)
819
upDir = targetDir.cross (Vec3<T> (1, 0, 0));
820
if (upDir.length() == 0)
821
upDir = targetDir.cross(Vec3<T> (0, 0, 1));
825
// Compute the x-, y-, and z-axis vectors of the new coordinate system.
828
Vec3<T> targetPerpDir = upDir.cross (targetDir);
829
Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
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).
838
row[0] = targetPerpDir.normalized ();
839
row[1] = targetUpDir .normalized ();
840
row[2] = targetDir .normalized ();
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,
852
//-----------------------------------------------------------------------------
853
// Implementation for 3x3 Matrix
854
//------------------------------
859
extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
864
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
873
sansScaling (const Matrix33<T> &mat, bool exc)
880
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
895
removeScaling (Matrix33<T> &mat, bool exc)
902
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
906
mat.translate (tran);
916
extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
920
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
929
sansScalingAndShear (const Matrix33<T> &mat, bool exc)
935
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
944
removeScalingAndShear (Matrix33<T> &mat, bool exc)
949
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
957
extractAndRemoveScalingAndShear (Matrix33<T> &mat,
958
Vec2<T> &scl, T &shr, bool exc)
962
row[0] = Vec2<T> (mat[0][0], mat[0][1]);
963
row[1] = Vec2<T> (mat[1][0], mat[1][1]);
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]);
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).
981
for (int i=0; i < 2; i++)
982
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
988
// Compute X scale factor.
989
scl.x = row[0].length ();
990
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
993
// Normalize first row.
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.
1001
// shear matrix < 1, YX, 0,
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];
1009
// Now, compute Y scale.
1010
scl.y = row[1].length ();
1011
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
1014
// Normalize 2nd row and correct the XY shear factor for Y scaling.
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)
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++)
1034
mat[i][0] = row[i][0];
1035
mat[i][1] = row[i][1];
1046
extractEuler (const Matrix33<T> &mat, T &rot)
1049
// Normalize the local x and y axes to remove scaling.
1052
Vec2<T> i (mat[0][0], mat[0][1]);
1053
Vec2<T> j (mat[1][0], mat[1][1]);
1059
// Extract the angle, rot.
1062
rot = - Math<T>::atan2 (j[0], i[0]);
1068
extractSHRT (const Matrix33<T> &mat,
1078
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
1081
extractEuler (rot, r);
1092
checkForZeroScaleInRow (const T& scl,
1094
bool exc /* = true */ )
1096
for (int i = 0; i < 2; i++)
1098
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
1101
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
1112
} // namespace Imath
1
///////////////////////////////////////////////////////////////////////////
3
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
6
// All rights reserved.
8
// Redistribution and use in source and binary forms, with or without
9
// modification, are permitted provided that the following conditions are
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
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.
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.
33
///////////////////////////////////////////////////////////////////////////
36
#ifndef INCLUDED_IMATHMATRIXALGO_H
37
#define INCLUDED_IMATHMATRIXALGO_H
39
//-------------------------------------------------------------------------
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.
47
// This file also defines a few predefined constant matrices.
49
//-------------------------------------------------------------------------
51
#include "ImathMatrix.h"
52
#include "ImathQuat.h"
53
#include "ImathEuler.h"
61
#define IMATH_EXPORT_CONST extern __declspec(dllexport)
63
#define IMATH_EXPORT_CONST extern __declspec(dllimport)
66
#define IMATH_EXPORT_CONST extern const
76
IMATH_EXPORT_CONST M33f identity33f;
77
IMATH_EXPORT_CONST M44f identity44f;
78
IMATH_EXPORT_CONST M33d identity33d;
79
IMATH_EXPORT_CONST M44d identity44d;
81
//----------------------------------------------------------------------
82
// Extract scale, shear, rotation, and translation values from a matrix:
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.
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:
94
// If exc is true, the functions throw an Imath::ZeroScale exception.
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,
106
// checkForZeroScaleInRow () returns false
107
// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid
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...() .
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.
122
//----------------------------------------------------------------------
126
// Declarations for 4x4 matrix.
129
template <class T> bool extractScaling
130
(const Matrix44<T> &mat,
134
template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat,
137
template <class T> bool removeScaling
141
template <class T> bool extractScalingAndShear
142
(const Matrix44<T> &mat,
147
template <class T> Matrix44<T> sansScalingAndShear
148
(const Matrix44<T> &mat,
151
template <class T> bool removeScalingAndShear
155
template <class T> bool extractAndRemoveScalingAndShear
161
template <class T> void extractEulerXYZ
162
(const Matrix44<T> &mat,
165
template <class T> void extractEulerZYX
166
(const Matrix44<T> &mat,
169
template <class T> Quat<T> extractQuat (const Matrix44<T> &mat);
171
template <class T> bool extractSHRT
172
(const Matrix44<T> &mat,
178
typename Euler<T>::Order rOrder);
180
template <class T> bool extractSHRT
181
(const Matrix44<T> &mat,
188
template <class T> bool extractSHRT
189
(const Matrix44<T> &mat,
197
// Internal utility function.
200
template <class T> bool checkForZeroScaleInRow
206
// Returns a matrix that rotates "fromDirection" vector to "toDirection"
210
template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection,
211
const Vec3<T> &toDirection);
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".
222
template <class T> Matrix44<T> rotationMatrixWithUpDir
223
(const Vec3<T> &fromDir,
224
const Vec3<T> &toDir,
225
const Vec3<T> &upDir);
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".
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
241
template <class T> Matrix44<T> alignZAxisWithTargetDir
246
//----------------------------------------------------------------------
250
// Declarations for 3x3 matrix.
254
template <class T> bool extractScaling
255
(const Matrix33<T> &mat,
259
template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat,
262
template <class T> bool removeScaling
266
template <class T> bool extractScalingAndShear
267
(const Matrix33<T> &mat,
272
template <class T> Matrix33<T> sansScalingAndShear
273
(const Matrix33<T> &mat,
276
template <class T> bool removeScalingAndShear
280
template <class T> bool extractAndRemoveScalingAndShear
286
template <class T> void extractEuler
287
(const Matrix33<T> &mat,
290
template <class T> bool extractSHRT (const Matrix33<T> &mat,
297
template <class T> bool checkForZeroScaleInRow
305
//-----------------------------------------------------------------------------
306
// Implementation for 4x4 Matrix
307
//------------------------------
312
extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
317
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
326
sansScaling (const Matrix44<T> &mat, bool exc)
333
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
348
removeScaling (Matrix44<T> &mat, bool exc)
355
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
359
mat.translate (tran);
369
extractScalingAndShear (const Matrix44<T> &mat,
370
Vec3<T> &scl, Vec3<T> &shr, bool exc)
374
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
383
sansScalingAndShear (const Matrix44<T> &mat, bool exc)
389
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
398
removeScalingAndShear (Matrix44<T> &mat, bool exc)
403
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
412
extractAndRemoveScalingAndShear (Matrix44<T> &mat,
413
Vec3<T> &scl, Vec3<T> &shr, bool exc)
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.
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]);
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]);
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).
443
for (int i=0; i < 3; i++)
444
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
450
// Compute X scale factor.
451
scl.x = row[0].length ();
452
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
455
// Normalize first row.
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.
463
// shear matrix < 1, YX, ZX, 0,
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];
472
// Now, compute Y scale.
473
scl.y = row[1].length ();
474
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
477
// Normalize 2nd row and correct the XY shear factor for Y scaling.
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];
487
// Next, get Z scale.
488
scl.z = row[2].length ();
489
if (! checkForZeroScaleInRow (scl.z, row[2], exc))
492
// Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
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++)
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++)
511
mat[i][0] = row[i][0];
512
mat[i][1] = row[i][1];
513
mat[i][2] = row[i][2];
516
// Correct the scaling factors for the normalization step that we
517
// performed above; shear and rotation are not affected by the
527
extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
530
// Normalize the local x, y and z axes to remove scaling.
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]);
541
Matrix44<T> M (i[0], i[1], i[2], 0,
547
// Extract the first angle, rot.x.
550
rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
553
// Remove the rot.x rotation from M, so that the remaining
554
// rotation, N, is only around two axes, and gimbal lock
559
N.rotate (Vec3<T> (-rot.x, 0, 0));
563
// Extract the other two angles, rot.y and rot.z, from N.
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]);
574
extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
577
// Normalize the local x, y and z axes to remove scaling.
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]);
588
Matrix44<T> M (i[0], i[1], i[2], 0,
594
// Extract the first angle, rot.x.
597
rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
600
// Remove the x rotation from M, so that the remaining
601
// rotation, N, is only around two axes, and gimbal lock
606
N.rotate (Vec3<T> (0, 0, -rot.x));
610
// Extract the other two angles, rot.y and rot.z, from N.
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]);
621
extractQuat (const Matrix44<T> &mat)
630
int nxt[3] = {1, 2, 0};
631
tr = mat[0][0] + mat[1][1] + mat[2][2];
633
// check the diagonal
635
s = Math<T>::sqrt (tr + 1.0);
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;
644
// diagonal is negative
646
if (mat[1][1] > mat[0][0])
648
if (mat[2][2] > mat[i][i])
653
s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
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;
674
extractSHRT (const Matrix44<T> &mat,
679
bool exc /* = true */ ,
680
typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
685
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
688
extractEulerXYZ (rot, r);
694
if (rOrder != Euler<T>::XYZ)
696
Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
697
Imath::Euler<T> e (eXYZ, rOrder);
698
r = e.toXYZVector ();
706
extractSHRT (const Matrix44<T> &mat,
713
return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
718
extractSHRT (const Matrix44<T> &mat,
723
bool exc /* = true */)
725
return extractSHRT (mat, s, h, r, t, exc, r.order ());
731
checkForZeroScaleInRow (const T& scl,
733
bool exc /* = true */ )
735
for (int i = 0; i < 3; i++)
737
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
740
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
753
rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
756
q.setRotation(from, to);
757
return q.toMatrix44();
763
rotationMatrixWithUpDir (const Vec3<T> &fromDir,
764
const Vec3<T> &toDir,
765
const Vec3<T> &upDir)
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"
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> ();
781
Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir
782
(fromDir, Vec3<T> (0, 1, 0));
784
Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed ();
786
Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir);
788
return fromDir2zAxis * zAxis2ToDir;
795
alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
798
// Ensure that the target direction is non-zero.
801
if ( targetDir.length () == 0 )
802
targetDir = Vec3<T> (0, 0, 1);
805
// Ensure that the up direction is non-zero.
808
if ( upDir.length () == 0 )
809
upDir = Vec3<T> (0, 1, 0);
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.
817
if (upDir.cross (targetDir).length () == 0)
819
upDir = targetDir.cross (Vec3<T> (1, 0, 0));
820
if (upDir.length() == 0)
821
upDir = targetDir.cross(Vec3<T> (0, 0, 1));
825
// Compute the x-, y-, and z-axis vectors of the new coordinate system.
828
Vec3<T> targetPerpDir = upDir.cross (targetDir);
829
Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
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).
838
row[0] = targetPerpDir.normalized ();
839
row[1] = targetUpDir .normalized ();
840
row[2] = targetDir .normalized ();
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,
852
//-----------------------------------------------------------------------------
853
// Implementation for 3x3 Matrix
854
//------------------------------
859
extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
864
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
873
sansScaling (const Matrix33<T> &mat, bool exc)
880
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
895
removeScaling (Matrix33<T> &mat, bool exc)
902
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
906
mat.translate (tran);
916
extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
920
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
929
sansScalingAndShear (const Matrix33<T> &mat, bool exc)
935
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
944
removeScalingAndShear (Matrix33<T> &mat, bool exc)
949
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
957
extractAndRemoveScalingAndShear (Matrix33<T> &mat,
958
Vec2<T> &scl, T &shr, bool exc)
962
row[0] = Vec2<T> (mat[0][0], mat[0][1]);
963
row[1] = Vec2<T> (mat[1][0], mat[1][1]);
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]);
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).
981
for (int i=0; i < 2; i++)
982
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
988
// Compute X scale factor.
989
scl.x = row[0].length ();
990
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
993
// Normalize first row.
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.
1001
// shear matrix < 1, YX, 0,
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];
1009
// Now, compute Y scale.
1010
scl.y = row[1].length ();
1011
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
1014
// Normalize 2nd row and correct the XY shear factor for Y scaling.
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)
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++)
1034
mat[i][0] = row[i][0];
1035
mat[i][1] = row[i][1];
1046
extractEuler (const Matrix33<T> &mat, T &rot)
1049
// Normalize the local x and y axes to remove scaling.
1052
Vec2<T> i (mat[0][0], mat[0][1]);
1053
Vec2<T> j (mat[1][0], mat[1][1]);
1059
// Extract the angle, rot.
1062
rot = - Math<T>::atan2 (j[0], i[0]);
1068
extractSHRT (const Matrix33<T> &mat,
1078
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
1081
extractEuler (rot, r);
1092
checkForZeroScaleInRow (const T& scl,
1094
bool exc /* = true */ )
1096
for (int i = 0; i < 2; i++)
1098
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
1101
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
1112
} // namespace Imath
added
removed
Source/OpenEXR/Imath/ImathMatrixAlgo.h
