Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity
Open Source Computer Vision Library https://opencv.org/
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
100 Commits
6 Branches
127 Tags
3.0 GiB
 
 
 
 
 
 
Tag: Branch: Tree: d5ef5b88f0
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd5ef5b88f0'
${ noResults }
opencv/3rdparty/include/OpenEXR/ImathMatrixAlgo.h

1114 lines
27 KiB
Raw Blame History

///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// * Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
#ifndef INCLUDED_IMATHMATRIXALGO_H
#define INCLUDED_IMATHMATRIXALGO_H
//-------------------------------------------------------------------------
//
// This file contains algorithms applied to or in conjunction with
// transformation matrices (Imath::Matrix33 and Imath::Matrix44).
// The assumption made is that these functions are called much less
// often than the basic point functions or these functions require
// more support classes.
//
// This file also defines a few predefined constant matrices.
//
//-------------------------------------------------------------------------
#include "ImathMatrix.h"
#include "ImathQuat.h"
#include "ImathEuler.h"
#include "ImathExc.h"
#include "ImathVec.h"
#include <math.h>
#ifdef OPENEXR_DLL
#ifdef IMATH_EXPORTS
#define IMATH_EXPORT_CONST extern __declspec(dllexport)
#else
#define IMATH_EXPORT_CONST extern __declspec(dllimport)
#endif
#else
#define IMATH_EXPORT_CONST extern const
#endif
namespace Imath {
//------------------
// Identity matrices
//------------------
IMATH_EXPORT_CONST M33f identity33f;
IMATH_EXPORT_CONST M44f identity44f;
IMATH_EXPORT_CONST M33d identity33d;
IMATH_EXPORT_CONST M44d identity44d;
//----------------------------------------------------------------------
// Extract scale, shear, rotation, and translation values from a matrix:
//
// Notes:
//
// This implementation follows the technique described in the paper by
// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
// Matrix into Simple Transformations", p. 320.
//
// - Some of the functions below have an optional exc parameter
// that determines the functions' behavior when the matrix'
// scaling is very close to zero:
//
// If exc is true, the functions throw an Imath::ZeroScale exception.
//
// If exc is false:
//
// extractScaling (m, s) returns false, s is invalid
// sansScaling (m) returns m
// removeScaling (m) returns false, m is unchanged
// sansScalingAndShear (m) returns m
// removeScalingAndShear (m) returns false, m is unchanged
// extractAndRemoveScalingAndShear (m, s, h)
// returns false, m is unchanged,
// (sh) are invalid
// checkForZeroScaleInRow () returns false
// extractSHRT (m, s, h, r, t) returns false, (shrt) are invalid
//
// - Functions extractEuler(), extractEulerXYZ() and extractEulerZYX()
// assume that the matrix does not include shear or non-uniform scaling,
// but they do not examine the matrix to verify this assumption.
// Matrices with shear or non-uniform scaling are likely to produce
// meaningless results. Therefore, you should use the
// removeScalingAndShear() routine, if necessary, prior to calling
// extractEuler...() .
//
// - All functions assume that the matrix does not include perspective
// transformation(s), but they do not examine the matrix to verify
// this assumption. Matrices with perspective transformations are
// likely to produce meaningless results.
//
//----------------------------------------------------------------------
//
// Declarations for 4x4 matrix.
//
template <class T> bool extractScaling
(const Matrix44<T> &mat,
Vec3<T> &scl,
bool exc = true);
template <class T> Matrix44<T> sansScaling (const Matrix44<T> &mat,
bool exc = true);
template <class T> bool removeScaling
(Matrix44<T> &mat,
bool exc = true);
template <class T> bool extractScalingAndShear
(const Matrix44<T> &mat,
Vec3<T> &scl,
Vec3<T> &shr,
bool exc = true);
template <class T> Matrix44<T> sansScalingAndShear
(const Matrix44<T> &mat,
bool exc = true);
template <class T> bool removeScalingAndShear
(Matrix44<T> &mat,
bool exc = true);
template <class T> bool extractAndRemoveScalingAndShear
(Matrix44<T> &mat,
Vec3<T> &scl,
Vec3<T> &shr,
bool exc = true);
template <class T> void extractEulerXYZ
(const Matrix44<T> &mat,
Vec3<T> &rot);
template <class T> void extractEulerZYX
(const Matrix44<T> &mat,
Vec3<T> &rot);
template <class T> Quat<T> extractQuat (const Matrix44<T> &mat);
template <class T> bool extractSHRT
(const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Vec3<T> &r,
Vec3<T> &t,
bool exc /*= true*/,
typename Euler<T>::Order rOrder);
template <class T> bool extractSHRT
(const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Vec3<T> &r,
Vec3<T> &t,
bool exc = true);
template <class T> bool extractSHRT
(const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Euler<T> &r,
Vec3<T> &t,
bool exc = true);
//
// Internal utility function.
//
template <class T> bool checkForZeroScaleInRow
(const T &scl,
const Vec3<T> &row,
bool exc = true);
//
// Returns a matrix that rotates "fromDirection" vector to "toDirection"
// vector.
//
template <class T> Matrix44<T> rotationMatrix (const Vec3<T> &fromDirection,
const Vec3<T> &toDirection);
//
// Returns a matrix that rotates the "fromDir" vector
// so that it points towards "toDir". You may also
// specify that you want the up vector to be pointing
// in a certain direction "upDir".
//
template <class T> Matrix44<T> rotationMatrixWithUpDir
(const Vec3<T> &fromDir,
const Vec3<T> &toDir,
const Vec3<T> &upDir);
//
// Returns a matrix that rotates the z-axis so that it
// points towards "targetDir". You must also specify
// that you want the up vector to be pointing in a
// certain direction "upDir".
//
// Notes: The following degenerate cases are handled:
// (a) when the directions given by "toDir" and "upDir"
// are parallel or opposite;
// (the direction vectors must have a non-zero cross product)
// (b) when any of the given direction vectors have zero length
//
template <class T> Matrix44<T> alignZAxisWithTargetDir
(Vec3<T> targetDir,
Vec3<T> upDir);
//----------------------------------------------------------------------
//
// Declarations for 3x3 matrix.
//
template <class T> bool extractScaling
(const Matrix33<T> &mat,
Vec2<T> &scl,
bool exc = true);
template <class T> Matrix33<T> sansScaling (const Matrix33<T> &mat,
bool exc = true);
template <class T> bool removeScaling
(Matrix33<T> &mat,
bool exc = true);
template <class T> bool extractScalingAndShear
(const Matrix33<T> &mat,
Vec2<T> &scl,
T &h,
bool exc = true);
template <class T> Matrix33<T> sansScalingAndShear
(const Matrix33<T> &mat,
bool exc = true);
template <class T> bool removeScalingAndShear
(Matrix33<T> &mat,
bool exc = true);
template <class T> bool extractAndRemoveScalingAndShear
(Matrix33<T> &mat,
Vec2<T> &scl,
T &shr,
bool exc = true);
template <class T> void extractEuler
(const Matrix33<T> &mat,
T &rot);
template <class T> bool extractSHRT (const Matrix33<T> &mat,
Vec2<T> &s,
T &h,
T &r,
Vec2<T> &t,
bool exc = true);
template <class T> bool checkForZeroScaleInRow
(const T &scl,
const Vec2<T> &row,
bool exc = true);
//-----------------------------------------------------------------------------
// Implementation for 4x4 Matrix
//------------------------------
template <class T>
bool
extractScaling (const Matrix44<T> &mat, Vec3<T> &scl, bool exc)
{
Vec3<T> shr;
Matrix44<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix44<T>
sansScaling (const Matrix44<T> &mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Vec3<T> rot;
Vec3<T> tran;
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
return mat;
Matrix44<T> M;
M.translate (tran);
M.rotate (rot);
M.shear (shr);
return M;
}
template <class T>
bool
removeScaling (Matrix44<T> &mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Vec3<T> rot;
Vec3<T> tran;
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
return false;
mat.makeIdentity ();
mat.translate (tran);
mat.rotate (rot);
mat.shear (shr);
return true;
}
template <class T>
bool
extractScalingAndShear (const Matrix44<T> &mat,
Vec3<T> &scl, Vec3<T> &shr, bool exc)
{
Matrix44<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix44<T>
sansScalingAndShear (const Matrix44<T> &mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
Matrix44<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return mat;
return M;
}
template <class T>
bool
removeScalingAndShear (Matrix44<T> &mat, bool exc)
{
Vec3<T> scl;
Vec3<T> shr;
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
return false;
return true;
}
template <class T>
bool
extractAndRemoveScalingAndShear (Matrix44<T> &mat,
Vec3<T> &scl, Vec3<T> &shr, bool exc)
{
//
// This implementation follows the technique described in the paper by
// Spencer W. Thomas in the Graphics Gems II article: "Decomposing a
// Matrix into Simple Transformations", p. 320.
//
Vec3<T> row[3];
row[0] = Vec3<T> (mat[0][0], mat[0][1], mat[0][2]);
row[1] = Vec3<T> (mat[1][0], mat[1][1], mat[1][2]);
row[2] = Vec3<T> (mat[2][0], mat[2][1], mat[2][2]);
T maxVal = 0;
for (int i=0; i < 3; i++)
for (int j=0; j < 3; j++)
if (Imath::abs (row[i][j]) > maxVal)
maxVal = Imath::abs (row[i][j]);
//
// We normalize the 3x3 matrix here.
// It was noticed that this can improve numerical stability significantly,
// especially when many of the upper 3x3 matrix's coefficients are very
// close to zero; we correct for this step at the end by multiplying the
// scaling factors by maxVal at the end (shear and rotation are not
// affected by the normalization).
if (maxVal != 0)
{
for (int i=0; i < 3; i++)
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
return false;
else
row[i] /= maxVal;
}
// Compute X scale factor.
scl.x = row[0].length ();
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
return false;
// Normalize first row.
row[0] /= scl.x;
// An XY shear factor will shear the X coord. as the Y coord. changes.
// There are 6 combinations (XY, XZ, YZ, YX, ZX, ZY), although we only
// extract the first 3 because we can effect the last 3 by shearing in
// XY, XZ, YZ combined rotations and scales.
//
// shear matrix < 1, YX, ZX, 0,
// XY, 1, ZY, 0,
// XZ, YZ, 1, 0,
// 0, 0, 0, 1 >
// Compute XY shear factor and make 2nd row orthogonal to 1st.
shr[0] = row[0].dot (row[1]);
row[1] -= shr[0] * row[0];
// Now, compute Y scale.
scl.y = row[1].length ();
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
return false;
// Normalize 2nd row and correct the XY shear factor for Y scaling.
row[1] /= scl.y;
shr[0] /= scl.y;
// Compute XZ and YZ shears, orthogonalize 3rd row.
shr[1] = row[0].dot (row[2]);
row[2] -= shr[1] * row[0];
shr[2] = row[1].dot (row[2]);
row[2] -= shr[2] * row[1];
// Next, get Z scale.
scl.z = row[2].length ();
if (! checkForZeroScaleInRow (scl.z, row[2], exc))
return false;
// Normalize 3rd row and correct the XZ and YZ shear factors for Z scaling.
row[2] /= scl.z;
shr[1] /= scl.z;
shr[2] /= scl.z;
// At this point, the upper 3x3 matrix in mat is orthonormal.
// Check for a coordinate system flip. If the determinant
// is less than zero, then negate the matrix and the scaling factors.
if (row[0].dot (row[1].cross (row[2])) < 0)
for (int i=0; i < 3; i++)
{
scl[i] *= -1;
row[i] *= -1;
}
// Copy over the orthonormal rows into the returned matrix.
// The upper 3x3 matrix in mat is now a rotation matrix.
for (int i=0; i < 3; i++)
{
mat[i][0] = row[i][0];
mat[i][1] = row[i][1];
mat[i][2] = row[i][2];
}
// Correct the scaling factors for the normalization step that we
// performed above; shear and rotation are not affected by the
// normalization.
scl *= maxVal;
return true;
}
template <class T>
void
extractEulerXYZ (const Matrix44<T> &mat, Vec3<T> &rot)
{
//
// Normalize the local x, y and z axes to remove scaling.
//
Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
i.normalize();
j.normalize();
k.normalize();
Matrix44<T> M (i[0], i[1], i[2], 0,
j[0], j[1], j[2], 0,
k[0], k[1], k[2], 0,
0, 0, 0, 1);
//
// Extract the first angle, rot.x.
//
rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
//
// Remove the rot.x rotation from M, so that the remaining
// rotation, N, is only around two axes, and gimbal lock
// cannot occur.
//
Matrix44<T> N;
N.rotate (Vec3<T> (-rot.x, 0, 0));
N = N * M;
//
// Extract the other two angles, rot.y and rot.z, from N.
//
T cy = Math<T>::sqrt (N[0][0]*N[0][0] + N[0][1]*N[0][1]);
rot.y = Math<T>::atan2 (-N[0][2], cy);
rot.z = Math<T>::atan2 (-N[1][0], N[1][1]);
}
template <class T>
void
extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
{
//
// Normalize the local x, y and z axes to remove scaling.
//
Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
i.normalize();
j.normalize();
k.normalize();
Matrix44<T> M (i[0], i[1], i[2], 0,
j[0], j[1], j[2], 0,
k[0], k[1], k[2], 0,
0, 0, 0, 1);
//
// Extract the first angle, rot.x.
//
rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
//
// Remove the x rotation from M, so that the remaining
// rotation, N, is only around two axes, and gimbal lock
// cannot occur.
//
Matrix44<T> N;
N.rotate (Vec3<T> (0, 0, -rot.x));
N = N * M;
//
// Extract the other two angles, rot.y and rot.z, from N.
//
T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]);
rot.y = -Math<T>::atan2 (-N[2][0], cy);
rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]);
}
template <class T>
Quat<T>
extractQuat (const Matrix44<T> &mat)
{
Matrix44<T> rot;
T tr, s;
T q[4];
int i, j, k;
Quat<T> quat;
int nxt[3] = {1, 2, 0};
tr = mat[0][0] + mat[1][1] + mat[2][2];
// check the diagonal
if (tr > 0.0) {
s = Math<T>::sqrt (tr + 1.0);
quat.r = s / 2.0;
s = 0.5 / s;
quat.v.x = (mat[1][2] - mat[2][1]) * s;
quat.v.y = (mat[2][0] - mat[0][2]) * s;
quat.v.z = (mat[0][1] - mat[1][0]) * s;
}
else {
// diagonal is negative
i = 0;
if (mat[1][1] > mat[0][0])
i=1;
if (mat[2][2] > mat[i][i])
i=2;
j = nxt[i];
k = nxt[j];
s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
q[i] = s * 0.5;
if (s != 0.0)
s = 0.5 / s;
q[3] = (mat[j][k] - mat[k][j]) * s;
q[j] = (mat[i][j] + mat[j][i]) * s;
q[k] = (mat[i][k] + mat[k][i]) * s;
quat.v.x = q[0];
quat.v.y = q[1];
quat.v.z = q[2];
quat.r = q[3];
}
return quat;
}
template <class T>
bool
extractSHRT (const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Vec3<T> &r,
Vec3<T> &t,
bool exc /* = true */ ,
typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
{
Matrix44<T> rot;
rot = mat;
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
return false;
extractEulerXYZ (rot, r);
t.x = mat[3][0];
t.y = mat[3][1];
t.z = mat[3][2];
if (rOrder != Euler<T>::XYZ)
{
Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
Imath::Euler<T> e (eXYZ, rOrder);
r = e.toXYZVector ();
}
return true;
}
template <class T>
bool
extractSHRT (const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Vec3<T> &r,
Vec3<T> &t,
bool exc)
{
return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
}
template <class T>
bool
extractSHRT (const Matrix44<T> &mat,
Vec3<T> &s,
Vec3<T> &h,
Euler<T> &r,
Vec3<T> &t,
bool exc /* = true */)
{
return extractSHRT (mat, s, h, r, t, exc, r.order ());
}
template <class T>
bool
checkForZeroScaleInRow (const T& scl,
const Vec3<T> &row,
bool exc /* = true */ )
{
for (int i = 0; i < 3; i++)
{
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
{
if (exc)
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
"from matrix.");
else
return false;
}
}
return true;
}
template <class T>
Matrix44<T>
rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
{
Quat<T> q;
q.setRotation(from, to);
return q.toMatrix44();
}
template <class T>
Matrix44<T>
rotationMatrixWithUpDir (const Vec3<T> &fromDir,
const Vec3<T> &toDir,
const Vec3<T> &upDir)
{
//
// The goal is to obtain a rotation matrix that takes
// "fromDir" to "toDir". We do this in two steps and
// compose the resulting rotation matrices;
// (a) rotate "fromDir" into the z-axis
// (b) rotate the z-axis into "toDir"
//
// The from direction must be non-zero; but we allow zero to and up dirs.
if (fromDir.length () == 0)
return Matrix44<T> ();
else
{
Matrix44<T> zAxis2FromDir = alignZAxisWithTargetDir
(fromDir, Vec3<T> (0, 1, 0));
Matrix44<T> fromDir2zAxis = zAxis2FromDir.transposed ();
Matrix44<T> zAxis2ToDir = alignZAxisWithTargetDir (toDir, upDir);
return fromDir2zAxis * zAxis2ToDir;
}
}
template <class T>
Matrix44<T>
alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
{
//
// Ensure that the target direction is non-zero.
//
if ( targetDir.length () == 0 )
targetDir = Vec3<T> (0, 0, 1);
//
// Ensure that the up direction is non-zero.
//
if ( upDir.length () == 0 )
upDir = Vec3<T> (0, 1, 0);
//
// Check for degeneracies. If the upDir and targetDir are parallel
// or opposite, then compute a new, arbitrary up direction that is
// not parallel or opposite to the targetDir.
//
if (upDir.cross (targetDir).length () == 0)
{
upDir = targetDir.cross (Vec3<T> (1, 0, 0));
if (upDir.length() == 0)
upDir = targetDir.cross(Vec3<T> (0, 0, 1));
}
//
// Compute the x-, y-, and z-axis vectors of the new coordinate system.
//
Vec3<T> targetPerpDir = upDir.cross (targetDir);
Vec3<T> targetUpDir = targetDir.cross (targetPerpDir);
//
// Rotate the x-axis into targetPerpDir (row 0),
// rotate the y-axis into targetUpDir (row 1),
// rotate the z-axis into targetDir (row 2).
//
Vec3<T> row[3];
row[0] = targetPerpDir.normalized ();
row[1] = targetUpDir .normalized ();
row[2] = targetDir .normalized ();
Matrix44<T> mat ( row[0][0], row[0][1], row[0][2], 0,
row[1][0], row[1][1], row[1][2], 0,
row[2][0], row[2][1], row[2][2], 0,
0, 0, 0, 1 );
return mat;
}
//-----------------------------------------------------------------------------
// Implementation for 3x3 Matrix
//------------------------------
template <class T>
bool
extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
{
T shr;
Matrix33<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix33<T>
sansScaling (const Matrix33<T> &mat, bool exc)
{
Vec2<T> scl;
T shr;
T rot;
Vec2<T> tran;
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
return mat;
Matrix33<T> M;
M.translate (tran);
M.rotate (rot);
M.shear (shr);
return M;
}
template <class T>
bool
removeScaling (Matrix33<T> &mat, bool exc)
{
Vec2<T> scl;
T shr;
T rot;
Vec2<T> tran;
if (! extractSHRT (mat, scl, shr, rot, tran, exc))
return false;
mat.makeIdentity ();
mat.translate (tran);
mat.rotate (rot);
mat.shear (shr);
return true;
}
template <class T>
bool
extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
{
Matrix33<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return false;
return true;
}
template <class T>
Matrix33<T>
sansScalingAndShear (const Matrix33<T> &mat, bool exc)
{
Vec2<T> scl;
T shr;
Matrix33<T> M (mat);
if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
return mat;
return M;
}
template <class T>
bool
removeScalingAndShear (Matrix33<T> &mat, bool exc)
{
Vec2<T> scl;
T shr;
if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
return false;
return true;
}
template <class T>
bool
extractAndRemoveScalingAndShear (Matrix33<T> &mat,
Vec2<T> &scl, T &shr, bool exc)
{
Vec2<T> row[2];
row[0] = Vec2<T> (mat[0][0], mat[0][1]);
row[1] = Vec2<T> (mat[1][0], mat[1][1]);
T maxVal = 0;
for (int i=0; i < 2; i++)
for (int j=0; j < 2; j++)
if (Imath::abs (row[i][j]) > maxVal)
maxVal = Imath::abs (row[i][j]);
//
// We normalize the 2x2 matrix here.
// It was noticed that this can improve numerical stability significantly,
// especially when many of the upper 2x2 matrix's coefficients are very
// close to zero; we correct for this step at the end by multiplying the
// scaling factors by maxVal at the end (shear and rotation are not
// affected by the normalization).
if (maxVal != 0)
{
for (int i=0; i < 2; i++)
if (! checkForZeroScaleInRow (maxVal, row[i], exc))
return false;
else
row[i] /= maxVal;
}
// Compute X scale factor.
scl.x = row[0].length ();
if (! checkForZeroScaleInRow (scl.x, row[0], exc))
return false;
// Normalize first row.
row[0] /= scl.x;
// An XY shear factor will shear the X coord. as the Y coord. changes.
// There are 2 combinations (XY, YX), although we only extract the XY
// shear factor because we can effect the an YX shear factor by
// shearing in XY combined with rotations and scales.
//
// shear matrix < 1, YX, 0,
// XY, 1, 0,
// 0, 0, 1 >
// Compute XY shear factor and make 2nd row orthogonal to 1st.
shr = row[0].dot (row[1]);
row[1] -= shr * row[0];
// Now, compute Y scale.
scl.y = row[1].length ();
if (! checkForZeroScaleInRow (scl.y, row[1], exc))
return false;
// Normalize 2nd row and correct the XY shear factor for Y scaling.
row[1] /= scl.y;
shr /= scl.y;
// At this point, the upper 2x2 matrix in mat is orthonormal.
// Check for a coordinate system flip. If the determinant
// is -1, then flip the rotation matrix and adjust the scale(Y)
// and shear(XY) factors to compensate.
if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
{
row[1][0] *= -1;
row[1][1] *= -1;
scl[1] *= -1;
shr *= -1;
}
// Copy over the orthonormal rows into the returned matrix.
// The upper 2x2 matrix in mat is now a rotation matrix.
for (int i=0; i < 2; i++)
{
mat[i][0] = row[i][0];
mat[i][1] = row[i][1];
}
scl *= maxVal;
return true;
}
template <class T>
void
extractEuler (const Matrix33<T> &mat, T &rot)
{
//
// Normalize the local x and y axes to remove scaling.
//
Vec2<T> i (mat[0][0], mat[0][1]);
Vec2<T> j (mat[1][0], mat[1][1]);
i.normalize();
j.normalize();
//
// Extract the angle, rot.
//
rot = - Math<T>::atan2 (j[0], i[0]);
}
template <class T>
bool
extractSHRT (const Matrix33<T> &mat,
Vec2<T> &s,
T &h,
T &r,
Vec2<T> &t,
bool exc)
{
Matrix33<T> rot;
rot = mat;
if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
return false;
extractEuler (rot, r);
t.x = mat[2][0];
t.y = mat[2][1];
return true;
}
template <class T>
bool
checkForZeroScaleInRow (const T& scl,
const Vec2<T> &row,
bool exc /* = true */ )
{
for (int i = 0; i < 2; i++)
{
if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
{
if (exc)
throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
"from matrix.");
else
return false;
}
}
return true;
}
} // namespace Imath
#endif