source: GTP/trunk/App/Games/Jungle_Rumble/src/physic/foundation/include/NxMat33.h @ 1378

Revision 1378, 36.6 KB checked in by giegl, 18 years ago (diff)

GTPD - Jungle Rumble - integrate into GTP SVN structure
Line 
1#ifndef NX_FOUNDATION_NxMat33T
2#define NX_FOUNDATION_NxMat33T
3/*----------------------------------------------------------------------------*\
4|
5|                                               Public Interface to NovodeX Technology
6|
7|                                                            www.novodex.com
8|
9\*----------------------------------------------------------------------------*/
10/** \addtogroup foundation
11  @{
12*/
13
14#include "NxVec3.h"
15#include "NxQuat.h"
16
17
18/**
19\brief Identifies a special matrix. Can be passed to the #NxMat33 constructor.
20*/
21enum NxMatrixType
22        {
23        /**
24        \brief Matrix of all zeros.
25        */
26        NX_ZERO_MATRIX,
27
28        /**
29        \brief Identity matrix.
30        */
31        NX_IDENTITY_MATRIX
32        };
33
34#include "Nx9F32.h"                             //change this if changing type below, to Nx9F32, Nx12F32, Nx16F32               
35
36typedef Nx9Real Mat33DataType;  //takes Nx9Real, Nx12Real, Nx16Real
37
38/**
39\brief 3x3 Matrix Class.
40
41 The idea of the matrix/vector classes is to partition them into two parts:
42 One is the data structure which may have different formatting (3x3, 3x4, 4x4),
43 row or column major.  The other is a template class which has all the operators
44 but is storage format independent.
45
46 This way it should be easier to change formats depending on what is faster/slower
47 on a particular platform.
48
49 Design issue: We use nameless struct/unions here.
50 Design issue: this used to be implemented with a template.  This had no benefit
51 but it added syntactic complexity.  Currently we just use a typedef and a preprocessor switch
52 to change between different memory layouts.
53
54 The matrix math in this class is storage format (row/col major) independent as far
55 as the user is concerned.
56 When the user wants to get/set raw data, he needs to specify what order the data is
57 coming in. 
58 
59*/
60class NxMat33
61        {
62        public:
63        NX_INLINE NxMat33();
64
65        /**
66        \param type Special matrix type to initialize with.
67
68        @see NxMatrixType
69        */
70        NX_INLINE NxMat33(NxMatrixType type);
71        NX_INLINE NxMat33(const NxVec3 &row0, const NxVec3 &row1, const NxVec3 &row2);
72
73        NX_INLINE NxMat33(const NxMat33&m);
74        NX_INLINE NxMat33(const NxQuat &m);
75        NX_INLINE ~NxMat33();
76        NX_INLINE const NxMat33& operator=(const NxMat33 &src);
77
78        // Access elements
79
80        //low level data access, single or double precision, with eventual translation:
81        //for dense 9 element data
82        NX_INLINE void setRowMajor(const NxF32 *);
83        NX_INLINE void setColumnMajor(const NxF32 *);
84        NX_INLINE void getRowMajor(NxF32 *) const;
85        NX_INLINE void getColumnMajor(NxF32 *) const;
86
87        NX_INLINE void setRowMajor(const NxF64 *);
88        NX_INLINE void setColumnMajor(const NxF64 *);
89        NX_INLINE void getRowMajor(NxF64 *) const;
90        NX_INLINE void getColumnMajor(NxF64 *) const;
91
92
93        //for loose 4-padded data.
94        NX_INLINE void setRowMajorStride4(const NxF32 *);
95        NX_INLINE void setColumnMajorStride4(const NxF32 *);
96        NX_INLINE void getRowMajorStride4(NxF32 *) const;
97        NX_INLINE void getColumnMajorStride4(NxF32 *) const;
98
99        NX_INLINE void setRowMajorStride4(const NxF64 *);
100        NX_INLINE void setColumnMajorStride4(const NxF64 *);
101        NX_INLINE void getRowMajorStride4(NxF64 *) const;
102        NX_INLINE void getColumnMajorStride4(NxF64 *) const;
103
104
105        NX_INLINE void setRow(int row, const NxVec3 &);
106        NX_INLINE void setColumn(int col, const NxVec3 &);
107        NX_INLINE void getRow(int row, NxVec3 &) const;
108        NX_INLINE void getColumn(int col, NxVec3 &) const;
109
110        NX_INLINE NxVec3 getRow(int row) const;
111        NX_INLINE NxVec3 getColumn(int col) const;
112
113
114        //element access:
115    NX_INLINE NxReal & operator()(int row, int col);
116    NX_INLINE const NxReal & operator() (int row, int col) const;
117
118        /**
119        \brief returns true for identity matrix
120        */
121        NX_INLINE bool isIdentity() const;
122
123        /**
124        \brief returns true for zero matrix
125        */
126        NX_INLINE bool isZero() const;
127
128        /**
129        \brief returns true if all elems are finite (not NAN or INF, etc.)
130        */
131        NX_INLINE bool isFinite() const;
132
133        //create special matrices:
134
135        /**
136        \brief sets this matrix to the zero matrix.
137        */
138        NX_INLINE void zero();
139
140        /**
141        \brief sets this matrix to the identity matrix.
142        */
143        NX_INLINE void id();
144
145        /**
146        \brief this = -this
147        */
148        NX_INLINE void setNegative();
149
150        /**
151        \brief sets this matrix to the diagonal matrix.
152        */
153        NX_INLINE void diagonal(const NxVec3 &vec);
154
155        /**
156        \brief Sets this matrix to the Star(Skew Symetric) matrix.
157
158        So that star(v) * x = v.cross(x) .
159        */
160        NX_INLINE void star(const NxVec3 &vec);
161
162
163        NX_INLINE void fromQuat(const NxQuat &);
164        NX_INLINE void toQuat(NxQuat &) const;
165
166        //modifications:
167
168        NX_INLINE const NxMat33 &operator +=(const NxMat33 &s);
169        NX_INLINE const NxMat33 &operator -=(const NxMat33 &s);
170        NX_INLINE const NxMat33 &operator *=(NxReal s);
171        NX_INLINE const NxMat33 &operator /=(NxReal s);
172
173        /*
174        Gram-Schmidt orthogonalization to correct numerical drift, plus column normalization
175        Caution: I believe the current implementation does not work right!
176        */
177//      NX_INLINE void orthonormalize();
178
179
180        /**
181        \brief returns determinant
182        */
183        NX_INLINE NxReal determinant() const;
184
185        /**
186        \brief assigns inverse to dest.
187       
188        Returns false if singular (i.e. if no inverse exists), setting dest to identity.
189        */
190        NX_INLINE bool getInverse(NxMat33& dest) const;
191
192        /**
193        \brief this = transpose(other)
194
195        this == other is OK.
196        */
197        NX_INLINE void setTransposed(const NxMat33& other);
198
199        /**
200        \brief this = transpose(this)
201        */
202        NX_INLINE void setTransposed();
203
204        /**
205        \brief this = this * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
206        */
207        NX_INLINE void multiplyDiagonal(const NxVec3 &d);
208
209        /**
210        */
211        NX_INLINE void multiplyDiagonalTranspose(const NxVec3 &d);
212
213        /**
214        \brief dst = this * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
215        */
216        NX_INLINE void multiplyDiagonal(const NxVec3 &d, NxMat33 &dst) const;
217
218        /**
219        \brief dst = this * [ d.x 0 0; 0 d.y 0; 0 0 d.z];
220        */
221        NX_INLINE void multiplyDiagonalTranspose(const NxVec3 &d, NxMat33 &dst) const;
222
223        /**
224        \brief dst = this * src
225        */
226        NX_INLINE void multiply(const NxVec3 &src, NxVec3 &dst) const;
227        /**
228        \brief dst = transpose(this) * src
229        */
230        NX_INLINE void multiplyByTranspose(const NxVec3 &src, NxVec3 &dst) const;
231
232        /**
233        \brief this = a + b
234        */
235        NX_INLINE void  add(const NxMat33 & a, const NxMat33 & b);
236        /***
237        \brief this = a - b
238        */
239        NX_INLINE void  subtract(const NxMat33 &a, const NxMat33 &b);
240        /**
241        \brief this = s * a;
242        */
243        NX_INLINE void  multiply(NxReal s,  const NxMat33 & a);
244        /**
245        \brief this = left * right
246        */
247        NX_INLINE void multiply(const NxMat33& left, const NxMat33& right);
248        /**
249        \brief this = transpose(left) * right
250
251        \note #multiplyByTranspose() is faster.
252        */
253        NX_INLINE void multiplyTransposeLeft(const NxMat33& left, const NxMat33& right);
254        /**
255        \brief this = left * transpose(right)
256       
257        \note faster than #multiplyByTranspose().
258        */
259        NX_INLINE void multiplyTransposeRight(const NxMat33& left, const NxMat33& right);
260
261        /**
262        \brief this = left * transpose(right)
263        */
264        NX_INLINE void multiplyTransposeRight(const NxVec3 &left, const NxVec3 &right);
265
266        /**
267        \brief this = rotation matrix around X axis
268
269        <b>Unit:</b> Radians
270        */
271        NX_INLINE void rotX(NxReal angle);
272
273        /**
274        \brief this = rotation matrix around Y axis
275
276        <b>Unit:</b> Radians
277        */
278        NX_INLINE void rotY(NxReal angle);
279
280        /**
281        \brief this = rotation matrix around Z axis
282
283        <b>Unit:</b> Radians
284        */
285        NX_INLINE void rotZ(NxReal angle);
286
287
288        //overloaded multiply, and transposed-multiply ops:
289
290        /**
291        \brief returns transpose(this)*src
292        */
293        NX_INLINE NxVec3 operator%  (const NxVec3 & src) const;
294        /**
295        \brief matrix vector product
296        */
297        NX_INLINE NxVec3 operator*  (const NxVec3 & src) const;
298        /**
299        \brief matrix product
300        */
301        NX_INLINE NxMat33&      operator*= (const NxMat33& mat);
302        /**
303        \brief matrix difference
304        */
305        NX_INLINE NxMat33       operator-  (const NxMat33& mat) const;
306        /**
307        \brief matrix addition
308        */
309        NX_INLINE NxMat33       operator+  (const NxMat33& mat) const;
310        /**
311        \brief matrix product
312        */
313        NX_INLINE NxMat33       operator*  (const NxMat33& mat) const;
314        /**
315        \brief matrix scalar product
316        */
317        NX_INLINE NxMat33       operator*  (float s)                            const;
318
319        private:
320        Mat33DataType data;
321        };
322
323
324NX_INLINE NxMat33::NxMat33()
325        {
326        }
327
328
329NX_INLINE NxMat33::NxMat33(NxMatrixType type)
330        {
331                switch(type)
332                {
333                        case NX_ZERO_MATRIX:            zero(); break;
334                        case NX_IDENTITY_MATRIX:        id();   break;
335                }
336        }
337
338
339NX_INLINE NxMat33::NxMat33(const NxMat33& a)
340        {
341        data = a.data;
342        }
343
344
345NX_INLINE NxMat33::NxMat33(const NxQuat &q)
346        {
347        fromQuat(q);
348        }
349
350NX_INLINE NxMat33::NxMat33(const NxVec3 &row0, const NxVec3 &row1, const NxVec3 &row2)
351{
352        data.s._11 = row0.x;  data.s._12 = row0.y;  data.s._13 = row0.z;
353        data.s._21 = row1.x;  data.s._22 = row1.y;  data.s._23 = row1.z;
354        data.s._31 = row2.x;  data.s._32 = row2.y;  data.s._33 = row2.z;
355}
356
357
358NX_INLINE NxMat33::~NxMat33()
359        {
360        //nothing
361        }
362
363
364NX_INLINE const NxMat33& NxMat33::operator=(const NxMat33 &a)
365        {
366        data = a.data;
367        return *this;
368        }
369
370
371NX_INLINE void NxMat33::setRowMajor(const NxF32* d)
372        {
373        //we are also row major, so this is a direct copy
374        data.s._11 = (NxReal)d[0];
375        data.s._12 = (NxReal)d[1];
376        data.s._13 = (NxReal)d[2];
377
378        data.s._21 = (NxReal)d[3];
379        data.s._22 = (NxReal)d[4];
380        data.s._23 = (NxReal)d[5];
381
382        data.s._31 = (NxReal)d[6];
383        data.s._32 = (NxReal)d[7];
384        data.s._33 = (NxReal)d[8];
385        }
386
387
388NX_INLINE void NxMat33::setColumnMajor(const NxF32* d)
389        {
390        //we are column major, so copy transposed.
391        data.s._11 = (NxReal)d[0];
392        data.s._12 = (NxReal)d[3];
393        data.s._13 = (NxReal)d[6];
394
395        data.s._21 = (NxReal)d[1];
396        data.s._22 = (NxReal)d[4];
397        data.s._23 = (NxReal)d[7];
398
399        data.s._31 = (NxReal)d[2];
400        data.s._32 = (NxReal)d[5];
401        data.s._33 = (NxReal)d[8];
402        }
403
404
405NX_INLINE void NxMat33::getRowMajor(NxF32* d) const
406        {
407        //we are also row major, so this is a direct copy
408        d[0] = (NxF32)data.s._11;
409        d[1] = (NxF32)data.s._12;
410        d[2] = (NxF32)data.s._13;
411
412        d[3] = (NxF32)data.s._21;
413        d[4] = (NxF32)data.s._22;
414        d[5] = (NxF32)data.s._23;
415
416        d[6] = (NxF32)data.s._31;
417        d[7] = (NxF32)data.s._32;
418        d[8] = (NxF32)data.s._33;
419        }
420
421
422NX_INLINE void NxMat33::getColumnMajor(NxF32* d) const
423        {
424        //we are column major, so copy transposed.
425        d[0] = (NxF32)data.s._11;
426        d[3] = (NxF32)data.s._12;
427        d[6] = (NxF32)data.s._13;
428
429        d[1] = (NxF32)data.s._21;
430        d[4] = (NxF32)data.s._22;
431        d[7] = (NxF32)data.s._23;
432
433        d[2] = (NxF32)data.s._31;
434        d[5] = (NxF32)data.s._32;
435        d[8] = (NxF32)data.s._33;
436        }
437
438
439NX_INLINE void NxMat33::setRowMajorStride4(const NxF32* d)
440        {
441        //we are also row major, so this is a direct copy
442        //however we've got to skip every 4th element.
443        data.s._11 = (NxReal)d[0];
444        data.s._12 = (NxReal)d[1];
445        data.s._13 = (NxReal)d[2];
446
447        data.s._21 = (NxReal)d[4];
448        data.s._22 = (NxReal)d[5];
449        data.s._23 = (NxReal)d[6];
450
451        data.s._31 = (NxReal)d[8];
452        data.s._32 = (NxReal)d[9];
453        data.s._33 = (NxReal)d[10];
454        }
455
456
457NX_INLINE void NxMat33::setColumnMajorStride4(const NxF32* d)
458        {
459        //we are column major, so copy transposed.
460        //however we've got to skip every 4th element.
461        data.s._11 = (NxReal)d[0];
462        data.s._12 = (NxReal)d[4];
463        data.s._13 = (NxReal)d[8];
464
465        data.s._21 = (NxReal)d[1];
466        data.s._22 = (NxReal)d[5];
467        data.s._23 = (NxReal)d[9];
468
469        data.s._31 = (NxReal)d[2];
470        data.s._32 = (NxReal)d[6];
471        data.s._33 = (NxReal)d[10];
472        }
473
474
475NX_INLINE void NxMat33::getRowMajorStride4(NxF32* d) const
476        {
477        //we are also row major, so this is a direct copy
478        //however we've got to skip every 4th element.
479        d[0] = (NxF32)data.s._11;
480        d[1] = (NxF32)data.s._12;
481        d[2] = (NxF32)data.s._13;
482
483        d[4] = (NxF32)data.s._21;
484        d[5] = (NxF32)data.s._22;
485        d[6] = (NxF32)data.s._23;
486
487        d[8] = (NxF32)data.s._31;
488        d[9] = (NxF32)data.s._32;
489        d[10]= (NxF32)data.s._33;
490        }
491
492
493NX_INLINE void NxMat33::getColumnMajorStride4(NxF32* d) const
494        {
495        //we are column major, so copy transposed.
496        //however we've got to skip every 4th element.
497        d[0] = (NxF32)data.s._11;
498        d[4] = (NxF32)data.s._12;
499        d[8] = (NxF32)data.s._13;
500
501        d[1] = (NxF32)data.s._21;
502        d[5] = (NxF32)data.s._22;
503        d[9] = (NxF32)data.s._23;
504
505        d[2] = (NxF32)data.s._31;
506        d[6] = (NxF32)data.s._32;
507        d[10]= (NxF32)data.s._33;
508        }
509
510
511NX_INLINE void NxMat33::setRowMajor(const NxF64*d)
512        {
513        //we are also row major, so this is a direct copy
514        data.s._11 = (NxReal)d[0];
515        data.s._12 = (NxReal)d[1];
516        data.s._13 = (NxReal)d[2];
517
518        data.s._21 = (NxReal)d[3];
519        data.s._22 = (NxReal)d[4];
520        data.s._23 = (NxReal)d[5];
521
522        data.s._31 = (NxReal)d[6];
523        data.s._32 = (NxReal)d[7];
524        data.s._33 = (NxReal)d[8];
525        }
526
527
528NX_INLINE void NxMat33::setColumnMajor(const NxF64*d)
529        {
530        //we are column major, so copy transposed.
531        data.s._11 = (NxReal)d[0];
532        data.s._12 = (NxReal)d[3];
533        data.s._13 = (NxReal)d[6];
534
535        data.s._21 = (NxReal)d[1];
536        data.s._22 = (NxReal)d[4];
537        data.s._23 = (NxReal)d[7];
538
539        data.s._31 = (NxReal)d[2];
540        data.s._32 = (NxReal)d[5];
541        data.s._33 = (NxReal)d[8];
542        }
543
544
545NX_INLINE void NxMat33::getRowMajor(NxF64*d) const
546        {
547        //we are also row major, so this is a direct copy
548        d[0] = (NxF64)data.s._11;
549        d[1] = (NxF64)data.s._12;
550        d[2] = (NxF64)data.s._13;
551
552        d[3] = (NxF64)data.s._21;
553        d[4] = (NxF64)data.s._22;
554        d[5] = (NxF64)data.s._23;
555
556        d[6] = (NxF64)data.s._31;
557        d[7] = (NxF64)data.s._32;
558        d[8] = (NxF64)data.s._33;
559        }
560
561
562NX_INLINE void NxMat33::getColumnMajor(NxF64*d) const
563        {
564        //we are column major, so copy transposed.
565        d[0] = (NxF64)data.s._11;
566        d[3] = (NxF64)data.s._12;
567        d[6] = (NxF64)data.s._13;
568
569        d[1] = (NxF64)data.s._21;
570        d[4] = (NxF64)data.s._22;
571        d[7] = (NxF64)data.s._23;
572
573        d[2] = (NxF64)data.s._31;
574        d[5] = (NxF64)data.s._32;
575        d[8] = (NxF64)data.s._33;
576        }
577
578
579NX_INLINE void NxMat33::setRowMajorStride4(const NxF64*d)
580        {
581        //we are also row major, so this is a direct copy
582        //however we've got to skip every 4th element.
583        data.s._11 = (NxReal)d[0];
584        data.s._12 = (NxReal)d[1];
585        data.s._13 = (NxReal)d[2];
586
587        data.s._21 = (NxReal)d[4];
588        data.s._22 = (NxReal)d[5];
589        data.s._23 = (NxReal)d[6];
590
591        data.s._31 = (NxReal)d[8];
592        data.s._32 = (NxReal)d[9];
593        data.s._33 = (NxReal)d[10];
594        }
595
596
597NX_INLINE void NxMat33::setColumnMajorStride4(const NxF64*d)
598        {
599        //we are column major, so copy transposed.
600        //however we've got to skip every 4th element.
601        data.s._11 = (NxReal)d[0];
602        data.s._12 = (NxReal)d[4];
603        data.s._13 = (NxReal)d[8];
604
605        data.s._21 = (NxReal)d[1];
606        data.s._22 = (NxReal)d[5];
607        data.s._23 = (NxReal)d[9];
608
609        data.s._31 = (NxReal)d[2];
610        data.s._32 = (NxReal)d[6];
611        data.s._33 = (NxReal)d[10];
612        }
613
614
615NX_INLINE void NxMat33::getRowMajorStride4(NxF64*d) const
616        {
617        //we are also row major, so this is a direct copy
618        //however we've got to skip every 4th element.
619        d[0] = (NxF64)data.s._11;
620        d[1] = (NxF64)data.s._12;
621        d[2] = (NxF64)data.s._13;
622
623        d[4] = (NxF64)data.s._21;
624        d[5] = (NxF64)data.s._22;
625        d[6] = (NxF64)data.s._23;
626
627        d[8] = (NxF64)data.s._31;
628        d[9] = (NxF64)data.s._32;
629        d[10]= (NxF64)data.s._33;
630        }
631
632
633NX_INLINE void NxMat33::getColumnMajorStride4(NxF64*d) const
634
635        {
636        //we are column major, so copy transposed.
637        //however we've got to skip every 4th element.
638        d[0] = (NxF64)data.s._11;
639        d[4] = (NxF64)data.s._12;
640        d[8] = (NxF64)data.s._13;
641
642        d[1] = (NxF64)data.s._21;
643        d[5] = (NxF64)data.s._22;
644        d[9] = (NxF64)data.s._23;
645
646        d[2] = (NxF64)data.s._31;
647        d[6] = (NxF64)data.s._32;
648        d[10]= (NxF64)data.s._33;
649        }
650
651
652NX_INLINE void NxMat33::setRow(int row, const NxVec3 & v)
653        {
654#ifndef TRANSPOSED_MAT33
655        data.m[row][0] = v.x;
656        data.m[row][1] = v.y;
657        data.m[row][2] = v.z;
658#else
659        data.m[0][row] = v.x;
660        data.m[1][row] = v.y;
661        data.m[2][row] = v.z;
662#endif
663        }
664
665
666NX_INLINE void NxMat33::setColumn(int col, const NxVec3 & v)
667        {
668#ifndef TRANSPOSED_MAT33
669        data.m[0][col] = v.x;
670        data.m[1][col] = v.y;
671        data.m[2][col] = v.z;
672#else
673        data.m[col][0] = v.x;
674        data.m[col][1] = v.y;
675        data.m[col][2] = v.z;
676#endif
677        }
678
679
680NX_INLINE void NxMat33::getRow(int row, NxVec3 & v) const
681        {
682#ifndef TRANSPOSED_MAT33
683        v.x = data.m[row][0];
684        v.y = data.m[row][1];
685        v.z = data.m[row][2];
686#else
687        v.x = data.m[0][row];
688        v.y = data.m[1][row];
689        v.z = data.m[2][row];
690#endif
691        }
692
693
694NX_INLINE void NxMat33::getColumn(int col, NxVec3 & v) const
695        {
696#ifndef TRANSPOSED_MAT33
697        v.x = data.m[0][col];
698        v.y = data.m[1][col];
699        v.z = data.m[2][col];
700#else
701        v.x = data.m[col][0];
702        v.y = data.m[col][1];
703        v.z = data.m[col][2];
704#endif
705        }
706
707
708NX_INLINE NxVec3 NxMat33::getRow(int row) const
709{
710#ifndef TRANSPOSED_MAT33
711        return NxVec3(data.m[row][0],data.m[row][1],data.m[row][2]);
712#else
713        return NxVec3(data.m[0][row],data.m[1][row],data.m[2][row]);
714#endif
715}
716
717NX_INLINE NxVec3 NxMat33::getColumn(int col) const
718{
719#ifndef TRANSPOSED_MAT33
720        return NxVec3(data.m[0][col],data.m[1][col],data.m[2][col]);
721#else
722        return NxVec3(data.m[col][0],data.m[col][1],data.m[col][2]);
723#endif
724}
725
726NX_INLINE NxReal & NxMat33::operator()(int row, int col)
727        {
728#ifndef TRANSPOSED_MAT33
729        return data.m[row][col];
730#else
731        return data.m[col][row];
732#endif
733        }
734
735
736NX_INLINE const NxReal & NxMat33::operator() (int row, int col) const
737        {
738#ifndef TRANSPOSED_MAT33
739        return data.m[row][col];
740#else
741        return data.m[col][row];
742#endif
743        }
744
745//const methods
746
747
748NX_INLINE bool NxMat33::isIdentity() const
749        {
750        if(NX_IR(data.s._11)!=NX_IEEE_1_0)      return false;
751        if(NX_IR(data.s._12))                           return false;
752        if(NX_IR(data.s._13))                           return false;
753
754        if(NX_IR(data.s._21))                           return false;
755        if(NX_IR(data.s._22)!=NX_IEEE_1_0)      return false;
756        if(NX_IR(data.s._23))                           return false;
757
758        if(NX_IR(data.s._31))                           return false;
759        if(NX_IR(data.s._32))                           return false;
760        if(NX_IR(data.s._33)!=NX_IEEE_1_0)      return false;
761        return true;
762        }
763
764
765NX_INLINE bool NxMat33::isZero() const
766        {
767        if(NX_IR(data.s._11))   return false;
768        if(NX_IR(data.s._12))   return false;
769        if(NX_IR(data.s._13))   return false;
770
771        if(NX_IR(data.s._21))   return false;
772        if(NX_IR(data.s._22))   return false;
773        if(NX_IR(data.s._23))   return false;
774
775        if(NX_IR(data.s._31))   return false;
776        if(NX_IR(data.s._32))   return false;
777        if(NX_IR(data.s._33))   return false;
778        return true;
779        }
780
781
782NX_INLINE bool NxMat33::isFinite() const
783        {
784        return NxMath::isFinite(data.s._11)
785        && NxMath::isFinite(data.s._12)
786        && NxMath::isFinite(data.s._13)
787
788        && NxMath::isFinite(data.s._21)
789        && NxMath::isFinite(data.s._22)
790        && NxMath::isFinite(data.s._23)
791
792        && NxMath::isFinite(data.s._31)
793        && NxMath::isFinite(data.s._32)
794        && NxMath::isFinite(data.s._33);
795        }
796
797
798
799NX_INLINE void NxMat33::zero()
800        {
801        data.s._11 = NxReal(0.0);
802        data.s._12 = NxReal(0.0);
803        data.s._13 = NxReal(0.0);
804
805        data.s._21 = NxReal(0.0);
806        data.s._22 = NxReal(0.0);
807        data.s._23 = NxReal(0.0);
808
809        data.s._31 = NxReal(0.0);
810        data.s._32 = NxReal(0.0);
811        data.s._33 = NxReal(0.0);
812        }
813
814
815NX_INLINE void NxMat33::id()
816        {
817        data.s._11 = NxReal(1.0);
818        data.s._12 = NxReal(0.0);
819        data.s._13 = NxReal(0.0);
820
821        data.s._21 = NxReal(0.0);
822        data.s._22 = NxReal(1.0);
823        data.s._23 = NxReal(0.0);
824
825        data.s._31 = NxReal(0.0);
826        data.s._32 = NxReal(0.0);
827        data.s._33 = NxReal(1.0);
828        }
829
830
831NX_INLINE void NxMat33::setNegative()
832        {
833        data.s._11 = -data.s._11;
834        data.s._12 = -data.s._12;
835        data.s._13 = -data.s._13;
836
837        data.s._21 = -data.s._21;
838        data.s._22 = -data.s._22;
839        data.s._23 = -data.s._23;
840
841        data.s._31 = -data.s._31;
842        data.s._32 = -data.s._32;
843        data.s._33 = -data.s._33;
844        }
845
846
847NX_INLINE void NxMat33::diagonal(const NxVec3 &v)
848        {
849        data.s._11 = v.x;
850        data.s._12 = NxReal(0.0);
851        data.s._13 = NxReal(0.0);
852
853        data.s._21 = NxReal(0.0);
854        data.s._22 = v.y;
855        data.s._23 = NxReal(0.0);
856
857        data.s._31 = NxReal(0.0);
858        data.s._32 = NxReal(0.0);
859        data.s._33 = v.z;
860        }
861
862
863NX_INLINE void NxMat33::star(const NxVec3 &v)
864        {
865        data.s._11 = NxReal(0.0);       data.s._12 =-v.z;       data.s._13 = v.y;
866        data.s._21 = v.z;       data.s._22 = NxReal(0.0);       data.s._23 =-v.x;
867        data.s._31 =-v.y;       data.s._32 = v.x;       data.s._33 = NxReal(0.0);
868        }
869
870
871NX_INLINE void NxMat33::fromQuat(const NxQuat & q)
872        {
873        const NxReal w = q.w;
874        const NxReal x = q.x;
875        const NxReal y = q.y;
876        const NxReal z = q.z;
877
878        data.s._11 = NxReal(1.0) - y*y*NxReal(2.0) - z*z*NxReal(2.0);
879        data.s._12 = x*y*NxReal(2.0) - w*z*NxReal(2.0);
880        data.s._13 = x*z*NxReal(2.0) + w*y*NxReal(2.0);
881
882        data.s._21 = x*y*NxReal(2.0) + w*z*NxReal(2.0);
883        data.s._22 = NxReal(1.0) - x*x*NxReal(2.0) - z*z*NxReal(2.0);   
884        data.s._23 = y*z*NxReal(2.0) - w*x*NxReal(2.0);
885       
886        data.s._31 = x*z*NxReal(2.0) - w*y*NxReal(2.0);
887        data.s._32 = y*z*NxReal(2.0) + w*x*NxReal(2.0);
888        data.s._33 = NxReal(1.0) - x*x*NxReal(2.0) - y*y*NxReal(2.0);   
889        }
890
891
892NX_INLINE void NxMat33::toQuat(NxQuat & q) const                                        // set the NxQuat from a rotation matrix
893        {
894    NxReal tr, s;
895    tr = data.s._11 + data.s._22 + data.s._33;
896    if(tr >= 0)
897                {
898                s = (NxReal)NxMath::sqrt(tr +1);
899                q.w = NxReal(0.5) * s;
900                s = NxReal(0.5) / s;
901                q.x = ((*this)(2,1) - (*this)(1,2)) * s;
902                q.y = ((*this)(0,2) - (*this)(2,0)) * s;
903                q.z = ((*this)(1,0) - (*this)(0,1)) * s;
904                }
905    else
906                {
907                int i = 0;
908                if (data.s._22 > data.s._11)
909                        i = 1;
910                if(data.s._33 > (*this)(i,i))
911                        i=2;
912                switch (i)
913                        {
914                        case 0:
915                                s = (NxReal)NxMath::sqrt((data.s._11 - (data.s._22 + data.s._33)) + 1);
916                                q.x = NxReal(0.5) * s;
917                                s = NxReal(0.5) / s;
918                                q.y = ((*this)(0,1) + (*this)(1,0)) * s;
919                                q.z = ((*this)(2,0) + (*this)(0,2)) * s;
920                                q.w = ((*this)(2,1) - (*this)(1,2)) * s;
921                                break;
922                        case 1:
923                                s = (NxReal)NxMath::sqrt((data.s._22 - (data.s._33 + data.s._11)) + 1);
924                                q.y = NxReal(0.5) * s;
925                                s = NxReal(0.5) / s;
926                                q.z = ((*this)(1,2) + (*this)(2,1)) * s;
927                                q.x = ((*this)(0,1) + (*this)(1,0)) * s;
928                                q.w = ((*this)(0,2) - (*this)(2,0)) * s;
929                                break;
930                        case 2:
931                                s = (NxReal)NxMath::sqrt((data.s._33 - (data.s._11 + data.s._22)) + 1);
932                                q.z = NxReal(0.5) * s;
933                                s = NxReal(0.5) / s;
934                                q.x = ((*this)(2,0) + (*this)(0,2)) * s;
935                                q.y = ((*this)(1,2) + (*this)(2,1)) * s;
936                                q.w = ((*this)(1,0) - (*this)(0,1)) * s;
937                        }
938                }
939        }
940/*
941
942NX_INLINE void NxMat33::orthonormalize()        //Gram-Schmidt orthogonalization to correct numerical drift, plus column normalization
943        {
944        //TODO: This is buggy!
945        NxVec3 w,t1,t2,t3;
946        NxReal norm_sq;
947
948    const NxReal m=3;                   //m := linalg[rowdim](A);
949    const NxReal n=3;                   //n := linalg[coldim](A);
950        int i, j, k = 0;                                //k := 0;
951
952
953    Mat33d v = *this;                           //v := linalg[col](A, 1 .. n); -- 3 column vectors indexable
954    NxVec3 norm_u_sq;
955                                                                                                                                //# orthogonalize v[i]
956    for (i=0; i<n; i++)//for i to n do
957                {
958        v.getColumn(i,w);               //i-th column
959        for (j=0; j<k; j++)                                                                     //# pull w along projection of v[i] with u[j]
960                        {
961                        this->getColumn(j,t1);
962                        this->getColumn(j,t2);
963                        v.getColumn(i,t3);
964                        NxVec3 temp = (t2 * (NxReal(1.0)/norm_u_sq[j]));
965                        NxVec3 temp2 = temp  * t3.dot( t1 );
966                        w -= temp;     
967                        }
968                                                                                                                                //        # compute norm of orthogonalized v[i]
969      norm_sq = w.Dot(w);
970
971                if (norm_sq != NxReal(0.0))
972                        {                                                                                                       //           # linearly independent new orthogonal vector
973                                                                                                                                //       # add to list of u and norm_u_sq
974                        this->SetColumn(i,w);                                                                   //u = [op(u), evalm(w)];
975            norm_u_sq[i] = norm_sq;                                             //norm_u_sq = [op(norm_u_sq), norm_sq];
976            k ++;
977                        }
978                }
979       
980
981        NxVec3 temp;                                                                                                    //may want to do this in-place -- dunno if optimizer does this for me
982        for (i=0; i<3; i++)
983                {
984                getColumn(i,temp);
985                temp.normalize();
986                setColumn(i,temp);
987                }
988        }
989        */
990
991
992NX_INLINE void NxMat33::setTransposed(const NxMat33& other)
993        {
994        //gotta special case in-place case
995        if (this != &other)
996                {
997                data.s._11 = other.data.s._11;
998                data.s._12 = other.data.s._21;
999                data.s._13 = other.data.s._31;
1000
1001                data.s._21 = other.data.s._12;
1002                data.s._22 = other.data.s._22;
1003                data.s._23 = other.data.s._32;
1004
1005                data.s._31 = other.data.s._13;
1006                data.s._32 = other.data.s._23;
1007                data.s._33 = other.data.s._33;
1008                }
1009        else
1010                {
1011                NxReal tx, ty, tz;
1012                tx = data.s._21;        data.s._21 = other.data.s._12;  data.s._12 = tx;
1013                ty = data.s._31;        data.s._31 = other.data.s._13;  data.s._13 = ty;
1014                tz = data.s._32;        data.s._32 = other.data.s._23;  data.s._23 = tz;
1015                }
1016        }
1017
1018
1019NX_INLINE void NxMat33::setTransposed()
1020        {
1021                NX_Swap(data.s._12, data.s._21);
1022                NX_Swap(data.s._23, data.s._32);
1023                NX_Swap(data.s._13, data.s._31);
1024        }
1025
1026
1027NX_INLINE void NxMat33::multiplyDiagonal(const NxVec3 &d)
1028        {
1029        data.s._11 *= d.x;
1030        data.s._12 *= d.y;
1031        data.s._13 *= d.z;
1032
1033        data.s._21 *= d.x;
1034        data.s._22 *= d.y;
1035        data.s._23 *= d.z;
1036
1037        data.s._31 *= d.x;
1038        data.s._32 *= d.y;
1039        data.s._33 *= d.z;
1040        }
1041
1042
1043NX_INLINE void NxMat33::multiplyDiagonalTranspose(const NxVec3 &d)
1044        {
1045        data.s._11 *= d.x;
1046        data.s._12 *= d.x;
1047        data.s._13 *= d.x;
1048
1049        data.s._21 *= d.y;
1050        data.s._22 *= d.y;
1051        data.s._23 *= d.y;
1052
1053        data.s._31 *= d.z;
1054        data.s._32 *= d.z;
1055        data.s._33 *= d.z;
1056        }
1057
1058
1059NX_INLINE void NxMat33::multiplyDiagonal(const NxVec3 &d, NxMat33& dst) const
1060        {
1061        dst.data.s._11 = data.s._11 * d.x;
1062        dst.data.s._12 = data.s._12 * d.y;
1063        dst.data.s._13 = data.s._13 * d.z;
1064
1065        dst.data.s._21 = data.s._21 * d.x;
1066        dst.data.s._22 = data.s._22 * d.y;
1067        dst.data.s._23 = data.s._23 * d.z;
1068
1069        dst.data.s._31 = data.s._31 * d.x;
1070        dst.data.s._32 = data.s._32 * d.y;
1071        dst.data.s._33 = data.s._33 * d.z;
1072        }
1073
1074
1075NX_INLINE void NxMat33::multiplyDiagonalTranspose(const NxVec3 &d, NxMat33& dst) const
1076        {
1077        dst.data.s._11 = data.s._11 * d.x;
1078        dst.data.s._12 = data.s._21 * d.y;
1079        dst.data.s._13 = data.s._31 * d.z;
1080
1081        dst.data.s._21 = data.s._12 * d.x;
1082        dst.data.s._22 = data.s._22 * d.y;
1083        dst.data.s._23 = data.s._32 * d.z;
1084
1085        dst.data.s._31 = data.s._13 * d.x;
1086        dst.data.s._32 = data.s._23 * d.y;
1087        dst.data.s._33 = data.s._33 * d.z;
1088        }
1089
1090
1091NX_INLINE void NxMat33::multiply(const NxVec3 &src, NxVec3 &dst) const
1092        {
1093        NxReal x,y,z;   //so it works if src == dst
1094        x = data.s._11 * src.x + data.s._12 * src.y + data.s._13 * src.z;
1095        y = data.s._21 * src.x + data.s._22 * src.y + data.s._23 * src.z;
1096        z = data.s._31 * src.x + data.s._32 * src.y + data.s._33 * src.z;
1097
1098        dst.x = x;
1099        dst.y = y;
1100        dst.z = z;     
1101        }
1102
1103
1104NX_INLINE void NxMat33::multiplyByTranspose(const NxVec3 &src, NxVec3 &dst) const
1105        {
1106        NxReal x,y,z;   //so it works if src == dst
1107        x = data.s._11 * src.x + data.s._21 * src.y + data.s._31 * src.z;
1108        y = data.s._12 * src.x + data.s._22 * src.y + data.s._32 * src.z;
1109        z = data.s._13 * src.x + data.s._23 * src.y + data.s._33 * src.z;
1110
1111        dst.x = x;
1112        dst.y = y;
1113        dst.z = z;     
1114        }
1115
1116
1117NX_INLINE void NxMat33::add(const NxMat33 & a, const NxMat33 & b)
1118        {
1119        data.s._11 = a.data.s._11 + b.data.s._11;
1120        data.s._12 = a.data.s._12 + b.data.s._12;
1121        data.s._13 = a.data.s._13 + b.data.s._13;
1122
1123        data.s._21 = a.data.s._21 + b.data.s._21;
1124        data.s._22 = a.data.s._22 + b.data.s._22;
1125        data.s._23 = a.data.s._23 + b.data.s._23;
1126
1127        data.s._31 = a.data.s._31 + b.data.s._31;
1128        data.s._32 = a.data.s._32 + b.data.s._32;
1129        data.s._33 = a.data.s._33 + b.data.s._33;
1130        }
1131
1132
1133NX_INLINE void NxMat33::subtract(const NxMat33 &a, const NxMat33 &b)
1134        {
1135        data.s._11 = a.data.s._11 - b.data.s._11;
1136        data.s._12 = a.data.s._12 - b.data.s._12;
1137        data.s._13 = a.data.s._13 - b.data.s._13;
1138
1139        data.s._21 = a.data.s._21 - b.data.s._21;
1140        data.s._22 = a.data.s._22 - b.data.s._22;
1141        data.s._23 = a.data.s._23 - b.data.s._23;
1142
1143        data.s._31 = a.data.s._31 - b.data.s._31;
1144        data.s._32 = a.data.s._32 - b.data.s._32;
1145        data.s._33 = a.data.s._33 - b.data.s._33;
1146        }
1147
1148
1149NX_INLINE void NxMat33::multiply(NxReal d,  const NxMat33 & a)
1150        {
1151        data.s._11 = a.data.s._11 * d;
1152        data.s._12 = a.data.s._12 * d;
1153        data.s._13 = a.data.s._13 * d;
1154
1155        data.s._21 = a.data.s._21 * d;
1156        data.s._22 = a.data.s._22 * d;
1157        data.s._23 = a.data.s._23 * d;
1158
1159        data.s._31 = a.data.s._31 * d;
1160        data.s._32 = a.data.s._32 * d;
1161        data.s._33 = a.data.s._33 * d;
1162        }
1163
1164
1165NX_INLINE void NxMat33::multiply(const NxMat33& left, const NxMat33& right)
1166        {
1167        NxReal a,b,c, d,e,f, g,h,i;
1168        //note: temps needed so that x.multiply(x,y) works OK.
1169        a =left.data.s._11 * right.data.s._11 +left.data.s._12 * right.data.s._21 +left.data.s._13 * right.data.s._31;
1170        b =left.data.s._11 * right.data.s._12 +left.data.s._12 * right.data.s._22 +left.data.s._13 * right.data.s._32;
1171        c =left.data.s._11 * right.data.s._13 +left.data.s._12 * right.data.s._23 +left.data.s._13 * right.data.s._33;
1172
1173        d =left.data.s._21 * right.data.s._11 +left.data.s._22 * right.data.s._21 +left.data.s._23 * right.data.s._31;
1174        e =left.data.s._21 * right.data.s._12 +left.data.s._22 * right.data.s._22 +left.data.s._23 * right.data.s._32;
1175        f =left.data.s._21 * right.data.s._13 +left.data.s._22 * right.data.s._23 +left.data.s._23 * right.data.s._33;
1176
1177        g =left.data.s._31 * right.data.s._11 +left.data.s._32 * right.data.s._21 +left.data.s._33 * right.data.s._31;
1178        h =left.data.s._31 * right.data.s._12 +left.data.s._32 * right.data.s._22 +left.data.s._33 * right.data.s._32;
1179        i =left.data.s._31 * right.data.s._13 +left.data.s._32 * right.data.s._23 +left.data.s._33 * right.data.s._33;
1180
1181
1182        data.s._11 = a;
1183        data.s._12 = b;
1184        data.s._13 = c;
1185
1186        data.s._21 = d;
1187        data.s._22 = e;
1188        data.s._23 = f;
1189
1190        data.s._31 = g;
1191        data.s._32 = h;
1192        data.s._33 = i;
1193        }
1194
1195
1196NX_INLINE void NxMat33::multiplyTransposeLeft(const NxMat33& left, const NxMat33& right)
1197        {
1198        NxReal a,b,c, d,e,f, g,h,i;
1199        //note: temps needed so that x.multiply(x,y) works OK.
1200        a =left.data.s._11 * right.data.s._11 +left.data.s._21 * right.data.s._21 +left.data.s._31 * right.data.s._31;
1201        b =left.data.s._11 * right.data.s._12 +left.data.s._21 * right.data.s._22 +left.data.s._31 * right.data.s._32;
1202        c =left.data.s._11 * right.data.s._13 +left.data.s._21 * right.data.s._23 +left.data.s._31 * right.data.s._33;
1203
1204        d =left.data.s._12 * right.data.s._11 +left.data.s._22 * right.data.s._21 +left.data.s._32 * right.data.s._31;
1205        e =left.data.s._12 * right.data.s._12 +left.data.s._22 * right.data.s._22 +left.data.s._32 * right.data.s._32;
1206        f =left.data.s._12 * right.data.s._13 +left.data.s._22 * right.data.s._23 +left.data.s._32 * right.data.s._33;
1207
1208        g =left.data.s._13 * right.data.s._11 +left.data.s._23 * right.data.s._21 +left.data.s._33 * right.data.s._31;
1209        h =left.data.s._13 * right.data.s._12 +left.data.s._23 * right.data.s._22 +left.data.s._33 * right.data.s._32;
1210        i =left.data.s._13 * right.data.s._13 +left.data.s._23 * right.data.s._23 +left.data.s._33 * right.data.s._33;
1211
1212        data.s._11 = a;
1213        data.s._12 = b;
1214        data.s._13 = c;
1215
1216        data.s._21 = d;
1217        data.s._22 = e;
1218        data.s._23 = f;
1219
1220        data.s._31 = g;
1221        data.s._32 = h;
1222        data.s._33 = i;
1223        }
1224
1225
1226NX_INLINE void NxMat33::multiplyTransposeRight(const NxMat33& left, const NxMat33& right)
1227        {
1228        NxReal a,b,c, d,e,f, g,h,i;
1229        //note: temps needed so that x.multiply(x,y) works OK.
1230        a =left.data.s._11 * right.data.s._11 +left.data.s._12 * right.data.s._12 +left.data.s._13 * right.data.s._13;
1231        b =left.data.s._11 * right.data.s._21 +left.data.s._12 * right.data.s._22 +left.data.s._13 * right.data.s._23;
1232        c =left.data.s._11 * right.data.s._31 +left.data.s._12 * right.data.s._32 +left.data.s._13 * right.data.s._33;
1233
1234        d =left.data.s._21 * right.data.s._11 +left.data.s._22 * right.data.s._12 +left.data.s._23 * right.data.s._13;
1235        e =left.data.s._21 * right.data.s._21 +left.data.s._22 * right.data.s._22 +left.data.s._23 * right.data.s._23;
1236        f =left.data.s._21 * right.data.s._31 +left.data.s._22 * right.data.s._32 +left.data.s._23 * right.data.s._33;
1237
1238        g =left.data.s._31 * right.data.s._11 +left.data.s._32 * right.data.s._12 +left.data.s._33 * right.data.s._13;
1239        h =left.data.s._31 * right.data.s._21 +left.data.s._32 * right.data.s._22 +left.data.s._33 * right.data.s._23;
1240        i =left.data.s._31 * right.data.s._31 +left.data.s._32 * right.data.s._32 +left.data.s._33 * right.data.s._33;
1241
1242        data.s._11 = a;
1243        data.s._12 = b;
1244        data.s._13 = c;
1245
1246        data.s._21 = d;
1247        data.s._22 = e;
1248        data.s._23 = f;
1249
1250        data.s._31 = g;
1251        data.s._32 = h;
1252        data.s._33 = i;
1253        }
1254
1255
1256NX_INLINE void NxMat33::multiplyTransposeRight(const NxVec3 &left, const NxVec3 &right)
1257        {
1258        data.s._11 = left.x * right.x;
1259        data.s._12 = left.x * right.y;
1260        data.s._13 = left.x * right.z;
1261
1262        data.s._21 = left.y * right.x;
1263        data.s._22 = left.y * right.y;
1264        data.s._23 = left.y * right.z;
1265
1266        data.s._31 = left.z * right.x;
1267        data.s._32 = left.z * right.y;
1268        data.s._33 = left.z * right.z;
1269        }
1270
1271NX_INLINE void NxMat33::rotX(NxReal angle)
1272        {
1273        NxReal Cos = cosf(angle);
1274        NxReal Sin = sinf(angle);
1275        id();
1276        data.m[1][1] = data.m[2][2] = Cos;
1277        data.m[1][2] = -Sin;
1278        data.m[2][1] = Sin;
1279        }
1280
1281NX_INLINE void NxMat33::rotY(NxReal angle)
1282        {
1283        NxReal Cos = cosf(angle);
1284        NxReal Sin = sinf(angle);
1285        id();
1286        data.m[0][0] = data.m[2][2] = Cos;
1287        data.m[0][2] = Sin;
1288        data.m[2][0] = -Sin;
1289        }
1290
1291NX_INLINE void NxMat33::rotZ(NxReal angle)
1292        {
1293        NxReal Cos = cosf(angle);
1294        NxReal Sin = sinf(angle);
1295        id();
1296        data.m[0][0] = data.m[1][1] = Cos;
1297        data.m[0][1] = -Sin;
1298        data.m[1][0] = Sin;
1299        }
1300
1301NX_INLINE NxVec3  NxMat33::operator%(const NxVec3 & src) const
1302        {
1303        NxVec3 dest;
1304        this->multiplyByTranspose(src, dest);
1305        return dest;
1306        }
1307
1308
1309NX_INLINE NxVec3  NxMat33::operator*(const NxVec3 & src) const
1310        {
1311        NxVec3 dest;
1312        this->multiply(src, dest);
1313        return dest;
1314        }
1315
1316
1317NX_INLINE const NxMat33 &NxMat33::operator +=(const NxMat33 &d)
1318        {
1319        data.s._11 += d.data.s._11;
1320        data.s._12 += d.data.s._12;
1321        data.s._13 += d.data.s._13;
1322
1323        data.s._21 += d.data.s._21;
1324        data.s._22 += d.data.s._22;
1325        data.s._23 += d.data.s._23;
1326
1327        data.s._31 += d.data.s._31;
1328        data.s._32 += d.data.s._32;
1329        data.s._33 += d.data.s._33;
1330        return *this;
1331        }
1332
1333
1334NX_INLINE const NxMat33 &NxMat33::operator -=(const NxMat33 &d)
1335        {
1336        data.s._11 -= d.data.s._11;
1337        data.s._12 -= d.data.s._12;
1338        data.s._13 -= d.data.s._13;
1339
1340        data.s._21 -= d.data.s._21;
1341        data.s._22 -= d.data.s._22;
1342        data.s._23 -= d.data.s._23;
1343
1344        data.s._31 -= d.data.s._31;
1345        data.s._32 -= d.data.s._32;
1346        data.s._33 -= d.data.s._33;
1347        return *this;
1348        }
1349
1350
1351NX_INLINE const NxMat33 &NxMat33::operator *=(NxReal f)
1352        {
1353        data.s._11 *= f;
1354        data.s._12 *= f;
1355        data.s._13 *= f;
1356
1357        data.s._21 *= f;
1358        data.s._22 *= f;
1359        data.s._23 *= f;
1360
1361        data.s._31 *= f;
1362        data.s._32 *= f;
1363        data.s._33 *= f;
1364        return *this;
1365        }
1366
1367
1368NX_INLINE const NxMat33 &NxMat33::operator /=(NxReal x)
1369        {
1370        NxReal f = NxReal(1.0) / x;
1371        data.s._11 *= f;
1372        data.s._12 *= f;
1373        data.s._13 *= f;
1374
1375        data.s._21 *= f;
1376        data.s._22 *= f;
1377        data.s._23 *= f;
1378
1379        data.s._31 *= f;
1380        data.s._32 *= f;
1381        data.s._33 *= f;
1382        return *this;
1383        }
1384
1385
1386NX_INLINE NxReal NxMat33::determinant() const
1387        {
1388        return  data.s._11*data.s._22*data.s._33 + data.s._12*data.s._23*data.s._31 + data.s._13*data.s._21*data.s._32
1389                  - data.s._13*data.s._22*data.s._31 - data.s._12*data.s._21*data.s._33 - data.s._11*data.s._23*data.s._32;
1390        }
1391
1392
1393bool NxMat33::getInverse(NxMat33& dest) const
1394        {
1395        NxReal b00,b01,b02,b10,b11,b12,b20,b21,b22;
1396
1397        b00 = data.s._22*data.s._33-data.s._23*data.s._32;      b01 = data.s._13*data.s._32-data.s._12*data.s._33;      b02 = data.s._12*data.s._23-data.s._13*data.s._22;
1398        b10 = data.s._23*data.s._31-data.s._21*data.s._33;      b11 = data.s._11*data.s._33-data.s._13*data.s._31;      b12 = data.s._13*data.s._21-data.s._11*data.s._23;
1399        b20 = data.s._21*data.s._32-data.s._22*data.s._31;      b21 = data.s._12*data.s._31-data.s._11*data.s._32;      b22 = data.s._11*data.s._22-data.s._12*data.s._21;
1400       
1401
1402
1403        /*
1404        compute determinant:
1405        NxReal d =   a00*a11*a22 + a01*a12*a20 + a02*a10*a21    - a02*a11*a20 - a01*a10*a22 - a00*a12*a21;
1406                                0                               1                       2                       3                               4                       5
1407
1408        this is a subset of the multiplies done above:
1409
1410        NxReal d = b00*a00                              +               b01*a10                                          + b02 * a20;
1411        NxReal d = (a11*a22-a12*a21)*a00 +              (a02*a21-a01*a22)*a10            + (a01*a12-a02*a11) * a20;
1412
1413        NxReal d = a11*a22*a00-a12*a21*a00 +            a02*a21*a10-a01*a22*a10          + a01*a12*a20-a02*a11*a20;
1414                        0                       5                                       2                       4                                       1                       3
1415        */
1416
1417        NxReal d = b00*data.s._11               +               b01*data.s._21                           + b02 * data.s._31;
1418       
1419        if (d == NxReal(0.0))           //singular?
1420                {
1421                dest.id();
1422                return false;
1423                }
1424       
1425        d = NxReal(1.0)/d;
1426
1427        //only do assignment at the end, in case dest == this:
1428
1429
1430        dest.data.s._11 = b00*d; dest.data.s._12 = b01*d; dest.data.s._13 = b02*d;
1431        dest.data.s._21 = b10*d; dest.data.s._22 = b11*d; dest.data.s._23 = b12*d;
1432        dest.data.s._31 = b20*d; dest.data.s._32 = b21*d; dest.data.s._33 = b22*d;
1433
1434        return true;
1435        }
1436
1437
1438NX_INLINE NxMat33&      NxMat33::operator*= (const NxMat33& mat)
1439        {
1440        this->multiply(*this, mat);
1441        return *this;
1442        }
1443
1444
1445NX_INLINE NxMat33       NxMat33::operator-  (const NxMat33& mat)        const
1446        {
1447        NxMat33 temp;
1448        temp.subtract(*this, mat);
1449        return temp;
1450        }
1451
1452
1453NX_INLINE NxMat33       NxMat33::operator+  (const NxMat33& mat)        const
1454        {
1455        NxMat33 temp;
1456        temp.add(*this, mat);
1457        return temp;
1458        }
1459
1460
1461NX_INLINE NxMat33       NxMat33::operator*  (const NxMat33& mat)        const
1462        {
1463        NxMat33 temp;
1464        temp.multiply(*this, mat);
1465        return temp;
1466        }
1467
1468
1469NX_INLINE NxMat33       NxMat33::operator*  (float s)                   const
1470        {
1471        NxMat33 temp;
1472        temp.multiply(s, *this);
1473        return temp;
1474        }
1475
1476NX_INLINE NxQuat::NxQuat(const class NxMat33 &m)
1477{
1478        m.toQuat(*this);
1479}
1480
1481 /** @} */
1482#endif
1483
1484
1485//AGCOPYRIGHTBEGIN
1486///////////////////////////////////////////////////////////////////////////
1487// Copyright © 2005 AGEIA Technologies.
1488// All rights reserved. www.ageia.com
1489///////////////////////////////////////////////////////////////////////////
1490//AGCOPYRIGHTEND
1491
Note: See TracBrowser for help on using the repository browser.