Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members   File Members  

Matrix.h

Go to the documentation of this file.
00001 
00048 #ifndef _AMIRE_COMMON_MATRIX_H
00049 #define _AMIRE_COMMON_MATRIX_H
00050 
00051 #include <iostream>
00052 
00053 #include <amire/common/Defines.h>
00054 #include <amire/common/Vector.h>
00055 #include <amire/common/Utils.h>
00056 
00057 namespace amire {
00058   namespace common {
00059     #define OPENGL_MATRIX
00060  
00061     #ifdef OPENGL_MATRIX
00062       #define MATRIX_EXTERN_ELEMENT(m, row, col) (m[((col) << 2) + (row)])
00063       #define MATRIX_THIS_ELEMENT(row, col) (elements[((col) << 2) + (row)])
00064       #define MATRIX_ELEMENT(m, row, col) ((m).elements[((col) << 2) + (row)])
00065     #else
00066       #define MATRIX_EXTERN_ELEMENT(m, row, col) (m[(row)>> 2 + (col)])
00067       #define MATRIX_THIS_ELEMENT(row, col) (elements[(row)][(col)])
00068       #define MATRIX_ELEMENT(m, row, col) ((m).elements[(row)][(col)])
00069     #endif
00070 
00074     template <class type> class TMatrix : public virtual osg::Referenced {
00075     public:
00076     #ifdef OPENGL_MATRIX
00077       type elements[4 * 4];
00078     #else
00079       type elements[4][4];
00080     #endif
00081 
00085       inline TMatrix() {
00086         identity();
00087       }
00088 
00092       inline TMatrix(const double* m) {
00093         set(m);
00094       }
00095 
00099       inline TMatrix(const float* m) {
00100         set(m);
00101       }
00102 
00106       inline TMatrix(const TMatrix<double> &m) {
00107         set(m);
00108       }
00109 
00113       inline TMatrix(const TMatrix<float> &m) {
00114         set(m);
00115       }
00116 
00120       inline TMatrix<type> &operator=(const TMatrix<float> &m) {
00121         set(m);
00122         return *this;
00123       }
00124 
00128       inline TMatrix<type> &operator=(const TMatrix<double> &m) {
00129         set(m);
00130         return *this;
00131       }
00132 
00136       inline std::ostream& print(std::ostream& out) const {
00137         out << '[';
00138         for (int i = 0; i < 4; ++i) {
00139           out << '[';
00140           for (int j = 0; j < 4; ++j) {
00141             out << MATRIX_THIS_ELEMENT(i, j);
00142             if (j < 3) {
00143               out << ", ";
00144             }
00145           }
00146           out << ']';
00147           if (i < 3) {
00148             out << ',' << std::endl << ' ';
00149           }
00150         }
00151         out << ']' << std::endl << std::flush;
00152         return out;
00153       }
00154 
00158       friend inline std::ostream &operator<<(std::ostream &out, const TMatrix<type> &m) {
00159         return m.print(out);
00160       }
00161 
00165       inline void set(const double *m) {
00166         MATRIX_THIS_ELEMENT(0, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 0);
00167         MATRIX_THIS_ELEMENT(0, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 1);
00168         MATRIX_THIS_ELEMENT(0, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 2);
00169         MATRIX_THIS_ELEMENT(0, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 3);
00170         MATRIX_THIS_ELEMENT(1, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 0);
00171         MATRIX_THIS_ELEMENT(1, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 1);
00172         MATRIX_THIS_ELEMENT(1, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 2);
00173         MATRIX_THIS_ELEMENT(1, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 3);
00174         MATRIX_THIS_ELEMENT(2, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 0);
00175         MATRIX_THIS_ELEMENT(2, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 1);
00176         MATRIX_THIS_ELEMENT(2, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 2);
00177         MATRIX_THIS_ELEMENT(2, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 3);
00178         MATRIX_THIS_ELEMENT(3, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 0);
00179         MATRIX_THIS_ELEMENT(3, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 1);
00180         MATRIX_THIS_ELEMENT(3, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 2);
00181         MATRIX_THIS_ELEMENT(3, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 3);
00182       }
00183 
00187       inline void set(const float *m) {
00188         MATRIX_THIS_ELEMENT(0, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 0);
00189         MATRIX_THIS_ELEMENT(0, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 1);
00190         MATRIX_THIS_ELEMENT(0, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 2);
00191         MATRIX_THIS_ELEMENT(0, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 0, 3);
00192         MATRIX_THIS_ELEMENT(1, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 0);
00193         MATRIX_THIS_ELEMENT(1, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 1);
00194         MATRIX_THIS_ELEMENT(1, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 2);
00195         MATRIX_THIS_ELEMENT(1, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 1, 3);
00196         MATRIX_THIS_ELEMENT(2, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 0);
00197         MATRIX_THIS_ELEMENT(2, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 1);
00198         MATRIX_THIS_ELEMENT(2, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 2);
00199         MATRIX_THIS_ELEMENT(2, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 2, 3);
00200         MATRIX_THIS_ELEMENT(3, 0) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 0);
00201         MATRIX_THIS_ELEMENT(3, 1) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 1);
00202         MATRIX_THIS_ELEMENT(3, 2) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 2);
00203         MATRIX_THIS_ELEMENT(3, 3) = (type)MATRIX_EXTERN_ELEMENT(m, 3, 3);
00204       }
00205 
00209       inline void set(const TMatrix<double> &m) {
00210         MATRIX_THIS_ELEMENT(0, 0) = (type)MATRIX_ELEMENT(m, 0, 0);
00211         MATRIX_THIS_ELEMENT(0, 1) = (type)MATRIX_ELEMENT(m, 0, 1);
00212         MATRIX_THIS_ELEMENT(0, 2) = (type)MATRIX_ELEMENT(m, 0, 2);
00213         MATRIX_THIS_ELEMENT(0, 3) = (type)MATRIX_ELEMENT(m, 0, 3);
00214         MATRIX_THIS_ELEMENT(1, 0) = (type)MATRIX_ELEMENT(m, 1, 0);
00215         MATRIX_THIS_ELEMENT(1, 1) = (type)MATRIX_ELEMENT(m, 1, 1);
00216         MATRIX_THIS_ELEMENT(1, 2) = (type)MATRIX_ELEMENT(m, 1, 2);
00217         MATRIX_THIS_ELEMENT(1, 3) = (type)MATRIX_ELEMENT(m, 1, 3);
00218         MATRIX_THIS_ELEMENT(2, 0) = (type)MATRIX_ELEMENT(m, 2, 0);
00219         MATRIX_THIS_ELEMENT(2, 1) = (type)MATRIX_ELEMENT(m, 2, 1);
00220         MATRIX_THIS_ELEMENT(2, 2) = (type)MATRIX_ELEMENT(m, 2, 2);
00221         MATRIX_THIS_ELEMENT(2, 3) = (type)MATRIX_ELEMENT(m, 2, 3);
00222         MATRIX_THIS_ELEMENT(3, 0) = (type)MATRIX_ELEMENT(m, 3, 0);
00223         MATRIX_THIS_ELEMENT(3, 1) = (type)MATRIX_ELEMENT(m, 3, 1);
00224         MATRIX_THIS_ELEMENT(3, 2) = (type)MATRIX_ELEMENT(m, 3, 2);
00225         MATRIX_THIS_ELEMENT(3, 3) = (type)MATRIX_ELEMENT(m, 3, 3);
00226       }
00227 
00231       inline void set(const TMatrix<float> &m) {
00232         MATRIX_THIS_ELEMENT(0, 0) = (type)MATRIX_ELEMENT(m, 0, 0);
00233         MATRIX_THIS_ELEMENT(0, 1) = (type)MATRIX_ELEMENT(m, 0, 1);
00234         MATRIX_THIS_ELEMENT(0, 2) = (type)MATRIX_ELEMENT(m, 0, 2);
00235         MATRIX_THIS_ELEMENT(0, 3) = (type)MATRIX_ELEMENT(m, 0, 3);
00236         MATRIX_THIS_ELEMENT(1, 0) = (type)MATRIX_ELEMENT(m, 1, 0);
00237         MATRIX_THIS_ELEMENT(1, 1) = (type)MATRIX_ELEMENT(m, 1, 1);
00238         MATRIX_THIS_ELEMENT(1, 2) = (type)MATRIX_ELEMENT(m, 1, 2);
00239         MATRIX_THIS_ELEMENT(1, 3) = (type)MATRIX_ELEMENT(m, 1, 3);
00240         MATRIX_THIS_ELEMENT(2, 0) = (type)MATRIX_ELEMENT(m, 2, 0);
00241         MATRIX_THIS_ELEMENT(2, 1) = (type)MATRIX_ELEMENT(m, 2, 1);
00242         MATRIX_THIS_ELEMENT(2, 2) = (type)MATRIX_ELEMENT(m, 2, 2);
00243         MATRIX_THIS_ELEMENT(2, 3) = (type)MATRIX_ELEMENT(m, 2, 3);
00244         MATRIX_THIS_ELEMENT(3, 0) = (type)MATRIX_ELEMENT(m, 3, 0);
00245         MATRIX_THIS_ELEMENT(3, 1) = (type)MATRIX_ELEMENT(m, 3, 1);
00246         MATRIX_THIS_ELEMENT(3, 2) = (type)MATRIX_ELEMENT(m, 3, 2);
00247         MATRIX_THIS_ELEMENT(3, 3) = (type)MATRIX_ELEMENT(m, 3, 3);
00248       }
00249 
00250 
00254       inline void setPerspective(type fovy, type aspect, type zNear, type zFar) {
00255 //        ASSERT(fovy != (type)0.0);
00256 //        ASSERT(aspect != (type)0.0);
00257         type divDepth = zFar - zNear;
00258 //        ASSERT(divDepth != (type)0.0);
00259         type f = (type)(1.0 / tan((double)(fovy * (180.0 / M_PI))));
00260         divDepth = ((type)1.0) / divDepth;
00261         type X = f / aspect;
00262         type Y = f;
00263         type Z = -(zFar + zNear) * divDepth;
00264         type D = ((type)(2.0)) * zFar * zNear * divDepth;
00265         MATRIX_THIS_ELEMENT(0, 0) = X;
00266         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00267         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;;
00268         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00269         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00270         MATRIX_THIS_ELEMENT(1, 1) = Y;
00271         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;;
00272         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00273         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00274         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00275         MATRIX_THIS_ELEMENT(2, 2) = Z;
00276         MATRIX_THIS_ELEMENT(2, 3) = D;
00277         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00278         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00279         MATRIX_THIS_ELEMENT(3, 2) = (type)-1.0;
00280         MATRIX_THIS_ELEMENT(3, 3) = (type)0.0;
00281       }
00282 
00286       inline bool getPerspective(type fovy, type aspect, type zNear, type zFar) {
00287         if (!(epsilonEqual(MATRIX_THIS_ELEMENT(0, 1), (type)0.0) &&
00288               epsilonEqual(MATRIX_THIS_ELEMENT(0, 2), (type)0.0) &&
00289               epsilonEqual(MATRIX_THIS_ELEMENT(0, 3), (type)0.0) &&
00290               epsilonEqual(MATRIX_THIS_ELEMENT(1, 0), (type)0.0) &&
00291               epsilonEqual(MATRIX_THIS_ELEMENT(1, 2), (type)0.0) &&
00292               epsilonEqual(MATRIX_THIS_ELEMENT(1, 3), (type)0.0) &&
00293               epsilonEqual(MATRIX_THIS_ELEMENT(2, 0), (type)0.0) &&
00294               epsilonEqual(MATRIX_THIS_ELEMENT(2, 1), (type)0.0) &&
00295               epsilonEqual(MATRIX_THIS_ELEMENT(3, 0), (type)0.0) &&
00296               epsilonEqual(MATRIX_THIS_ELEMENT(3, 1), (type)0.0) &&
00297               epsilonEqual(MATRIX_THIS_ELEMENT(3, 3), (type)0.0) &&
00298               epsilonEqual(MATRIX_THIS_ELEMENT(3, 2), (type)-1.0))) {
00299           return false;
00300         }
00301         type X = MATRIX_THIS_ELEMENT(0, 0);
00302         type Y = MATRIX_THIS_ELEMENT(1, 1);
00303         type Z = MATRIX_THIS_ELEMENT(2, 2);
00304         type D = MATRIX_THIS_ELEMENT(2, 3);
00305         if ((X == (type)0.0) || (Y == (type)0.0)) {
00306           return false;
00307         }
00308         type divFar = (Z + (type)1.0);
00309         type divNear = (Z - (type)1.0);
00310         if ((divFar == (type)0.0) || (divNear == (type)0.0)) {
00311           return false;
00312         }
00313         zNear = D / divNear;
00314         zFar = D / divFar;
00315         fovy = (type)(atan(1.0 / (double)Y) * (M_PI / 180.0));
00316         aspect = fovy / X;
00317         return true;
00318       }
00319 
00323       inline void setFrustum(type left, type right, type bottom, type top, type zNear, type zFar) {
00324         type n = 2 * zNear;
00325         type divWidth = (right - left);
00326         type divHeight = (top - bottom);
00327         type divDepth =(zFar - zNear);
00328         ASSERT(divWidth != (type)0.0);
00329         ASSERT(divHeight != (type)0.0);
00330         ASSERT(divDepth != (type)0.0);
00331         divWidth = ((type)1.0) / divWidth;
00332         divHeight = ((type)1.0) / divHeight;
00333         divDepth = ((type)1.0) / divDepth;
00334         type X = n * divWidth;
00335         type Y = n * divHeight;
00336         type A = (right + left) * divWidth;
00337         type B = (top + bottom) * divHeight;
00338         type C = -(zFar + zNear) * divDepth;
00339         type D = zFar * n * divDepth;
00340         MATRIX_THIS_ELEMENT(0, 0) = X;
00341         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00342         MATRIX_THIS_ELEMENT(0, 2) = A;
00343         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00344         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00345         MATRIX_THIS_ELEMENT(1, 1) = Y;
00346         MATRIX_THIS_ELEMENT(1, 2) = B;
00347         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00348         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00349         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00350         MATRIX_THIS_ELEMENT(2, 2) = C;
00351         MATRIX_THIS_ELEMENT(2, 3) = D;
00352         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00353         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00354         MATRIX_THIS_ELEMENT(3, 2) = (type)-1.0;
00355         MATRIX_THIS_ELEMENT(3, 3) = (type)0.0;
00356       }
00357 
00361       inline bool getFrustum(double &left, double &right, double &bottom, double &top, double &zNear, double &zFar) {
00362         if (!(epsilonEqual(MATRIX_THIS_ELEMENT(1, 0), (type)0.0) &&
00363               epsilonEqual(MATRIX_THIS_ELEMENT(2, 0), (type)0.0) &&
00364               epsilonEqual(MATRIX_THIS_ELEMENT(3, 0), (type)0.0) &&
00365               epsilonEqual(MATRIX_THIS_ELEMENT(0, 1), (type)0.0) &&
00366               epsilonEqual(MATRIX_THIS_ELEMENT(2, 1), (type)0.0) &&
00367               epsilonEqual(MATRIX_THIS_ELEMENT(3, 1), (type)0.0) &&
00368               epsilonEqual(MATRIX_THIS_ELEMENT(0, 3), (type)0.0) &&
00369               epsilonEqual(MATRIX_THIS_ELEMENT(1, 3), (type)0.0) &&
00370               epsilonEqual(MATRIX_THIS_ELEMENT(3, 3), (type)0.0) &&
00371               epsilonEqual(MATRIX_THIS_ELEMENT(3, 2), (type)-1.0))) {
00372           return false;
00373         }
00374         type X = MATRIX_THIS_ELEMENT(0, 0);
00375         type Y = MATRIX_THIS_ELEMENT(1, 1);
00376         if ((X == (type)0.0) || (Y == (type)0.0)) {
00377           return false;
00378         }
00379         type A = MATRIX_THIS_ELEMENT(0, 2);
00380         type B = MATRIX_THIS_ELEMENT(1, 2);
00381         type C = MATRIX_THIS_ELEMENT(2, 2);
00382         type D = MATRIX_THIS_ELEMENT(2, 3);
00383         type divFar = (C + (type)1.0);
00384         type divNear = (C - (type)1.0);
00385         if ((divFar == (type)0.0) || (divNear == (type)0.0)) {
00386           return false;
00387         }
00388         zNear = D / divNear;
00389         zFar = D / divFar;
00390         type divX = ((type)1.0) / X;
00391         type divY = ((type)1.0) / Y;
00392         left = zNear * (A - ((type)1.0)) * divX ;
00393         right = zNear * (A + ((type)1.0)) * divX;
00394         bottom = zNear * (B - ((type)1.0)) * divY;
00395         top = zNear * (B + ((type)1.0)) * divY;
00396         return true;
00397       }
00398 
00402       inline void setOrtho(type left, type right, type bottom, type top, type zNear, type zFar) {
00403         type divWidth = (right - left);
00404         type divHeight = (top - bottom);
00405         type divDepth =(zFar - zNear);
00406         ASSERT(divWidth != (type)0.0);
00407         ASSERT(divHeight != (type)0.0);
00408         ASSERT(divDepth != (type)0.0);
00409         divWidth = ((type)1.0) / divWidth;
00410         divHeight = ((type)1.0) / divHeight;
00411         divDepth = ((type)1.0) / divDepth;
00412         type X = 2.0 * divWidth;
00413         type Y = 2.0 * divHeight;
00414         type Z = -2.0 * divDepth;
00415         type TX = -(right + left) * divWidth;
00416         type TY = -(top + bottom) * divHeight;
00417         type TZ = -(zFar + zNear) * divDepth;
00418         MATRIX_THIS_ELEMENT(0, 0) = X;
00419         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00420         MATRIX_THIS_ELEMENT(0, 2) = TX;
00421         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00422         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00423         MATRIX_THIS_ELEMENT(1, 1) = Y;
00424         MATRIX_THIS_ELEMENT(1, 2) = TY;
00425         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00426         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00427         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00428         MATRIX_THIS_ELEMENT(2, 2) = Z;
00429         MATRIX_THIS_ELEMENT(2, 3) = TZ;
00430         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00431         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00432         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00433         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00434       }
00435 
00436       inline bool getOrtho(type &left, type &right, type &bottom, type &top, type &zNear, type &zFar) {
00437         if (!(epsilonEqual(MATRIX_THIS_ELEMENT(0, 1), (type)0.0) &&
00438               epsilonEqual(MATRIX_THIS_ELEMENT(0, 3), (type)0.0) &&
00439               epsilonEqual(MATRIX_THIS_ELEMENT(1, 0), (type)0.0) &&
00440               epsilonEqual(MATRIX_THIS_ELEMENT(1, 3), (type)0.0) &&
00441               epsilonEqual(MATRIX_THIS_ELEMENT(2, 0), (type)0.0) &&
00442               epsilonEqual(MATRIX_THIS_ELEMENT(2, 1), (type)0.0) &&
00443               epsilonEqual(MATRIX_THIS_ELEMENT(3, 0), (type)0.0) &&
00444               epsilonEqual(MATRIX_THIS_ELEMENT(3, 1), (type)0.0) &&
00445               epsilonEqual(MATRIX_THIS_ELEMENT(3, 2), (type)0.0) &&
00446               epsilonEqual(MATRIX_THIS_ELEMENT(3, 3), (type)1.0))) {
00447           return false;
00448         }
00449         type X = MATRIX_THIS_ELEMENT(0, 0);
00450         type TX = MATRIX_THIS_ELEMENT(0, 2);
00451         type Y = MATRIX_THIS_ELEMENT(1, 1);
00452         type TY = MATRIX_THIS_ELEMENT(1, 2);
00453         type Z = MATRIX_THIS_ELEMENT(2, 2);
00454         type TZ = MATRIX_THIS_ELEMENT(2, 3);
00455         if ((X == (type)0.0) || (Y == (type)0.0) || (Z == (type)0.0)) {
00456           return false;
00457         }
00458         type divX = ((type)1.0) / X;
00459         type divY = ((type)1.0) / Y;
00460         type divZ = ((type)1.0) / Z;
00461         left = (TX - ((type)1.0)) * divX;
00462         right = (TX + ((type)1.0)) * divX;
00463         bottom = (TY - ((type)1.0)) * divY;
00464         top = (TY + ((type)1.0)) * divY;
00465         zNear = (TZ - ((type)1.0)) * divZ;
00466         zFar = (TZ + ((type)1.0)) * divZ;
00467         return true;
00468       }
00469 
00473       inline void setRotateXAxis(type angle) {
00474         type c = (type)cos((double)angle);
00475         type s = (type)sin((double)angle);
00476         MATRIX_THIS_ELEMENT(0, 0) = (type)1.0;
00477         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00478         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;
00479         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00480         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00481         MATRIX_THIS_ELEMENT(1, 1) = c;
00482         MATRIX_THIS_ELEMENT(1, 2) = -s;
00483         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00484         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00485         MATRIX_THIS_ELEMENT(2, 1) = s;
00486         MATRIX_THIS_ELEMENT(2, 2) = c;
00487         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
00488         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00489         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00490         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00491         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00492       }
00493 
00497       inline void setRotateYAxis(type angle) {
00498         type c = (type)cos((double)angle);
00499         type s = (type)sin((double)angle);
00500         MATRIX_THIS_ELEMENT(0, 0) = c;
00501         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00502         MATRIX_THIS_ELEMENT(0, 2) = s;
00503         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00504         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00505         MATRIX_THIS_ELEMENT(1, 1) = (type)1.0;
00506         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;
00507         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00508         MATRIX_THIS_ELEMENT(2, 0) = -s;
00509         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00510         MATRIX_THIS_ELEMENT(2, 2) = c;
00511         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
00512         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00513         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00514         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00515         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00516       }
00517 
00521       inline void setRotateZAxis(type angle) {
00522         type c = (type)cos((double)angle);
00523         type s = (type)sin((double)angle);
00524         MATRIX_THIS_ELEMENT(0, 0) = c;
00525         MATRIX_THIS_ELEMENT(0, 1) = -s;
00526         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;
00527         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00528         MATRIX_THIS_ELEMENT(1, 0) = s;
00529         MATRIX_THIS_ELEMENT(1, 1) = c;
00530         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;
00531         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00532         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00533         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00534         MATRIX_THIS_ELEMENT(2, 2) = (type)1.0;
00535         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
00536         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00537         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00538         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00539         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00540       }
00541 
00545       inline void setRotation(type angle, const TVector3D<type> &rotationAxis) {
00546         setRotation(angle, rotationAxis.x, rotationAxis.y, rotationAxis.z);
00547       }
00548 
00552       inline void setRotation(type angle, type x, type y, type z) {
00553         type c = (type)cos((double)angle);
00554         type s = (type)sin((double)angle);
00555         type c1 = ((type)1.0) - c;
00556         type f1 = x * c1;
00557         type f2 = y * c1;
00558         type f3 = y * f1;
00559         type f4 = z * f1;
00560         type f5 = z * f2;
00561         type f6 = z * s;
00562         type f7 = x * s;
00563         type f8 = y * s;
00564         MATRIX_THIS_ELEMENT(0, 0) = x * f1 + c;
00565         MATRIX_THIS_ELEMENT(0, 1) = f3 - f6;
00566         MATRIX_THIS_ELEMENT(0, 2) = f4 + f8;
00567         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00568         MATRIX_THIS_ELEMENT(1, 0) = f3 + f6;
00569         MATRIX_THIS_ELEMENT(1, 1) = y * f2 + c;
00570         MATRIX_THIS_ELEMENT(1, 2) = f5 - f7;
00571         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00572         MATRIX_THIS_ELEMENT(2, 0) = f4 - f8;
00573         MATRIX_THIS_ELEMENT(2, 1) = f5 + f7;
00574         MATRIX_THIS_ELEMENT(2, 2) = z * z * c1 + c;
00575         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
00576         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00577         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00578         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00579         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00580       }
00581 
00585       inline void rotateXAxisDotLeft(type angle) {
00586         // TODO optimize it
00587         TMatrix<type> r;
00588         r.setRotateXAxis(angle);
00589         dotLeft(r);
00590       }
00591 
00595       inline void rotateXAxisDotLeft(const TMatrix<type> &m, type angle) {
00596         // TODO optimize it
00597         TMatrix<type> r;
00598         r.setRotateXAxis(angle);
00599         dotLeft(m, r);
00600       }
00601 
00605       inline void rotateYAxisDotLeft(type angle) {
00606         // TODO optimize it
00607         TMatrix<type> r;
00608         r.setRotateYAxis(angle);
00609         dotLeft(r);
00610       }
00611 
00615       inline void rotateYAxisDotLeft(const TMatrix<type> &m, type angle) {
00616         // TODO optimize it
00617         TMatrix<type> r;
00618         r.setRotateYAxis(angle);
00619         dotLeft(m, r);
00620       }
00621 
00625       inline void rotateZAxisDotLeft(type angle) {
00626         type c = (type)cos((double)angle);
00627         type s = (type)sin((double)angle);
00628         type f0 = MATRIX_THIS_ELEMENT(0, 0);
00629         MATRIX_THIS_ELEMENT(0, 0) = c * f0 - s * MATRIX_THIS_ELEMENT(1, 0);
00630         MATRIX_THIS_ELEMENT(1, 0) = s * f0 + c * MATRIX_THIS_ELEMENT(1, 0);
00631         type f1 = MATRIX_THIS_ELEMENT(0, 1);
00632         MATRIX_THIS_ELEMENT(0, 1) = c * f1 - s * MATRIX_THIS_ELEMENT(1, 1);
00633         MATRIX_THIS_ELEMENT(1, 1) = s * f1 + c * MATRIX_THIS_ELEMENT(1, 1);
00634         type f2 = MATRIX_THIS_ELEMENT(0, 2);
00635         MATRIX_THIS_ELEMENT(0, 2) = c * f2 - s * MATRIX_THIS_ELEMENT(1, 2);
00636         MATRIX_THIS_ELEMENT(1, 2) = s * f2 + c * MATRIX_THIS_ELEMENT(1, 2);
00637         type f3 = MATRIX_THIS_ELEMENT(0, 3);
00638         MATRIX_THIS_ELEMENT(0, 3) = c * f3 - s * MATRIX_THIS_ELEMENT(1, 3);
00639         MATRIX_THIS_ELEMENT(1, 3) = s * f3 + c * MATRIX_THIS_ELEMENT(1, 3);
00640       }
00641 
00645       inline void rotateZAxisDotLeft(const TMatrix<type> &m, type angle) {
00646         type c = (type)cos((double)angle);
00647         type s = (type)sin((double)angle);
00648         type f0 = MATRIX_ELEMENT(m, 0, 0);
00649         MATRIX_THIS_ELEMENT(0, 0) = c * f0 - s * MATRIX_THIS_ELEMENT(1, 0);
00650         MATRIX_THIS_ELEMENT(1, 0) = s * f0 + c * MATRIX_THIS_ELEMENT(1, 0);
00651         type f1 = MATRIX_ELEMENT(m, 0, 1);
00652         MATRIX_THIS_ELEMENT(0, 1) = c * f1 - s * MATRIX_THIS_ELEMENT(1, 1);
00653         MATRIX_THIS_ELEMENT(1, 1) = s * f1 + c * MATRIX_THIS_ELEMENT(1, 1);
00654         type f2 = MATRIX_ELEMENT(m, 0, 2);
00655         MATRIX_THIS_ELEMENT(0, 2) = c * f2 - s * MATRIX_THIS_ELEMENT(1, 2);
00656         MATRIX_THIS_ELEMENT(1, 2) = s * f2 + c * MATRIX_THIS_ELEMENT(1, 2);
00657         type f3 = MATRIX_ELEMENT(m, 0, 3);
00658         MATRIX_THIS_ELEMENT(0, 3) = c * f3 - s * MATRIX_THIS_ELEMENT(1, 3);
00659         MATRIX_THIS_ELEMENT(1, 3) = s * f3 + c * MATRIX_THIS_ELEMENT(1, 3);
00660       }
00661 
00665       inline void rotateZAxisDotRight(type angle) {
00666         TMatrix<type> m(*this);
00667         rotateZAxisDotRight(m, angle);
00668       }
00669 
00673       inline void rotateZAxisDotRight(const TMatrix<type> &m, type angle) {
00674         type c = (type)cos((double)angle);
00675         type s = (type)sin((double)angle);
00676         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(m, 0, 0) * c +
00677                                     MATRIX_ELEMENT(m, 0, 1) * s;
00678         MATRIX_THIS_ELEMENT(0, 1) = -MATRIX_ELEMENT(m, 0, 0) * s +
00679                                     MATRIX_ELEMENT(m, 0, 1) * c;
00680         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(m, 0, 2);
00681         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(m, 0, 3);
00682         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(m, 1, 0) * c +
00683                                     MATRIX_ELEMENT(m, 1, 1) * s;
00684         MATRIX_THIS_ELEMENT(1, 1) = -MATRIX_ELEMENT(m, 1, 0) * s +
00685                                     MATRIX_ELEMENT(m, 1, 1) * c;
00686         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(m, 1, 2);
00687         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(m, 1, 3);
00688         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(m, 2, 0) * c +
00689                                     MATRIX_ELEMENT(m, 2, 1) * s;
00690         MATRIX_THIS_ELEMENT(2, 1) = -MATRIX_ELEMENT(m, 2, 0) * s +
00691                                     MATRIX_ELEMENT(m, 2, 1) * c;
00692         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(m, 2, 2);
00693         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(m, 2, 3);
00694         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0) * c +
00695                                     MATRIX_ELEMENT(m, 3, 1) * s;
00696         MATRIX_THIS_ELEMENT(3, 1) = -MATRIX_ELEMENT(m, 3, 0) * s +
00697                                     MATRIX_ELEMENT(m, 3, 1) * c;
00698         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 2);
00699         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
00700       }
00701 
00705       inline void rotateDotLeft(type angle, const TVector3D<type> &v) {
00706         rotateDotLeft(angle, v.x, v.y, v.z);
00707       }
00708 
00712       inline void rotateDotLeft(type angle, type x, type y, type z) {
00713         TMatrix<type> m(*this);
00714         rotateDotLeft(m, angle, x, y, z);
00715       }
00716 
00720       inline void rotateDotLeft(const TMatrix<type> &m, type angle, type x, type y, type z) {
00721         type c = (type)cos((double)angle);
00722         type s = (type)sin((double)angle);
00723         type c1 = ((type)1.0) - c;
00724         type f1 = x * c1;
00725         type f2 = y * c1;
00726         type f3 = y * f1;
00727         type f4 = z * f1;
00728         type f5 = z * f2;
00729         type f6 = z * s;
00730         type f7 = x * s;
00731         type f8 = y * s;
00732         type f9 = x * f1 + c;
00733         type f10 = f3 - f6;
00734         type f11 = f4 + f8;
00735         type f13 = f3 + f6;
00736         type f14 = y * f2 + c;
00737         type f15 = f5 - f7;
00738         type f17 = f4 - f8;
00739         type f18 = f5 + f7;
00740         type f19 = z * z * c1 + c;
00741         MATRIX_THIS_ELEMENT(0, 0) = f9 * MATRIX_ELEMENT(m, 0, 0) +
00742                                     f10 * MATRIX_ELEMENT(m, 1, 0) +
00743                                     f11 * MATRIX_ELEMENT(m, 2, 0);
00744         MATRIX_THIS_ELEMENT(0, 1) = f9 * MATRIX_ELEMENT(m, 0, 1) +
00745                                     f10 * MATRIX_ELEMENT(m, 1, 1) +
00746                                     f11 * MATRIX_ELEMENT(m, 2, 1);
00747         MATRIX_THIS_ELEMENT(0, 2) = f9 * MATRIX_ELEMENT(m, 0, 2) +
00748                                     f10 * MATRIX_ELEMENT(m, 1, 2) +
00749                                     f11 * MATRIX_ELEMENT(m, 2, 2);
00750         MATRIX_THIS_ELEMENT(0, 3) = f9 * MATRIX_ELEMENT(m, 0, 3) +
00751                                     f10 * MATRIX_ELEMENT(m, 1, 3) +
00752                                     f11 * MATRIX_ELEMENT(m, 2, 3);
00753         MATRIX_THIS_ELEMENT(1, 0) = f13 * MATRIX_ELEMENT(m, 0, 0) +
00754                                     f14 * MATRIX_ELEMENT(m, 1, 0) +
00755                                     f15 * MATRIX_ELEMENT(m, 2, 0);
00756         MATRIX_THIS_ELEMENT(1, 1) = f13 * MATRIX_ELEMENT(m, 0, 1) +
00757                                     f14 * MATRIX_ELEMENT(m, 1, 1) +
00758                                     f15 * MATRIX_ELEMENT(m, 2, 1);
00759         MATRIX_THIS_ELEMENT(1, 2) = f13 * MATRIX_ELEMENT(m, 0, 2) +
00760                                     f14 * MATRIX_ELEMENT(m, 1, 2) +
00761                                     f15 * MATRIX_ELEMENT(m, 2, 2);
00762         MATRIX_THIS_ELEMENT(1, 3) = f13 * MATRIX_ELEMENT(m, 0, 3) +
00763                                     f14 * MATRIX_ELEMENT(m, 1, 3) +
00764                                     f15 * MATRIX_ELEMENT(m, 2, 3);
00765         MATRIX_THIS_ELEMENT(2, 0) = f17 * MATRIX_ELEMENT(m, 0, 0) +
00766                                     f18 * MATRIX_ELEMENT(m, 1, 0) +
00767                                     f19 * MATRIX_ELEMENT(m, 2, 0);
00768         MATRIX_THIS_ELEMENT(2, 1) = f17 * MATRIX_ELEMENT(m, 0, 1) +
00769                                     f18 * MATRIX_ELEMENT(m, 1, 1) +
00770                                     f19 * MATRIX_ELEMENT(m, 2, 1);
00771         MATRIX_THIS_ELEMENT(2, 2) = f17 * MATRIX_ELEMENT(m, 0, 2) +
00772                                     f18 * MATRIX_ELEMENT(m, 1, 2) +
00773                                     f19 * MATRIX_ELEMENT(m, 2, 2);
00774         MATRIX_THIS_ELEMENT(2, 3) = f17 * MATRIX_ELEMENT(m, 0, 3) +
00775                                     f18 * MATRIX_ELEMENT(m, 1, 3) +
00776                                     f19 * MATRIX_ELEMENT(m, 2, 3);
00777         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0);
00778         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(m, 3, 1);
00779         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 2);
00780         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
00781       }
00782 
00786       inline void rotateDotRight(type angle, const TVector3D<type> &v) {
00787         rotateDotRight(angle, v.x, v.y, v.z);
00788       }
00789 
00793       inline void rotateDotRight(type angle, type x, type y, type z) {
00794         TMatrix<type> m(*this);
00795         rotateDotRight(m, angle, x, y, z);
00796       }
00797 
00801       inline void rotateDotRight(const TMatrix<type> &m, type angle, type x, type y, type z) {
00802         type c = (type)cos((double)angle);
00803         type s = (type)sin((double)angle);
00804         type c1 = ((type)1.0) - c;
00805         type f1 = x * c1;
00806         type f2 = y * c1;
00807         type f3 = y * f1;
00808         type f4 = z * f1;
00809         type f5 = z * f2;
00810         type f6 = z * s;
00811         type f7 = x * s;
00812         type f8 = y * s;
00813         type f9 = x * f1 + c;
00814         type f10 = f3 - f6;
00815         type f11 = f4 + f8;
00816         type f13 = f3 + f6;
00817         type f14 = y * f2 + c;
00818         type f15 = f5 - f7;
00819         type f17 = f4 - f8;
00820         type f18 = f5 + f7;
00821         type f19 = z * z * c1 + c;
00822         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(m, 0, 0) * f9 +
00823                                     MATRIX_ELEMENT(m, 0, 1) * f13 +
00824                                     MATRIX_ELEMENT(m, 0, 2) * f17;
00825         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(m, 0, 0) * f10 +
00826                                     MATRIX_ELEMENT(m, 0, 1) * f14 +
00827                                     MATRIX_ELEMENT(m, 0, 2) * f18;
00828         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(m, 0, 0) * f11 +
00829                                     MATRIX_ELEMENT(m, 0, 1) * f15 +
00830                                     MATRIX_ELEMENT(m, 0, 2) * f19;
00831         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(m, 0, 3);
00832         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(m, 1, 0) * f9 +
00833                                     MATRIX_ELEMENT(m, 1, 1) * f13 +
00834                                     MATRIX_ELEMENT(m, 1, 2) * f17;
00835         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(m, 1, 0) * f10 +
00836                                     MATRIX_ELEMENT(m, 1, 1) * f14 +
00837                                     MATRIX_ELEMENT(m, 1, 2) * f18;
00838         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(m, 1, 0) * f11 +
00839                                     MATRIX_ELEMENT(m, 1, 1) * f15 +
00840                                     MATRIX_ELEMENT(m, 1, 2) * f19;
00841         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(m, 1, 3);
00842         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(m, 2, 0) * f9 +
00843                                     MATRIX_ELEMENT(m, 2, 1) * f13 +
00844                                     MATRIX_ELEMENT(m, 2, 2) * f17;
00845         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(m, 2, 0) * f10 +
00846                                     MATRIX_ELEMENT(m, 2, 1) * f14 +
00847                                     MATRIX_ELEMENT(m, 2, 2) * f18;
00848         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(m, 2, 0) * f11 +
00849                                     MATRIX_ELEMENT(m, 2, 1) * f15 +
00850                                     MATRIX_ELEMENT(m, 2, 2) * f19;
00851         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(m, 2, 3);
00852         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0) * f9 +
00853                                     MATRIX_ELEMENT(m, 3, 1) * f13 +
00854                                     MATRIX_ELEMENT(m, 3, 2) * f17;
00855         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(m, 3, 0) * f10 +
00856                                     MATRIX_ELEMENT(m, 3, 1) * f14 +
00857                                     MATRIX_ELEMENT(m, 3, 2) * f18;
00858         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 0) * f11 +
00859                                     MATRIX_ELEMENT(m, 3, 1) * f15 +
00860                                     MATRIX_ELEMENT(m, 3, 2) * f19;
00861         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
00862       }
00863 
00867       inline void setScale(float *scale) {
00868         setScale((type)scale[0], (type)scale[1], (type)scale[2]);
00869       }
00870 
00874       inline void setScale(double *scale) {
00875         setScale((type)scale[0], (type)scale[1], (type)scale[2]);
00876       }
00877 
00881       inline void setScale(const TVector3D<type> &scale) {
00882         setScale(scale.x, scale.y, scale.z);
00883       }
00884 
00888       inline void setScale(type scaleX, type scaleY, type scaleZ) {
00889         MATRIX_THIS_ELEMENT(0, 0) = scaleX;
00890         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00891         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;
00892         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
00893         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00894         MATRIX_THIS_ELEMENT(1, 1) = scaleY;
00895         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;
00896         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
00897         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00898         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00899         MATRIX_THIS_ELEMENT(2, 2) = scaleZ;
00900         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
00901         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00902         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00903         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00904         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00905       }
00906 
00910       inline void setTranslate(double *translate) {
00911         setTranslate((type)translate[0], (type)translate[1], (type)translate[2]);
00912       }
00913 
00917       inline void setTranslate(float *translate) {
00918         setTranslate((type)translate[0], (type)translate[1], (type)translate[2]);
00919       }
00920 
00924       inline void setTranslate(const TVector3D<type> &translate) {
00925         setTranslate(translate.x, translate.y, translate.z);
00926       }
00927 
00931       inline void setTranslate(type translateX, type translateY, type translateZ) {
00932         MATRIX_THIS_ELEMENT(0, 0) = (type)1.0;
00933         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
00934         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;
00935         MATRIX_THIS_ELEMENT(0, 3) = translateX;
00936         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
00937         MATRIX_THIS_ELEMENT(1, 1) = (type)1.0;
00938         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;
00939         MATRIX_THIS_ELEMENT(1, 3) = translateY;
00940         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00941         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
00942         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
00943         MATRIX_THIS_ELEMENT(2, 2) = (type)1.0;
00944         MATRIX_THIS_ELEMENT(2, 3) = translateZ;
00945         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
00946         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
00947         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
00948         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
00949       }
00950 
00954       inline void setOrientation(double *forward, double *up) {
00955         setOrientation((type)forward[0], (type)forward[1], (type)forward[2], (type)up[0], (type)up[1], (type)up[2]);
00956       }
00957 
00961       inline void setOrientation(float *forward, float *up) {
00962         setOrientation((type)forward[0], (type)forward[1], (type)forward[2], (type)up[0], (type)up[1], (type)up[2]);
00963       }
00964 
00968       inline void setOrientation(const TVector3D<type> &forward, const TVector3D<type> &up) {
00969         setOrientation(forward.x, forward.y, forward.z, up.x, up.y, up.z);
00970       }
00971 
00975       inline void setOrientation(type forwardX, type forwardY, type forwardZ, type upX, type upY, type upZ) {
00976         TVector3D<type> f(forwardX, forwardY, forwardZ);
00977         TVector3D<type> u(upX, upY, upZ);
00978         f.normalize();
00979         u.normalize();
00980         TVector3D<type> s(f);
00981         s.cross(u);
00982         s.normalize();
00983         u.cross(s, f);
00984         setOrientation(f.x, f.y, f.z, u.x, u.y, u.z, s.x, s.y, s.z);
00985       }
00986 
00990       inline void setOrientation(double *forward, double *up, double *side) {
00991         setOrientation((type)forward[0], (type)forward[1], (type)forward[2], (type)up[0], (type)up[1], (type)up[2], side[0], side[1], side[2]);
00992       }
00993 
00997       inline void setOrientation(float *forward, float *up, float *side) {
00998         setOrientation((type)forward[0], (type)forward[1], (type)forward[2], (type)up[0], (type)up[1], (type)up[2], side[0], side[1], side[2]);
00999       }
01000 
01004       inline void setOrientation(const TVector3D<type> &forward, const TVector3D<type> &up, const TVector3D<type> &side) {
01005         setOrientation(forward.x, forward.y, forward.z, up.x, up.y, up.z, side.x, side.y, side.z);
01006       }
01007 
01011       inline void setOrientation(type forwardX, type forwardY, type forwardZ, type upX, type upY, type upZ, type sideX, type sideY, type sideZ) {
01012         MATRIX_THIS_ELEMENT(0, 0) = sideX;
01013         MATRIX_THIS_ELEMENT(1, 0) = sideY;
01014         MATRIX_THIS_ELEMENT(2, 0) = sideZ;
01015         MATRIX_THIS_ELEMENT(3, 0) = 0.0;
01016         MATRIX_THIS_ELEMENT(0, 1) = upX;
01017         MATRIX_THIS_ELEMENT(1, 1) = upY;
01018         MATRIX_THIS_ELEMENT(2, 1) = upZ;
01019         MATRIX_THIS_ELEMENT(3, 1) = 0.0;
01020         MATRIX_THIS_ELEMENT(0, 2) = forwardX;
01021         MATRIX_THIS_ELEMENT(1, 2) = forwardY;
01022         MATRIX_THIS_ELEMENT(2, 2) = forwardZ;
01023         MATRIX_THIS_ELEMENT(3, 2) = 0.0;
01024         MATRIX_THIS_ELEMENT(0, 3) = 0.0;
01025         MATRIX_THIS_ELEMENT(1, 3) = 0.0;
01026         MATRIX_THIS_ELEMENT(2, 3) = 0.0;
01027         MATRIX_THIS_ELEMENT(3, 3) = 1.0;
01028       }
01029 
01033       inline void transpose() {
01034         type swap01 = MATRIX_THIS_ELEMENT(0, 1);
01035         type swap02 = MATRIX_THIS_ELEMENT(0, 2);
01036         type swap03 = MATRIX_THIS_ELEMENT(0, 3);
01037         type swap12 = MATRIX_THIS_ELEMENT(1, 2);
01038         type swap13 = MATRIX_THIS_ELEMENT(1, 3);
01039         type swap23 = MATRIX_THIS_ELEMENT(2, 3);
01040         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_THIS_ELEMENT(1, 0);
01041         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_THIS_ELEMENT(2, 0);
01042         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_THIS_ELEMENT(3, 0);
01043         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_THIS_ELEMENT(2, 1);
01044         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_THIS_ELEMENT(3, 1);
01045         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_THIS_ELEMENT(3, 2);
01046         MATRIX_THIS_ELEMENT(1, 0) = swap01;
01047         MATRIX_THIS_ELEMENT(2, 0) = swap02;
01048         MATRIX_THIS_ELEMENT(3, 0) = swap03;
01049         MATRIX_THIS_ELEMENT(2, 1) = swap12;
01050         MATRIX_THIS_ELEMENT(3, 1) = swap13;
01051         MATRIX_THIS_ELEMENT(3, 2) = swap23;
01052       }
01053 
01057       inline void identity() {
01058         MATRIX_THIS_ELEMENT(0, 0) = (type)1.0;
01059         MATRIX_THIS_ELEMENT(0, 1) = (type)0.0;
01060         MATRIX_THIS_ELEMENT(0, 2) = (type)0.0;
01061         MATRIX_THIS_ELEMENT(0, 3) = (type)0.0;
01062         MATRIX_THIS_ELEMENT(1, 0) = (type)0.0;
01063         MATRIX_THIS_ELEMENT(1, 1) = (type)1.0;
01064         MATRIX_THIS_ELEMENT(1, 2) = (type)0.0;
01065         MATRIX_THIS_ELEMENT(1, 3) = (type)0.0;
01066         MATRIX_THIS_ELEMENT(2, 0) = (type)0.0;
01067         MATRIX_THIS_ELEMENT(2, 1) = (type)0.0;
01068         MATRIX_THIS_ELEMENT(2, 2) = (type)1.0;
01069         MATRIX_THIS_ELEMENT(2, 3) = (type)0.0;
01070         MATRIX_THIS_ELEMENT(3, 0) = (type)0.0;
01071         MATRIX_THIS_ELEMENT(3, 1) = (type)0.0;
01072         MATRIX_THIS_ELEMENT(3, 2) = (type)0.0;
01073         MATRIX_THIS_ELEMENT(3, 3) = (type)1.0;
01074       }
01075 
01079       inline type det() {
01080         type m00 = MATRIX_THIS_ELEMENT(0, 1) * MATRIX_THIS_ELEMENT(1, 0);
01081         type m01 = MATRIX_THIS_ELEMENT(0, 2) * MATRIX_THIS_ELEMENT(1, 0);
01082         type m02 = MATRIX_THIS_ELEMENT(0, 3) * MATRIX_THIS_ELEMENT(1, 0);
01083         type m03 = MATRIX_THIS_ELEMENT(0, 0) * MATRIX_THIS_ELEMENT(1, 1);
01084         type m04 = MATRIX_THIS_ELEMENT(0, 2) * MATRIX_THIS_ELEMENT(1, 1);
01085         type m05 = MATRIX_THIS_ELEMENT(0, 3) * MATRIX_THIS_ELEMENT(1, 1);
01086         type m06 = MATRIX_THIS_ELEMENT(0, 0) * MATRIX_THIS_ELEMENT(1, 2);
01087         type m07 = MATRIX_THIS_ELEMENT(0, 1) * MATRIX_THIS_ELEMENT(1, 2);
01088         type m08 = MATRIX_THIS_ELEMENT(0, 3) * MATRIX_THIS_ELEMENT(1, 2);
01089         type m09 = MATRIX_THIS_ELEMENT(0, 0) * MATRIX_THIS_ELEMENT(1, 3);
01090         type m10 = MATRIX_THIS_ELEMENT(0, 1) * MATRIX_THIS_ELEMENT(1, 3);
01091         type m11 = MATRIX_THIS_ELEMENT(0, 2) * MATRIX_THIS_ELEMENT(1, 3);
01092         type m12 = MATRIX_THIS_ELEMENT(2, 1) * MATRIX_THIS_ELEMENT(3, 0);
01093         type m13 = MATRIX_THIS_ELEMENT(2, 2) * MATRIX_THIS_ELEMENT(3, 0);
01094         type m14 = MATRIX_THIS_ELEMENT(2, 3) * MATRIX_THIS_ELEMENT(3, 0);
01095         type m15 = MATRIX_THIS_ELEMENT(2, 0) * MATRIX_THIS_ELEMENT(3, 1);
01096         type m16 = MATRIX_THIS_ELEMENT(2, 2) * MATRIX_THIS_ELEMENT(3, 1);
01097         type m17 = MATRIX_THIS_ELEMENT(2, 3) * MATRIX_THIS_ELEMENT(3, 1);
01098         type m18 = MATRIX_THIS_ELEMENT(2, 0) * MATRIX_THIS_ELEMENT(3, 2);
01099         type m19 = MATRIX_THIS_ELEMENT(2, 1) * MATRIX_THIS_ELEMENT(3, 2);
01100         type m20 = MATRIX_THIS_ELEMENT(2, 3) * MATRIX_THIS_ELEMENT(3, 2);
01101         type m21 = MATRIX_THIS_ELEMENT(2, 0) * MATRIX_THIS_ELEMENT(3, 3);
01102         type m22 = MATRIX_THIS_ELEMENT(2, 1) * MATRIX_THIS_ELEMENT(3, 3);
01103         type m23 = MATRIX_THIS_ELEMENT(2, 2) * MATRIX_THIS_ELEMENT(3, 3);
01104         return m08 * m12 - m11 * m12 - m05 * m13 + m10 * m13 + 
01105                m04 * m14 - m07 * m14 - m08 * m15 + m11 * m15 + 
01106                m02 * m16 - m09 * m16 - m01 * m17 + m06 * m17 + 
01107                m05 * m18 - m10 * m18 - m02 * m19 + m09 * m19 + 
01108                m00 * m20 - m03 * m20 - m04 * m21 + m07 * m21 + 
01109                m01 * m22 - m06 * m22 - m00 * m23 + m03 * m23;
01110       }
01111 
01115       inline TVector3D<type> dotLeft(type x, type y, type z) const {
01116         dotLeft(x, y, z, x, y, z);
01117         return TVector3D<type>(x, y, z);
01118       }
01119 
01123       inline TVector3D<type> dotRight(type x, type y, type z) const {
01124         dotRight(x, y, z, x, y, z);
01125         return TVector3D<type>(x, y, z);
01126       }
01127 
01131       inline TVector3D<type> dotLeft(const TVector3D<type> &v) const {
01132         type x;
01133         type y;
01134         type z;
01135         dotLeft(v.x, v.y, v.z, x, y, z);
01136         return TVector3D<type>(x, y, z);
01137       }
01138 
01142       inline TVector3D<type> dotRight(const TVector3D<type> &v) const {
01143         type x;
01144         type y;
01145         type z;
01146         dotRight(v.x, v.y, v.z, x, y, z);
01147         return TVector3D<type>(x, y, z);
01148       }
01149 
01153       inline void dotLeft(const TVector3D<type> &v1, TVector3D<type> &v2) const {
01154         dotLeft(v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);
01155       }
01156 
01160       inline void dotRight(const TVector3D<type> &v1, TVector3D<type> &v2) const {
01161         dotRight(v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);
01162       }
01163 
01167       inline void dotLeft(type &x, type &y, type &z, TVector3D<type> &v) const {
01168         dotLeft(x, y, z, v.x, v.y, v.z);
01169       }
01170 
01174       inline void dotRight(type x, type y, type z, TVector3D<type> &v) const {
01175         dotRight(x, y, z, v.x, v.y, v.z);
01176       }
01177 
01181       inline void dotLeft(const TVector3D<type> &v, type &x, type &y, type &z) const {
01182         dotLeft(v.x, v.y, v.z, x, y, z);
01183       }
01184 
01188       inline void dotRight(const TVector3D<type> &v, type &x, type &y, type &z) const {
01189         dotRight(v.x, v.y, v.z, x, y, z);
01190       }
01191 
01195       inline void dotLeft(type x1, type y1, type z1, type &x2, type &y2, type &z2) const {
01196         type w;
01197         dotLeft(x1, y1, z1, x2, y2, z2, w);
01198         type divW = ((type)1.0) / w;
01199         x2 *= divW;
01200         y2 *= divW;
01201         z2 *= divW;
01202       }
01203 
01207       inline void dotRight(type x1, type y1, type z1, type &x2, type &y2, type &z2) const {
01208         type w;
01209         dotRight(x1, y1, z1, x2, y2, z2, w);
01210         type divW = ((type)1.0) / w;
01211         x2 *= divW;
01212         y2 *= divW;
01213         z2 *= divW;
01214       }
01215 
01219       inline void dotLeft(const TVector3D<type> &v, type &x, type &y, type &z, type &w) const {
01220         dotLeft(v.x, v.y, v.z, x, y, z, w);
01221       }
01222 
01226       inline void dotRight(const TVector3D<type> &v, type &x, type &y, type &z, type &w) const {
01227         dotRight(v.x, v.y, v.z, x, y, z, w);
01228       }
01229 
01233       inline void dotLeft(type x1, type y1, type z1, type &x2, type &y2, type &z2, type &w2) const {
01234         x2 = x1 * MATRIX_THIS_ELEMENT(0, 0) + y1 * MATRIX_THIS_ELEMENT(1, 0) + z1 * MATRIX_THIS_ELEMENT(2, 0) + MATRIX_THIS_ELEMENT(3, 0);
01235         y2 = x1 * MATRIX_THIS_ELEMENT(0, 1) + y1 * MATRIX_THIS_ELEMENT(1, 1) + z1 * MATRIX_THIS_ELEMENT(2, 1) + MATRIX_THIS_ELEMENT(3, 1);
01236         z2 = x1 * MATRIX_THIS_ELEMENT(0, 2) + y1 * MATRIX_THIS_ELEMENT(1, 2) + z1 * MATRIX_THIS_ELEMENT(2, 2) + MATRIX_THIS_ELEMENT(3, 2);
01237         w2 = x1 * MATRIX_THIS_ELEMENT(0, 3) + y1 * MATRIX_THIS_ELEMENT(1, 3) + z1 * MATRIX_THIS_ELEMENT(2, 3) + MATRIX_THIS_ELEMENT(3, 3);
01238       }
01239 
01243       inline void dotRight(type x1, type y1, type z1, type &x2, type &y2, type &z2, type &w2) const {
01244         x2 = x1 * MATRIX_THIS_ELEMENT(0, 0) + y1 * MATRIX_THIS_ELEMENT(0, 1) + z1 * MATRIX_THIS_ELEMENT(0, 2) + MATRIX_THIS_ELEMENT(0, 3);
01245         y2 = x1 * MATRIX_THIS_ELEMENT(1, 0) + y1 * MATRIX_THIS_ELEMENT(1, 1) + z1 * MATRIX_THIS_ELEMENT(1, 2) + MATRIX_THIS_ELEMENT(1, 3);
01246         z2 = x1 * MATRIX_THIS_ELEMENT(2, 0) + y1 * MATRIX_THIS_ELEMENT(2, 1) + z1 * MATRIX_THIS_ELEMENT(2, 2) + MATRIX_THIS_ELEMENT(2, 3);
01247         w2 = x1 * MATRIX_THIS_ELEMENT(3, 0) + y1 * MATRIX_THIS_ELEMENT(3, 1) + z1 * MATRIX_THIS_ELEMENT(3, 2) + MATRIX_THIS_ELEMENT(3, 3);
01248       }
01249 
01253       inline void dotLeft(type x1, type y1, type z1, type w1, type &x2, type &y2, type &z2, type &w2) const {
01254         x2 = x1 * MATRIX_THIS_ELEMENT(0, 0) + y1 * MATRIX_THIS_ELEMENT(1, 0) + z1 * MATRIX_THIS_ELEMENT(2, 0) + w1 * MATRIX_THIS_ELEMENT(3, 0);
01255         y2 = x1 * MATRIX_THIS_ELEMENT(0, 1) + y1 * MATRIX_THIS_ELEMENT(1, 1) + z1 * MATRIX_THIS_ELEMENT(2, 1) + w1 * MATRIX_THIS_ELEMENT(3, 1);
01256         z2 = x1 * MATRIX_THIS_ELEMENT(0, 2) + y1 * MATRIX_THIS_ELEMENT(1, 2) + z1 * MATRIX_THIS_ELEMENT(2, 2) + w1 * MATRIX_THIS_ELEMENT(3, 2);
01257         w2 = x1 * MATRIX_THIS_ELEMENT(0, 3) + y1 * MATRIX_THIS_ELEMENT(1, 3) + z1 * MATRIX_THIS_ELEMENT(2, 3) + w1 * MATRIX_THIS_ELEMENT(3, 3);
01258       }
01259 
01263       inline void dotRight(type x1, type y1, type z1, type w1, type &x2, type &y2, type &z2, type &w2) const {
01264         x2 = x1 * MATRIX_THIS_ELEMENT(0, 0) + y1 * MATRIX_THIS_ELEMENT(0, 1) + z1 * MATRIX_THIS_ELEMENT(0, 2) + w1 * MATRIX_THIS_ELEMENT(0, 3);
01265         y2 = x1 * MATRIX_THIS_ELEMENT(1, 0) + y1 * MATRIX_THIS_ELEMENT(1, 1) + z1 * MATRIX_THIS_ELEMENT(1, 2) + w1 * MATRIX_THIS_ELEMENT(1, 3);
01266         z2 = x1 * MATRIX_THIS_ELEMENT(2, 0) + y1 * MATRIX_THIS_ELEMENT(2, 1) + z1 * MATRIX_THIS_ELEMENT(2, 2) + w1 * MATRIX_THIS_ELEMENT(2, 3);
01267         w2 = x1 * MATRIX_THIS_ELEMENT(3, 0) + y1 * MATRIX_THIS_ELEMENT(3, 1) + z1 * MATRIX_THIS_ELEMENT(3, 2) + w1 * MATRIX_THIS_ELEMENT(3, 3);
01268       }
01269 
01273       inline void dotLeft(const TMatrix<type> &m) {
01274         TMatrix<type> t;
01275         t.dotRight(m, *this);
01276         set(t);
01277       }
01278 
01282       inline void dotRight(const TMatrix<type> &m) {
01283         TMatrix<type> t;
01284         t.dotRight(*this, m);
01285         set(t);
01286       }
01287 
01291       inline void dotLeft(const TMatrix<type> &first, const TMatrix<type> &second) {
01292         dotRight(second, first);
01293       }
01294 
01298       inline void dotRight(const TMatrix<type> &first, const TMatrix<type> &second) {
01299         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(first, 0, 0) * MATRIX_ELEMENT(second, 0, 0) +
01300                                     MATRIX_ELEMENT(first, 0, 1) * MATRIX_ELEMENT(second, 1, 0) +
01301                                     MATRIX_ELEMENT(first, 0, 2) * MATRIX_ELEMENT(second, 2, 0) +
01302                                     MATRIX_ELEMENT(first, 0, 3) * MATRIX_ELEMENT(second, 3, 0);
01303         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(first, 0, 0) * MATRIX_ELEMENT(second, 0, 1) +
01304                                     MATRIX_ELEMENT(first, 0, 1) * MATRIX_ELEMENT(second, 1, 1) +
01305                                     MATRIX_ELEMENT(first, 0, 2) * MATRIX_ELEMENT(second, 2, 1) +
01306                                     MATRIX_ELEMENT(first, 0, 3) * MATRIX_ELEMENT(second, 3, 1);
01307         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(first, 0, 0) * MATRIX_ELEMENT(second, 0, 2) +
01308                                     MATRIX_ELEMENT(first, 0, 1) * MATRIX_ELEMENT(second, 1, 2) +
01309                                     MATRIX_ELEMENT(first, 0, 2) * MATRIX_ELEMENT(second, 2, 2) +
01310                                     MATRIX_ELEMENT(first, 0, 3) * MATRIX_ELEMENT(second, 3, 2);
01311         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(first, 0, 0) * MATRIX_ELEMENT(second, 0, 3) +
01312                                     MATRIX_ELEMENT(first, 0, 1) * MATRIX_ELEMENT(second, 1, 3) +
01313                                     MATRIX_ELEMENT(first, 0, 2) * MATRIX_ELEMENT(second, 2, 3) +
01314                                     MATRIX_ELEMENT(first, 0, 3) * MATRIX_ELEMENT(second, 3, 3);
01315         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(first, 1, 0) * MATRIX_ELEMENT(second, 0, 0) +
01316                                     MATRIX_ELEMENT(first, 1, 1) * MATRIX_ELEMENT(second, 1, 0) +
01317                                     MATRIX_ELEMENT(first, 1, 2) * MATRIX_ELEMENT(second, 2, 0) +
01318                                     MATRIX_ELEMENT(first, 1, 3) * MATRIX_ELEMENT(second, 3, 0);
01319         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(first, 1, 0) * MATRIX_ELEMENT(second, 0, 1) +
01320                                     MATRIX_ELEMENT(first, 1, 1) * MATRIX_ELEMENT(second, 1, 1) +
01321                                     MATRIX_ELEMENT(first, 1, 2) * MATRIX_ELEMENT(second, 2, 1) +
01322                                     MATRIX_ELEMENT(first, 1, 3) * MATRIX_ELEMENT(second, 3, 1);
01323         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(first, 1, 0) * MATRIX_ELEMENT(second, 0, 2) +
01324                                     MATRIX_ELEMENT(first, 1, 1) * MATRIX_ELEMENT(second, 1, 2) +
01325                                     MATRIX_ELEMENT(first, 1, 2) * MATRIX_ELEMENT(second, 2, 2) +
01326                                     MATRIX_ELEMENT(first, 1, 3) * MATRIX_ELEMENT(second, 3, 2);
01327         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(first, 1, 0) * MATRIX_ELEMENT(second, 0, 3) +
01328                                     MATRIX_ELEMENT(first, 1, 1) * MATRIX_ELEMENT(second, 1, 3) +
01329                                     MATRIX_ELEMENT(first, 1, 2) * MATRIX_ELEMENT(second, 2, 3) +
01330                                     MATRIX_ELEMENT(first, 1, 3) * MATRIX_ELEMENT(second, 3, 3);
01331         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(first, 2, 0) * MATRIX_ELEMENT(second, 0, 0) +
01332                                     MATRIX_ELEMENT(first, 2, 1) * MATRIX_ELEMENT(second, 1, 0) +
01333                                     MATRIX_ELEMENT(first, 2, 2) * MATRIX_ELEMENT(second, 2, 0) +
01334                                     MATRIX_ELEMENT(first, 2, 3) * MATRIX_ELEMENT(second, 3, 0);
01335         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(first, 2, 0) * MATRIX_ELEMENT(second, 0, 1) +
01336                                     MATRIX_ELEMENT(first, 2, 1) * MATRIX_ELEMENT(second, 1, 1) +
01337                                     MATRIX_ELEMENT(first, 2, 2) * MATRIX_ELEMENT(second, 2, 1) +
01338                                     MATRIX_ELEMENT(first, 2, 3) * MATRIX_ELEMENT(second, 3, 1);
01339         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(first, 2, 0) * MATRIX_ELEMENT(second, 0, 2) +
01340                                     MATRIX_ELEMENT(first, 2, 1) * MATRIX_ELEMENT(second, 1, 2) +
01341                                     MATRIX_ELEMENT(first, 2, 2) * MATRIX_ELEMENT(second, 2, 2) +
01342                                     MATRIX_ELEMENT(first, 2, 3) * MATRIX_ELEMENT(second, 3, 2);
01343         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(first, 2, 0) * MATRIX_ELEMENT(second, 0, 3) +
01344                                     MATRIX_ELEMENT(first, 2, 1) * MATRIX_ELEMENT(second, 1, 3) +
01345                                     MATRIX_ELEMENT(first, 2, 2) * MATRIX_ELEMENT(second, 2, 3) +
01346                                     MATRIX_ELEMENT(first, 2, 3) * MATRIX_ELEMENT(second, 3, 3);
01347         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(first, 3, 0) * MATRIX_ELEMENT(second, 0, 0) +
01348                                     MATRIX_ELEMENT(first, 3, 1) * MATRIX_ELEMENT(second, 1, 0) +
01349                                     MATRIX_ELEMENT(first, 3, 2) * MATRIX_ELEMENT(second, 2, 0) +
01350                                     MATRIX_ELEMENT(first, 3, 3) * MATRIX_ELEMENT(second, 3, 0);
01351         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(first, 3, 0) * MATRIX_ELEMENT(second, 0, 1) +
01352                                     MATRIX_ELEMENT(first, 3, 1) * MATRIX_ELEMENT(second, 1, 1) +
01353                                     MATRIX_ELEMENT(first, 3, 2) * MATRIX_ELEMENT(second, 2, 1) +
01354                                     MATRIX_ELEMENT(first, 3, 3) * MATRIX_ELEMENT(second, 3, 1);
01355         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(first, 3, 0) * MATRIX_ELEMENT(second, 0, 2) +
01356                                     MATRIX_ELEMENT(first, 3, 1) * MATRIX_ELEMENT(second, 1, 2) +
01357                                     MATRIX_ELEMENT(first, 3, 2) * MATRIX_ELEMENT(second, 2, 2) +
01358                                     MATRIX_ELEMENT(first, 3, 3) * MATRIX_ELEMENT(second, 3, 2);
01359         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(first, 3, 0) * MATRIX_ELEMENT(second, 0, 3) +
01360                                     MATRIX_ELEMENT(first, 3, 1) * MATRIX_ELEMENT(second, 1, 3) +
01361                                     MATRIX_ELEMENT(first, 3, 2) * MATRIX_ELEMENT(second, 2, 3) +
01362                                     MATRIX_ELEMENT(first, 3, 3) * MATRIX_ELEMENT(second, 3, 3);
01363       }
01364 
01368       inline void translateDotLeft(const TVector3D<type> &translate) {
01369         translateDotLeft(translate.x, translate.y, translate.z);
01370       }
01371 
01375       inline void translateDotLeft(type translateX, type translateY, type translateZ) {
01376         MATRIX_THIS_ELEMENT(3, 0) += translateX * MATRIX_THIS_ELEMENT(0, 0) +
01377                                      translateY * MATRIX_THIS_ELEMENT(1, 0) +
01378                                      translateZ * MATRIX_THIS_ELEMENT(2, 0);
01379         MATRIX_THIS_ELEMENT(3, 1) += translateX * MATRIX_THIS_ELEMENT(0, 1) +
01380                                      translateY * MATRIX_THIS_ELEMENT(1, 1) +
01381                                      translateZ * MATRIX_THIS_ELEMENT(2, 1);
01382         MATRIX_THIS_ELEMENT(3, 2) += translateX * MATRIX_THIS_ELEMENT(0, 2) +
01383                                      translateY * MATRIX_THIS_ELEMENT(1, 2) +
01384                                      translateZ * MATRIX_THIS_ELEMENT(2, 2);
01385         MATRIX_THIS_ELEMENT(3, 3) += translateX * MATRIX_THIS_ELEMENT(0, 3) +
01386                                      translateY * MATRIX_THIS_ELEMENT(1, 3) +
01387                                      translateZ * MATRIX_THIS_ELEMENT(2, 3);
01388       }
01389 
01393       inline void translateDotLeft(const TMatrix<type> &m, const TVector3D<type> &translate) {
01394         translateDotLeft(m, translate.x, translate.y, translate.z);
01395       }
01396 
01400       inline void translateDotLeft(const TMatrix<type> &m, type translateX, type translateY, type translateZ) {
01401         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(m, 0, 0);
01402         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(m, 0, 1);
01403         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(m, 0, 2);
01404         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(m, 0, 3);
01405         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(m, 1, 0);
01406         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(m, 1, 1);
01407         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(m, 1, 2);
01408         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(m, 1, 3);
01409         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(m, 2, 0);
01410         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(m, 2, 1);
01411         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(m, 2, 2);
01412         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(m, 2, 3);
01413         MATRIX_THIS_ELEMENT(3, 0) = translateX * MATRIX_ELEMENT(m, 0, 0) +
01414                                     translateY * MATRIX_ELEMENT(m, 1, 0) +
01415                                     translateZ * MATRIX_ELEMENT(m, 2, 0) +
01416                                     MATRIX_ELEMENT(m, 3, 0);
01417         MATRIX_THIS_ELEMENT(3, 1) = translateX * MATRIX_ELEMENT(m, 0, 1) +
01418                                     translateY * MATRIX_ELEMENT(m, 1, 1) +
01419                                     translateZ * MATRIX_ELEMENT(m, 2, 1) +
01420                                     MATRIX_ELEMENT(m, 3, 1);
01421         MATRIX_THIS_ELEMENT(3, 2) = translateX * MATRIX_ELEMENT(m, 0, 2) +
01422                                     translateY * MATRIX_ELEMENT(m, 1, 2) +
01423                                     translateZ * MATRIX_ELEMENT(m, 2, 2) +
01424                                     MATRIX_ELEMENT(m, 3, 2);
01425         MATRIX_THIS_ELEMENT(3, 3) = translateX * MATRIX_ELEMENT(m, 0, 3) +
01426                                     translateY * MATRIX_ELEMENT(m, 1, 3) +
01427                                     translateZ * MATRIX_ELEMENT(m, 2, 3) +
01428                                     MATRIX_ELEMENT(m, 3, 3);
01429       }
01430 
01434       inline void translateDotRight(const TVector3D<type> &translate) {
01435         translateDotRight(translate.x, translate.y, translate.z);
01436       }
01437 
01441       inline void translateDotRight(type translateX, type translateY, type translateZ) {
01442         MATRIX_THIS_ELEMENT(0, 0) += MATRIX_THIS_ELEMENT(0, 3) * translateX;
01443         MATRIX_THIS_ELEMENT(1, 0) += MATRIX_THIS_ELEMENT(1, 3) * translateX;
01444         MATRIX_THIS_ELEMENT(2, 0) += MATRIX_THIS_ELEMENT(2, 3) * translateX;
01445         MATRIX_THIS_ELEMENT(3, 0) += MATRIX_THIS_ELEMENT(3, 3) * translateX;
01446         MATRIX_THIS_ELEMENT(0, 1) += MATRIX_THIS_ELEMENT(0, 3) * translateY;
01447         MATRIX_THIS_ELEMENT(1, 1) += MATRIX_THIS_ELEMENT(1, 3) * translateY;
01448         MATRIX_THIS_ELEMENT(2, 1) += MATRIX_THIS_ELEMENT(2, 3) * translateY;
01449         MATRIX_THIS_ELEMENT(3, 1) += MATRIX_THIS_ELEMENT(3, 3) * translateY;
01450         MATRIX_THIS_ELEMENT(0, 2) += MATRIX_THIS_ELEMENT(0, 3) * translateZ;
01451         MATRIX_THIS_ELEMENT(1, 2) += MATRIX_THIS_ELEMENT(1, 3) * translateZ;
01452         MATRIX_THIS_ELEMENT(2, 2) += MATRIX_THIS_ELEMENT(2, 3) * translateZ;
01453         MATRIX_THIS_ELEMENT(3, 2) += MATRIX_THIS_ELEMENT(3, 3) * translateZ;
01454       }
01455 
01459       inline void translateDotRight(const TMatrix<type> &m, const TVector3D<type> &translate) {
01460         translateDotRight(m, translate.x, translate.y, translate.z);
01461       }
01462 
01466       inline void translateDotRight(const TMatrix<type> &m, type translateX, type translateY, type translateZ) {
01467         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(m, 0, 0) + MATRIX_ELEMENT(m, 0, 3) * translateX;
01468         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(m, 2, 0) + MATRIX_ELEMENT(m, 2, 3) * translateX;
01469         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(m, 1, 0) + MATRIX_ELEMENT(m, 1, 3) * translateX;
01470         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0) + MATRIX_ELEMENT(m, 3, 3) * translateX;
01471         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(m, 0, 1) + MATRIX_ELEMENT(m, 0, 3) * translateY;
01472         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(m, 1, 1) + MATRIX_ELEMENT(m, 1, 3) * translateY;
01473         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(m, 2, 1) + MATRIX_ELEMENT(m, 2, 3) * translateY;
01474         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(m, 3, 1) + MATRIX_ELEMENT(m, 3, 3) * translateY;
01475         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(m, 0, 2) + MATRIX_ELEMENT(m, 0, 3) * translateZ;
01476         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(m, 1, 2) + MATRIX_ELEMENT(m, 1, 3) * translateZ;
01477         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(m, 2, 2) + MATRIX_ELEMENT(m, 2, 3) * translateZ;
01478         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 2) + MATRIX_ELEMENT(m, 3, 3) * translateZ;
01479         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(m, 0, 3);
01480         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(m, 1, 3);
01481         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(m, 2, 3);
01482         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
01483       }
01484 
01488       inline void scaleDotRight(const TVector3D<type> &scale) {
01489         scaleDotRight(scale.x, scale.y, scale.z);
01490       }
01491 
01495       inline void scaleDotRight(type scaleX, type scaleY, type scaleZ) {
01496         MATRIX_THIS_ELEMENT(0, 0) *= scaleX;
01497         MATRIX_THIS_ELEMENT(1, 0) *= scaleX;
01498         MATRIX_THIS_ELEMENT(2, 0) *= scaleX;
01499         MATRIX_THIS_ELEMENT(3, 0) *= scaleX;
01500         MATRIX_THIS_ELEMENT(0, 1) *= scaleY;
01501         MATRIX_THIS_ELEMENT(1, 1) *= scaleY;
01502         MATRIX_THIS_ELEMENT(2, 1) *= scaleY;
01503         MATRIX_THIS_ELEMENT(3, 1) *= scaleY;
01504         MATRIX_THIS_ELEMENT(0, 2) *= scaleZ;
01505         MATRIX_THIS_ELEMENT(1, 2) *= scaleZ;
01506         MATRIX_THIS_ELEMENT(2, 2) *= scaleZ;
01507         MATRIX_THIS_ELEMENT(3, 2) *= scaleZ;
01508       }
01509 
01513       inline void scaleDotRight(const TMatrix<type> &m, const TVector3D<type> &scale) {
01514         scaleDotRight(m, scale.x, scale.y, scale.z);
01515       }
01516 
01520       inline void scaleDotRight(const TMatrix<type> &m, type scaleX, type scaleY, type scaleZ) {
01521         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(m, 0, 0) * scaleX;
01522         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(m, 1, 0) * scaleX;
01523         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(m, 2, 0) * scaleX;
01524         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0) * scaleX;
01525         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(m, 0, 1) * scaleY;
01526         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(m, 1, 1) * scaleY;
01527         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(m, 2, 1) * scaleY;
01528         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(m, 3, 1) * scaleY;
01529         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(m, 0, 2) * scaleZ;
01530         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(m, 1, 2) * scaleZ;
01531         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(m, 2, 2) * scaleZ;
01532         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 2) * scaleZ;
01533         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(m, 0, 3);
01534         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(m, 1, 3);
01535         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(m, 2, 3);
01536         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
01537       }
01538 
01542       inline void scaleDotLeft(const TVector3D<type> &scale) {
01543         scaleDotLeft(scale.x, scale.y, scale.z);
01544       }
01545 
01549       inline void scaleDotLeft(type scaleX, type scaleY, type scaleZ) {
01550         MATRIX_THIS_ELEMENT(0, 0) *= scaleX;
01551         MATRIX_THIS_ELEMENT(0, 1) *= scaleX;
01552         MATRIX_THIS_ELEMENT(0, 2) *= scaleX;
01553         MATRIX_THIS_ELEMENT(0, 3) *= scaleX;
01554         MATRIX_THIS_ELEMENT(1, 0) *= scaleY;
01555         MATRIX_THIS_ELEMENT(1, 1) *= scaleY;
01556         MATRIX_THIS_ELEMENT(1, 2) *= scaleY;
01557         MATRIX_THIS_ELEMENT(1, 3) *= scaleY;
01558         MATRIX_THIS_ELEMENT(2, 0) *= scaleZ;
01559         MATRIX_THIS_ELEMENT(2, 1) *= scaleZ;
01560         MATRIX_THIS_ELEMENT(2, 2) *= scaleZ;
01561         MATRIX_THIS_ELEMENT(2, 3) *= scaleZ;
01562       }
01563 
01567       inline void scaleDotLeft(const TMatrix<type> &m, const TVector3D<type> &scale) {
01568         scaleDotLeft(m, scale.x, scale.y, scale.z);
01569       }
01570 
01574       inline void scaleDotLeft(const TMatrix<type> &m, type scaleX, type scaleY, type scaleZ) {
01575         MATRIX_THIS_ELEMENT(0, 0) = scaleX * MATRIX_ELEMENT(m, 0, 0);
01576         MATRIX_THIS_ELEMENT(0, 1) = scaleX * MATRIX_ELEMENT(m, 0, 1);
01577         MATRIX_THIS_ELEMENT(0, 2) = scaleX * MATRIX_ELEMENT(m, 0, 2);
01578         MATRIX_THIS_ELEMENT(0, 3) = scaleX * MATRIX_ELEMENT(m, 0, 3);
01579         MATRIX_THIS_ELEMENT(1, 0) = scaleY * MATRIX_ELEMENT(m, 1, 0);
01580         MATRIX_THIS_ELEMENT(1, 1) = scaleY * MATRIX_ELEMENT(m, 1, 1);
01581         MATRIX_THIS_ELEMENT(1, 2) = scaleY * MATRIX_ELEMENT(m, 1, 2);
01582         MATRIX_THIS_ELEMENT(1, 3) = scaleY * MATRIX_ELEMENT(m, 1, 3);
01583         MATRIX_THIS_ELEMENT(2, 0) = scaleZ * MATRIX_ELEMENT(m, 2, 0);
01584         MATRIX_THIS_ELEMENT(2, 1) = scaleZ * MATRIX_ELEMENT(m, 2, 1);
01585         MATRIX_THIS_ELEMENT(2, 2) = scaleZ * MATRIX_ELEMENT(m, 2, 2);
01586         MATRIX_THIS_ELEMENT(2, 3) = scaleZ * MATRIX_ELEMENT(m, 2, 3);
01587         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(m, 3, 0);
01588         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(m, 3, 1);
01589         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(m, 3, 2);
01590         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(m, 3, 3);
01591       }
01592 
01596       inline void add(const TMatrix<type> &m) {
01597         MATRIX_THIS_ELEMENT(0, 0) += MATRIX_ELEMENT(m, 0, 0);
01598         MATRIX_THIS_ELEMENT(0, 1) += MATRIX_ELEMENT(m, 0, 1);
01599         MATRIX_THIS_ELEMENT(0, 2) += MATRIX_ELEMENT(m, 0, 2);
01600         MATRIX_THIS_ELEMENT(0, 3) += MATRIX_ELEMENT(m, 0, 3);
01601         MATRIX_THIS_ELEMENT(1, 0) += MATRIX_ELEMENT(m, 1, 0);
01602         MATRIX_THIS_ELEMENT(1, 1) += MATRIX_ELEMENT(m, 1, 1);
01603         MATRIX_THIS_ELEMENT(1, 2) += MATRIX_ELEMENT(m, 1, 2);
01604         MATRIX_THIS_ELEMENT(1, 3) += MATRIX_ELEMENT(m, 1, 3);
01605         MATRIX_THIS_ELEMENT(2, 0) += MATRIX_ELEMENT(m, 2, 0);
01606         MATRIX_THIS_ELEMENT(2, 1) += MATRIX_ELEMENT(m, 2, 1);
01607         MATRIX_THIS_ELEMENT(2, 2) += MATRIX_ELEMENT(m, 2, 2);
01608         MATRIX_THIS_ELEMENT(2, 3) += MATRIX_ELEMENT(m, 2, 3);
01609         MATRIX_THIS_ELEMENT(3, 0) += MATRIX_ELEMENT(m, 3, 0);
01610         MATRIX_THIS_ELEMENT(3, 1) += MATRIX_ELEMENT(m, 3, 1);
01611         MATRIX_THIS_ELEMENT(3, 2) += MATRIX_ELEMENT(m, 3, 2);
01612         MATRIX_THIS_ELEMENT(3, 3) += MATRIX_ELEMENT(m, 3, 3);
01613       }
01614 
01618       inline void add(const TMatrix<type> &first, const TMatrix<type> &second) {
01619         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(first, 0, 0) + MATRIX_ELEMENT(second, 0, 0);
01620         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(first, 0, 1) + MATRIX_ELEMENT(second, 0, 1);
01621         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(first, 0, 2) + MATRIX_ELEMENT(second, 0, 2);
01622         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(first, 0, 3) + MATRIX_ELEMENT(second, 0, 3);
01623         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(first, 1, 0) + MATRIX_ELEMENT(second, 1, 0);
01624         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(first, 1, 1) + MATRIX_ELEMENT(second, 1, 1);
01625         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(first, 1, 2) + MATRIX_ELEMENT(second, 1, 2);
01626         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(first, 1, 3) + MATRIX_ELEMENT(second, 1, 3);
01627         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(first, 2, 0) + MATRIX_ELEMENT(second, 2, 0);
01628         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(first, 2, 1) + MATRIX_ELEMENT(second, 2, 1);
01629         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(first, 2, 2) + MATRIX_ELEMENT(second, 2, 2);
01630         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(first, 2, 3) + MATRIX_ELEMENT(second, 2, 3);
01631         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(first, 3, 0) + MATRIX_ELEMENT(second, 3, 0);
01632         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(first, 3, 1) + MATRIX_ELEMENT(second, 3, 1);
01633         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(first, 3, 2) + MATRIX_ELEMENT(second, 3, 2);
01634         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(first, 3, 3) + MATRIX_ELEMENT(second, 3, 2);
01635       }
01636 
01640       inline void sub(const TMatrix<type> &m) {
01641         MATRIX_THIS_ELEMENT(0, 0) -= MATRIX_ELEMENT(m, 0, 0);
01642         MATRIX_THIS_ELEMENT(0, 1) -= MATRIX_ELEMENT(m, 0, 1);
01643         MATRIX_THIS_ELEMENT(0, 2) -= MATRIX_ELEMENT(m, 0, 2);
01644         MATRIX_THIS_ELEMENT(0, 3) -= MATRIX_ELEMENT(m, 0, 3);
01645         MATRIX_THIS_ELEMENT(1, 0) -= MATRIX_ELEMENT(m, 1, 0);
01646         MATRIX_THIS_ELEMENT(1, 1) -= MATRIX_ELEMENT(m, 1, 1);
01647         MATRIX_THIS_ELEMENT(1, 2) -= MATRIX_ELEMENT(m, 1, 2);
01648         MATRIX_THIS_ELEMENT(1, 3) -= MATRIX_ELEMENT(m, 1, 3);
01649         MATRIX_THIS_ELEMENT(2, 0) -= MATRIX_ELEMENT(m, 2, 0);
01650         MATRIX_THIS_ELEMENT(2, 1) -= MATRIX_ELEMENT(m, 2, 1);
01651         MATRIX_THIS_ELEMENT(2, 2) -= MATRIX_ELEMENT(m, 2, 2);
01652         MATRIX_THIS_ELEMENT(2, 3) -= MATRIX_ELEMENT(m, 2, 3);
01653         MATRIX_THIS_ELEMENT(3, 0) -= MATRIX_ELEMENT(m, 3, 0);
01654         MATRIX_THIS_ELEMENT(3, 1) -= MATRIX_ELEMENT(m, 3, 1);
01655         MATRIX_THIS_ELEMENT(3, 2) -= MATRIX_ELEMENT(m, 3, 2);
01656         MATRIX_THIS_ELEMENT(3, 3) -= MATRIX_ELEMENT(m, 3, 3);
01657       }
01658 
01662       inline void sub(const TMatrix<type> &first, const TMatrix<type> &second) {
01663         MATRIX_THIS_ELEMENT(0, 0) = MATRIX_ELEMENT(first, 0, 0) - MATRIX_ELEMENT(second, 0, 0);
01664         MATRIX_THIS_ELEMENT(0, 1) = MATRIX_ELEMENT(first, 0, 1) - MATRIX_ELEMENT(second, 0, 1);
01665         MATRIX_THIS_ELEMENT(0, 2) = MATRIX_ELEMENT(first, 0, 2) - MATRIX_ELEMENT(second, 0, 2);
01666         MATRIX_THIS_ELEMENT(0, 3) = MATRIX_ELEMENT(first, 0, 3) - MATRIX_ELEMENT(second, 0, 3);
01667         MATRIX_THIS_ELEMENT(1, 0) = MATRIX_ELEMENT(first, 1, 0) - MATRIX_ELEMENT(second, 1, 0);
01668         MATRIX_THIS_ELEMENT(1, 1) = MATRIX_ELEMENT(first, 1, 1) - MATRIX_ELEMENT(second, 1, 1);
01669         MATRIX_THIS_ELEMENT(1, 2) = MATRIX_ELEMENT(first, 1, 2) - MATRIX_ELEMENT(second, 1, 2);
01670         MATRIX_THIS_ELEMENT(1, 3) = MATRIX_ELEMENT(first, 1, 3) - MATRIX_ELEMENT(second, 1, 3);
01671         MATRIX_THIS_ELEMENT(2, 0) = MATRIX_ELEMENT(first, 2, 0) - MATRIX_ELEMENT(second, 2, 0);
01672         MATRIX_THIS_ELEMENT(2, 1) = MATRIX_ELEMENT(first, 2, 1) - MATRIX_ELEMENT(second, 2, 1);
01673         MATRIX_THIS_ELEMENT(2, 2) = MATRIX_ELEMENT(first, 2, 2) - MATRIX_ELEMENT(second, 2, 2);
01674         MATRIX_THIS_ELEMENT(2, 3) = MATRIX_ELEMENT(first, 2, 3) - MATRIX_ELEMENT(second, 2, 3);
01675         MATRIX_THIS_ELEMENT(3, 0) = MATRIX_ELEMENT(first, 3, 0) - MATRIX_ELEMENT(second, 3, 0);
01676         MATRIX_THIS_ELEMENT(3, 1) = MATRIX_ELEMENT(first, 3, 1) - MATRIX_ELEMENT(second, 3, 1);
01677         MATRIX_THIS_ELEMENT(3, 2) = MATRIX_ELEMENT(first, 3, 2) - MATRIX_ELEMENT(second, 3, 2);
01678         MATRIX_THIS_ELEMENT(3, 3) = MATRIX_ELEMENT(first, 3, 3) - MATRIX_ELEMENT(second, 3, 2);
01679       }
01680 
01684       inline void inverse() {
01685         TMatrix<type> m;
01686         m.inverse(*this);
01687         set(m);
01688       }
01689 
01693       inline void inverse(const TMatrix<type> &m) {
01694         type m00 = MATRIX_ELEMENT(m, 0, 1) * MATRIX_ELEMENT(m, 1, 0);
01695         type m01 = MATRIX_ELEMENT(m, 0, 2) * MATRIX_ELEMENT(m, 1, 0);
01696         type m02 = MATRIX_ELEMENT(m, 0, 3) * MATRIX_ELEMENT(m, 1, 0);
01697         type m03 = MATRIX_ELEMENT(m, 0, 0) * MATRIX_ELEMENT(m, 1, 1);
01698         type m04 = MATRIX_ELEMENT(m, 0, 2) * MATRIX_ELEMENT(m, 1, 1);
01699         type m05 = MATRIX_ELEMENT(m, 0, 3) * MATRIX_ELEMENT(m, 1, 1);
01700         type m06 = MATRIX_ELEMENT(m, 0, 0) * MATRIX_ELEMENT(m, 1, 2);
01701         type m07 = MATRIX_ELEMENT(m, 0, 1) * MATRIX_ELEMENT(m, 1, 2);
01702         type m08 = MATRIX_ELEMENT(m, 0, 3) * MATRIX_ELEMENT(m, 1, 2);
01703         type m09 = MATRIX_ELEMENT(m, 0, 0) * MATRIX_ELEMENT(m, 1, 3);
01704         type m10 = MATRIX_ELEMENT(m, 0, 1) * MATRIX_ELEMENT(m, 1, 3);
01705         type m11 = MATRIX_ELEMENT(m, 0, 2) * MATRIX_ELEMENT(m, 1, 3);
01706         type m12 = MATRIX_ELEMENT(m, 2, 1) * MATRIX_ELEMENT(m, 3, 0);
01707         type m13 = MATRIX_ELEMENT(m, 2, 2) * MATRIX_ELEMENT(m, 3, 0);
01708         type m14 = MATRIX_ELEMENT(m, 2, 3) * MATRIX_ELEMENT(m, 3, 0);
01709         type m15 = MATRIX_ELEMENT(m, 2, 0) * MATRIX_ELEMENT(m, 3, 1);
01710         type m16 = MATRIX_ELEMENT(m, 2, 2) * MATRIX_ELEMENT(m, 3, 1);
01711         type m17 = MATRIX_ELEMENT(m, 2, 3) * MATRIX_ELEMENT(m, 3, 1);
01712         type m18 = MATRIX_ELEMENT(m, 2, 0) * MATRIX_ELEMENT(m, 3, 2);
01713         type m19 = MATRIX_ELEMENT(m, 2, 1) * MATRIX_ELEMENT(m, 3, 2);
01714         type m20 = MATRIX_ELEMENT(m, 2, 3) * MATRIX_ELEMENT(m, 3, 2);
01715         type m21 = MATRIX_ELEMENT(m, 2, 0) * MATRIX_ELEMENT(m, 3, 3);
01716         type m22 = MATRIX_ELEMENT(m, 2, 1) * MATRIX_ELEMENT(m, 3, 3);
01717         type m23 = MATRIX_ELEMENT(m, 2, 2) * MATRIX_ELEMENT(m, 3, 3);
01718         type divDet = ((type)(1.0)) / 
01719                       (m08 * m12 - m11 * m12 - m05 * m13 + m10 * m13 + 
01720                        m04 * m14 - m07 * m14 - m08 * m15 + m11 * m15 + 
01721                        m02 * m16 - m09 * m16 - m01 * m17 + m06 * m17 + 
01722                        m05 * m18 - m10 * m18 - m02 * m19 + m09 * m19 + 
01723                        m00 * m20 - m03 * m20 - m04 * m21 + m07 * m21 + 
01724                        m01 * m22 - m06 * m22 - m00 * m23 + m03 * m23);
01725         MATRIX_THIS_ELEMENT(0, 0) = (- MATRIX_ELEMENT(m, 1, 1) * m20
01726                                      + MATRIX_ELEMENT(m, 1, 1) * m23
01727                                      + MATRIX_ELEMENT(m, 1, 2) * m17
01728                                      - MATRIX_ELEMENT(m, 1, 3) * m16
01729                                      - MATRIX_ELEMENT(m, 1, 2) * m22
01730                                      + MATRIX_ELEMENT(m, 1, 3) * m19)
01731                                     * divDet;
01732         MATRIX_THIS_ELEMENT(0, 1) = (  MATRIX_ELEMENT(m, 0, 1) * m20
01733                                      - MATRIX_ELEMENT(m, 0, 1) * m23
01734                                      - MATRIX_ELEMENT(m, 0, 2) * m17
01735                                      + MATRIX_ELEMENT(m, 0, 3) * m16
01736                                      + MATRIX_ELEMENT(m, 0, 2) * m22
01737                                      - MATRIX_ELEMENT(m, 0, 3) * m19)
01738                                     * divDet;
01739         MATRIX_THIS_ELEMENT(0, 2) = (- m04 * MATRIX_ELEMENT(m, 3, 3)
01740                                      + m05 * MATRIX_ELEMENT(m, 3, 2)
01741                                      + m07 * MATRIX_ELEMENT(m, 3, 3)
01742                                      - m08 * MATRIX_ELEMENT(m, 3, 1)
01743                                      - m10 * MATRIX_ELEMENT(m, 3, 2)
01744                                      + m11 * MATRIX_ELEMENT(m, 3, 1))
01745                                     * divDet;
01746         MATRIX_THIS_ELEMENT(0, 3) = (  m04 * MATRIX_ELEMENT(m, 2, 3)
01747                                      - m05 * MATRIX_ELEMENT(m, 2, 2)
01748                                      - m07 * MATRIX_ELEMENT(m, 2, 3)
01749                                      + m10 * MATRIX_ELEMENT(m, 2, 2)
01750                                      + m08 * MATRIX_ELEMENT(m, 2, 1)
01751                                      - m11 * MATRIX_ELEMENT(m, 2, 1))
01752                                     * divDet;
01753         MATRIX_THIS_ELEMENT(1, 0) = (  MATRIX_ELEMENT(m, 1, 0) * m20
01754                                      - MATRIX_ELEMENT(m, 1, 0) * m23
01755                                      - MATRIX_ELEMENT(m, 1, 2) * m14
01756                                      + MATRIX_ELEMENT(m, 1, 3) * m13
01757                                      + MATRIX_ELEMENT(m, 1, 2) * m21
01758                                      - MATRIX_ELEMENT(m, 1, 3) * m18)
01759                                     * divDet;
01760         MATRIX_THIS_ELEMENT(1, 1) = (- MATRIX_ELEMENT(m, 0, 0) * m20
01761                                      + MATRIX_ELEMENT(m, 0, 0) * m23
01762                                      + MATRIX_ELEMENT(m, 0, 2) * m14
01763                                      - MATRIX_ELEMENT(m, 0, 3) * m13
01764                                      - MATRIX_ELEMENT(m, 0, 2) * m21
01765                                      + MATRIX_ELEMENT(m, 0, 3) * m18)
01766                                     * divDet;
01767         MATRIX_THIS_ELEMENT(1, 2) = (  m01 * MATRIX_ELEMENT(m, 3, 3)
01768                                      - m02 * MATRIX_ELEMENT(m, 3, 2)
01769                                      - m06 * MATRIX_ELEMENT(m, 3, 3)
01770                                      + m08 * MATRIX_ELEMENT(m, 3, 0)
01771                                      + m09 * MATRIX_ELEMENT(m, 3, 2)
01772                                      - m11 * MATRIX_ELEMENT(m, 3, 0))
01773                                     * divDet;
01774         MATRIX_THIS_ELEMENT(1, 3) = (- m01 * MATRIX_ELEMENT(m, 2, 3)
01775                                      + m02 * MATRIX_ELEMENT(m, 2, 2)
01776                                      + m06 * MATRIX_ELEMENT(m, 2, 3)
01777                                      - m09 * MATRIX_ELEMENT(m, 2, 2)
01778                                      - m08 * MATRIX_ELEMENT(m, 2, 0)
01779                                      + m11 * MATRIX_ELEMENT(m, 2, 0))
01780                                     * divDet;
01781         MATRIX_THIS_ELEMENT(2, 0) = (- MATRIX_ELEMENT(m, 1, 0) * m17
01782                                      + MATRIX_ELEMENT(m, 1, 1) * m14
01783                                      - MATRIX_ELEMENT(m, 1, 1) * m21
01784                                      + MATRIX_ELEMENT(m, 1, 3) * m15
01785                                      + MATRIX_ELEMENT(m, 1, 0) * m22
01786                                      - MATRIX_ELEMENT(m, 1, 3) * m12)
01787                                     * divDet;
01788         MATRIX_THIS_ELEMENT(2, 1) = (- MATRIX_ELEMENT(m, 0, 1) * m14
01789                                      + MATRIX_ELEMENT(m, 0, 0) * m17
01790                                      + MATRIX_ELEMENT(m, 0, 1) * m21
01791                                      - MATRIX_ELEMENT(m, 0, 3) * m15
01792                                      - MATRIX_ELEMENT(m, 0, 0) * m22
01793                                      + MATRIX_ELEMENT(m, 0, 3) * m12)
01794                                     * divDet;
01795         MATRIX_THIS_ELEMENT(2, 2) = (- m00 * MATRIX_ELEMENT(m, 3, 3)
01796                                      + m02 * MATRIX_ELEMENT(m, 3, 1)
01797                                      + m03 * MATRIX_ELEMENT(m, 3, 3)
01798                                      - m05 * MATRIX_ELEMENT(m, 3, 0)
01799                                      + m10 * MATRIX_ELEMENT(m, 3, 0)
01800                                      - m09 * MATRIX_ELEMENT(m, 3, 1))
01801                                     * divDet;
01802         MATRIX_THIS_ELEMENT(2, 3) = (  m00 * MATRIX_ELEMENT(m, 2, 3)
01803                                      - m03 * MATRIX_ELEMENT(m, 2, 3)
01804                                      + m05 * MATRIX_ELEMENT(m, 2, 0)
01805                                      - m10 * MATRIX_ELEMENT(m, 2, 0)
01806                                      - m02 * MATRIX_ELEMENT(m, 2, 1)
01807                                      + m09 * MATRIX_ELEMENT(m, 2, 1))
01808                                     * divDet;
01809         MATRIX_THIS_ELEMENT(3, 0) = (  MATRIX_ELEMENT(m, 1, 0) * m16
01810                                      - MATRIX_ELEMENT(m, 1, 1) * m13
01811                                      + MATRIX_ELEMENT(m, 1, 1) * m18
01812                                      - MATRIX_ELEMENT(m, 1, 2) * m15
01813                                      - MATRIX_ELEMENT(m, 1, 0) * m19
01814                                      + MATRIX_ELEMENT(m, 1, 2) * m12)
01815                                     * divDet;
01816         MATRIX_THIS_ELEMENT(3, 1) = (  MATRIX_ELEMENT(m, 0, 1) * m13
01817                                      - MATRIX_ELEMENT(m, 0, 0) * m16
01818                                      - MATRIX_ELEMENT(m, 0, 1) * m18
01819                                      + MATRIX_ELEMENT(m, 0, 2) * m15
01820                                      + MATRIX_ELEMENT(m, 0, 0) * m19
01821                                      - MATRIX_ELEMENT(m, 0, 2) * m12)
01822                                     * divDet;
01823         MATRIX_THIS_ELEMENT(3, 2) = (  m00 * MATRIX_ELEMENT(m, 3, 2)
01824                                      - m01 * MATRIX_ELEMENT(m, 3, 1)
01825                                      - m03 * MATRIX_ELEMENT(m, 3, 2)
01826                                      + m04 * MATRIX_ELEMENT(m, 3, 0)
01827                                      - m07 * MATRIX_ELEMENT(m, 3, 0)
01828                                      + m06 * MATRIX_ELEMENT(m, 3, 1))
01829                                     * divDet;
01830         MATRIX_THIS_ELEMENT(3, 3) = (- m00 * MATRIX_ELEMENT(m, 2, 2)
01831                                      + m03 * MATRIX_ELEMENT(m, 2, 2)
01832                                      - m04 * MATRIX_ELEMENT(m, 2, 0)
01833                                      + m07 * MATRIX_ELEMENT(m, 2, 0)
01834                                      + m01 * MATRIX_ELEMENT(m, 2, 1)
01835                                      - m06 * MATRIX_ELEMENT(m, 2, 1))
01836                                     * divDet;
01837       }
01838     };
01839 
01843     typedef TMatrix<Real> Matrix;
01844   }
01845 }
01846 
01847 #endif

Generated on Tue May 20 17:23:53 2003 for AMIRE-Framework by doxygen1.3