Coverage Report

Created: 2024-09-08 06:41

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