Coverage Report

Created: 2024-09-08 06:41

/usr/local/include/OpenEXR/ImathMatrix.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
37
#ifndef INCLUDED_IMATHMATRIX_H
38
#define INCLUDED_IMATHMATRIX_H
39
40
//----------------------------------------------------------------
41
//
42
//      2D (3x3) and 3D (4x4) transformation matrix templates.
43
//
44
//----------------------------------------------------------------
45
46
#include "ImathPlatform.h"
47
#include "ImathFun.h"
48
#include "ImathExc.h"
49
#include "ImathVec.h"
50
#include "ImathShear.h"
51
#include "ImathNamespace.h"
52
53
#include <cstring>
54
#include <iostream>
55
#include <iomanip>
56
#include <string.h>
57
58
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
59
// suppress exception specification warnings
60
#pragma warning(disable:4290)
61
#endif
62
63
64
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
65
66
enum Uninitialized {UNINITIALIZED};
67
68
69
template <class T> class Matrix22
70
{
71
  public:
72
73
    //-------------------
74
    // Access to elements
75
    //-------------------
76
77
    T           x[2][2];
78
79
    T *         operator [] (int i);
80
    const T *   operator [] (int i) const;
81
82
83
    //-------------
84
    // Constructors
85
    //-------------
86
87
    Matrix22 (Uninitialized) {}
88
89
    Matrix22 ();
90
                                // 1 0
91
                                // 0 1
92
93
    Matrix22 (T a);
94
                                // a a
95
                                // a a
96
97
    Matrix22 (const T a[2][2]);
98
                                // a[0][0] a[0][1]
99
                                // a[1][0] a[1][1]
100
101
    Matrix22 (T a, T b, T c, T d);
102
103
                                // a b
104
                                // c d
105
106
107
    //--------------------------------
108
    // Copy constructor and assignment
109
    //--------------------------------
110
111
    Matrix22 (const Matrix22 &v);
112
    template <class S> explicit Matrix22 (const Matrix22<S> &v);
113
114
    const Matrix22 &    operator = (const Matrix22 &v);
115
    const Matrix22 &    operator = (T a);
116
117
118
    //------------
119
    // Destructor
120
    //------------
121
122
    ~Matrix22 () = default;
123
  
124
    //----------------------
125
    // Compatibility with Sb
126
    //----------------------
127
    
128
    T *                 getValue ();
129
    const T *           getValue () const;
130
131
    template <class S>
132
    void                getValue (Matrix22<S> &v) const;
133
    template <class S>
134
    Matrix22 &          setValue (const Matrix22<S> &v);
135
136
    template <class S>
137
    Matrix22 &          setTheMatrix (const Matrix22<S> &v);
138
139
140
    //---------
141
    // Identity
142
    //---------
143
144
    void                makeIdentity();
145
146
147
    //---------
148
    // Equality
149
    //---------
150
151
    bool                operator == (const Matrix22 &v) const;
152
    bool                operator != (const Matrix22 &v) const;
153
154
    //-----------------------------------------------------------------------
155
    // Compare two matrices and test if they are "approximately equal":
156
    //
157
    // equalWithAbsError (m, e)
158
    //
159
    //      Returns true if the coefficients of this and m are the same with
160
    //      an absolute error of no more than e, i.e., for all i, j
161
    //
162
    //      abs (this[i][j] - m[i][j]) <= e
163
    //
164
    // equalWithRelError (m, e)
165
    //
166
    //      Returns true if the coefficients of this and m are the same with
167
    //      a relative error of no more than e, i.e., for all i, j
168
    //
169
    //      abs (this[i] - v[i][j]) <= e * abs (this[i][j])
170
    //-----------------------------------------------------------------------
171
172
    bool                equalWithAbsError (const Matrix22<T> &v, T e) const;
173
    bool                equalWithRelError (const Matrix22<T> &v, T e) const;
174
175
176
    //------------------------
177
    // Component-wise addition
178
    //------------------------
179
180
    const Matrix22 &    operator += (const Matrix22 &v);
181
    const Matrix22 &    operator += (T a);
182
    Matrix22            operator + (const Matrix22 &v) const;
183
184
185
    //---------------------------
186
    // Component-wise subtraction
187
    //---------------------------
188
189
    const Matrix22 &    operator -= (const Matrix22 &v);
190
    const Matrix22 &    operator -= (T a);
191
    Matrix22            operator - (const Matrix22 &v) const;
192
193
194
    //------------------------------------
195
    // Component-wise multiplication by -1
196
    //------------------------------------
197
198
    Matrix22            operator - () const;
199
    const Matrix22 &    negate ();
200
201
202
    //------------------------------
203
    // Component-wise multiplication
204
    //------------------------------
205
206
    const Matrix22 &    operator *= (T a);
207
    Matrix22            operator * (T a) const;
208
209
210
    //-----------------------------------
211
    // Matrix-times-matrix multiplication
212
    //-----------------------------------
213
214
    const Matrix22 &    operator *= (const Matrix22 &v);
215
    Matrix22            operator * (const Matrix22 &v) const;
216
217
218
    //-----------------------------------------------------------------
219
    // Vector-times-matrix multiplication; see also the "operator *"
220
    // functions defined below.
221
    //
222
    // m.multDirMatrix(src,dst) multiplies src by the matrix m.
223
    //-----------------------------------------------------------------
224
225
    template <class S>
226
    void                multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
227
228
229
    //------------------------
230
    // Component-wise division
231
    //------------------------
232
233
    const Matrix22 &    operator /= (T a);
234
    Matrix22            operator / (T a) const;
235
236
237
    //------------------
238
    // Transposed matrix
239
    //------------------
240
241
    const Matrix22 &    transpose ();
242
    Matrix22            transposed () const;
243
244
245
    //------------------------------------------------------------
246
    // Inverse matrix: If singExc is false, inverting a singular
247
    // matrix produces an identity matrix.  If singExc is true,
248
    // inverting a singular matrix throws a SingMatrixExc.
249
    //
250
    // inverse() and invert() invert matrices using determinants.
251
    //
252
    //------------------------------------------------------------
253
254
    const Matrix22 &    invert (bool singExc = false);
255
256
    Matrix22<T>         inverse (bool singExc = false) const;
257
258
    //------------
259
    // Determinant
260
    //------------
261
262
    T                   determinant() const;
263
264
    //-----------------------------------------
265
    // Set matrix to rotation by r (in radians)
266
    //-----------------------------------------
267
268
    template <class S>
269
    const Matrix22 &    setRotation (S r);
270
271
272
    //-----------------------------
273
    // Rotate the given matrix by r
274
    //-----------------------------
275
276
    template <class S>
277
    const Matrix22 &    rotate (S r);
278
279
280
    //--------------------------------------------
281
    // Set matrix to scale by given uniform factor
282
    //--------------------------------------------
283
284
    const Matrix22 &    setScale (T s);
285
286
287
    //------------------------------------
288
    // Set matrix to scale by given vector
289
    //------------------------------------
290
291
    template <class S>
292
    const Matrix22 &    setScale (const Vec2<S> &s);
293
294
295
    //----------------------
296
    // Scale the matrix by s
297
    //----------------------
298
299
    template <class S>
300
    const Matrix22 &    scale (const Vec2<S> &s);
301
302
303
    //--------------------------------------------------------
304
    // Number of the row and column dimensions, since
305
    // Matrix22 is a square matrix.
306
    //--------------------------------------------------------
307
308
    static unsigned int dimensions() {return 2;}
309
310
311
    //-------------------------------------------------
312
    // Limitations of type T (see also class limits<T>)
313
    //-------------------------------------------------
314
315
    static T            baseTypeMin()           {return limits<T>::min();}
316
    static T            baseTypeMax()           {return limits<T>::max();}
317
    static T            baseTypeSmallest()      {return limits<T>::smallest();}
318
    static T            baseTypeEpsilon()       {return limits<T>::epsilon();}
319
320
    typedef T   BaseType;
321
    typedef Vec2<T> BaseVecType;
322
    
323
  private:
324
325
    template <typename R, typename S>
326
    struct isSameType
327
    {
328
        enum {value = 0};
329
    };
330
331
    template <typename R>
332
    struct isSameType<R, R>
333
    {
334
        enum {value = 1};
335
    };
336
};
337
338
339
template <class T> class Matrix33
340
{
341
  public:
342
343
    //-------------------
344
    // Access to elements
345
    //-------------------
346
347
    T           x[3][3];
348
349
    T *         operator [] (int i);
350
    const T *   operator [] (int i) const;
351
352
353
    //-------------
354
    // Constructors
355
    //-------------
356
357
    Matrix33 (Uninitialized) {}
358
359
    Matrix33 ();
360
                                // 1 0 0
361
                                // 0 1 0
362
                                // 0 0 1
363
364
    Matrix33 (T a);
365
                                // a a a
366
                                // a a a
367
                                // a a a
368
369
    Matrix33 (const T a[3][3]);
370
                                // a[0][0] a[0][1] a[0][2]
371
                                // a[1][0] a[1][1] a[1][2]
372
                                // a[2][0] a[2][1] a[2][2]
373
374
    Matrix33 (T a, T b, T c, T d, T e, T f, T g, T h, T i);
375
376
                                // a b c
377
                                // d e f
378
                                // g h i
379
380
381
    //--------------------------------
382
    // Copy constructor and assignment
383
    //--------------------------------
384
385
    Matrix33 (const Matrix33 &v);
386
    template <class S> explicit Matrix33 (const Matrix33<S> &v);
387
388
    const Matrix33 &    operator = (const Matrix33 &v);
389
    const Matrix33 &    operator = (T a);
390
391
392
    //------------
393
    // Destructor
394
    //------------
395
396
    ~Matrix33 () = default;
397
  
398
    //----------------------
399
    // Compatibility with Sb
400
    //----------------------
401
    
402
    T *                 getValue ();
403
    const T *           getValue () const;
404
405
    template <class S>
406
    void                getValue (Matrix33<S> &v) const;
407
    template <class S>
408
    Matrix33 &          setValue (const Matrix33<S> &v);
409
410
    template <class S>
411
    Matrix33 &          setTheMatrix (const Matrix33<S> &v);
412
413
414
    //---------
415
    // Identity
416
    //---------
417
418
    void                makeIdentity();
419
420
421
    //---------
422
    // Equality
423
    //---------
424
425
    bool                operator == (const Matrix33 &v) const;
426
    bool                operator != (const Matrix33 &v) const;
427
428
    //-----------------------------------------------------------------------
429
    // Compare two matrices and test if they are "approximately equal":
430
    //
431
    // equalWithAbsError (m, e)
432
    //
433
    //      Returns true if the coefficients of this and m are the same with
434
    //      an absolute error of no more than e, i.e., for all i, j
435
    //
436
    //      abs (this[i][j] - m[i][j]) <= e
437
    //
438
    // equalWithRelError (m, e)
439
    //
440
    //      Returns true if the coefficients of this and m are the same with
441
    //      a relative error of no more than e, i.e., for all i, j
442
    //
443
    //      abs (this[i] - v[i][j]) <= e * abs (this[i][j])
444
    //-----------------------------------------------------------------------
445
446
    bool                equalWithAbsError (const Matrix33<T> &v, T e) const;
447
    bool                equalWithRelError (const Matrix33<T> &v, T e) const;
448
449
450
    //------------------------
451
    // Component-wise addition
452
    //------------------------
453
454
    const Matrix33 &    operator += (const Matrix33 &v);
455
    const Matrix33 &    operator += (T a);
456
    Matrix33            operator + (const Matrix33 &v) const;
457
458
459
    //---------------------------
460
    // Component-wise subtraction
461
    //---------------------------
462
463
    const Matrix33 &    operator -= (const Matrix33 &v);
464
    const Matrix33 &    operator -= (T a);
465
    Matrix33            operator - (const Matrix33 &v) const;
466
467
468
    //------------------------------------
469
    // Component-wise multiplication by -1
470
    //------------------------------------
471
472
    Matrix33            operator - () const;
473
    const Matrix33 &    negate ();
474
475
476
    //------------------------------
477
    // Component-wise multiplication
478
    //------------------------------
479
480
    const Matrix33 &    operator *= (T a);
481
    Matrix33            operator * (T a) const;
482
483
484
    //-----------------------------------
485
    // Matrix-times-matrix multiplication
486
    //-----------------------------------
487
488
    const Matrix33 &    operator *= (const Matrix33 &v);
489
    Matrix33            operator * (const Matrix33 &v) const;
490
491
492
    //-----------------------------------------------------------------
493
    // Vector-times-matrix multiplication; see also the "operator *"
494
    // functions defined below.
495
    //
496
    // m.multVecMatrix(src,dst) implements a homogeneous transformation
497
    // by computing Vec3 (src.x, src.y, 1) * m and dividing by the
498
    // result's third element.
499
    //
500
    // m.multDirMatrix(src,dst) multiplies src by the upper left 2x2
501
    // submatrix, ignoring the rest of matrix m.
502
    //-----------------------------------------------------------------
503
504
    template <class S>
505
    void                multVecMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
506
507
    template <class S>
508
    void                multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const;
509
510
511
    //------------------------
512
    // Component-wise division
513
    //------------------------
514
515
    const Matrix33 &    operator /= (T a);
516
    Matrix33            operator / (T a) const;
517
518
519
    //------------------
520
    // Transposed matrix
521
    //------------------
522
523
    const Matrix33 &    transpose ();
524
    Matrix33            transposed () const;
525
526
527
    //------------------------------------------------------------
528
    // Inverse matrix: If singExc is false, inverting a singular
529
    // matrix produces an identity matrix.  If singExc is true,
530
    // inverting a singular matrix throws a SingMatrixExc.
531
    //
532
    // inverse() and invert() invert matrices using determinants;
533
    // gjInverse() and gjInvert() use the Gauss-Jordan method.
534
    //
535
    // inverse() and invert() are significantly faster than
536
    // gjInverse() and gjInvert(), but the results may be slightly
537
    // less accurate.
538
    // 
539
    //------------------------------------------------------------
540
541
    const Matrix33 &    invert (bool singExc = false);
542
543
    Matrix33<T>         inverse (bool singExc = false) const;
544
545
    const Matrix33 &    gjInvert (bool singExc = false);
546
547
    Matrix33<T>         gjInverse (bool singExc = false) const;
548
549
550
    //------------------------------------------------
551
    // Calculate the matrix minor of the (r,c) element
552
    //------------------------------------------------
553
554
    T                   minorOf (const int r, const int c) const;
555
556
    //---------------------------------------------------
557
    // Build a minor using the specified rows and columns
558
    //---------------------------------------------------
559
560
    T                   fastMinor (const int r0, const int r1, 
561
                                   const int c0, const int c1) const;
562
563
    //------------
564
    // Determinant
565
    //------------
566
567
    T                   determinant() const;
568
569
    //-----------------------------------------
570
    // Set matrix to rotation by r (in radians)
571
    //-----------------------------------------
572
573
    template <class S>
574
    const Matrix33 &    setRotation (S r);
575
576
577
    //-----------------------------
578
    // Rotate the given matrix by r
579
    //-----------------------------
580
581
    template <class S>
582
    const Matrix33 &    rotate (S r);
583
584
585
    //--------------------------------------------
586
    // Set matrix to scale by given uniform factor
587
    //--------------------------------------------
588
589
    const Matrix33 &    setScale (T s);
590
591
592
    //------------------------------------
593
    // Set matrix to scale by given vector
594
    //------------------------------------
595
596
    template <class S>
597
    const Matrix33 &    setScale (const Vec2<S> &s);
598
599
600
    //----------------------
601
    // Scale the matrix by s
602
    //----------------------
603
604
    template <class S>
605
    const Matrix33 &    scale (const Vec2<S> &s);
606
607
608
    //------------------------------------------
609
    // Set matrix to translation by given vector
610
    //------------------------------------------
611
612
    template <class S>
613
    const Matrix33 &    setTranslation (const Vec2<S> &t);
614
615
616
    //-----------------------------
617
    // Return translation component
618
    //-----------------------------
619
620
    Vec2<T>             translation () const;
621
622
623
    //--------------------------
624
    // Translate the matrix by t
625
    //--------------------------
626
627
    template <class S>
628
    const Matrix33 &    translate (const Vec2<S> &t);
629
630
631
    //-----------------------------------------------------------
632
    // Set matrix to shear x for each y coord. by given factor xy
633
    //-----------------------------------------------------------
634
635
    template <class S>
636
    const Matrix33 &    setShear (const S &h);
637
638
639
    //-------------------------------------------------------------
640
    // Set matrix to shear x for each y coord. by given factor h[0]
641
    // and to shear y for each x coord. by given factor h[1]
642
    //-------------------------------------------------------------
643
644
    template <class S>
645
    const Matrix33 &    setShear (const Vec2<S> &h);
646
647
648
    //-----------------------------------------------------------
649
    // Shear the matrix in x for each y coord. by given factor xy
650
    //-----------------------------------------------------------
651
652
    template <class S>
653
    const Matrix33 &    shear (const S &xy);
654
655
656
    //-----------------------------------------------------------
657
    // Shear the matrix in x for each y coord. by given factor xy
658
    // and shear y for each x coord. by given factor yx
659
    //-----------------------------------------------------------
660
661
    template <class S>
662
    const Matrix33 &    shear (const Vec2<S> &h);
663
664
665
    //--------------------------------------------------------
666
    // Number of the row and column dimensions, since
667
    // Matrix33 is a square matrix.
668
    //--------------------------------------------------------
669
670
    static unsigned int dimensions() {return 3;}
671
672
673
    //-------------------------------------------------
674
    // Limitations of type T (see also class limits<T>)
675
    //-------------------------------------------------
676
677
    static T            baseTypeMin()           {return limits<T>::min();}
678
    static T            baseTypeMax()           {return limits<T>::max();}
679
    static T            baseTypeSmallest()      {return limits<T>::smallest();}
680
    static T            baseTypeEpsilon()       {return limits<T>::epsilon();}
681
682
    typedef T   BaseType;
683
    typedef Vec3<T> BaseVecType;
684
685
  private:
686
687
    template <typename R, typename S>
688
    struct isSameType
689
    {
690
        enum {value = 0};
691
    };
692
693
    template <typename R>
694
    struct isSameType<R, R>
695
    {
696
        enum {value = 1};
697
    };
698
};
699
700
701
template <class T> class Matrix44
702
{
703
  public:
704
705
    //-------------------
706
    // Access to elements
707
    //-------------------
708
709
    T           x[4][4];
710
711
    T *         operator [] (int i);
712
    const T *   operator [] (int i) const;
713
714
715
    //-------------
716
    // Constructors
717
    //-------------
718
719
    Matrix44 (Uninitialized) {}
720
721
    Matrix44 ();
722
                                // 1 0 0 0
723
                                // 0 1 0 0
724
                                // 0 0 1 0
725
                                // 0 0 0 1
726
727
    Matrix44 (T a);
728
                                // a a a a
729
                                // a a a a
730
                                // a a a a
731
                                // a a a a
732
733
    Matrix44 (const T a[4][4]) ;
734
                                // a[0][0] a[0][1] a[0][2] a[0][3]
735
                                // a[1][0] a[1][1] a[1][2] a[1][3]
736
                                // a[2][0] a[2][1] a[2][2] a[2][3]
737
                                // a[3][0] a[3][1] a[3][2] a[3][3]
738
739
    Matrix44 (T a, T b, T c, T d, T e, T f, T g, T h,
740
              T i, T j, T k, T l, T m, T n, T o, T p);
741
742
                                // a b c d
743
                                // e f g h
744
                                // i j k l
745
                                // m n o p
746
747
    Matrix44 (Matrix33<T> r, Vec3<T> t);
748
                                // r r r 0
749
                                // r r r 0
750
                                // r r r 0
751
                                // t t t 1
752
753
    //------------
754
    // Destructor
755
    //------------
756
757
    ~Matrix44 () = default;
758
759
    //--------------------------------
760
    // Copy constructor and assignment
761
    //--------------------------------
762
763
    Matrix44 (const Matrix44 &v);
764
    template <class S> explicit Matrix44 (const Matrix44<S> &v);
765
766
    const Matrix44 &    operator = (const Matrix44 &v);
767
    const Matrix44 &    operator = (T a);
768
769
770
    //----------------------
771
    // Compatibility with Sb
772
    //----------------------
773
    
774
    T *                 getValue ();
775
    const T *           getValue () const;
776
777
    template <class S>
778
    void                getValue (Matrix44<S> &v) const;
779
    template <class S>
780
    Matrix44 &          setValue (const Matrix44<S> &v);
781
782
    template <class S>
783
    Matrix44 &          setTheMatrix (const Matrix44<S> &v);
784
785
    //---------
786
    // Identity
787
    //---------
788
789
    void                makeIdentity();
790
791
792
    //---------
793
    // Equality
794
    //---------
795
796
    bool                operator == (const Matrix44 &v) const;
797
    bool                operator != (const Matrix44 &v) const;
798
799
    //-----------------------------------------------------------------------
800
    // Compare two matrices and test if they are "approximately equal":
801
    //
802
    // equalWithAbsError (m, e)
803
    //
804
    //      Returns true if the coefficients of this and m are the same with
805
    //      an absolute error of no more than e, i.e., for all i, j
806
    //
807
    //      abs (this[i][j] - m[i][j]) <= e
808
    //
809
    // equalWithRelError (m, e)
810
    //
811
    //      Returns true if the coefficients of this and m are the same with
812
    //      a relative error of no more than e, i.e., for all i, j
813
    //
814
    //      abs (this[i] - v[i][j]) <= e * abs (this[i][j])
815
    //-----------------------------------------------------------------------
816
817
    bool                equalWithAbsError (const Matrix44<T> &v, T e) const;
818
    bool                equalWithRelError (const Matrix44<T> &v, T e) const;
819
820
821
    //------------------------
822
    // Component-wise addition
823
    //------------------------
824
825
    const Matrix44 &    operator += (const Matrix44 &v);
826
    const Matrix44 &    operator += (T a);
827
    Matrix44            operator + (const Matrix44 &v) const;
828
829
830
    //---------------------------
831
    // Component-wise subtraction
832
    //---------------------------
833
834
    const Matrix44 &    operator -= (const Matrix44 &v);
835
    const Matrix44 &    operator -= (T a);
836
    Matrix44            operator - (const Matrix44 &v) const;
837
838
839
    //------------------------------------
840
    // Component-wise multiplication by -1
841
    //------------------------------------
842
843
    Matrix44            operator - () const;
844
    const Matrix44 &    negate ();
845
846
847
    //------------------------------
848
    // Component-wise multiplication
849
    //------------------------------
850
851
    const Matrix44 &    operator *= (T a);
852
    Matrix44            operator * (T a) const;
853
854
855
    //-----------------------------------
856
    // Matrix-times-matrix multiplication
857
    //-----------------------------------
858
859
    const Matrix44 &    operator *= (const Matrix44 &v);
860
    Matrix44            operator * (const Matrix44 &v) const;
861
862
    static void         multiply (const Matrix44 &a,    // assumes that
863
                                  const Matrix44 &b,    // &a != &c and
864
                                  Matrix44 &c);         // &b != &c.
865
866
867
    //-----------------------------------------------------------------
868
    // Vector-times-matrix multiplication; see also the "operator *"
869
    // functions defined below.
870
    //
871
    // m.multVecMatrix(src,dst) implements a homogeneous transformation
872
    // by computing Vec4 (src.x, src.y, src.z, 1) * m and dividing by
873
    // the result's third element.
874
    //
875
    // m.multDirMatrix(src,dst) multiplies src by the upper left 3x3
876
    // submatrix, ignoring the rest of matrix m.
877
    //-----------------------------------------------------------------
878
879
    template <class S>
880
    void                multVecMatrix(const Vec3<S> &src, Vec3<S> &dst) const;
881
882
    template <class S>
883
    void                multDirMatrix(const Vec3<S> &src, Vec3<S> &dst) const;
884
885
886
    //------------------------
887
    // Component-wise division
888
    //------------------------
889
890
    const Matrix44 &    operator /= (T a);
891
    Matrix44            operator / (T a) const;
892
893
894
    //------------------
895
    // Transposed matrix
896
    //------------------
897
898
    const Matrix44 &    transpose ();
899
    Matrix44            transposed () const;
900
901
902
    //------------------------------------------------------------
903
    // Inverse matrix: If singExc is false, inverting a singular
904
    // matrix produces an identity matrix.  If singExc is true,
905
    // inverting a singular matrix throws a SingMatrixExc.
906
    //
907
    // inverse() and invert() invert matrices using determinants;
908
    // gjInverse() and gjInvert() use the Gauss-Jordan method.
909
    //
910
    // inverse() and invert() are significantly faster than
911
    // gjInverse() and gjInvert(), but the results may be slightly
912
    // less accurate.
913
    // 
914
    //------------------------------------------------------------
915
916
    const Matrix44 &    invert (bool singExc = false);
917
918
    Matrix44<T>         inverse (bool singExc = false) const;
919
920
    const Matrix44 &    gjInvert (bool singExc = false);
921
922
    Matrix44<T>         gjInverse (bool singExc = false) const;
923
924
925
    //------------------------------------------------
926
    // Calculate the matrix minor of the (r,c) element
927
    //------------------------------------------------
928
929
    T                   minorOf (const int r, const int c) const;
930
931
    //---------------------------------------------------
932
    // Build a minor using the specified rows and columns
933
    //---------------------------------------------------
934
935
    T                   fastMinor (const int r0, const int r1, const int r2,
936
                                   const int c0, const int c1, const int c2) const;
937
938
    //------------
939
    // Determinant
940
    //------------
941
942
    T                   determinant() const;
943
944
    //--------------------------------------------------------
945
    // Set matrix to rotation by XYZ euler angles (in radians)
946
    //--------------------------------------------------------
947
948
    template <class S>
949
    const Matrix44 &    setEulerAngles (const Vec3<S>& r);
950
951
952
    //--------------------------------------------------------
953
    // Set matrix to rotation around given axis by given angle
954
    //--------------------------------------------------------
955
956
    template <class S>
957
    const Matrix44 &    setAxisAngle (const Vec3<S>& ax, S ang);
958
959
960
    //-------------------------------------------
961
    // Rotate the matrix by XYZ euler angles in r
962
    //-------------------------------------------
963
964
    template <class S>
965
    const Matrix44 &    rotate (const Vec3<S> &r);
966
967
968
    //--------------------------------------------
969
    // Set matrix to scale by given uniform factor
970
    //--------------------------------------------
971
972
    const Matrix44 &    setScale (T s);
973
974
975
    //------------------------------------
976
    // Set matrix to scale by given vector
977
    //------------------------------------
978
979
    template <class S>
980
    const Matrix44 &    setScale (const Vec3<S> &s);
981
982
983
    //----------------------
984
    // Scale the matrix by s
985
    //----------------------
986
987
    template <class S>
988
    const Matrix44 &    scale (const Vec3<S> &s);
989
990
991
    //------------------------------------------
992
    // Set matrix to translation by given vector
993
    //------------------------------------------
994
995
    template <class S>
996
    const Matrix44 &    setTranslation (const Vec3<S> &t);
997
998
999
    //-----------------------------
1000
    // Return translation component
1001
    //-----------------------------
1002
1003
    const Vec3<T>       translation () const;
1004
1005
1006
    //--------------------------
1007
    // Translate the matrix by t
1008
    //--------------------------
1009
1010
    template <class S>
1011
    const Matrix44 &    translate (const Vec3<S> &t);
1012
1013
1014
    //-------------------------------------------------------------
1015
    // Set matrix to shear by given vector h.  The resulting matrix
1016
    //    will shear x for each y coord. by a factor of h[0] ;
1017
    //    will shear x for each z coord. by a factor of h[1] ;
1018
    //    will shear y for each z coord. by a factor of h[2] .
1019
    //-------------------------------------------------------------
1020
1021
    template <class S>
1022
    const Matrix44 &    setShear (const Vec3<S> &h);
1023
1024
1025
    //------------------------------------------------------------
1026
    // Set matrix to shear by given factors.  The resulting matrix
1027
    //    will shear x for each y coord. by a factor of h.xy ;
1028
    //    will shear x for each z coord. by a factor of h.xz ;
1029
    //    will shear y for each z coord. by a factor of h.yz ; 
1030
    //    will shear y for each x coord. by a factor of h.yx ;
1031
    //    will shear z for each x coord. by a factor of h.zx ;
1032
    //    will shear z for each y coord. by a factor of h.zy .
1033
    //------------------------------------------------------------
1034
1035
    template <class S>
1036
    const Matrix44 &    setShear (const Shear6<S> &h);
1037
1038
1039
    //--------------------------------------------------------
1040
    // Shear the matrix by given vector.  The composed matrix 
1041
    // will be <shear> * <this>, where the shear matrix ...
1042
    //    will shear x for each y coord. by a factor of h[0] ;
1043
    //    will shear x for each z coord. by a factor of h[1] ;
1044
    //    will shear y for each z coord. by a factor of h[2] .
1045
    //--------------------------------------------------------
1046
1047
    template <class S>
1048
    const Matrix44 &    shear (const Vec3<S> &h);
1049
1050
    //--------------------------------------------------------
1051
    // Number of the row and column dimensions, since
1052
    // Matrix44 is a square matrix.
1053
    //--------------------------------------------------------
1054
1055
    static unsigned int dimensions() {return 4;}
1056
1057
1058
    //------------------------------------------------------------
1059
    // Shear the matrix by the given factors.  The composed matrix 
1060
    // will be <shear> * <this>, where the shear matrix ...
1061
    //    will shear x for each y coord. by a factor of h.xy ;
1062
    //    will shear x for each z coord. by a factor of h.xz ;
1063
    //    will shear y for each z coord. by a factor of h.yz ;
1064
    //    will shear y for each x coord. by a factor of h.yx ;
1065
    //    will shear z for each x coord. by a factor of h.zx ;
1066
    //    will shear z for each y coord. by a factor of h.zy .
1067
    //------------------------------------------------------------
1068
1069
    template <class S>
1070
    const Matrix44 &    shear (const Shear6<S> &h);
1071
1072
1073
    //-------------------------------------------------
1074
    // Limitations of type T (see also class limits<T>)
1075
    //-------------------------------------------------
1076
1077
    static T            baseTypeMin()           {return limits<T>::min();}
1078
    static T            baseTypeMax()           {return limits<T>::max();}
1079
    static T            baseTypeSmallest()      {return limits<T>::smallest();}
1080
    static T            baseTypeEpsilon()       {return limits<T>::epsilon();}
1081
1082
    typedef T   BaseType;
1083
    typedef Vec4<T> BaseVecType;
1084
1085
  private:
1086
1087
    template <typename R, typename S>
1088
    struct isSameType
1089
    {
1090
        enum {value = 0};
1091
    };
1092
1093
    template <typename R>
1094
    struct isSameType<R, R>
1095
    {
1096
        enum {value = 1};
1097
    };
1098
};
1099
1100
1101
//--------------
1102
// Stream output
1103
//--------------
1104
1105
template <class T>
1106
std::ostream &  operator << (std::ostream & s, const Matrix22<T> &m); 
1107
1108
template <class T>
1109
std::ostream &  operator << (std::ostream & s, const Matrix33<T> &m); 
1110
1111
template <class T>
1112
std::ostream &  operator << (std::ostream & s, const Matrix44<T> &m); 
1113
1114
1115
//---------------------------------------------
1116
// Vector-times-matrix multiplication operators
1117
//---------------------------------------------
1118
1119
1120
template <class S, class T>
1121
const Vec2<S> &            operator *= (Vec2<S> &v, const Matrix22<T> &m);
1122
1123
template <class S, class T>
1124
Vec2<S>                    operator * (const Vec2<S> &v, const Matrix22<T> &m);
1125
1126
template <class S, class T>
1127
const Vec2<S> &            operator *= (Vec2<S> &v, const Matrix33<T> &m);
1128
1129
template <class S, class T>
1130
Vec2<S>                    operator * (const Vec2<S> &v, const Matrix33<T> &m);
1131
1132
template <class S, class T>
1133
const Vec3<S> &            operator *= (Vec3<S> &v, const Matrix33<T> &m);
1134
1135
template <class S, class T>
1136
Vec3<S>                    operator * (const Vec3<S> &v, const Matrix33<T> &m);
1137
1138
template <class S, class T>
1139
const Vec3<S> &            operator *= (Vec3<S> &v, const Matrix44<T> &m);
1140
1141
template <class S, class T>
1142
Vec3<S>                    operator * (const Vec3<S> &v, const Matrix44<T> &m);
1143
1144
template <class S, class T>
1145
const Vec4<S> &            operator *= (Vec4<S> &v, const Matrix44<T> &m);
1146
1147
template <class S, class T>
1148
Vec4<S>                    operator * (const Vec4<S> &v, const Matrix44<T> &m);
1149
1150
//-------------------------
1151
// Typedefs for convenience
1152
//-------------------------
1153
1154
typedef Matrix22 <float>  M22f;
1155
typedef Matrix22 <double> M22d;
1156
typedef Matrix33 <float>  M33f;
1157
typedef Matrix33 <double> M33d;
1158
typedef Matrix44 <float>  M44f;
1159
typedef Matrix44 <double> M44d;
1160
1161
1162
//---------------------------
1163
// Implementation of Matrix22
1164
//---------------------------
1165
1166
template <class T>
1167
inline T *
1168
Matrix22<T>::operator [] (int i)
1169
{
1170
    return x[i];
1171
}
1172
1173
template <class T>
1174
inline const T *
1175
Matrix22<T>::operator [] (int i) const
1176
{
1177
    return x[i];
1178
}
1179
1180
template <class T>
1181
inline
1182
Matrix22<T>::Matrix22 ()
1183
{
1184
    x[0][0] = 1;
1185
    x[0][1] = 0;
1186
    x[1][0] = 0;
1187
    x[1][1] = 1;
1188
}
1189
1190
template <class T>
1191
inline
1192
Matrix22<T>::Matrix22 (T a)
1193
{
1194
    x[0][0] = a;
1195
    x[0][1] = a;
1196
    x[1][0] = a;
1197
    x[1][1] = a;
1198
}
1199
1200
template <class T>
1201
inline
1202
Matrix22<T>::Matrix22 (const T a[2][2]) 
1203
{
1204
    memcpy (x, a, sizeof (x));
1205
}
1206
1207
template <class T>
1208
inline
1209
Matrix22<T>::Matrix22 (T a, T b, T c, T d)
1210
{
1211
    x[0][0] = a;
1212
    x[0][1] = b;
1213
    x[1][0] = c;
1214
    x[1][1] = d;
1215
}
1216
1217
template <class T>
1218
inline
1219
Matrix22<T>::Matrix22 (const Matrix22 &v)
1220
{
1221
    memcpy (x, v.x, sizeof (x));
1222
}
1223
1224
template <class T>
1225
template <class S>
1226
inline
1227
Matrix22<T>::Matrix22 (const Matrix22<S> &v)
1228
{
1229
    x[0][0] = T (v.x[0][0]);
1230
    x[0][1] = T (v.x[0][1]);
1231
    x[1][0] = T (v.x[1][0]);
1232
    x[1][1] = T (v.x[1][1]);
1233
}
1234
1235
template <class T>
1236
inline const Matrix22<T> &
1237
Matrix22<T>::operator = (const Matrix22 &v)
1238
{
1239
    memcpy (x, v.x, sizeof (x));
1240
    return *this;
1241
}
1242
1243
template <class T>
1244
inline const Matrix22<T> &
1245
Matrix22<T>::operator = (T a)
1246
{
1247
    x[0][0] = a;
1248
    x[0][1] = a;
1249
    x[1][0] = a;
1250
    x[1][1] = a;
1251
    return *this;
1252
}
1253
1254
template <class T>
1255
inline T *
1256
Matrix22<T>::getValue ()
1257
{
1258
    return (T *) &x[0][0];
1259
}
1260
1261
template <class T>
1262
inline const T *
1263
Matrix22<T>::getValue () const
1264
{
1265
    return (const T *) &x[0][0];
1266
}
1267
1268
template <class T>
1269
template <class S>
1270
inline void
1271
Matrix22<T>::getValue (Matrix22<S> &v) const
1272
{
1273
    if (isSameType<S,T>::value)
1274
    {
1275
        memcpy (v.x, x, sizeof (x));
1276
    }
1277
    else
1278
    {
1279
        v.x[0][0] = x[0][0];
1280
        v.x[0][1] = x[0][1];
1281
        v.x[1][0] = x[1][0];
1282
        v.x[1][1] = x[1][1];
1283
    }
1284
}
1285
1286
template <class T>
1287
template <class S>
1288
inline Matrix22<T> &
1289
Matrix22<T>::setValue (const Matrix22<S> &v)
1290
{
1291
    if (isSameType<S,T>::value)
1292
    {
1293
        memcpy (x, v.x, sizeof (x));
1294
    }
1295
    else
1296
    {
1297
        x[0][0] = v.x[0][0];
1298
        x[0][1] = v.x[0][1];
1299
        x[1][0] = v.x[1][0];
1300
        x[1][1] = v.x[1][1];
1301
    }
1302
1303
    return *this;
1304
}
1305
1306
template <class T>
1307
template <class S>
1308
inline Matrix22<T> &
1309
Matrix22<T>::setTheMatrix (const Matrix22<S> &v)
1310
{
1311
    if (isSameType<S,T>::value)
1312
    {
1313
        memcpy (x, v.x, sizeof (x));
1314
    }
1315
    else
1316
    {
1317
        x[0][0] = v.x[0][0];
1318
        x[0][1] = v.x[0][1];
1319
        x[1][0] = v.x[1][0];
1320
        x[1][1] = v.x[1][1];
1321
    }
1322
1323
    return *this;
1324
}
1325
1326
template <class T>
1327
inline void
1328
Matrix22<T>::makeIdentity()
1329
{
1330
    x[0][0] = 1;
1331
    x[0][1] = 0;
1332
    x[1][0] = 0;
1333
    x[1][1] = 1;
1334
}
1335
1336
template <class T>
1337
bool
1338
Matrix22<T>::operator == (const Matrix22 &v) const
1339
{
1340
    return x[0][0] == v.x[0][0] &&
1341
           x[0][1] == v.x[0][1] &&
1342
           x[1][0] == v.x[1][0] &&
1343
           x[1][1] == v.x[1][1];
1344
}
1345
1346
template <class T>
1347
bool
1348
Matrix22<T>::operator != (const Matrix22 &v) const
1349
{
1350
    return x[0][0] != v.x[0][0] ||
1351
           x[0][1] != v.x[0][1] ||
1352
           x[1][0] != v.x[1][0] ||
1353
           x[1][1] != v.x[1][1];
1354
}
1355
1356
template <class T>
1357
bool
1358
Matrix22<T>::equalWithAbsError (const Matrix22<T> &m, T e) const
1359
{
1360
    for (int i = 0; i < 2; i++)
1361
        for (int j = 0; j < 2; j++)
1362
            if (!IMATH_INTERNAL_NAMESPACE::equalWithAbsError ((*this)[i][j], m[i][j], e))
1363
                return false;
1364
1365
    return true;
1366
}
1367
1368
template <class T>
1369
bool
1370
Matrix22<T>::equalWithRelError (const Matrix22<T> &m, T e) const
1371
{
1372
    for (int i = 0; i < 2; i++)
1373
        for (int j = 0; j < 2; j++)
1374
            if (!IMATH_INTERNAL_NAMESPACE::equalWithRelError ((*this)[i][j], m[i][j], e))
1375
                return false;
1376
1377
    return true;
1378
}
1379
1380
template <class T>
1381
const Matrix22<T> &
1382
Matrix22<T>::operator += (const Matrix22<T> &v)
1383
{
1384
    x[0][0] += v.x[0][0];
1385
    x[0][1] += v.x[0][1];
1386
    x[1][0] += v.x[1][0];
1387
    x[1][1] += v.x[1][1];
1388
1389
    return *this;
1390
}
1391
1392
template <class T>
1393
const Matrix22<T> &
1394
Matrix22<T>::operator += (T a)
1395
{
1396
    x[0][0] += a;
1397
    x[0][1] += a;
1398
    x[1][0] += a;
1399
    x[1][1] += a;
1400
  
1401
    return *this;
1402
}
1403
1404
template <class T>
1405
Matrix22<T>
1406
Matrix22<T>::operator + (const Matrix22<T> &v) const
1407
{
1408
    return Matrix22 (x[0][0] + v.x[0][0],
1409
                     x[0][1] + v.x[0][1],
1410
                     x[1][0] + v.x[1][0],
1411
                     x[1][1] + v.x[1][1]);
1412
}
1413
1414
template <class T>
1415
const Matrix22<T> &
1416
Matrix22<T>::operator -= (const Matrix22<T> &v)
1417
{
1418
    x[0][0] -= v.x[0][0];
1419
    x[0][1] -= v.x[0][1];
1420
    x[1][0] -= v.x[1][0];
1421
    x[1][1] -= v.x[1][1];
1422
  
1423
    return *this;
1424
}
1425
1426
template <class T>
1427
const Matrix22<T> &
1428
Matrix22<T>::operator -= (T a)
1429
{
1430
    x[0][0] -= a;
1431
    x[0][1] -= a;
1432
    x[1][0] -= a;
1433
    x[1][1] -= a;
1434
  
1435
    return *this;
1436
}
1437
1438
template <class T>
1439
Matrix22<T>
1440
Matrix22<T>::operator - (const Matrix22<T> &v) const
1441
{
1442
    return Matrix22 (x[0][0] - v.x[0][0],
1443
                     x[0][1] - v.x[0][1],
1444
                     x[1][0] - v.x[1][0],
1445
                     x[1][1] - v.x[1][1]);
1446
}
1447
1448
template <class T>
1449
Matrix22<T>
1450
Matrix22<T>::operator - () const
1451
{
1452
    return Matrix22 (-x[0][0],
1453
                     -x[0][1],
1454
                     -x[1][0],
1455
                     -x[1][1]);
1456
}
1457
1458
template <class T>
1459
const Matrix22<T> &
1460
Matrix22<T>::negate ()
1461
{
1462
    x[0][0] = -x[0][0];
1463
    x[0][1] = -x[0][1];
1464
    x[1][0] = -x[1][0];
1465
    x[1][1] = -x[1][1];
1466
1467
    return *this;
1468
}
1469
1470
template <class T>
1471
const Matrix22<T> &
1472
Matrix22<T>::operator *= (T a)
1473
{
1474
    x[0][0] *= a;
1475
    x[0][1] *= a;
1476
    x[1][0] *= a;
1477
    x[1][1] *= a;
1478
  
1479
    return *this;
1480
}
1481
1482
template <class T>
1483
Matrix22<T>
1484
Matrix22<T>::operator * (T a) const
1485
{
1486
    return Matrix22 (x[0][0] * a,
1487
                     x[0][1] * a,
1488
                     x[1][0] * a,
1489
                     x[1][1] * a);
1490
}
1491
1492
template <class T>
1493
inline Matrix22<T>
1494
operator * (T a, const Matrix22<T> &v)
1495
{
1496
    return v * a;
1497
}
1498
1499
template <class T>
1500
const Matrix22<T> &
1501
Matrix22<T>::operator *= (const Matrix22<T> &v)
1502
{
1503
    Matrix22 tmp (T (0));
1504
1505
    for (int i = 0; i < 2; i++)
1506
        for (int j = 0; j < 2; j++)
1507
            for (int k = 0; k < 2; k++)
1508
                tmp.x[i][j] += x[i][k] * v.x[k][j];
1509
1510
    *this = tmp;
1511
    return *this;
1512
}
1513
1514
template <class T>
1515
Matrix22<T>
1516
Matrix22<T>::operator * (const Matrix22<T> &v) const
1517
{
1518
    Matrix22 tmp (T (0));
1519
1520
    for (int i = 0; i < 2; i++)
1521
        for (int j = 0; j < 2; j++)
1522
            for (int k = 0; k < 2; k++)
1523
                tmp.x[i][j] += x[i][k] * v.x[k][j];
1524
1525
    return tmp;
1526
}
1527
1528
template <class T>
1529
template <class S>
1530
void
1531
Matrix22<T>::multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const
1532
{
1533
    S a, b;
1534
1535
    a = src[0] * x[0][0] + src[1] * x[1][0];
1536
    b = src[0] * x[0][1] + src[1] * x[1][1];
1537
1538
    dst.x = a;
1539
    dst.y = b;
1540
}
1541
1542
template <class T>
1543
const Matrix22<T> &
1544
Matrix22<T>::operator /= (T a)
1545
{
1546
    x[0][0] /= a;
1547
    x[0][1] /= a;
1548
    x[1][0] /= a;
1549
    x[1][1] /= a;
1550
  
1551
    return *this;
1552
}
1553
1554
template <class T>
1555
Matrix22<T>
1556
Matrix22<T>::operator / (T a) const
1557
{
1558
    return Matrix22 (x[0][0] / a,
1559
                     x[0][1] / a,
1560
                     x[1][0] / a,
1561
                     x[1][1] / a);
1562
}
1563
1564
template <class T>
1565
const Matrix22<T> &
1566
Matrix22<T>::transpose ()
1567
{
1568
    Matrix22 tmp (x[0][0],
1569
                  x[1][0],
1570
                  x[0][1],
1571
                  x[1][1]);
1572
    *this = tmp;
1573
    return *this;
1574
}
1575
1576
template <class T>
1577
Matrix22<T>
1578
Matrix22<T>::transposed () const
1579
{
1580
    return Matrix22 (x[0][0],
1581
                     x[1][0],
1582
                     x[0][1],
1583
                     x[1][1]);
1584
}
1585
1586
template <class T>
1587
const Matrix22<T> &
1588
Matrix22<T>::invert (bool singExc)
1589
{
1590
    *this = inverse (singExc);
1591
    return *this;
1592
}
1593
1594
template <class T>
1595
Matrix22<T>
1596
Matrix22<T>::inverse (bool singExc) const
1597
{
1598
    Matrix22 s ( x[1][1],  -x[0][1],
1599
                -x[1][0],   x[0][0]);
1600
1601
    T r = x[0][0] * x[1][1] - x[1][0] * x[0][1];
1602
1603
    if (IMATH_INTERNAL_NAMESPACE::abs (r) >= 1)
1604
    {
1605
        for (int i = 0; i < 2; ++i)
1606
        {
1607
            for (int j = 0; j < 2; ++j)
1608
            {
1609
                s[i][j] /= r;
1610
            }
1611
        }
1612
    }
1613
    else
1614
    {
1615
        T mr = IMATH_INTERNAL_NAMESPACE::abs (r) / limits<T>::smallest();
1616
1617
        for (int i = 0; i < 2; ++i)
1618
        {
1619
            for (int j = 0; j < 2; ++j)
1620
            {
1621
                if (mr > IMATH_INTERNAL_NAMESPACE::abs (s[i][j]))
1622
                {
1623
                    s[i][j] /= r;
1624
                }
1625
                else
1626
                {
1627
                    if (singExc)
1628
                        throw SingMatrixExc ("Cannot invert "
1629
                                                "singular matrix.");
1630
                    return Matrix22();
1631
                }
1632
            }
1633
        }
1634
    }
1635
    return s;
1636
}
1637
1638
template <class T>
1639
inline T
1640
Matrix22<T>::determinant () const
1641
{
1642
    return x[0][0] * x[1][1] - x[1][0] * x[0][1];
1643
}
1644
1645
template <class T>
1646
template <class S>
1647
const Matrix22<T> &
1648
Matrix22<T>::setRotation (S r)
1649
{
1650
    S cos_r, sin_r;
1651
1652
    cos_r = Math<T>::cos (r);
1653
    sin_r = Math<T>::sin (r);
1654
1655
    x[0][0] =  cos_r;
1656
    x[0][1] =  sin_r;
1657
1658
    x[1][0] =  -sin_r;
1659
    x[1][1] =  cos_r;
1660
1661
    return *this;
1662
}
1663
1664
template <class T>
1665
template <class S>
1666
const Matrix22<T> &
1667
Matrix22<T>::rotate (S r)
1668
{
1669
    *this *= Matrix22<T>().setRotation (r);
1670
    return *this;
1671
}
1672
1673
template <class T>
1674
const Matrix22<T> &
1675
Matrix22<T>::setScale (T s)
1676
{
1677
    x[0][0] = s;
1678
    x[0][1] = static_cast<T>(0);
1679
    x[1][0] = static_cast<T>(0);
1680
    x[1][1] = s;
1681
1682
    return *this;
1683
}
1684
1685
template <class T>
1686
template <class S>
1687
const Matrix22<T> &
1688
Matrix22<T>::setScale (const Vec2<S> &s)
1689
{
1690
    x[0][0] = s[0];
1691
    x[0][1] = static_cast<T>(0);
1692
    x[1][0] = static_cast<T>(0);
1693
    x[1][1] = s[1];
1694
1695
    return *this;
1696
}
1697
1698
template <class T>
1699
template <class S>
1700
const Matrix22<T> &
1701
Matrix22<T>::scale (const Vec2<S> &s)
1702
{
1703
    x[0][0] *= s[0];
1704
    x[0][1] *= s[0];
1705
1706
    x[1][0] *= s[1];
1707
    x[1][1] *= s[1];
1708
1709
    return *this;
1710
}
1711
1712
1713
//---------------------------
1714
// Implementation of Matrix33
1715
//---------------------------
1716
1717
template <class T>
1718
inline T *
1719
Matrix33<T>::operator [] (int i)
1720
{
1721
    return x[i];
1722
}
1723
1724
template <class T>
1725
inline const T *
1726
Matrix33<T>::operator [] (int i) const
1727
{
1728
    return x[i];
1729
}
1730
1731
template <class T>
1732
inline
1733
Matrix33<T>::Matrix33 ()
1734
{
1735
    memset (x, 0, sizeof (x));
1736
    x[0][0] = 1;
1737
    x[1][1] = 1;
1738
    x[2][2] = 1;
1739
}
1740
1741
template <class T>
1742
inline
1743
Matrix33<T>::Matrix33 (T a)
1744
{
1745
    x[0][0] = a;
1746
    x[0][1] = a;
1747
    x[0][2] = a;
1748
    x[1][0] = a;
1749
    x[1][1] = a;
1750
    x[1][2] = a;
1751
    x[2][0] = a;
1752
    x[2][1] = a;
1753
    x[2][2] = a;
1754
}
1755
1756
template <class T>
1757
inline
1758
Matrix33<T>::Matrix33 (const T a[3][3]) 
1759
{
1760
    memcpy (x, a, sizeof (x));
1761
}
1762
1763
template <class T>
1764
inline
1765
Matrix33<T>::Matrix33 (T a, T b, T c, T d, T e, T f, T g, T h, T i)
1766
{
1767
    x[0][0] = a;
1768
    x[0][1] = b;
1769
    x[0][2] = c;
1770
    x[1][0] = d;
1771
    x[1][1] = e;
1772
    x[1][2] = f;
1773
    x[2][0] = g;
1774
    x[2][1] = h;
1775
    x[2][2] = i;
1776
}
1777
1778
template <class T>
1779
inline
1780
Matrix33<T>::Matrix33 (const Matrix33 &v)
1781
{
1782
    memcpy (x, v.x, sizeof (x));
1783
}
1784
1785
template <class T>
1786
template <class S>
1787
inline
1788
Matrix33<T>::Matrix33 (const Matrix33<S> &v)
1789
{
1790
    x[0][0] = T (v.x[0][0]);
1791
    x[0][1] = T (v.x[0][1]);
1792
    x[0][2] = T (v.x[0][2]);
1793
    x[1][0] = T (v.x[1][0]);
1794
    x[1][1] = T (v.x[1][1]);
1795
    x[1][2] = T (v.x[1][2]);
1796
    x[2][0] = T (v.x[2][0]);
1797
    x[2][1] = T (v.x[2][1]);
1798
    x[2][2] = T (v.x[2][2]);
1799
}
1800
1801
template <class T>
1802
inline const Matrix33<T> &
1803
Matrix33<T>::operator = (const Matrix33 &v)
1804
{
1805
    memcpy (x, v.x, sizeof (x));
1806
    return *this;
1807
}
1808
1809
template <class T>
1810
inline const Matrix33<T> &
1811
Matrix33<T>::operator = (T a)
1812
{
1813
    x[0][0] = a;
1814
    x[0][1] = a;
1815
    x[0][2] = a;
1816
    x[1][0] = a;
1817
    x[1][1] = a;
1818
    x[1][2] = a;
1819
    x[2][0] = a;
1820
    x[2][1] = a;
1821
    x[2][2] = a;
1822
    return *this;
1823
}
1824
1825
template <class T>
1826
inline T *
1827
Matrix33<T>::getValue ()
1828
{
1829
    return (T *) &x[0][0];
1830
}
1831
1832
template <class T>
1833
inline const T *
1834
Matrix33<T>::getValue () const
1835
{
1836
    return (const T *) &x[0][0];
1837
}
1838
1839
template <class T>
1840
template <class S>
1841
inline void
1842
Matrix33<T>::getValue (Matrix33<S> &v) const
1843
{
1844
    if (isSameType<S,T>::value)
1845
    {
1846
        memcpy (v.x, x, sizeof (x));
1847
    }
1848
    else
1849
    {
1850
        v.x[0][0] = x[0][0];
1851
        v.x[0][1] = x[0][1];
1852
        v.x[0][2] = x[0][2];
1853
        v.x[1][0] = x[1][0];
1854
        v.x[1][1] = x[1][1];
1855
        v.x[1][2] = x[1][2];
1856
        v.x[2][0] = x[2][0];
1857
        v.x[2][1] = x[2][1];
1858
        v.x[2][2] = x[2][2];
1859
    }
1860
}
1861
1862
template <class T>
1863
template <class S>
1864
inline Matrix33<T> &
1865
Matrix33<T>::setValue (const Matrix33<S> &v)
1866
{
1867
    if (isSameType<S,T>::value)
1868
    {
1869
        memcpy (x, v.x, sizeof (x));
1870
    }
1871
    else
1872
    {
1873
        x[0][0] = v.x[0][0];
1874
        x[0][1] = v.x[0][1];
1875
        x[0][2] = v.x[0][2];
1876
        x[1][0] = v.x[1][0];
1877
        x[1][1] = v.x[1][1];
1878
        x[1][2] = v.x[1][2];
1879
        x[2][0] = v.x[2][0];
1880
        x[2][1] = v.x[2][1];
1881
        x[2][2] = v.x[2][2];
1882
    }
1883
1884
    return *this;
1885
}
1886
1887
template <class T>
1888
template <class S>
1889
inline Matrix33<T> &
1890
Matrix33<T>::setTheMatrix (const Matrix33<S> &v)
1891
{
1892
    if (isSameType<S,T>::value)
1893
    {
1894
        memcpy (x, v.x, sizeof (x));
1895
    }
1896
    else
1897
    {
1898
        x[0][0] = v.x[0][0];
1899
        x[0][1] = v.x[0][1];
1900
        x[0][2] = v.x[0][2];
1901
        x[1][0] = v.x[1][0];
1902
        x[1][1] = v.x[1][1];
1903
        x[1][2] = v.x[1][2];
1904
        x[2][0] = v.x[2][0];
1905
        x[2][1] = v.x[2][1];
1906
        x[2][2] = v.x[2][2];
1907
    }
1908
1909
    return *this;
1910
}
1911
1912
template <class T>
1913
inline void
1914
Matrix33<T>::makeIdentity()
1915
{
1916
    memset (x, 0, sizeof (x));
1917
    x[0][0] = 1;
1918
    x[1][1] = 1;
1919
    x[2][2] = 1;
1920
}
1921
1922
template <class T>
1923
bool
1924
Matrix33<T>::operator == (const Matrix33 &v) const
1925
{
1926
    return x[0][0] == v.x[0][0] &&
1927
           x[0][1] == v.x[0][1] &&
1928
           x[0][2] == v.x[0][2] &&
1929
           x[1][0] == v.x[1][0] &&
1930
           x[1][1] == v.x[1][1] &&
1931
           x[1][2] == v.x[1][2] &&
1932
           x[2][0] == v.x[2][0] &&
1933
           x[2][1] == v.x[2][1] &&
1934
           x[2][2] == v.x[2][2];
1935
}
1936
1937
template <class T>
1938
bool
1939
Matrix33<T>::operator != (const Matrix33 &v) const
1940
{
1941
    return x[0][0] != v.x[0][0] ||
1942
           x[0][1] != v.x[0][1] ||
1943
           x[0][2] != v.x[0][2] ||
1944
           x[1][0] != v.x[1][0] ||
1945
           x[1][1] != v.x[1][1] ||
1946
           x[1][2] != v.x[1][2] ||
1947
           x[2][0] != v.x[2][0] ||
1948
           x[2][1] != v.x[2][1] ||
1949
           x[2][2] != v.x[2][2];
1950
}
1951
1952
template <class T>
1953
bool
1954
Matrix33<T>::equalWithAbsError (const Matrix33<T> &m, T e) const
1955
{
1956
    for (int i = 0; i < 3; i++)
1957
        for (int j = 0; j < 3; j++)
1958
            if (!IMATH_INTERNAL_NAMESPACE::equalWithAbsError ((*this)[i][j], m[i][j], e))
1959
                return false;
1960
1961
    return true;
1962
}
1963
1964
template <class T>
1965
bool
1966
Matrix33<T>::equalWithRelError (const Matrix33<T> &m, T e) const
1967
{
1968
    for (int i = 0; i < 3; i++)
1969
        for (int j = 0; j < 3; j++)
1970
            if (!IMATH_INTERNAL_NAMESPACE::equalWithRelError ((*this)[i][j], m[i][j], e))
1971
                return false;
1972
1973
    return true;
1974
}
1975
1976
template <class T>
1977
const Matrix33<T> &
1978
Matrix33<T>::operator += (const Matrix33<T> &v)
1979
{
1980
    x[0][0] += v.x[0][0];
1981
    x[0][1] += v.x[0][1];
1982
    x[0][2] += v.x[0][2];
1983
    x[1][0] += v.x[1][0];
1984
    x[1][1] += v.x[1][1];
1985
    x[1][2] += v.x[1][2];
1986
    x[2][0] += v.x[2][0];
1987
    x[2][1] += v.x[2][1];
1988
    x[2][2] += v.x[2][2];
1989
1990
    return *this;
1991
}
1992
1993
template <class T>
1994
const Matrix33<T> &
1995
Matrix33<T>::operator += (T a)
1996
{
1997
    x[0][0] += a;
1998
    x[0][1] += a;
1999
    x[0][2] += a;
2000
    x[1][0] += a;
2001
    x[1][1] += a;
2002
    x[1][2] += a;
2003
    x[2][0] += a;
2004
    x[2][1] += a;
2005
    x[2][2] += a;
2006
  
2007
    return *this;
2008
}
2009
2010
template <class T>
2011
Matrix33<T>
2012
Matrix33<T>::operator + (const Matrix33<T> &v) const
2013
{
2014
    return Matrix33 (x[0][0] + v.x[0][0],
2015
                     x[0][1] + v.x[0][1],
2016
                     x[0][2] + v.x[0][2],
2017
                     x[1][0] + v.x[1][0],
2018
                     x[1][1] + v.x[1][1],
2019
                     x[1][2] + v.x[1][2],
2020
                     x[2][0] + v.x[2][0],
2021
                     x[2][1] + v.x[2][1],
2022
                     x[2][2] + v.x[2][2]);
2023
}
2024
2025
template <class T>
2026
const Matrix33<T> &
2027
Matrix33<T>::operator -= (const Matrix33<T> &v)
2028
{
2029
    x[0][0] -= v.x[0][0];
2030
    x[0][1] -= v.x[0][1];
2031
    x[0][2] -= v.x[0][2];
2032
    x[1][0] -= v.x[1][0];
2033
    x[1][1] -= v.x[1][1];
2034
    x[1][2] -= v.x[1][2];
2035
    x[2][0] -= v.x[2][0];
2036
    x[2][1] -= v.x[2][1];
2037
    x[2][2] -= v.x[2][2];
2038
  
2039
    return *this;
2040
}
2041
2042
template <class T>
2043
const Matrix33<T> &
2044
Matrix33<T>::operator -= (T a)
2045
{
2046
    x[0][0] -= a;
2047
    x[0][1] -= a;
2048
    x[0][2] -= a;
2049
    x[1][0] -= a;
2050
    x[1][1] -= a;
2051
    x[1][2] -= a;
2052
    x[2][0] -= a;
2053
    x[2][1] -= a;
2054
    x[2][2] -= a;
2055
  
2056
    return *this;
2057
}
2058
2059
template <class T>
2060
Matrix33<T>
2061
Matrix33<T>::operator - (const Matrix33<T> &v) const
2062
{
2063
    return Matrix33 (x[0][0] - v.x[0][0],
2064
                     x[0][1] - v.x[0][1],
2065
                     x[0][2] - v.x[0][2],
2066
                     x[1][0] - v.x[1][0],
2067
                     x[1][1] - v.x[1][1],
2068
                     x[1][2] - v.x[1][2],
2069
                     x[2][0] - v.x[2][0],
2070
                     x[2][1] - v.x[2][1],
2071
                     x[2][2] - v.x[2][2]);
2072
}
2073
2074
template <class T>
2075
Matrix33<T>
2076
Matrix33<T>::operator - () const
2077
{
2078
    return Matrix33 (-x[0][0],
2079
                     -x[0][1],
2080
                     -x[0][2],
2081
                     -x[1][0],
2082
                     -x[1][1],
2083
                     -x[1][2],
2084
                     -x[2][0],
2085
                     -x[2][1],
2086
                     -x[2][2]);
2087
}
2088
2089
template <class T>
2090
const Matrix33<T> &
2091
Matrix33<T>::negate ()
2092
{
2093
    x[0][0] = -x[0][0];
2094
    x[0][1] = -x[0][1];
2095
    x[0][2] = -x[0][2];
2096
    x[1][0] = -x[1][0];
2097
    x[1][1] = -x[1][1];
2098
    x[1][2] = -x[1][2];
2099
    x[2][0] = -x[2][0];
2100
    x[2][1] = -x[2][1];
2101
    x[2][2] = -x[2][2];
2102
2103
    return *this;
2104
}
2105
2106
template <class T>
2107
const Matrix33<T> &
2108
Matrix33<T>::operator *= (T a)
2109
{
2110
    x[0][0] *= a;
2111
    x[0][1] *= a;
2112
    x[0][2] *= a;
2113
    x[1][0] *= a;
2114
    x[1][1] *= a;
2115
    x[1][2] *= a;
2116
    x[2][0] *= a;
2117
    x[2][1] *= a;
2118
    x[2][2] *= a;
2119
  
2120
    return *this;
2121
}
2122
2123
template <class T>
2124
Matrix33<T>
2125
Matrix33<T>::operator * (T a) const
2126
{
2127
    return Matrix33 (x[0][0] * a,
2128
                     x[0][1] * a,
2129
                     x[0][2] * a,
2130
                     x[1][0] * a,
2131
                     x[1][1] * a,
2132
                     x[1][2] * a,
2133
                     x[2][0] * a,
2134
                     x[2][1] * a,
2135
                     x[2][2] * a);
2136
}
2137
2138
template <class T>
2139
inline Matrix33<T>
2140
operator * (T a, const Matrix33<T> &v)
2141
{
2142
    return v * a;
2143
}
2144
2145
template <class T>
2146
const Matrix33<T> &
2147
Matrix33<T>::operator *= (const Matrix33<T> &v)
2148
{
2149
    Matrix33 tmp (T (0));
2150
2151
    for (int i = 0; i < 3; i++)
2152
        for (int j = 0; j < 3; j++)
2153
            for (int k = 0; k < 3; k++)
2154
                tmp.x[i][j] += x[i][k] * v.x[k][j];
2155
2156
    *this = tmp;
2157
    return *this;
2158
}
2159
2160
template <class T>
2161
Matrix33<T>
2162
Matrix33<T>::operator * (const Matrix33<T> &v) const
2163
{
2164
    Matrix33 tmp (T (0));
2165
2166
    for (int i = 0; i < 3; i++)
2167
        for (int j = 0; j < 3; j++)
2168
            for (int k = 0; k < 3; k++)
2169
                tmp.x[i][j] += x[i][k] * v.x[k][j];
2170
2171
    return tmp;
2172
}
2173
2174
template <class T>
2175
template <class S>
2176
void
2177
Matrix33<T>::multVecMatrix(const Vec2<S> &src, Vec2<S> &dst) const
2178
{
2179
    S a, b, w;
2180
2181
    a = src[0] * x[0][0] + src[1] * x[1][0] + x[2][0];
2182
    b = src[0] * x[0][1] + src[1] * x[1][1] + x[2][1];
2183
    w = src[0] * x[0][2] + src[1] * x[1][2] + x[2][2];
2184
2185
    dst.x = a / w;
2186
    dst.y = b / w;
2187
}
2188
2189
template <class T>
2190
template <class S>
2191
void
2192
Matrix33<T>::multDirMatrix(const Vec2<S> &src, Vec2<S> &dst) const
2193
{
2194
    S a, b;
2195
2196
    a = src[0] * x[0][0] + src[1] * x[1][0];
2197
    b = src[0] * x[0][1] + src[1] * x[1][1];
2198
2199
    dst.x = a;
2200
    dst.y = b;
2201
}
2202
2203
template <class T>
2204
const Matrix33<T> &
2205
Matrix33<T>::operator /= (T a)
2206
{
2207
    x[0][0] /= a;
2208
    x[0][1] /= a;
2209
    x[0][2] /= a;
2210
    x[1][0] /= a;
2211
    x[1][1] /= a;
2212
    x[1][2] /= a;
2213
    x[2][0] /= a;
2214
    x[2][1] /= a;
2215
    x[2][2] /= a;
2216
  
2217
    return *this;
2218
}
2219
2220
template <class T>
2221
Matrix33<T>
2222
Matrix33<T>::operator / (T a) const
2223
{
2224
    return Matrix33 (x[0][0] / a,
2225
                     x[0][1] / a,
2226
                     x[0][2] / a,
2227
                     x[1][0] / a,
2228
                     x[1][1] / a,
2229
                     x[1][2] / a,
2230
                     x[2][0] / a,
2231
                     x[2][1] / a,
2232
                     x[2][2] / a);
2233
}
2234
2235
template <class T>
2236
const Matrix33<T> &
2237
Matrix33<T>::transpose ()
2238
{
2239
    Matrix33 tmp (x[0][0],
2240
                  x[1][0],
2241
                  x[2][0],
2242
                  x[0][1],
2243
                  x[1][1],
2244
                  x[2][1],
2245
                  x[0][2],
2246
                  x[1][2],
2247
                  x[2][2]);
2248
    *this = tmp;
2249
    return *this;
2250
}
2251
2252
template <class T>
2253
Matrix33<T>
2254
Matrix33<T>::transposed () const
2255
{
2256
    return Matrix33 (x[0][0],
2257
                     x[1][0],
2258
                     x[2][0],
2259
                     x[0][1],
2260
                     x[1][1],
2261
                     x[2][1],
2262
                     x[0][2],
2263
                     x[1][2],
2264
                     x[2][2]);
2265
}
2266
2267
template <class T>
2268
const Matrix33<T> &
2269
Matrix33<T>::gjInvert (bool singExc)
2270
{
2271
    *this = gjInverse (singExc);
2272
    return *this;
2273
}
2274
2275
template <class T>
2276
Matrix33<T>
2277
Matrix33<T>::gjInverse (bool singExc) const
2278
{
2279
    int i, j, k;
2280
    Matrix33 s;
2281
    Matrix33 t (*this);
2282
2283
    // Forward elimination
2284
2285
    for (i = 0; i < 2 ; i++)
2286
    {
2287
        int pivot = i;
2288
2289
        T pivotsize = t[i][i];
2290
2291
        if (pivotsize < 0)
2292
            pivotsize = -pivotsize;
2293
2294
        for (j = i + 1; j < 3; j++)
2295
        {
2296
            T tmp = t[j][i];
2297
2298
            if (tmp < 0)
2299
                tmp = -tmp;
2300
2301
            if (tmp > pivotsize)
2302
            {
2303
                pivot = j;
2304
                pivotsize = tmp;
2305
            }
2306
        }
2307
2308
        if (pivotsize == 0)
2309
        {
2310
            if (singExc)
2311
                throw ::IMATH_INTERNAL_NAMESPACE::SingMatrixExc ("Cannot invert singular matrix.");
2312
2313
            return Matrix33();
2314
        }
2315
2316
        if (pivot != i)
2317
        {
2318
            for (j = 0; j < 3; j++)
2319
            {
2320
                T tmp;
2321
2322
                tmp = t[i][j];
2323
                t[i][j] = t[pivot][j];
2324
                t[pivot][j] = tmp;
2325
2326
                tmp = s[i][j];
2327
                s[i][j] = s[pivot][j];
2328
                s[pivot][j] = tmp;
2329
            }
2330
        }
2331
2332
        for (j = i + 1; j < 3; j++)
2333
        {
2334
            T f = t[j][i] / t[i][i];
2335
2336
            for (k = 0; k < 3; k++)
2337
            {
2338
                t[j][k] -= f * t[i][k];
2339
                s[j][k] -= f * s[i][k];
2340
            }
2341
        }
2342
    }
2343
2344
    // Backward substitution
2345
2346
    for (i = 2; i >= 0; --i)
2347
    {
2348
        T f;
2349
2350
        if ((f = t[i][i]) == 0)
2351
        {
2352
            if (singExc)
2353
                throw ::IMATH_INTERNAL_NAMESPACE::SingMatrixExc ("Cannot invert singular matrix.");
2354
2355
            return Matrix33();
2356
        }
2357
2358
        for (j = 0; j < 3; j++)
2359
        {
2360
            t[i][j] /= f;
2361
            s[i][j] /= f;
2362
        }
2363
2364
        for (j = 0; j < i; j++)
2365
        {
2366
            f = t[j][i];
2367
2368
            for (k = 0; k < 3; k++)
2369
            {
2370
                t[j][k] -= f * t[i][k];
2371
                s[j][k] -= f * s[i][k];
2372
            }
2373
        }
2374
    }
2375
2376
    return s;
2377
}
2378
2379
template <class T>
2380
const Matrix33<T> &
2381
Matrix33<T>::invert (bool singExc)
2382
{
2383
    *this = inverse (singExc);
2384
    return *this;
2385
}
2386
2387
template <class T>
2388
Matrix33<T>
2389
Matrix33<T>::inverse (bool singExc) const
2390
{
2391
    if (x[0][2] != 0 || x[1][2] != 0 || x[2][2] != 1)
2392
    {
2393
        Matrix33 s (x[1][1] * x[2][2] - x[2][1] * x[1][2],
2394
                    x[2][1] * x[0][2] - x[0][1] * x[2][2],
2395
                    x[0][1] * x[1][2] - x[1][1] * x[0][2],
2396
2397
                    x[2][0] * x[1][2] - x[1][0] * x[2][2],
2398
                    x[0][0] * x[2][2] - x[2][0] * x[0][2],
2399
                    x[1][0] * x[0][2] - x[0][0] * x[1][2],
2400
2401
                    x[1][0] * x[2][1] - x[2][0] * x[1][1],
2402
                    x[2][0] * x[0][1] - x[0][0] * x[2][1],
2403
                    x[0][0] * x[1][1] - x[1][0] * x[0][1]);
2404
2405
        T r = x[0][0] * s[0][0] + x[0][1] * s[1][0] + x[0][2] * s[2][0];
2406
2407
        if (IMATH_INTERNAL_NAMESPACE::abs (r) >= 1)
2408
        {
2409
            for (int i = 0; i < 3; ++i)
2410
            {
2411
                for (int j = 0; j < 3; ++j)
2412
                {
2413
                    s[i][j] /= r;
2414
                }
2415
            }
2416
        }
2417
        else
2418
        {
2419
            T mr = IMATH_INTERNAL_NAMESPACE::abs (r) / limits<T>::smallest();
2420
2421
            for (int i = 0; i < 3; ++i)
2422
            {
2423
                for (int j = 0; j < 3; ++j)
2424
                {
2425
                    if (mr > IMATH_INTERNAL_NAMESPACE::abs (s[i][j]))
2426
                    {
2427
                        s[i][j] /= r;
2428
                    }
2429
                    else
2430
                    {
2431
                        if (singExc)
2432
                            throw SingMatrixExc ("Cannot invert "
2433
                                                 "singular matrix.");
2434
                        return Matrix33();
2435
                    }
2436
                }
2437
            }
2438
        }
2439
2440
        return s;
2441
    }
2442
    else
2443
    {
2444
        Matrix33 s ( x[1][1],
2445
                    -x[0][1],
2446
                     0, 
2447
2448
                    -x[1][0],
2449
                     x[0][0],
2450
                     0,
2451
2452
                     0,
2453
                     0,
2454
                     1);
2455
2456
        T r = x[0][0] * x[1][1] - x[1][0] * x[0][1];
2457
2458
        if (IMATH_INTERNAL_NAMESPACE::abs (r) >= 1)
2459
        {
2460
            for (int i = 0; i < 2; ++i)
2461
            {
2462
                for (int j = 0; j < 2; ++j)
2463
                {
2464
                    s[i][j] /= r;
2465
                }
2466
            }
2467
        }
2468
        else
2469
        {
2470
            T mr = IMATH_INTERNAL_NAMESPACE::abs (r) / limits<T>::smallest();
2471
2472
            for (int i = 0; i < 2; ++i)
2473
            {
2474
                for (int j = 0; j < 2; ++j)
2475
                {
2476
                    if (mr > IMATH_INTERNAL_NAMESPACE::abs (s[i][j]))
2477
                    {
2478
                        s[i][j] /= r;
2479
                    }
2480
                    else
2481
                    {
2482
                        if (singExc)
2483
                            throw SingMatrixExc ("Cannot invert "
2484
                                                 "singular matrix.");
2485
                        return Matrix33();
2486
                    }
2487
                }
2488
            }
2489
        }
2490
2491
        s[2][0] = -x[2][0] * s[0][0] - x[2][1] * s[1][0];
2492
        s[2][1] = -x[2][0] * s[0][1] - x[2][1] * s[1][1];
2493
2494
        return s;
2495
    }
2496
}
2497
2498
template <class T>
2499
inline T
2500
Matrix33<T>::minorOf (const int r, const int c) const
2501
{
2502
    int r0 = 0 + (r < 1 ? 1 : 0);
2503
    int r1 = 1 + (r < 2 ? 1 : 0);
2504
    int c0 = 0 + (c < 1 ? 1 : 0);
2505
    int c1 = 1 + (c < 2 ? 1 : 0);
2506
2507
    return x[r0][c0]*x[r1][c1] - x[r1][c0]*x[r0][c1];
2508
}
2509
2510
template <class T>
2511
inline T
2512
Matrix33<T>::fastMinor( const int r0, const int r1,
2513
                        const int c0, const int c1) const
2514
{
2515
    return x[r0][c0]*x[r1][c1] - x[r0][c1]*x[r1][c0];
2516
}
2517
2518
template <class T>
2519
inline T
2520
Matrix33<T>::determinant () const
2521
{
2522
    return x[0][0]*(x[1][1]*x[2][2] - x[1][2]*x[2][1]) +
2523
           x[0][1]*(x[1][2]*x[2][0] - x[1][0]*x[2][2]) +
2524
           x[0][2]*(x[1][0]*x[2][1] - x[1][1]*x[2][0]);
2525
}
2526
2527
template <class T>
2528
template <class S>
2529
const Matrix33<T> &
2530
Matrix33<T>::setRotation (S r)
2531
{
2532
    S cos_r, sin_r;
2533
2534
    cos_r = Math<T>::cos (r);
2535
    sin_r = Math<T>::sin (r);
2536
2537
    x[0][0] =  cos_r;
2538
    x[0][1] =  sin_r;
2539
    x[0][2] =  0;
2540
2541
    x[1][0] =  -sin_r;
2542
    x[1][1] =  cos_r;
2543
    x[1][2] =  0;
2544
2545
    x[2][0] =  0;
2546
    x[2][1] =  0;
2547
    x[2][2] =  1;
2548
2549
    return *this;
2550
}
2551
2552
template <class T>
2553
template <class S>
2554
const Matrix33<T> &
2555
Matrix33<T>::rotate (S r)
2556
{
2557
    *this *= Matrix33<T>().setRotation (r);
2558
    return *this;
2559
}
2560
2561
template <class T>
2562
const Matrix33<T> &
2563
Matrix33<T>::setScale (T s)
2564
{
2565
    memset (x, 0, sizeof (x));
2566
    x[0][0] = s;
2567
    x[1][1] = s;
2568
    x[2][2] = 1;
2569
2570
    return *this;
2571
}
2572
2573
template <class T>
2574
template <class S>
2575
const Matrix33<T> &
2576
Matrix33<T>::setScale (const Vec2<S> &s)
2577
{
2578
    memset (x, 0, sizeof (x));
2579
    x[0][0] = s[0];
2580
    x[1][1] = s[1];
2581
    x[2][2] = 1;
2582
2583
    return *this;
2584
}
2585
2586
template <class T>
2587
template <class S>
2588
const Matrix33<T> &
2589
Matrix33<T>::scale (const Vec2<S> &s)
2590
{
2591
    x[0][0] *= s[0];
2592
    x[0][1] *= s[0];
2593
    x[0][2] *= s[0];
2594
2595
    x[1][0] *= s[1];
2596
    x[1][1] *= s[1];
2597
    x[1][2] *= s[1];
2598
2599
    return *this;
2600
}
2601
2602
template <class T>
2603
template <class S>
2604
const Matrix33<T> &
2605
Matrix33<T>::setTranslation (const Vec2<S> &t)
2606
{
2607
    x[0][0] = 1;
2608
    x[0][1] = 0;
2609
    x[0][2] = 0;
2610
2611
    x[1][0] = 0;
2612
    x[1][1] = 1;
2613
    x[1][2] = 0;
2614
2615
    x[2][0] = t[0];
2616
    x[2][1] = t[1];
2617
    x[2][2] = 1;
2618
2619
    return *this;
2620
}
2621
2622
template <class T>
2623
inline Vec2<T> 
2624
Matrix33<T>::translation () const
2625
{
2626
    return Vec2<T> (x[2][0], x[2][1]);
2627
}
2628
2629
template <class T>
2630
template <class S>
2631
const Matrix33<T> &
2632
Matrix33<T>::translate (const Vec2<S> &t)
2633
{
2634
    x[2][0] += t[0] * x[0][0] + t[1] * x[1][0];
2635
    x[2][1] += t[0] * x[0][1] + t[1] * x[1][1];
2636
    x[2][2] += t[0] * x[0][2] + t[1] * x[1][2];
2637
2638
    return *this;
2639
}
2640
2641
template <class T>
2642
template <class S>
2643
const Matrix33<T> &
2644
Matrix33<T>::setShear (const S &xy)
2645
{
2646
    x[0][0] = 1;
2647
    x[0][1] = 0;
2648
    x[0][2] = 0;
2649
2650
    x[1][0] = xy;
2651
    x[1][1] = 1;
2652
    x[1][2] = 0;
2653
2654
    x[2][0] = 0;
2655
    x[2][1] = 0;
2656
    x[2][2] = 1;
2657
2658
    return *this;
2659
}
2660
2661
template <class T>
2662
template <class S>
2663
const Matrix33<T> &
2664
Matrix33<T>::setShear (const Vec2<S> &h)
2665
{
2666
    x[0][0] = 1;
2667
    x[0][1] = h[1];
2668
    x[0][2] = 0;
2669
2670
    x[1][0] = h[0];
2671
    x[1][1] = 1;
2672
    x[1][2] = 0;
2673
2674
    x[2][0] = 0;
2675
    x[2][1] = 0;
2676
    x[2][2] = 1;
2677
2678
    return *this;
2679
}
2680
2681
template <class T>
2682
template <class S>
2683
const Matrix33<T> &
2684
Matrix33<T>::shear (const S &xy)
2685
{
2686
    //
2687
    // In this case, we don't need a temp. copy of the matrix 
2688
    // because we never use a value on the RHS after we've 
2689
    // changed it on the LHS.
2690
    // 
2691
2692
    x[1][0] += xy * x[0][0];
2693
    x[1][1] += xy * x[0][1];
2694
    x[1][2] += xy * x[0][2];
2695
2696
    return *this;
2697
}
2698
2699
template <class T>
2700
template <class S>
2701
const Matrix33<T> &
2702
Matrix33<T>::shear (const Vec2<S> &h)
2703
{
2704
    Matrix33<T> P (*this);
2705
    
2706
    x[0][0] = P[0][0] + h[1] * P[1][0];
2707
    x[0][1] = P[0][1] + h[1] * P[1][1];
2708
    x[0][2] = P[0][2] + h[1] * P[1][2];
2709
    
2710
    x[1][0] = P[1][0] + h[0] * P[0][0];
2711
    x[1][1] = P[1][1] + h[0] * P[0][1];
2712
    x[1][2] = P[1][2] + h[0] * P[0][2];
2713
2714
    return *this;
2715
}
2716
2717
2718
//---------------------------
2719
// Implementation of Matrix44
2720
//---------------------------
2721
2722
template <class T>
2723
inline T *
2724
Matrix44<T>::operator [] (int i)
2725
0
{
2726
0
    return x[i];
2727
0
}
2728
2729
template <class T>
2730
inline const T *
2731
Matrix44<T>::operator [] (int i) const
2732
0
{
2733
0
    return x[i];
2734
0
}
2735
2736
template <class T>
2737
inline
2738
Matrix44<T>::Matrix44 ()
2739
0
{
2740
0
    memset (x, 0, sizeof (x));
2741
0
    x[0][0] = 1;
2742
0
    x[1][1] = 1;
2743
0
    x[2][2] = 1;
2744
0
    x[3][3] = 1;
2745
0
}
2746
2747
template <class T>
2748
inline
2749
Matrix44<T>::Matrix44 (T a)
2750
0
{
2751
0
    x[0][0] = a;
2752
0
    x[0][1] = a;
2753
0
    x[0][2] = a;
2754
0
    x[0][3] = a;
2755
0
    x[1][0] = a;
2756
0
    x[1][1] = a;
2757
0
    x[1][2] = a;
2758
0
    x[1][3] = a;
2759
0
    x[2][0] = a;
2760
0
    x[2][1] = a;
2761
0
    x[2][2] = a;
2762
0
    x[2][3] = a;
2763
0
    x[3][0] = a;
2764
0
    x[3][1] = a;
2765
0
    x[3][2] = a;
2766
0
    x[3][3] = a;
2767
0
}
2768
2769
template <class T>
2770
inline
2771
Matrix44<T>::Matrix44 (const T a[4][4]) 
2772
{
2773
    memcpy (x, a, sizeof (x));
2774
}
2775
2776
template <class T>
2777
inline
2778
Matrix44<T>::Matrix44 (T a, T b, T c, T d, T e, T f, T g, T h,
2779
                       T i, T j, T k, T l, T m, T n, T o, T p)
2780
0
{
2781
0
    x[0][0] = a;
2782
0
    x[0][1] = b;
2783
0
    x[0][2] = c;
2784
0
    x[0][3] = d;
2785
0
    x[1][0] = e;
2786
0
    x[1][1] = f;
2787
0
    x[1][2] = g;
2788
0
    x[1][3] = h;
2789
0
    x[2][0] = i;
2790
0
    x[2][1] = j;
2791
0
    x[2][2] = k;
2792
0
    x[2][3] = l;
2793
0
    x[3][0] = m;
2794
0
    x[3][1] = n;
2795
0
    x[3][2] = o;
2796
0
    x[3][3] = p;
2797
0
}
2798
2799
2800
template <class T>
2801
inline
2802
Matrix44<T>::Matrix44 (Matrix33<T> r, Vec3<T> t)
2803
{
2804
    x[0][0] = r[0][0];
2805
    x[0][1] = r[0][1];
2806
    x[0][2] = r[0][2];
2807
    x[0][3] = 0;
2808
    x[1][0] = r[1][0];
2809
    x[1][1] = r[1][1];
2810
    x[1][2] = r[1][2];
2811
    x[1][3] = 0;
2812
    x[2][0] = r[2][0];
2813
    x[2][1] = r[2][1];
2814
    x[2][2] = r[2][2];
2815
    x[2][3] = 0;
2816
    x[3][0] = t[0];
2817
    x[3][1] = t[1];
2818
    x[3][2] = t[2];
2819
    x[3][3] = 1;
2820
}
2821
2822
template <class T>
2823
inline
2824
Matrix44<T>::Matrix44 (const Matrix44 &v)
2825
0
{
2826
0
    x[0][0] = v.x[0][0];
2827
0
    x[0][1] = v.x[0][1];
2828
0
    x[0][2] = v.x[0][2];
2829
0
    x[0][3] = v.x[0][3];
2830
0
    x[1][0] = v.x[1][0];
2831
0
    x[1][1] = v.x[1][1];
2832
0
    x[1][2] = v.x[1][2];
2833
0
    x[1][3] = v.x[1][3];
2834
0
    x[2][0] = v.x[2][0];
2835
0
    x[2][1] = v.x[2][1];
2836
0
    x[2][2] = v.x[2][2];
2837
0
    x[2][3] = v.x[2][3];
2838
0
    x[3][0] = v.x[3][0];
2839
0
    x[3][1] = v.x[3][1];
2840
0
    x[3][2] = v.x[3][2];
2841
0
    x[3][3] = v.x[3][3];
2842
0
}
2843
2844
template <class T>
2845
template <class S>
2846
inline
2847
Matrix44<T>::Matrix44 (const Matrix44<S> &v)
2848
{
2849
    x[0][0] = T (v.x[0][0]);
2850
    x[0][1] = T (v.x[0][1]);
2851
    x[0][2] = T (v.x[0][2]);
2852
    x[0][3] = T (v.x[0][3]);
2853
    x[1][0] = T (v.x[1][0]);
2854
    x[1][1] = T (v.x[1][1]);
2855
    x[1][2] = T (v.x[1][2]);
2856
    x[1][3] = T (v.x[1][3]);
2857
    x[2][0] = T (v.x[2][0]);
2858
    x[2][1] = T (v.x[2][1]);
2859
    x[2][2] = T (v.x[2][2]);
2860
    x[2][3] = T (v.x[2][3]);
2861
    x[3][0] = T (v.x[3][0]);
2862
    x[3][1] = T (v.x[3][1]);
2863
    x[3][2] = T (v.x[3][2]);
2864
    x[3][3] = T (v.x[3][3]);
2865
}
2866
2867
template <class T>
2868
inline const Matrix44<T> &
2869
Matrix44<T>::operator = (const Matrix44 &v)
2870
0
{
2871
0
    x[0][0] = v.x[0][0];
2872
0
    x[0][1] = v.x[0][1];
2873
0
    x[0][2] = v.x[0][2];
2874
0
    x[0][3] = v.x[0][3];
2875
0
    x[1][0] = v.x[1][0];
2876
0
    x[1][1] = v.x[1][1];
2877
0
    x[1][2] = v.x[1][2];
2878
0
    x[1][3] = v.x[1][3];
2879
0
    x[2][0] = v.x[2][0];
2880
0
    x[2][1] = v.x[2][1];
2881
0
    x[2][2] = v.x[2][2];
2882
0
    x[2][3] = v.x[2][3];
2883
0
    x[3][0] = v.x[3][0];
2884
0
    x[3][1] = v.x[3][1];
2885
0
    x[3][2] = v.x[3][2];
2886
0
    x[3][3] = v.x[3][3];
2887
0
    return *this;
2888
0
}
2889
2890
template <class T>
2891
inline const Matrix44<T> &
2892
Matrix44<T>::operator = (T a)
2893
{
2894
    x[0][0] = a;
2895
    x[0][1] = a;
2896
    x[0][2] = a;
2897
    x[0][3] = a;
2898
    x[1][0] = a;
2899
    x[1][1] = a;
2900
    x[1][2] = a;
2901
    x[1][3] = a;
2902
    x[2][0] = a;
2903
    x[2][1] = a;
2904
    x[2][2] = a;
2905
    x[2][3] = a;
2906
    x[3][0] = a;
2907
    x[3][1] = a;
2908
    x[3][2] = a;
2909
    x[3][3] = a;
2910
    return *this;
2911
}
2912
2913
template <class T>
2914
inline T *
2915
Matrix44<T>::getValue ()
2916
{
2917
    return (T *) &x[0][0];
2918
}
2919
2920
template <class T>
2921
inline const T *
2922
Matrix44<T>::getValue () const
2923
{
2924
    return (const T *) &x[0][0];
2925
}
2926
2927
template <class T>
2928
template <class S>
2929
inline void
2930
Matrix44<T>::getValue (Matrix44<S> &v) const
2931
{
2932
    if (isSameType<S,T>::value)
2933
    {
2934
        memcpy (v.x, x, sizeof (x));
2935
    }
2936
    else
2937
    {
2938
        v.x[0][0] = x[0][0];
2939
        v.x[0][1] = x[0][1];
2940
        v.x[0][2] = x[0][2];
2941
        v.x[0][3] = x[0][3];
2942
        v.x[1][0] = x[1][0];
2943
        v.x[1][1] = x[1][1];
2944
        v.x[1][2] = x[1][2];
2945
        v.x[1][3] = x[1][3];
2946
        v.x[2][0] = x[2][0];
2947
        v.x[2][1] = x[2][1];
2948
        v.x[2][2] = x[2][2];
2949
        v.x[2][3] = x[2][3];
2950
        v.x[3][0] = x[3][0];
2951
        v.x[3][1] = x[3][1];
2952
        v.x[3][2] = x[3][2];
2953
        v.x[3][3] = x[3][3];
2954
    }
2955
}
2956
2957
template <class T>
2958
template <class S>
2959
inline Matrix44<T> &
2960
Matrix44<T>::setValue (const Matrix44<S> &v)
2961
{
2962
    if (isSameType<S,T>::value)
2963
    {
2964
        memcpy (x, v.x, sizeof (x));
2965
    }
2966
    else
2967
    {
2968
        x[0][0] = v.x[0][0];
2969
        x[0][1] = v.x[0][1];
2970
        x[0][2] = v.x[0][2];
2971
        x[0][3] = v.x[0][3];
2972
        x[1][0] = v.x[1][0];
2973
        x[1][1] = v.x[1][1];
2974
        x[1][2] = v.x[1][2];
2975
        x[1][3] = v.x[1][3];
2976
        x[2][0] = v.x[2][0];
2977
        x[2][1] = v.x[2][1];
2978
        x[2][2] = v.x[2][2];
2979
        x[2][3] = v.x[2][3];
2980
        x[3][0] = v.x[3][0];
2981
        x[3][1] = v.x[3][1];
2982
        x[3][2] = v.x[3][2];
2983
        x[3][3] = v.x[3][3];
2984
    }
2985
2986
    return *this;
2987
}
2988
2989
template <class T>
2990
template <class S>
2991
inline Matrix44<T> &
2992
Matrix44<T>::setTheMatrix (const Matrix44<S> &v)
2993
{
2994
    if (isSameType<S,T>::value)
2995
    {
2996
        memcpy (x, v.x, sizeof (x));
2997
    }
2998
    else
2999
    {
3000
        x[0][0] = v.x[0][0];
3001
        x[0][1] = v.x[0][1];
3002
        x[0][2] = v.x[0][2];
3003
        x[0][3] = v.x[0][3];
3004
        x[1][0] = v.x[1][0];
3005
        x[1][1] = v.x[1][1];
3006
        x[1][2] = v.x[1][2];
3007
        x[1][3] = v.x[1][3];
3008
        x[2][0] = v.x[2][0];
3009
        x[2][1] = v.x[2][1];
3010
        x[2][2] = v.x[2][2];
3011
        x[2][3] = v.x[2][3];
3012
        x[3][0] = v.x[3][0];
3013
        x[3][1] = v.x[3][1];
3014
        x[3][2] = v.x[3][2];
3015
        x[3][3] = v.x[3][3];
3016
    }
3017
3018
    return *this;
3019
}
3020
3021
template <class T>
3022
inline void
3023
Matrix44<T>::makeIdentity()
3024
0
{
3025
0
    memset (x, 0, sizeof (x));
3026
0
    x[0][0] = 1;
3027
0
    x[1][1] = 1;
3028
0
    x[2][2] = 1;
3029
0
    x[3][3] = 1;
3030
0
}
3031
3032
template <class T>
3033
bool
3034
Matrix44<T>::operator == (const Matrix44 &v) const
3035
{
3036
    return x[0][0] == v.x[0][0] &&
3037
           x[0][1] == v.x[0][1] &&
3038
           x[0][2] == v.x[0][2] &&
3039
           x[0][3] == v.x[0][3] &&
3040
           x[1][0] == v.x[1][0] &&
3041
           x[1][1] == v.x[1][1] &&
3042
           x[1][2] == v.x[1][2] &&
3043
           x[1][3] == v.x[1][3] &&
3044
           x[2][0] == v.x[2][0] &&
3045
           x[2][1] == v.x[2][1] &&
3046
           x[2][2] == v.x[2][2] &&
3047
           x[2][3] == v.x[2][3] &&
3048
           x[3][0] == v.x[3][0] &&
3049
           x[3][1] == v.x[3][1] &&
3050
           x[3][2] == v.x[3][2] &&
3051
           x[3][3] == v.x[3][3];
3052
}
3053
3054
template <class T>
3055
bool
3056
Matrix44<T>::operator != (const Matrix44 &v) const
3057
{
3058
    return x[0][0] != v.x[0][0] ||
3059
           x[0][1] != v.x[0][1] ||
3060
           x[0][2] != v.x[0][2] ||
3061
           x[0][3] != v.x[0][3] ||
3062
           x[1][0] != v.x[1][0] ||
3063
           x[1][1] != v.x[1][1] ||
3064
           x[1][2] != v.x[1][2] ||
3065
           x[1][3] != v.x[1][3] ||
3066
           x[2][0] != v.x[2][0] ||
3067
           x[2][1] != v.x[2][1] ||
3068
           x[2][2] != v.x[2][2] ||
3069
           x[2][3] != v.x[2][3] ||
3070
           x[3][0] != v.x[3][0] ||
3071
           x[3][1] != v.x[3][1] ||
3072
           x[3][2] != v.x[3][2] ||
3073
           x[3][3] != v.x[3][3];
3074
}
3075
3076
template <class T>
3077
bool
3078
Matrix44<T>::equalWithAbsError (const Matrix44<T> &m, T e) const
3079
{
3080
    for (int i = 0; i < 4; i++)
3081
        for (int j = 0; j < 4; j++)
3082
            if (!IMATH_INTERNAL_NAMESPACE::equalWithAbsError ((*this)[i][j], m[i][j], e))
3083
                return false;
3084
3085
    return true;
3086
}
3087
3088
template <class T>
3089
bool
3090
Matrix44<T>::equalWithRelError (const Matrix44<T> &m, T e) const
3091
{
3092
    for (int i = 0; i < 4; i++)
3093
        for (int j = 0; j < 4; j++)
3094
            if (!IMATH_INTERNAL_NAMESPACE::equalWithRelError ((*this)[i][j], m[i][j], e))
3095
                return false;
3096
3097
    return true;
3098
}
3099
3100
template <class T>
3101
const Matrix44<T> &
3102
Matrix44<T>::operator += (const Matrix44<T> &v)
3103
{
3104
    x[0][0] += v.x[0][0];
3105
    x[0][1] += v.x[0][1];
3106
    x[0][2] += v.x[0][2];
3107
    x[0][3] += v.x[0][3];
3108
    x[1][0] += v.x[1][0];
3109
    x[1][1] += v.x[1][1];
3110
    x[1][2] += v.x[1][2];
3111
    x[1][3] += v.x[1][3];
3112
    x[2][0] += v.x[2][0];
3113
    x[2][1] += v.x[2][1];
3114
    x[2][2] += v.x[2][2];
3115
    x[2][3] += v.x[2][3];
3116
    x[3][0] += v.x[3][0];
3117
    x[3][1] += v.x[3][1];
3118
    x[3][2] += v.x[3][2];
3119
    x[3][3] += v.x[3][3];
3120
3121
    return *this;
3122
}
3123
3124
template <class T>
3125
const Matrix44<T> &
3126
Matrix44<T>::operator += (T a)
3127
{
3128
    x[0][0] += a;
3129
    x[0][1] += a;
3130
    x[0][2] += a;
3131
    x[0][3] += a;
3132
    x[1][0] += a;
3133
    x[1][1] += a;
3134
    x[1][2] += a;
3135
    x[1][3] += a;
3136
    x[2][0] += a;
3137
    x[2][1] += a;
3138
    x[2][2] += a;
3139
    x[2][3] += a;
3140
    x[3][0] += a;
3141
    x[3][1] += a;
3142
    x[3][2] += a;
3143
    x[3][3] += a;
3144
3145
    return *this;
3146
}
3147
3148
template <class T>
3149
Matrix44<T>
3150
Matrix44<T>::operator + (const Matrix44<T> &v) const
3151
{
3152
    return Matrix44 (x[0][0] + v.x[0][0],
3153
                     x[0][1] + v.x[0][1],
3154
                     x[0][2] + v.x[0][2],
3155
                     x[0][3] + v.x[0][3],
3156
                     x[1][0] + v.x[1][0],
3157
                     x[1][1] + v.x[1][1],
3158
                     x[1][2] + v.x[1][2],
3159
                     x[1][3] + v.x[1][3],
3160
                     x[2][0] + v.x[2][0],
3161
                     x[2][1] + v.x[2][1],
3162
                     x[2][2] + v.x[2][2],
3163
                     x[2][3] + v.x[2][3],
3164
                     x[3][0] + v.x[3][0],
3165
                     x[3][1] + v.x[3][1],
3166
                     x[3][2] + v.x[3][2],
3167
                     x[3][3] + v.x[3][3]);
3168
}
3169
3170
template <class T>
3171
const Matrix44<T> &
3172
Matrix44<T>::operator -= (const Matrix44<T> &v)
3173
{
3174
    x[0][0] -= v.x[0][0];
3175
    x[0][1] -= v.x[0][1];
3176
    x[0][2] -= v.x[0][2];
3177
    x[0][3] -= v.x[0][3];
3178
    x[1][0] -= v.x[1][0];
3179
    x[1][1] -= v.x[1][1];
3180
    x[1][2] -= v.x[1][2];
3181
    x[1][3] -= v.x[1][3];
3182
    x[2][0] -= v.x[2][0];
3183
    x[2][1] -= v.x[2][1];
3184
    x[2][2] -= v.x[2][2];
3185
    x[2][3] -= v.x[2][3];
3186
    x[3][0] -= v.x[3][0];
3187
    x[3][1] -= v.x[3][1];
3188
    x[3][2] -= v.x[3][2];
3189
    x[3][3] -= v.x[3][3];
3190
3191
    return *this;
3192
}
3193
3194
template <class T>
3195
const Matrix44<T> &
3196
Matrix44<T>::operator -= (T a)
3197
{
3198
    x[0][0] -= a;
3199
    x[0][1] -= a;
3200
    x[0][2] -= a;
3201
    x[0][3] -= a;
3202
    x[1][0] -= a;
3203
    x[1][1] -= a;
3204
    x[1][2] -= a;
3205
    x[1][3] -= a;
3206
    x[2][0] -= a;
3207
    x[2][1] -= a;
3208
    x[2][2] -= a;
3209
    x[2][3] -= a;
3210
    x[3][0] -= a;
3211
    x[3][1] -= a;
3212
    x[3][2] -= a;
3213
    x[3][3] -= a;
3214
3215
    return *this;
3216
}
3217
3218
template <class T>
3219
Matrix44<T>
3220
Matrix44<T>::operator - (const Matrix44<T> &v) const
3221
{
3222
    return Matrix44 (x[0][0] - v.x[0][0],
3223
                     x[0][1] - v.x[0][1],
3224
                     x[0][2] - v.x[0][2],
3225
                     x[0][3] - v.x[0][3],
3226
                     x[1][0] - v.x[1][0],
3227
                     x[1][1] - v.x[1][1],
3228
                     x[1][2] - v.x[1][2],
3229
                     x[1][3] - v.x[1][3],
3230
                     x[2][0] - v.x[2][0],
3231
                     x[2][1] - v.x[2][1],
3232
                     x[2][2] - v.x[2][2],
3233
                     x[2][3] - v.x[2][3],
3234
                     x[3][0] - v.x[3][0],
3235
                     x[3][1] - v.x[3][1],
3236
                     x[3][2] - v.x[3][2],
3237
                     x[3][3] - v.x[3][3]);
3238
}
3239
3240
template <class T>
3241
Matrix44<T>
3242
Matrix44<T>::operator - () const
3243
{
3244
    return Matrix44 (-x[0][0],
3245
                     -x[0][1],
3246
                     -x[0][2],
3247
                     -x[0][3],
3248
                     -x[1][0],
3249
                     -x[1][1],
3250
                     -x[1][2],
3251
                     -x[1][3],
3252
                     -x[2][0],
3253
                     -x[2][1],
3254
                     -x[2][2],
3255
                     -x[2][3],
3256
                     -x[3][0],
3257
                     -x[3][1],
3258
                     -x[3][2],
3259
                     -x[3][3]);
3260
}
3261
3262
template <class T>
3263
const Matrix44<T> &
3264
Matrix44<T>::negate ()
3265
{
3266
    x[0][0] = -x[0][0];
3267
    x[0][1] = -x[0][1];
3268
    x[0][2] = -x[0][2];
3269
    x[0][3] = -x[0][3];
3270
    x[1][0] = -x[1][0];
3271
    x[1][1] = -x[1][1];
3272
    x[1][2] = -x[1][2];
3273
    x[1][3] = -x[1][3];
3274
    x[2][0] = -x[2][0];
3275
    x[2][1] = -x[2][1];
3276
    x[2][2] = -x[2][2];
3277
    x[2][3] = -x[2][3];
3278
    x[3][0] = -x[3][0];
3279
    x[3][1] = -x[3][1];
3280
    x[3][2] = -x[3][2];
3281
    x[3][3] = -x[3][3];
3282
3283
    return *this;
3284
}
3285
3286
template <class T>
3287
const Matrix44<T> &
3288
Matrix44<T>::operator *= (T a)
3289
{
3290
    x[0][0] *= a;
3291
    x[0][1] *= a;
3292
    x[0][2] *= a;
3293
    x[0][3] *= a;
3294
    x[1][0] *= a;
3295
    x[1][1] *= a;
3296
    x[1][2] *= a;
3297
    x[1][3] *= a;
3298
    x[2][0] *= a;
3299
    x[2][1] *= a;
3300
    x[2][2] *= a;
3301
    x[2][3] *= a;
3302
    x[3][0] *= a;
3303
    x[3][1] *= a;
3304
    x[3][2] *= a;
3305
    x[3][3] *= a;
3306
3307
    return *this;
3308
}
3309
3310
template <class T>
3311
Matrix44<T>
3312
Matrix44<T>::operator * (T a) const
3313
{
3314
    return Matrix44 (x[0][0] * a,
3315
                     x[0][1] * a,
3316
                     x[0][2] * a,
3317
                     x[0][3] * a,
3318
                     x[1][0] * a,
3319
                     x[1][1] * a,
3320
                     x[1][2] * a,
3321
                     x[1][3] * a,
3322
                     x[2][0] * a,
3323
                     x[2][1] * a,
3324
                     x[2][2] * a,
3325
                     x[2][3] * a,
3326
                     x[3][0] * a,
3327
                     x[3][1] * a,
3328
                     x[3][2] * a,
3329
                     x[3][3] * a);
3330
}
3331
3332
template <class T>
3333
inline Matrix44<T>
3334
operator * (T a, const Matrix44<T> &v)
3335
{
3336
    return v * a;
3337
}
3338
3339
template <class T>
3340
inline const Matrix44<T> &
3341
Matrix44<T>::operator *= (const Matrix44<T> &v)
3342
{
3343
    Matrix44 tmp (T (0));
3344
3345
    multiply (*this, v, tmp);
3346
    *this = tmp;
3347
    return *this;
3348
}
3349
3350
template <class T>
3351
inline Matrix44<T>
3352
Matrix44<T>::operator * (const Matrix44<T> &v) const
3353
0
{
3354
0
    Matrix44 tmp (T (0));
3355
3356
0
    multiply (*this, v, tmp);
3357
0
    return tmp;
3358
0
}
3359
3360
template <class T>
3361
void
3362
Matrix44<T>::multiply (const Matrix44<T> &a,
3363
                       const Matrix44<T> &b,
3364
                       Matrix44<T> &c)
3365
0
{
3366
0
    const T * IMATH_RESTRICT ap = &a.x[0][0];
3367
0
    const T * IMATH_RESTRICT bp = &b.x[0][0];
3368
0
          T * IMATH_RESTRICT cp = &c.x[0][0];
3369
3370
0
    T a0, a1, a2, a3;
3371
3372
0
    a0 = ap[0];
3373
0
    a1 = ap[1];
3374
0
    a2 = ap[2];
3375
0
    a3 = ap[3];
3376
3377
0
    cp[0]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
3378
0
    cp[1]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
3379
0
    cp[2]  = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
3380
0
    cp[3]  = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];
3381
3382
0
    a0 = ap[4];
3383
0
    a1 = ap[5];
3384
0
    a2 = ap[6];
3385
0
    a3 = ap[7];
3386
3387
0
    cp[4]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
3388
0
    cp[5]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
3389
0
    cp[6]  = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
3390
0
    cp[7]  = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];
3391
3392
0
    a0 = ap[8];
3393
0
    a1 = ap[9];
3394
0
    a2 = ap[10];
3395
0
    a3 = ap[11];
3396
3397
0
    cp[8]  = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
3398
0
    cp[9]  = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
3399
0
    cp[10] = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
3400
0
    cp[11] = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];
3401
3402
0
    a0 = ap[12];
3403
0
    a1 = ap[13];
3404
0
    a2 = ap[14];
3405
0
    a3 = ap[15];
3406
3407
0
    cp[12] = a0 * bp[0]  + a1 * bp[4]  + a2 * bp[8]  + a3 * bp[12];
3408
0
    cp[13] = a0 * bp[1]  + a1 * bp[5]  + a2 * bp[9]  + a3 * bp[13];
3409
0
    cp[14] = a0 * bp[2]  + a1 * bp[6]  + a2 * bp[10] + a3 * bp[14];
3410
0
    cp[15] = a0 * bp[3]  + a1 * bp[7]  + a2 * bp[11] + a3 * bp[15];
3411
0
}
3412
3413
template <class T> template <class S>
3414
void
3415
Matrix44<T>::multVecMatrix(const Vec3<S> &src, Vec3<S> &dst) const
3416
{
3417
    S a, b, c, w;
3418
3419
    a = src[0] * x[0][0] + src[1] * x[1][0] + src[2] * x[2][0] + x[3][0];
3420
    b = src[0] * x[0][1] + src[1] * x[1][1] + src[2] * x[2][1] + x[3][1];
3421
    c = src[0] * x[0][2] + src[1] * x[1][2] + src[2] * x[2][2] + x[3][2];
3422
    w = src[0] * x[0][3] + src[1] * x[1][3] + src[2] * x[2][3] + x[3][3];
3423
3424
    dst.x = a / w;
3425
    dst.y = b / w;
3426
    dst.z = c / w;
3427
}
3428
3429
template <class T> template <class S>
3430
void
3431
Matrix44<T>::multDirMatrix(const Vec3<S> &src, Vec3<S> &dst) const
3432
{
3433
    S a, b, c;
3434
3435
    a = src[0] * x[0][0] + src[1] * x[1][0] + src[2] * x[2][0];
3436
    b = src[0] * x[0][1] + src[1] * x[1][1] + src[2] * x[2][1];
3437
    c = src[0] * x[0][2] + src[1] * x[1][2] + src[2] * x[2][2];
3438
3439
    dst.x = a;
3440
    dst.y = b;
3441
    dst.z = c;
3442
}
3443
3444
template <class T>
3445
const Matrix44<T> &
3446
Matrix44<T>::operator /= (T a)
3447
{
3448
    x[0][0] /= a;
3449
    x[0][1] /= a;
3450
    x[0][2] /= a;
3451
    x[0][3] /= a;
3452
    x[1][0] /= a;
3453
    x[1][1] /= a;
3454
    x[1][2] /= a;
3455
    x[1][3] /= a;
3456
    x[2][0] /= a;
3457
    x[2][1] /= a;
3458
    x[2][2] /= a;
3459
    x[2][3] /= a;
3460
    x[3][0] /= a;
3461
    x[3][1] /= a;
3462
    x[3][2] /= a;
3463
    x[3][3] /= a;
3464
3465
    return *this;
3466
}
3467
3468
template <class T>
3469
Matrix44<T>
3470
Matrix44<T>::operator / (T a) const
3471
{
3472
    return Matrix44 (x[0][0] / a,
3473
                     x[0][1] / a,
3474
                     x[0][2] / a,
3475
                     x[0][3] / a,
3476
                     x[1][0] / a,
3477
                     x[1][1] / a,
3478
                     x[1][2] / a,
3479
                     x[1][3] / a,
3480
                     x[2][0] / a,
3481
                     x[2][1] / a,
3482
                     x[2][2] / a,
3483
                     x[2][3] / a,
3484
                     x[3][0] / a,
3485
                     x[3][1] / a,
3486
                     x[3][2] / a,
3487
                     x[3][3] / a);
3488
}
3489
3490
template <class T>
3491
const Matrix44<T> &
3492
Matrix44<T>::transpose ()
3493
{
3494
    Matrix44 tmp (x[0][0],
3495
                  x[1][0],
3496
                  x[2][0],
3497
                  x[3][0],
3498
                  x[0][1],
3499
                  x[1][1],
3500
                  x[2][1],
3501
                  x[3][1],
3502
                  x[0][2],
3503
                  x[1][2],
3504
                  x[2][2],
3505
                  x[3][2],
3506
                  x[0][3],
3507
                  x[1][3],
3508
                  x[2][3],
3509
                  x[3][3]);
3510
    *this = tmp;
3511
    return *this;
3512
}
3513
3514
template <class T>
3515
Matrix44<T>
3516
Matrix44<T>::transposed () const
3517
{
3518
    return Matrix44 (x[0][0],
3519
                     x[1][0],
3520
                     x[2][0],
3521
                     x[3][0],
3522
                     x[0][1],
3523
                     x[1][1],
3524
                     x[2][1],
3525
                     x[3][1],
3526
                     x[0][2],
3527
                     x[1][2],
3528
                     x[2][2],
3529
                     x[3][2],
3530
                     x[0][3],
3531
                     x[1][3],
3532
                     x[2][3],
3533
                     x[3][3]);
3534
}
3535
3536
template <class T>
3537
const Matrix44<T> &
3538
Matrix44<T>::gjInvert (bool singExc)
3539
{
3540
    *this = gjInverse (singExc);
3541
    return *this;
3542
}
3543
3544
template <class T>
3545
Matrix44<T>
3546
Matrix44<T>::gjInverse (bool singExc) const
3547
{
3548
    int i, j, k;
3549
    Matrix44 s;
3550
    Matrix44 t (*this);
3551
3552
    // Forward elimination
3553
3554
    for (i = 0; i < 3 ; i++)
3555
    {
3556
        int pivot = i;
3557
3558
        T pivotsize = t[i][i];
3559
3560
        if (pivotsize < 0)
3561
            pivotsize = -pivotsize;
3562
3563
        for (j = i + 1; j < 4; j++)
3564
        {
3565
            T tmp = t[j][i];
3566
3567
            if (tmp < 0)
3568
                tmp = -tmp;
3569
3570
            if (tmp > pivotsize)
3571
            {
3572
                pivot = j;
3573
                pivotsize = tmp;
3574
            }
3575
        }
3576
3577
        if (pivotsize == 0)
3578
        {
3579
            if (singExc)
3580
                throw ::IMATH_INTERNAL_NAMESPACE::SingMatrixExc ("Cannot invert singular matrix.");
3581
3582
            return Matrix44();
3583
        }
3584
3585
        if (pivot != i)
3586
        {
3587
            for (j = 0; j < 4; j++)
3588
            {
3589
                T tmp;
3590
3591
                tmp = t[i][j];
3592
                t[i][j] = t[pivot][j];
3593
                t[pivot][j] = tmp;
3594
3595
                tmp = s[i][j];
3596
                s[i][j] = s[pivot][j];
3597
                s[pivot][j] = tmp;
3598
            }
3599
        }
3600
3601
        for (j = i + 1; j < 4; j++)
3602
        {
3603
            T f = t[j][i] / t[i][i];
3604
3605
            for (k = 0; k < 4; k++)
3606
            {
3607
                t[j][k] -= f * t[i][k];
3608
                s[j][k] -= f * s[i][k];
3609
            }
3610
        }
3611
    }
3612
3613
    // Backward substitution
3614
3615
    for (i = 3; i >= 0; --i)
3616
    {
3617
        T f;
3618
3619
        if ((f = t[i][i]) == 0)
3620
        {
3621
            if (singExc)
3622
                throw ::IMATH_INTERNAL_NAMESPACE::SingMatrixExc ("Cannot invert singular matrix.");
3623
3624
            return Matrix44();
3625
        }
3626
3627
        for (j = 0; j < 4; j++)
3628
        {
3629
            t[i][j] /= f;
3630
            s[i][j] /= f;
3631
        }
3632
3633
        for (j = 0; j < i; j++)
3634
        {
3635
            f = t[j][i];
3636
3637
            for (k = 0; k < 4; k++)
3638
            {
3639
                t[j][k] -= f * t[i][k];
3640
                s[j][k] -= f * s[i][k];
3641
            }
3642
        }
3643
    }
3644
3645
    return s;
3646
}
3647
3648
template <class T>
3649
const Matrix44<T> &
3650
Matrix44<T>::invert (bool singExc)
3651
{
3652
    *this = inverse (singExc);
3653
    return *this;
3654
}
3655
3656
template <class T>
3657
Matrix44<T>
3658
Matrix44<T>::inverse (bool singExc) const
3659
{
3660
    if (x[0][3] != 0 || x[1][3] != 0 || x[2][3] != 0 || x[3][3] != 1)
3661
        return gjInverse(singExc);
3662
3663
    Matrix44 s (x[1][1] * x[2][2] - x[2][1] * x[1][2],
3664
                x[2][1] * x[0][2] - x[0][1] * x[2][2],
3665
                x[0][1] * x[1][2] - x[1][1] * x[0][2],
3666
                0,
3667
3668
                x[2][0] * x[1][2] - x[1][0] * x[2][2],
3669
                x[0][0] * x[2][2] - x[2][0] * x[0][2],
3670
                x[1][0] * x[0][2] - x[0][0] * x[1][2],
3671
                0,
3672
3673
                x[1][0] * x[2][1] - x[2][0] * x[1][1],
3674
                x[2][0] * x[0][1] - x[0][0] * x[2][1],
3675
                x[0][0] * x[1][1] - x[1][0] * x[0][1],
3676
                0,
3677
3678
                0,
3679
                0,
3680
                0,
3681
                1);
3682
3683
    T r = x[0][0] * s[0][0] + x[0][1] * s[1][0] + x[0][2] * s[2][0];
3684
3685
    if (IMATH_INTERNAL_NAMESPACE::abs (r) >= 1)
3686
    {
3687
        for (int i = 0; i < 3; ++i)
3688
        {
3689
            for (int j = 0; j < 3; ++j)
3690
            {
3691
                s[i][j] /= r;
3692
            }
3693
        }
3694
    }
3695
    else
3696
    {
3697
        T mr = IMATH_INTERNAL_NAMESPACE::abs (r) / limits<T>::smallest();
3698
3699
        for (int i = 0; i < 3; ++i)
3700
        {
3701
            for (int j = 0; j < 3; ++j)
3702
            {
3703
                if (mr > IMATH_INTERNAL_NAMESPACE::abs (s[i][j]))
3704
                {
3705
                    s[i][j] /= r;
3706
                }
3707
                else
3708
                {
3709
                    if (singExc)
3710
                        throw SingMatrixExc ("Cannot invert singular matrix.");
3711
3712
                    return Matrix44();
3713
                }
3714
            }
3715
        }
3716
    }
3717
3718
    s[3][0] = -x[3][0] * s[0][0] - x[3][1] * s[1][0] - x[3][2] * s[2][0];
3719
    s[3][1] = -x[3][0] * s[0][1] - x[3][1] * s[1][1] - x[3][2] * s[2][1];
3720
    s[3][2] = -x[3][0] * s[0][2] - x[3][1] * s[1][2] - x[3][2] * s[2][2];
3721
3722
    return s;
3723
}
3724
3725
template <class T>
3726
inline T
3727
Matrix44<T>::fastMinor( const int r0, const int r1, const int r2,
3728
                        const int c0, const int c1, const int c2) const
3729
{
3730
    return x[r0][c0] * (x[r1][c1]*x[r2][c2] - x[r1][c2]*x[r2][c1])
3731
         + x[r0][c1] * (x[r1][c2]*x[r2][c0] - x[r1][c0]*x[r2][c2])
3732
         + x[r0][c2] * (x[r1][c0]*x[r2][c1] - x[r1][c1]*x[r2][c0]);
3733
}
3734
3735
template <class T>
3736
inline T
3737
Matrix44<T>::minorOf (const int r, const int c) const
3738
{
3739
    int r0 = 0 + (r < 1 ? 1 : 0);
3740
    int r1 = 1 + (r < 2 ? 1 : 0);
3741
    int r2 = 2 + (r < 3 ? 1 : 0);
3742
    int c0 = 0 + (c < 1 ? 1 : 0);
3743
    int c1 = 1 + (c < 2 ? 1 : 0);
3744
    int c2 = 2 + (c < 3 ? 1 : 0);
3745
3746
    Matrix33<T> working (x[r0][c0],x[r1][c0],x[r2][c0],
3747
                         x[r0][c1],x[r1][c1],x[r2][c1],
3748
                         x[r0][c2],x[r1][c2],x[r2][c2]);
3749
3750
    return working.determinant();
3751
}
3752
3753
template <class T>
3754
inline T
3755
Matrix44<T>::determinant () const
3756
{
3757
    T sum = (T)0;
3758
3759
    if (x[0][3] != 0.) sum -= x[0][3] * fastMinor(1,2,3,0,1,2);
3760
    if (x[1][3] != 0.) sum += x[1][3] * fastMinor(0,2,3,0,1,2);
3761
    if (x[2][3] != 0.) sum -= x[2][3] * fastMinor(0,1,3,0,1,2);
3762
    if (x[3][3] != 0.) sum += x[3][3] * fastMinor(0,1,2,0,1,2);
3763
3764
    return sum;
3765
}
3766
3767
template <class T>
3768
template <class S>
3769
const Matrix44<T> &
3770
Matrix44<T>::setEulerAngles (const Vec3<S>& r)
3771
{
3772
    S cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
3773
    
3774
    cos_rz = Math<T>::cos (r[2]);
3775
    cos_ry = Math<T>::cos (r[1]);
3776
    cos_rx = Math<T>::cos (r[0]);
3777
    
3778
    sin_rz = Math<T>::sin (r[2]);
3779
    sin_ry = Math<T>::sin (r[1]);
3780
    sin_rx = Math<T>::sin (r[0]);
3781
    
3782
    x[0][0] =  cos_rz * cos_ry;
3783
    x[0][1] =  sin_rz * cos_ry;
3784
    x[0][2] = -sin_ry;
3785
    x[0][3] =  0;
3786
    
3787
    x[1][0] = -sin_rz * cos_rx + cos_rz * sin_ry * sin_rx;
3788
    x[1][1] =  cos_rz * cos_rx + sin_rz * sin_ry * sin_rx;
3789
    x[1][2] =  cos_ry * sin_rx;
3790
    x[1][3] =  0;
3791
    
3792
    x[2][0] =  sin_rz * sin_rx + cos_rz * sin_ry * cos_rx;
3793
    x[2][1] = -cos_rz * sin_rx + sin_rz * sin_ry * cos_rx;
3794
    x[2][2] =  cos_ry * cos_rx;
3795
    x[2][3] =  0;
3796
3797
    x[3][0] =  0;
3798
    x[3][1] =  0;
3799
    x[3][2] =  0;
3800
    x[3][3] =  1;
3801
3802
    return *this;
3803
}
3804
3805
template <class T>
3806
template <class S>
3807
const Matrix44<T> &
3808
Matrix44<T>::setAxisAngle (const Vec3<S>& axis, S angle)
3809
0
{
3810
0
    Vec3<S> unit (axis.normalized());
3811
0
    S sine   = Math<T>::sin (angle);
3812
0
    S cosine = Math<T>::cos (angle);
3813
3814
0
    x[0][0] = unit[0] * unit[0] * (1 - cosine) + cosine;
3815
0
    x[0][1] = unit[0] * unit[1] * (1 - cosine) + unit[2] * sine;
3816
0
    x[0][2] = unit[0] * unit[2] * (1 - cosine) - unit[1] * sine;
3817
0
    x[0][3] = 0;
3818
3819
0
    x[1][0] = unit[0] * unit[1] * (1 - cosine) - unit[2] * sine;
3820
0
    x[1][1] = unit[1] * unit[1] * (1 - cosine) + cosine;
3821
0
    x[1][2] = unit[1] * unit[2] * (1 - cosine) + unit[0] * sine;
3822
0
    x[1][3] = 0;
3823
3824
0
    x[2][0] = unit[0] * unit[2] * (1 - cosine) + unit[1] * sine;
3825
0
    x[2][1] = unit[1] * unit[2] * (1 - cosine) - unit[0] * sine;
3826
0
    x[2][2] = unit[2] * unit[2] * (1 - cosine) + cosine;
3827
0
    x[2][3] = 0;
3828
3829
0
    x[3][0] = 0;
3830
0
    x[3][1] = 0;
3831
0
    x[3][2] = 0;
3832
0
    x[3][3] = 1;
3833
3834
0
    return *this;
3835
0
}
3836
3837
template <class T>
3838
template <class S>
3839
const Matrix44<T> &
3840
Matrix44<T>::rotate (const Vec3<S> &r)
3841
0
{
3842
0
    S cos_rz, sin_rz, cos_ry, sin_ry, cos_rx, sin_rx;
3843
0
    S m00, m01, m02;
3844
0
    S m10, m11, m12;
3845
0
    S m20, m21, m22;
3846
3847
0
    cos_rz = Math<S>::cos (r[2]);
3848
0
    cos_ry = Math<S>::cos (r[1]);
3849
0
    cos_rx = Math<S>::cos (r[0]);
3850
    
3851
0
    sin_rz = Math<S>::sin (r[2]);
3852
0
    sin_ry = Math<S>::sin (r[1]);
3853
0
    sin_rx = Math<S>::sin (r[0]);
3854
3855
0
    m00 =  cos_rz *  cos_ry;
3856
0
    m01 =  sin_rz *  cos_ry;
3857
0
    m02 = -sin_ry;
3858
0
    m10 = -sin_rz *  cos_rx + cos_rz * sin_ry * sin_rx;
3859
0
    m11 =  cos_rz *  cos_rx + sin_rz * sin_ry * sin_rx;
3860
0
    m12 =  cos_ry *  sin_rx;
3861
0
    m20 = -sin_rz * -sin_rx + cos_rz * sin_ry * cos_rx;
3862
0
    m21 =  cos_rz * -sin_rx + sin_rz * sin_ry * cos_rx;
3863
0
    m22 =  cos_ry *  cos_rx;
3864
3865
0
    Matrix44<T> P (*this);
3866
3867
0
    x[0][0] = P[0][0] * m00 + P[1][0] * m01 + P[2][0] * m02;
3868
0
    x[0][1] = P[0][1] * m00 + P[1][1] * m01 + P[2][1] * m02;
3869
0
    x[0][2] = P[0][2] * m00 + P[1][2] * m01 + P[2][2] * m02;
3870
0
    x[0][3] = P[0][3] * m00 + P[1][3] * m01 + P[2][3] * m02;
3871
3872
0
    x[1][0] = P[0][0] * m10 + P[1][0] * m11 + P[2][0] * m12;
3873
0
    x[1][1] = P[0][1] * m10 + P[1][1] * m11 + P[2][1] * m12;
3874
0
    x[1][2] = P[0][2] * m10 + P[1][2] * m11 + P[2][2] * m12;
3875
0
    x[1][3] = P[0][3] * m10 + P[1][3] * m11 + P[2][3] * m12;
3876
3877
0
    x[2][0] = P[0][0] * m20 + P[1][0] * m21 + P[2][0] * m22;
3878
0
    x[2][1] = P[0][1] * m20 + P[1][1] * m21 + P[2][1] * m22;
3879
0
    x[2][2] = P[0][2] * m20 + P[1][2] * m21 + P[2][2] * m22;
3880
0
    x[2][3] = P[0][3] * m20 + P[1][3] * m21 + P[2][3] * m22;
3881
3882
0
    return *this;
3883
0
}
3884
3885
template <class T>
3886
const Matrix44<T> &
3887
Matrix44<T>::setScale (T s)
3888
{
3889
    memset (x, 0, sizeof (x));
3890
    x[0][0] = s;
3891
    x[1][1] = s;
3892
    x[2][2] = s;
3893
    x[3][3] = 1;
3894
3895
    return *this;
3896
}
3897
3898
template <class T>
3899
template <class S>
3900
const Matrix44<T> &
3901
Matrix44<T>::setScale (const Vec3<S> &s)
3902
0
{
3903
0
    memset (x, 0, sizeof (x));
3904
0
    x[0][0] = s[0];
3905
0
    x[1][1] = s[1];
3906
0
    x[2][2] = s[2];
3907
0
    x[3][3] = 1;
3908
3909
0
    return *this;
3910
0
}
3911
3912
template <class T>
3913
template <class S>
3914
const Matrix44<T> &
3915
Matrix44<T>::scale (const Vec3<S> &s)
3916
{
3917
    x[0][0] *= s[0];
3918
    x[0][1] *= s[0];
3919
    x[0][2] *= s[0];
3920
    x[0][3] *= s[0];
3921
3922
    x[1][0] *= s[1];
3923
    x[1][1] *= s[1];
3924
    x[1][2] *= s[1];
3925
    x[1][3] *= s[1];
3926
3927
    x[2][0] *= s[2];
3928
    x[2][1] *= s[2];
3929
    x[2][2] *= s[2];
3930
    x[2][3] *= s[2];
3931
3932
    return *this;
3933
}
3934
3935
template <class T>
3936
template <class S>
3937
const Matrix44<T> &
3938
Matrix44<T>::setTranslation (const Vec3<S> &t)
3939
0
{
3940
0
    x[0][0] = 1;
3941
0
    x[0][1] = 0;
3942
0
    x[0][2] = 0;
3943
0
    x[0][3] = 0;
3944
3945
0
    x[1][0] = 0;
3946
0
    x[1][1] = 1;
3947
0
    x[1][2] = 0;
3948
0
    x[1][3] = 0;
3949
3950
0
    x[2][0] = 0;
3951
0
    x[2][1] = 0;
3952
0
    x[2][2] = 1;
3953
0
    x[2][3] = 0;
3954
3955
0
    x[3][0] = t[0];
3956
0
    x[3][1] = t[1];
3957
0
    x[3][2] = t[2];
3958
0
    x[3][3] = 1;
3959
3960
0
    return *this;
3961
0
}
3962
3963
template <class T>
3964
inline const Vec3<T>
3965
Matrix44<T>::translation () const
3966
0
{
3967
0
    return Vec3<T> (x[3][0], x[3][1], x[3][2]);
3968
0
}
3969
3970
template <class T>
3971
template <class S>
3972
const Matrix44<T> &
3973
Matrix44<T>::translate (const Vec3<S> &t)
3974
{
3975
    x[3][0] += t[0] * x[0][0] + t[1] * x[1][0] + t[2] * x[2][0];
3976
    x[3][1] += t[0] * x[0][1] + t[1] * x[1][1] + t[2] * x[2][1];
3977
    x[3][2] += t[0] * x[0][2] + t[1] * x[1][2] + t[2] * x[2][2];
3978
    x[3][3] += t[0] * x[0][3] + t[1] * x[1][3] + t[2] * x[2][3];
3979
3980
    return *this;
3981
}
3982
3983
template <class T>
3984
template <class S>
3985
const Matrix44<T> &
3986
Matrix44<T>::setShear (const Vec3<S> &h)
3987
{
3988
    x[0][0] = 1;
3989
    x[0][1] = 0;
3990
    x[0][2] = 0;
3991
    x[0][3] = 0;
3992
3993
    x[1][0] = h[0];
3994
    x[1][1] = 1;
3995
    x[1][2] = 0;
3996
    x[1][3] = 0;
3997
3998
    x[2][0] = h[1];
3999
    x[2][1] = h[2];
4000
    x[2][2] = 1;
4001
    x[2][3] = 0;
4002
4003
    x[3][0] = 0;
4004
    x[3][1] = 0;
4005
    x[3][2] = 0;
4006
    x[3][3] = 1;
4007
4008
    return *this;
4009
}
4010
4011
template <class T>
4012
template <class S>
4013
const Matrix44<T> &
4014
Matrix44<T>::setShear (const Shear6<S> &h)
4015
{
4016
    x[0][0] = 1;
4017
    x[0][1] = h.yx;
4018
    x[0][2] = h.zx;
4019
    x[0][3] = 0;
4020
4021
    x[1][0] = h.xy;
4022
    x[1][1] = 1;
4023
    x[1][2] = h.zy;
4024
    x[1][3] = 0;
4025
4026
    x[2][0] = h.xz;
4027
    x[2][1] = h.yz;
4028
    x[2][2] = 1;
4029
    x[2][3] = 0;
4030
4031
    x[3][0] = 0;
4032
    x[3][1] = 0;
4033
    x[3][2] = 0;
4034
    x[3][3] = 1;
4035
4036
    return *this;
4037
}
4038
4039
template <class T>
4040
template <class S>
4041
const Matrix44<T> &
4042
Matrix44<T>::shear (const Vec3<S> &h)
4043
{
4044
    //
4045
    // In this case, we don't need a temp. copy of the matrix 
4046
    // because we never use a value on the RHS after we've 
4047
    // changed it on the LHS.
4048
    // 
4049
4050
    for (int i=0; i < 4; i++)
4051
    {
4052
        x[2][i] += h[1] * x[0][i] + h[2] * x[1][i];
4053
        x[1][i] += h[0] * x[0][i];
4054
    }
4055
4056
    return *this;
4057
}
4058
4059
template <class T>
4060
template <class S>
4061
const Matrix44<T> &
4062
Matrix44<T>::shear (const Shear6<S> &h)
4063
{
4064
    Matrix44<T> P (*this);
4065
4066
    for (int i=0; i < 4; i++)
4067
    {
4068
        x[0][i] = P[0][i] + h.yx * P[1][i] + h.zx * P[2][i];
4069
        x[1][i] = h.xy * P[0][i] + P[1][i] + h.zy * P[2][i];
4070
        x[2][i] = h.xz * P[0][i] + h.yz * P[1][i] + P[2][i];
4071
    }
4072
4073
    return *this;
4074
}
4075
4076
4077
//--------------------------------
4078
// Implementation of stream output
4079
//--------------------------------
4080
4081
template <class T>
4082
std::ostream &
4083
operator << (std::ostream &s, const Matrix22<T> &m)
4084
{
4085
    std::ios_base::fmtflags oldFlags = s.flags();
4086
    int width;
4087
4088
    if (s.flags() & std::ios_base::fixed)
4089
    {
4090
        s.setf (std::ios_base::showpoint);
4091
        width = static_cast<int>(s.precision()) + 5;
4092
    }
4093
    else
4094
    {
4095
        s.setf (std::ios_base::scientific);
4096
        s.setf (std::ios_base::showpoint);
4097
        width = static_cast<int>(s.precision()) + 8;
4098
    }
4099
4100
    s << "(" << std::setw (width) << m[0][0] <<
4101
         " " << std::setw (width) << m[0][1] << "\n" <<
4102
4103
         " " << std::setw (width) << m[1][0] <<
4104
         " " << std::setw (width) << m[1][1] << ")\n";
4105
4106
    s.flags (oldFlags);
4107
    return s;
4108
}
4109
4110
template <class T>
4111
std::ostream &
4112
operator << (std::ostream &s, const Matrix33<T> &m)
4113
{
4114
    std::ios_base::fmtflags oldFlags = s.flags();
4115
    int width;
4116
4117
    if (s.flags() & std::ios_base::fixed)
4118
    {
4119
        s.setf (std::ios_base::showpoint);
4120
        width = static_cast<int>(s.precision()) + 5;
4121
    }
4122
    else
4123
    {
4124
        s.setf (std::ios_base::scientific);
4125
        s.setf (std::ios_base::showpoint);
4126
        width = static_cast<int>(s.precision()) + 8;
4127
    }
4128
4129
    s << "(" << std::setw (width) << m[0][0] <<
4130
         " " << std::setw (width) << m[0][1] <<
4131
         " " << std::setw (width) << m[0][2] << "\n" <<
4132
4133
         " " << std::setw (width) << m[1][0] <<
4134
         " " << std::setw (width) << m[1][1] <<
4135
         " " << std::setw (width) << m[1][2] << "\n" <<
4136
4137
         " " << std::setw (width) << m[2][0] <<
4138
         " " << std::setw (width) << m[2][1] <<
4139
         " " << std::setw (width) << m[2][2] << ")\n";
4140
4141
    s.flags (oldFlags);
4142
    return s;
4143
}
4144
4145
template <class T>
4146
std::ostream &
4147
operator << (std::ostream &s, const Matrix44<T> &m)
4148
{
4149
    std::ios_base::fmtflags oldFlags = s.flags();
4150
    int width;
4151
4152
    if (s.flags() & std::ios_base::fixed)
4153
    {
4154
        s.setf (std::ios_base::showpoint);
4155
        width = static_cast<int>(s.precision()) + 5;
4156
    }
4157
    else
4158
    {
4159
        s.setf (std::ios_base::scientific);
4160
        s.setf (std::ios_base::showpoint);
4161
        width = static_cast<int>(s.precision()) + 8;
4162
    }
4163
4164
    s << "(" << std::setw (width) << m[0][0] <<
4165
         " " << std::setw (width) << m[0][1] <<
4166
         " " << std::setw (width) << m[0][2] <<
4167
         " " << std::setw (width) << m[0][3] << "\n" <<
4168
4169
         " " << std::setw (width) << m[1][0] <<
4170
         " " << std::setw (width) << m[1][1] <<
4171
         " " << std::setw (width) << m[1][2] <<
4172
         " " << std::setw (width) << m[1][3] << "\n" <<
4173
4174
         " " << std::setw (width) << m[2][0] <<
4175
         " " << std::setw (width) << m[2][1] <<
4176
         " " << std::setw (width) << m[2][2] <<
4177
         " " << std::setw (width) << m[2][3] << "\n" <<
4178
4179
         " " << std::setw (width) << m[3][0] <<
4180
         " " << std::setw (width) << m[3][1] <<
4181
         " " << std::setw (width) << m[3][2] <<
4182
         " " << std::setw (width) << m[3][3] << ")\n";
4183
4184
    s.flags (oldFlags);
4185
    return s;
4186
}
4187
4188
4189
//---------------------------------------------------------------
4190
// Implementation of vector-times-matrix multiplication operators
4191
//---------------------------------------------------------------
4192
4193
template <class S, class T>
4194
inline const Vec2<S> &
4195
operator *= (Vec2<S> &v, const Matrix22<T> &m)
4196
{
4197
    S x = S(v.x * m[0][0] + v.y * m[1][0]);
4198
    S y = S(v.x * m[0][1] + v.y * m[1][1]);
4199
4200
    v.x = x;
4201
    v.y = y;
4202
4203
    return v;
4204
}
4205
4206
template <class S, class T>
4207
inline Vec2<S>
4208
operator * (const Vec2<S> &v, const Matrix22<T> &m)
4209
{
4210
    S x = S(v.x * m[0][0] + v.y * m[1][0]);
4211
    S y = S(v.x * m[0][1] + v.y * m[1][1]);
4212
4213
    return Vec2<S> (x, y);
4214
}
4215
4216
template <class S, class T>
4217
inline const Vec2<S> &
4218
operator *= (Vec2<S> &v, const Matrix33<T> &m)
4219
{
4220
    S x = S(v.x * m[0][0] + v.y * m[1][0] + m[2][0]);
4221
    S y = S(v.x * m[0][1] + v.y * m[1][1] + m[2][1]);
4222
    S w = S(v.x * m[0][2] + v.y * m[1][2] + m[2][2]);
4223
4224
    v.x = x / w;
4225
    v.y = y / w;
4226
4227
    return v;
4228
}
4229
4230
template <class S, class T>
4231
inline Vec2<S>
4232
operator * (const Vec2<S> &v, const Matrix33<T> &m)
4233
{
4234
    S x = S(v.x * m[0][0] + v.y * m[1][0] + m[2][0]);
4235
    S y = S(v.x * m[0][1] + v.y * m[1][1] + m[2][1]);
4236
    S w = S(v.x * m[0][2] + v.y * m[1][2] + m[2][2]);
4237
4238
    return Vec2<S> (x / w, y / w);
4239
}
4240
4241
4242
template <class S, class T>
4243
inline const Vec3<S> &
4244
operator *= (Vec3<S> &v, const Matrix33<T> &m)
4245
{
4246
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0]);
4247
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1]);
4248
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2]);
4249
4250
    v.x = x;
4251
    v.y = y;
4252
    v.z = z;
4253
4254
    return v;
4255
}
4256
4257
template <class S, class T>
4258
inline Vec3<S>
4259
operator * (const Vec3<S> &v, const Matrix33<T> &m)
4260
{
4261
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0]);
4262
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1]);
4263
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2]);
4264
4265
    return Vec3<S> (x, y, z);
4266
}
4267
4268
4269
template <class S, class T>
4270
inline const Vec3<S> &
4271
operator *= (Vec3<S> &v, const Matrix44<T> &m)
4272
{
4273
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + m[3][0]);
4274
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + m[3][1]);
4275
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + m[3][2]);
4276
    S w = S(v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + m[3][3]);
4277
4278
    v.x = x / w;
4279
    v.y = y / w;
4280
    v.z = z / w;
4281
4282
    return v;
4283
}
4284
4285
template <class S, class T>
4286
inline Vec3<S>
4287
operator * (const Vec3<S> &v, const Matrix44<T> &m)
4288
{
4289
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + m[3][0]);
4290
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + m[3][1]);
4291
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + m[3][2]);
4292
    S w = S(v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + m[3][3]);
4293
4294
    return Vec3<S> (x / w, y / w, z / w);
4295
}
4296
4297
4298
template <class S, class T>
4299
inline const Vec4<S> &
4300
operator *= (Vec4<S> &v, const Matrix44<T> &m)
4301
{
4302
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + v.w * m[3][0]);
4303
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + v.w * m[3][1]);
4304
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + v.w * m[3][2]);
4305
    S w = S(v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + v.w * m[3][3]);
4306
4307
    v.x = x;
4308
    v.y = y;
4309
    v.z = z;
4310
    v.w = w;
4311
4312
    return v;
4313
}
4314
4315
template <class S, class T>
4316
inline Vec4<S>
4317
operator * (const Vec4<S> &v, const Matrix44<T> &m)
4318
{
4319
    S x = S(v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + v.w * m[3][0]);
4320
    S y = S(v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + v.w * m[3][1]);
4321
    S z = S(v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + v.w * m[3][2]);
4322
    S w = S(v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + v.w * m[3][3]);
4323
4324
    return Vec4<S> (x, y, z, w);
4325
}
4326
4327
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
4328
4329
#endif // INCLUDED_IMATHMATRIX_H