PageRenderTime 100ms CodeModel.GetById 11ms app.highlight 81ms RepoModel.GetById 1ms app.codeStats 1ms

/src/FreeImage/Source/OpenEXR/Imath/ImathMatrixAlgo.h

https://bitbucket.org/cabalistic/ogredeps/
C++ Header | 1115 lines | 629 code | 234 blank | 252 comment | 64 complexity | 151e74894f916f81a6dfef581006074c MD5 | raw file
   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    //
 548    // Extract the first angle, rot.x.
 549    // 
 550
 551    rot.x = Math<T>::atan2 (M[1][2], M[2][2]);
 552
 553    //
 554    // Remove the rot.x rotation from M, so that the remaining
 555    // rotation, N, is only around two axes, and gimbal lock
 556    // cannot occur.
 557    //
 558    Matrix44<T> N;
 559    N.rotate (Vec3<T> (-rot.x, 0, 0));
 560
 561	N = N * M;
 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
 572
 573template <class T>
 574void
 575extractEulerZYX (const Matrix44<T> &mat, Vec3<T> &rot)
 576{
 577    //
 578    // Normalize the local x, y and z axes to remove scaling.
 579    //
 580
 581    Vec3<T> i (mat[0][0], mat[0][1], mat[0][2]);
 582    Vec3<T> j (mat[1][0], mat[1][1], mat[1][2]);
 583    Vec3<T> k (mat[2][0], mat[2][1], mat[2][2]);
 584
 585    i.normalize();
 586    j.normalize();
 587    k.normalize();
 588
 589    Matrix44<T> M (i[0], i[1], i[2], 0, 
 590		   j[0], j[1], j[2], 0, 
 591		   k[0], k[1], k[2], 0, 
 592		   0,    0,    0,    1);
 593
 594    //
 595    // Extract the first angle, rot.x.
 596    // 
 597
 598    rot.x = -Math<T>::atan2 (M[1][0], M[0][0]);
 599
 600    //
 601    // Remove the x rotation from M, so that the remaining
 602    // rotation, N, is only around two axes, and gimbal lock
 603    // cannot occur.
 604    //
 605
 606    Matrix44<T> N;
 607    N.rotate (Vec3<T> (0, 0, -rot.x));
 608    N = N * M;
 609
 610    //
 611    // Extract the other two angles, rot.y and rot.z, from N.
 612    //
 613
 614    T cy = Math<T>::sqrt (N[2][2]*N[2][2] + N[2][1]*N[2][1]);
 615    rot.y = -Math<T>::atan2 (-N[2][0], cy);
 616    rot.z = -Math<T>::atan2 (-N[1][2], N[1][1]);
 617}
 618
 619
 620template <class T>
 621Quat<T>
 622extractQuat (const Matrix44<T> &mat)
 623{
 624  Matrix44<T> rot;
 625
 626  T        tr, s;
 627  T         q[4];
 628  int    i, j, k;
 629  Quat<T>   quat;
 630
 631  int nxt[3] = {1, 2, 0};
 632  tr = mat[0][0] + mat[1][1] + mat[2][2];
 633
 634  // check the diagonal
 635  if (tr > 0.0) {
 636     s = Math<T>::sqrt (tr + 1.0);
 637     quat.r = s / 2.0;
 638     s = 0.5 / s;
 639
 640     quat.v.x = (mat[1][2] - mat[2][1]) * s;
 641     quat.v.y = (mat[2][0] - mat[0][2]) * s;
 642     quat.v.z = (mat[0][1] - mat[1][0]) * s;
 643  } 
 644  else {      
 645     // diagonal is negative
 646     i = 0;
 647     if (mat[1][1] > mat[0][0]) 
 648        i=1;
 649     if (mat[2][2] > mat[i][i]) 
 650        i=2;
 651    
 652     j = nxt[i];
 653     k = nxt[j];
 654     s = Math<T>::sqrt ((mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0);
 655      
 656     q[i] = s * 0.5;
 657     if (s != 0.0) 
 658        s = 0.5 / s;
 659
 660     q[3] = (mat[j][k] - mat[k][j]) * s;
 661     q[j] = (mat[i][j] + mat[j][i]) * s;
 662     q[k] = (mat[i][k] + mat[k][i]) * s;
 663
 664     quat.v.x = q[0];
 665     quat.v.y = q[1];
 666     quat.v.z = q[2];
 667     quat.r = q[3];
 668 }
 669
 670  return quat;
 671}
 672
 673template <class T>
 674bool 
 675extractSHRT (const Matrix44<T> &mat,
 676	     Vec3<T> &s,
 677	     Vec3<T> &h,
 678	     Vec3<T> &r,
 679	     Vec3<T> &t,
 680	     bool exc /* = true */ ,
 681	     typename Euler<T>::Order rOrder /* = Euler<T>::XYZ */ )
 682{
 683    Matrix44<T> rot;
 684
 685    rot = mat;
 686    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
 687	return false;
 688
 689    extractEulerXYZ (rot, r);
 690
 691    t.x = mat[3][0];
 692    t.y = mat[3][1];
 693    t.z = mat[3][2];
 694
 695    if (rOrder != Euler<T>::XYZ)
 696    {
 697	Imath::Euler<T> eXYZ (r, Imath::Euler<T>::XYZ);
 698	Imath::Euler<T> e (eXYZ, rOrder);
 699	r = e.toXYZVector ();
 700    }
 701
 702    return true;
 703}
 704
 705template <class T>
 706bool 
 707extractSHRT (const Matrix44<T> &mat,
 708	     Vec3<T> &s,
 709	     Vec3<T> &h,
 710	     Vec3<T> &r,
 711	     Vec3<T> &t,
 712	     bool exc)
 713{
 714    return extractSHRT(mat, s, h, r, t, exc, Imath::Euler<T>::XYZ);
 715}
 716
 717template <class T>
 718bool 
 719extractSHRT (const Matrix44<T> &mat,
 720	     Vec3<T> &s,
 721	     Vec3<T> &h,
 722	     Euler<T> &r,
 723	     Vec3<T> &t,
 724	     bool exc /* = true */)
 725{
 726    return extractSHRT (mat, s, h, r, t, exc, r.order ());
 727}
 728
 729
 730template <class T> 
 731bool		
 732checkForZeroScaleInRow (const T& scl, 
 733			const Vec3<T> &row,
 734			bool exc /* = true */ )
 735{
 736    for (int i = 0; i < 3; i++)
 737    {
 738	if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
 739	{
 740	    if (exc)
 741		throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
 742					   "from matrix.");
 743	    else
 744		return false;
 745	}
 746    }
 747
 748    return true;
 749}
 750
 751
 752template <class T>
 753Matrix44<T>
 754rotationMatrix (const Vec3<T> &from, const Vec3<T> &to)
 755{
 756    Quat<T> q;
 757    q.setRotation(from, to);
 758    return q.toMatrix44();
 759}
 760
 761
 762template <class T>
 763Matrix44<T>	
 764rotationMatrixWithUpDir (const Vec3<T> &fromDir,
 765			 const Vec3<T> &toDir,
 766			 const Vec3<T> &upDir)
 767{
 768    //
 769    // The goal is to obtain a rotation matrix that takes 
 770    // "fromDir" to "toDir".  We do this in two steps and 
 771    // compose the resulting rotation matrices; 
 772    //    (a) rotate "fromDir" into the z-axis
 773    //    (b) rotate the z-axis into "toDir"
 774    //
 775
 776    // The from direction must be non-zero; but we allow zero to and up dirs.
 777    if (fromDir.length () == 0)
 778	return Matrix44<T> ();
 779
 780    else
 781    {
 782	Matrix44<T> zAxis2FromDir  = alignZAxisWithTargetDir 
 783	                                 (fromDir, Vec3<T> (0, 1, 0));
 784
 785	Matrix44<T> fromDir2zAxis  = zAxis2FromDir.transposed ();
 786	
 787	Matrix44<T> zAxis2ToDir    = alignZAxisWithTargetDir (toDir, upDir);
 788
 789	return fromDir2zAxis * zAxis2ToDir;
 790    }
 791}
 792
 793
 794template <class T>
 795Matrix44<T>
 796alignZAxisWithTargetDir (Vec3<T> targetDir, Vec3<T> upDir)
 797{
 798    //
 799    // Ensure that the target direction is non-zero.
 800    //
 801
 802    if ( targetDir.length () == 0 )
 803	targetDir = Vec3<T> (0, 0, 1);
 804
 805    //
 806    // Ensure that the up direction is non-zero.
 807    //
 808
 809    if ( upDir.length () == 0 )
 810	upDir = Vec3<T> (0, 1, 0);
 811
 812    //
 813    // Check for degeneracies.  If the upDir and targetDir are parallel 
 814    // or opposite, then compute a new, arbitrary up direction that is
 815    // not parallel or opposite to the targetDir.
 816    //
 817
 818    if (upDir.cross (targetDir).length () == 0)
 819    {
 820	upDir = targetDir.cross (Vec3<T> (1, 0, 0));
 821	if (upDir.length() == 0)
 822	    upDir = targetDir.cross(Vec3<T> (0, 0, 1));
 823    }
 824
 825    //
 826    // Compute the x-, y-, and z-axis vectors of the new coordinate system.
 827    //
 828
 829    Vec3<T> targetPerpDir = upDir.cross (targetDir);    
 830    Vec3<T> targetUpDir   = targetDir.cross (targetPerpDir);
 831    
 832    //
 833    // Rotate the x-axis into targetPerpDir (row 0),
 834    // rotate the y-axis into targetUpDir   (row 1),
 835    // rotate the z-axis into targetDir     (row 2).
 836    //
 837    
 838    Vec3<T> row[3];
 839    row[0] = targetPerpDir.normalized ();
 840    row[1] = targetUpDir  .normalized ();
 841    row[2] = targetDir    .normalized ();
 842    
 843    Matrix44<T> mat ( row[0][0],  row[0][1],  row[0][2],  0,
 844		      row[1][0],  row[1][1],  row[1][2],  0,
 845		      row[2][0],  row[2][1],  row[2][2],  0,
 846		      0,          0,          0,          1 );
 847    
 848    return mat;
 849}
 850
 851
 852
 853//-----------------------------------------------------------------------------
 854// Implementation for 3x3 Matrix
 855//------------------------------
 856
 857
 858template <class T>
 859bool
 860extractScaling (const Matrix33<T> &mat, Vec2<T> &scl, bool exc)
 861{
 862    T shr;
 863    Matrix33<T> M (mat);
 864
 865    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
 866	return false;
 867
 868    return true;
 869}
 870
 871
 872template <class T>
 873Matrix33<T>
 874sansScaling (const Matrix33<T> &mat, bool exc)
 875{
 876    Vec2<T> scl;
 877    T shr;
 878    T rot;
 879    Vec2<T> tran;
 880
 881    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
 882	return mat;
 883
 884    Matrix33<T> M;
 885    
 886    M.translate (tran);
 887    M.rotate (rot);
 888    M.shear (shr);
 889
 890    return M;
 891}
 892
 893
 894template <class T>
 895bool
 896removeScaling (Matrix33<T> &mat, bool exc)
 897{
 898    Vec2<T> scl;
 899    T shr;
 900    T rot;
 901    Vec2<T> tran;
 902
 903    if (! extractSHRT (mat, scl, shr, rot, tran, exc))
 904	return false;
 905
 906    mat.makeIdentity ();
 907    mat.translate (tran);
 908    mat.rotate (rot);
 909    mat.shear (shr);
 910
 911    return true;
 912}
 913
 914
 915template <class T>
 916bool
 917extractScalingAndShear (const Matrix33<T> &mat, Vec2<T> &scl, T &shr, bool exc)
 918{
 919    Matrix33<T> M (mat);
 920
 921    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
 922	return false;
 923
 924    return true;
 925}
 926
 927
 928template <class T>
 929Matrix33<T>
 930sansScalingAndShear (const Matrix33<T> &mat, bool exc)
 931{
 932    Vec2<T> scl;
 933    T shr;
 934    Matrix33<T> M (mat);
 935
 936    if (! extractAndRemoveScalingAndShear (M, scl, shr, exc))
 937	return mat;
 938    
 939    return M;
 940}
 941
 942
 943template <class T>
 944bool
 945removeScalingAndShear (Matrix33<T> &mat, bool exc)
 946{
 947    Vec2<T> scl;
 948    T shr;
 949
 950    if (! extractAndRemoveScalingAndShear (mat, scl, shr, exc))
 951	return false;
 952    
 953    return true;
 954}
 955
 956template <class T>
 957bool
 958extractAndRemoveScalingAndShear (Matrix33<T> &mat, 
 959				 Vec2<T> &scl, T &shr, bool exc)
 960{
 961    Vec2<T> row[2];
 962
 963    row[0] = Vec2<T> (mat[0][0], mat[0][1]);
 964    row[1] = Vec2<T> (mat[1][0], mat[1][1]);
 965    
 966    T maxVal = 0;
 967    for (int i=0; i < 2; i++)
 968	for (int j=0; j < 2; j++)
 969	    if (Imath::abs (row[i][j]) > maxVal)
 970		maxVal = Imath::abs (row[i][j]);
 971
 972    //
 973    // We normalize the 2x2 matrix here.
 974    // It was noticed that this can improve numerical stability significantly,
 975    // especially when many of the upper 2x2 matrix's coefficients are very
 976    // close to zero; we correct for this step at the end by multiplying the 
 977    // scaling factors by maxVal at the end (shear and rotation are not 
 978    // affected by the normalization).
 979
 980    if (maxVal != 0)
 981    {
 982	for (int i=0; i < 2; i++)
 983	    if (! checkForZeroScaleInRow (maxVal, row[i], exc))
 984		return false;
 985	    else
 986		row[i] /= maxVal;
 987    }
 988
 989    // Compute X scale factor. 
 990    scl.x = row[0].length ();
 991    if (! checkForZeroScaleInRow (scl.x, row[0], exc))
 992	return false;
 993
 994    // Normalize first row.
 995    row[0] /= scl.x;
 996
 997    // An XY shear factor will shear the X coord. as the Y coord. changes.
 998    // There are 2 combinations (XY, YX), although we only extract the XY 
 999    // shear factor because we can effect the an YX shear factor by 
1000    // shearing in XY combined with rotations and scales.
1001    //
1002    // shear matrix <   1,  YX,  0,
1003    //                 XY,   1,  0,
1004    //                  0,   0,  1 >
1005
1006    // Compute XY shear factor and make 2nd row orthogonal to 1st.
1007    shr     = row[0].dot (row[1]);
1008    row[1] -= shr * row[0];
1009
1010    // Now, compute Y scale.
1011    scl.y = row[1].length ();
1012    if (! checkForZeroScaleInRow (scl.y, row[1], exc))
1013	return false;
1014
1015    // Normalize 2nd row and correct the XY shear factor for Y scaling.
1016    row[1] /= scl.y; 
1017    shr    /= scl.y;
1018
1019    // At this point, the upper 2x2 matrix in mat is orthonormal.
1020    // Check for a coordinate system flip. If the determinant
1021    // is -1, then flip the rotation matrix and adjust the scale(Y) 
1022    // and shear(XY) factors to compensate.
1023    if (row[0][0] * row[1][1] - row[0][1] * row[1][0] < 0)
1024    {
1025	row[1][0] *= -1;
1026	row[1][1] *= -1;
1027	scl[1] *= -1;
1028	shr *= -1;
1029    }
1030
1031    // Copy over the orthonormal rows into the returned matrix.
1032    // The upper 2x2 matrix in mat is now a rotation matrix.
1033    for (int i=0; i < 2; i++)
1034    {
1035	mat[i][0] = row[i][0]; 
1036	mat[i][1] = row[i][1]; 
1037    }
1038
1039    scl *= maxVal;
1040
1041    return true;
1042}
1043
1044
1045template <class T>
1046void
1047extractEuler (const Matrix33<T> &mat, T &rot)
1048{
1049    //
1050    // Normalize the local x and y axes to remove scaling.
1051    //
1052
1053    Vec2<T> i (mat[0][0], mat[0][1]);
1054    Vec2<T> j (mat[1][0], mat[1][1]);
1055
1056    i.normalize();
1057    j.normalize();
1058
1059    //
1060    // Extract the angle, rot.
1061    // 
1062
1063    rot = - Math<T>::atan2 (j[0], i[0]);
1064}
1065
1066
1067template <class T>
1068bool 
1069extractSHRT (const Matrix33<T> &mat,
1070	     Vec2<T> &s,
1071	     T       &h,
1072	     T       &r,
1073	     Vec2<T> &t,
1074	     bool    exc)
1075{
1076    Matrix33<T> rot;
1077
1078    rot = mat;
1079    if (! extractAndRemoveScalingAndShear (rot, s, h, exc))
1080	return false;
1081
1082    extractEuler (rot, r);
1083
1084    t.x = mat[2][0];
1085    t.y = mat[2][1];
1086
1087    return true;
1088}
1089
1090
1091template <class T> 
1092bool		
1093checkForZeroScaleInRow (const T& scl, 
1094			const Vec2<T> &row,
1095			bool exc /* = true */ )
1096{
1097    for (int i = 0; i < 2; i++)
1098    {
1099	if ((abs (scl) < 1 && abs (row[i]) >= limits<T>::max() * abs (scl)))
1100	{
1101	    if (exc)
1102		throw Imath::ZeroScaleExc ("Cannot remove zero scaling "
1103					   "from matrix.");
1104	    else
1105		return false;
1106	}
1107    }
1108
1109    return true;
1110}
1111
1112
1113} // namespace Imath
1114
1115#endif