nevrax.org : docs

00001 
+00007 /* Copyright, 2000 Nevrax Ltd.
+00008  *
+00009  * This file is part of NEVRAX NEL.
+00010  * NEVRAX NEL is free software; you can redistribute it and/or modify
+00011  * it under the terms of the GNU General Public License as published by
+00012  * the Free Software Foundation; either version 2, or (at your option)
+00013  * any later version.
+00014 
+00015  * NEVRAX NEL is distributed in the hope that it will be useful, but
+00016  * WITHOUT ANY WARRANTY; without even the implied warranty of
+00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+00018  * General Public License for more details.
+00019 
+00020  * You should have received a copy of the GNU General Public License
+00021  * along with NEVRAX NEL; see the file COPYING. If not, write to the
+00022  * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
+00023  * MA 02111-1307, USA.
+00024  */
+00025 
+00026 #include "stdmisc.h"
+00027 
+00028 #include "nel/misc/matrix.h"
+00029 #include "nel/misc/plane.h"
+00030 #include "nel/misc/debug.h"
+00031 
+00032 using namespace std;
+00033 
+00034 
+00035 
+00036 namespace       NLMISC
+00037 {
+00038 
+00039 
+00040 // ======================================================================================================
+00041 const CMatrix   CMatrix::Identity;
+00042 
+00043 
+00044 // ======================================================================================================
+00045 // ======================================================================================================
+00046 // ======================================================================================================
+00047 
+00048 
+00049 // State Bits.
+00050 #define MAT_TRANS               1
+00051 #define MAT_ROT                 2
+00052 #define MAT_SCALEUNI    4
+00053 #define MAT_SCALEANY    8
+00054 #define MAT_PROJ                16
+00055 // Validity bits. These means that the part may be yet identity, but is valid in the floats.
+00056 // NB: MAT_VALIDTRANS no more used for faster Pos access
+00057 #define MAT_VALIDROT    64
+00058 #define MAT_VALIDPROJ   128
+00059 #define MAT_VALIDALL    (MAT_VALIDROT | MAT_VALIDPROJ)
+00060 // The identity is nothing.
+00061 #define MAT_IDENTITY    0
+00062 
+00063 
+00064 
+00065 // Matrix elements.
+00066 #define a11             M[0]
+00067 #define a21             M[1]
+00068 #define a31             M[2]
+00069 #define a41             M[3]
+00070 #define a12             M[4]
+00071 #define a22             M[5]
+00072 #define a32             M[6]
+00073 #define a42             M[7]
+00074 #define a13             M[8]
+00075 #define a23             M[9]
+00076 #define a33             M[10]
+00077 #define a43             M[11]
+00078 #define a14             M[12]
+00079 #define a24             M[13]
+00080 #define a34             M[14]
+00081 #define a44             M[15]
+00082 
+00083 
+00084 
+00085 // ======================================================================================================
+00086 // ======================================================================================================
+00087 // ======================================================================================================
+00088 
+00089 
+00090 
+00091 // ======================================================================================================
+00092 bool            CMatrix::hasScalePart() const
+00093 {
+00094         return (StateBit&(MAT_SCALEUNI|MAT_SCALEANY))!=0;
+00095 }
+00096 bool            CMatrix::hasProjectionPart() const
+00097 {
+00098         return (StateBit&MAT_PROJ)!=0;
+00099 }
+00100 
+00101 
+00102 bool            CMatrix::hasScaleUniform() const
+00103 {
+00104         return (StateBit&MAT_SCALEUNI)!=0 && (StateBit&MAT_SCALEANY)==0;
+00105 }
+00106 float           CMatrix::getScaleUniform() const
+00107 {
+00108         if(hasScaleUniform())
+00109                 return Scale33;
+00110         else
+00111                 return 1;
+00112 }
+00113 
+00114 
+00115 
+00116 // ======================================================================================================
+00117 inline bool     CMatrix::hasRot() const
+00118 {
+00119         return (StateBit&(MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY))!=0;
+00120 }
+00121 inline bool     CMatrix::hasTrans() const
+00122 {
+00123         return (StateBit&MAT_TRANS)!=0;
+00124 }
+00125 inline bool     CMatrix::hasProj() const
+00126 {
+00127         return (StateBit&MAT_PROJ)!=0;
+00128 }
+00129 inline bool     CMatrix::hasAll() const
+00130 {
+00131         return (hasRot() && hasTrans() && hasProj());
+00132 }
+00133 
+00134 
+00135 inline void CMatrix::testExpandRot() const
+00136 {
+00137         if(hasRot())
+00138                 return;
+00139         if(!(StateBit&MAT_VALIDROT))
+00140         {
+00141                 CMatrix *self= const_cast<CMatrix*>(this);
+00142                 self->StateBit|=MAT_VALIDROT;
+00143                 self->a11= 1; self->a12=0; self->a13=0;
+00144                 self->a21= 0; self->a22=1; self->a23=0;
+00145                 self->a31= 0; self->a32=0; self->a33=1;
+00146                 self->Scale33= 1;
+00147         }
+00148 }
+00149 inline void CMatrix::testExpandProj() const
+00150 {
+00151         if(hasProj())
+00152                 return;
+00153         if(!(StateBit&MAT_VALIDPROJ))
+00154         {
+00155                 CMatrix *self= const_cast<CMatrix*>(this);
+00156                 self->StateBit|=MAT_VALIDPROJ;
+00157                 self->a41=0; self->a42=0; self->a43=0; self->a44=1; 
+00158         }
+00159 }
+00160 
+00161 
+00162 // ======================================================================================================
+00163 CMatrix::CMatrix(const CMatrix &m)
+00164 {
+00165         (*this)= m;
+00166 }
+00167 // ======================================================================================================
+00168 CMatrix         &CMatrix::operator=(const CMatrix &m)
+00169 {
+00170         StateBit= m.StateBit & ~MAT_VALIDALL;
+00171         if(hasAll())
+00172         {
+00173                 memcpy(M, m.M, 16*sizeof(float));
+00174                 Scale33= m.Scale33;
+00175         }
+00176         else
+00177         {
+00178                 if(hasRot())
+00179                 {
+00180                         memcpy(&a11, &m.a11, 3*sizeof(float));
+00181                         memcpy(&a12, &m.a12, 3*sizeof(float));
+00182                         memcpy(&a13, &m.a13, 3*sizeof(float));
+00183                         Scale33= m.Scale33;
+00184                 }
+00185                 if(hasProj())
+00186                 {
+00187                         a41= m.a41;
+00188                         a42= m.a42;
+00189                         a43= m.a43;
+00190                         a44= m.a44;
+00191                 }
+00192                 // Must always copy Trans part.
+00193                 memcpy(&a14, &m.a14, 3*sizeof(float));
+00194         }
+00195         return *this;
+00196 }
+00197 
+00198 
+00199 // ======================================================================================================
+00200 void            CMatrix::identity()
+00201 {
+00202         StateBit= MAT_IDENTITY;
+00203         // Reset just Pos because must always be valid for faster getPos()
+00204         a14= a24= a34= 0;
+00205         // For optimisation it would be usefull to keep MAT_VALID states.
+00206         // But this slows identity(), and this may not be interesting...
+00207 }
+00208 // ======================================================================================================
+00209 void            CMatrix::setRot(const CVector &i, const CVector &j, const CVector &k, bool hintNoScale)
+00210 {
+00211         StateBit|= MAT_ROT | MAT_SCALEANY;
+00212         if(hintNoScale)
+00213                 StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
+00214         a11= i.x; a12= j.x; a13= k.x; 
+00215         a21= i.y; a22= j.y; a23= k.y; 
+00216         a31= i.z; a32= j.z; a33= k.z; 
+00217         Scale33= 1.0f;
+00218 }
+00219 // ======================================================================================================
+00220 void            CMatrix::setRot(const float m33[9], bool hintNoScale)
+00221 {
+00222         StateBit|= MAT_ROT | MAT_SCALEANY;
+00223         if(hintNoScale)
+00224                 StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
+00225         a11= m33[0]; a12= m33[3]; a13= m33[6]; 
+00226         a21= m33[1]; a22= m33[4]; a23= m33[7]; 
+00227         a31= m33[2]; a32= m33[5]; a33= m33[8]; 
+00228         Scale33= 1.0f;
+00229 }
+00230 // ======================================================================================================
+00231 void            CMatrix::setRot(const CVector &v, TRotOrder ro)
+00232 {
+00233         CMatrix         rot;
+00234         rot.identity();
+00235         rot.rotate(v, ro);
+00236         float   m33[9];
+00237         rot.getRot(m33);
+00238         setRot(m33, true);
+00239 }
+00240 
+00241 
+00242 // ======================================================================================================
+00243 void            CMatrix::setRot(const CMatrix &matrix)
+00244 {
+00245         // copy rotpart statebit from other.
+00246         StateBit&= ~(MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
+00247         StateBit|= matrix.StateBit & (MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
+00248         // copy values.
+00249         if(hasRot())
+00250         {
+00251                 a11= matrix.a11; a12= matrix.a12; a13= matrix.a13;
+00252                 a21= matrix.a21; a22= matrix.a22; a23= matrix.a23;
+00253                 a31= matrix.a31; a32= matrix.a32; a33= matrix.a33;
+00254                 // if has scale, copy from matrix.
+00255                 if(StateBit & MAT_SCALEUNI)
+00256                         Scale33= matrix.Scale33;
+00257         }
+00258         else
+00259         {
+00260                 // we are rot identity, with undefined values.
+00261                 StateBit&= ~MAT_VALIDROT;
+00262         }
+00263 }
+00264 
+00265 
+00266 // ======================================================================================================
+00267 void            CMatrix::setPos(const CVector &v)
+00268 {
+00269         a14= v.x;
+00270         a24= v.y;
+00271         a34= v.z;
+00272         if(a14!=0 || a24!=0 || a34!=0)
+00273                 StateBit|= MAT_TRANS;
+00274         else
+00275                 // The trans is identity
+00276                 StateBit&= ~MAT_TRANS;
+00277 }
+00278 // ======================================================================================================
+00279 void            CMatrix::movePos(const CVector &v)
+00280 {
+00281         a14+= v.x;
+00282         a24+= v.y;
+00283         a34+= v.z;
+00284         if(a14!=0 || a24!=0 || a34!=0)
+00285                 StateBit|= MAT_TRANS;
+00286         else
+00287                 // The trans is identity
+00288                 StateBit&= ~MAT_TRANS;
+00289 }
+00290 // ======================================================================================================
+00291 void            CMatrix::setProj(const float proj[4])
+00292 {
+00293         a41= proj[0];
+00294         a42= proj[1];
+00295         a43= proj[2];
+00296         a44= proj[3];
+00297 
+00298         // Check Proj state.
+00299         if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
+00300                 StateBit|= MAT_PROJ;
+00301         else
+00302         {
+00303                 // The proj is identity, and is correcly setup!
+00304                 StateBit&= ~MAT_PROJ;
+00305                 StateBit|= MAT_VALIDPROJ;
+00306         }
+00307 }
+00308 // ======================================================================================================
+00309 void            CMatrix::resetProj()
+00310 {
+00311         a41= 0;
+00312         a42= 0;
+00313         a43= 0;
+00314         a44= 1;
+00315         // The proj is identity, and is correcly setup!
+00316         StateBit&= ~MAT_PROJ;
+00317         StateBit|= MAT_VALIDPROJ;
+00318 }
+00319 // ======================================================================================================
+00320 void            CMatrix::set(const float m44[16])
+00321 {
+00322         StateBit= MAT_IDENTITY;
+00323 
+00324         StateBit|= MAT_ROT | MAT_SCALEANY;
+00325         memcpy(M, m44, 16*sizeof(float));
+00326         Scale33= 1.0f;
+00327 
+00328         // Check Trans state.
+00329         if(a14!=0 || a24!=0 || a34!=0)
+00330                 StateBit|= MAT_TRANS;
+00331         else
+00332                 // The trans is identity
+00333                 StateBit&= ~MAT_TRANS;
+00334 
+00335         // Check Proj state.
+00336         if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
+00337                 StateBit|= MAT_PROJ;
+00338         else
+00339         {
+00340                 // The proj is identity, and is correcly setup!
+00341                 StateBit&= ~MAT_PROJ;
+00342                 StateBit|= MAT_VALIDPROJ;
+00343         }
+00344 }
+00345 
+00346 
+00347 // ======================================================================================================
+00348 // ======================================================================================================
+00349 // ======================================================================================================
+00350 
+00351 
+00352 // ======================================================================================================
+00353 void            CMatrix::getRot(CVector &i, CVector &j, CVector &k) const
+00354 {
+00355         if(hasRot())
+00356         {
+00357                 i.set(a11, a21, a31);
+00358                 j.set(a12, a22, a32);
+00359                 k.set(a13, a23, a33);
+00360         }
+00361         else
+00362         {
+00363                 i.set(1, 0, 0);
+00364                 j.set(0, 1, 0);
+00365                 k.set(0, 0, 1);
+00366         }
+00367 }
+00368 // ======================================================================================================
+00369 void            CMatrix::getRot(float m33[9]) const
+00370 {
+00371         if(hasRot())
+00372         {
+00373                 m33[0]= a11;
+00374                 m33[1]= a21;
+00375                 m33[2]= a31;
+00376 
+00377                 m33[3]= a12;
+00378                 m33[4]= a22;
+00379                 m33[5]= a32;
+00380 
+00381                 m33[6]= a13;
+00382                 m33[7]= a23;
+00383                 m33[8]= a33;
+00384         }
+00385         else
+00386         {
+00387                 m33[0]= 1;
+00388                 m33[1]= 0;
+00389                 m33[2]= 0;
+00390 
+00391                 m33[3]= 0;
+00392                 m33[4]= 1;
+00393                 m33[5]= 0;
+00394 
+00395                 m33[6]= 0;
+00396                 m33[7]= 0;
+00397                 m33[8]= 1;
+00398         }
+00399 }
+00400 // ======================================================================================================
+00401 void            CMatrix::getProj(float proj[4]) const
+00402 {
+00403         if(hasProj())
+00404         {
+00405                 proj[0]= a41;
+00406                 proj[1]= a42;
+00407                 proj[2]= a43;
+00408                 proj[3]= a44;
+00409         }
+00410         else
+00411         {
+00412                 proj[0]= 0;
+00413                 proj[1]= 0;
+00414                 proj[2]= 0;
+00415                 proj[3]= 1;
+00416         }
+00417 }
+00418 // ======================================================================================================
+00419 CVector         CMatrix::getI() const
+00420 {
+00421         if(hasRot())
+00422                 return CVector(a11, a21, a31);
+00423         else
+00424                 return CVector(1, 0, 0);
+00425 }
+00426 // ======================================================================================================
+00427 CVector         CMatrix::getJ() const
+00428 {
+00429         if(hasRot())
+00430                 return CVector(a12, a22, a32);
+00431         else
+00432                 return CVector(0, 1, 0);
+00433 }
+00434 // ======================================================================================================
+00435 CVector         CMatrix::getK() const
+00436 {
+00437         if(hasRot())
+00438                 return CVector(a13, a23, a33);
+00439         else
+00440                 return CVector(0, 0, 1);
+00441 }
+00442 // ======================================================================================================
+00443 void            CMatrix::get(float m44[16]) const
+00444 {
+00445         // \todo yoyo: TODO_OPTIMIZE_it.
+00446         testExpandRot();
+00447         testExpandProj();
+00448         memcpy(m44, M, 16*sizeof(float));
+00449 }
+00450 // ======================================================================================================
+00451 const float *CMatrix::get() const
+00452 {
+00453         testExpandRot();
+00454         testExpandProj();
+00455         return M;
+00456 }
+00457 /*// ======================================================================================================
+00458 CVector         CMatrix::toEuler(TRotOrder ro) const
+00459 {
+00460 
+00461 }*/
+00462 
+00463 
+00464 // ======================================================================================================
+00465 // ======================================================================================================
+00466 // ======================================================================================================
+00467 
+00468 
+00469 // ======================================================================================================
+00470 void            CMatrix::translate(const CVector &v)
+00471 {
+00472         // SetTrans.
+00473         if( hasRot() )
+00474         {
+00475                 a14+= a11*v.x + a12*v.y + a13*v.z;
+00476                 a24+= a21*v.x + a22*v.y + a23*v.z;
+00477                 a34+= a31*v.x + a32*v.y + a33*v.z;
+00478         }
+00479         else
+00480         {
+00481                 a14+= v.x;
+00482                 a24+= v.y;
+00483                 a34+= v.z;
+00484         }
+00485 
+00486         // SetProj.
+00487         if( hasProj() )
+00488                 a44+= a41*v.x + a42*v.y + a43*v.z;
+00489 
+00490         // Check Trans.
+00491         if(a14!=0 || a24!=0 || a34!=0)
+00492                 StateBit|= MAT_TRANS;
+00493         else
+00494                 // The trans is identity, and is correcly setup!
+00495                 StateBit&= ~MAT_TRANS;
+00496 }
+00497 // ======================================================================================================
+00498 void            CMatrix::rotateX(float a)
+00499 {
+00500 
+00501         if(a==0)
+00502                 return;
+00503         double  ca,sa;
+00504         ca=cos(a);
+00505         sa=sin(a);
+00506 
+00507         // SetRot.
+00508         if( hasRot() )
+00509         {
+00510                 float   b12=a12, b22=a22, b32=a32;
+00511                 float   b13=a13, b23=a23, b33=a33;
+00512                 a12= (float)(b12*ca + b13*sa);
+00513                 a22= (float)(b22*ca + b23*sa);
+00514                 a32= (float)(b32*ca + b33*sa);
+00515                 a13= (float)(b13*ca - b12*sa);
+00516                 a23= (float)(b23*ca - b22*sa);
+00517                 a33= (float)(b33*ca - b32*sa);
+00518         }
+00519         else
+00520         {
+00521                 testExpandRot();
+00522                 a12= 0.0f; a22= (float)ca; a32= (float)sa;
+00523                 a13= 0.0f; a23= (float)-sa; a33= (float)ca;
+00524         }
+00525 
+00526         // SetProj.
+00527         if( hasProj() )
+00528         {
+00529                 float   b42=a42, b43=a43;
+00530                 a42= (float)(b42*ca + b43*sa);
+00531                 a43= (float)(b43*ca - b42*sa);
+00532         }
+00533 
+00534         // set Rot.
+00535         StateBit|= MAT_ROT;
+00536 }
+00537 // ======================================================================================================
+00538 void            CMatrix::rotateY(float a)
+00539 {
+00540 
+00541         if(a==0)
+00542                 return;
+00543         double  ca,sa;
+00544         ca=cos(a);
+00545         sa=sin(a);
+00546 
+00547         // SetRot.
+00548         if( hasRot() )
+00549         {
+00550                 float   b11=a11, b21=a21, b31=a31;
+00551                 float   b13=a13, b23=a23, b33=a33;
+00552                 a11= (float)(b11*ca - b13*sa);
+00553                 a21= (float)(b21*ca - b23*sa);
+00554                 a31= (float)(b31*ca - b33*sa);
+00555                 a13= (float)(b13*ca + b11*sa);
+00556                 a23= (float)(b23*ca + b21*sa);
+00557                 a33= (float)(b33*ca + b31*sa);
+00558         }
+00559         else
+00560         {
+00561                 testExpandRot();
+00562                 a11= (float)ca; a21=0.0f; a31= (float)-sa;
+00563                 a13= (float)sa; a23=0.0f; a33= (float)ca;
+00564         }
+00565 
+00566         // SetProj.
+00567         if( hasProj() )
+00568         {
+00569                 float   b41=a41, b43=a43;
+00570                 a41= (float)(b41*ca - b43*sa);
+00571                 a43= (float)(b43*ca + b41*sa);
+00572         }
+00573 
+00574         // set Rot.
+00575         StateBit|= MAT_ROT;
+00576 }
+00577 // ======================================================================================================
+00578 void            CMatrix::rotateZ(float a)
+00579 {
+00580 
+00581         if(a==0)
+00582                 return;
+00583         double  ca,sa;
+00584         ca=cos(a);
+00585         sa=sin(a);
+00586 
+00587         // SetRot.
+00588         if( StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY) )
+00589         {
+00590                 float   b11=a11, b21=a21, b31=a31;
+00591                 float   b12=a12, b22=a22, b32=a32;
+00592                 a11= (float)(b11*ca + b12*sa);
+00593                 a21= (float)(b21*ca + b22*sa);
+00594                 a31= (float)(b31*ca + b32*sa);
+00595                 a12= (float)(b12*ca - b11*sa);
+00596                 a22= (float)(b22*ca - b21*sa);
+00597                 a32= (float)(b32*ca - b31*sa);
+00598         }
+00599         else
+00600         {
+00601                 testExpandRot();
+00602                 a11= (float)ca; a21= (float)sa; a31=0.0f;
+00603                 a12= (float)-sa; a22= (float)ca; a32=0.0f;
+00604         }
+00605 
+00606         // SetProj.
+00607         if( hasProj() )
+00608         {
+00609                 float   b41=a41, b42=a42;
+00610                 a41= (float)(b41*ca + b42*sa);
+00611                 a42= (float)(b42*ca - b41*sa);
+00612         }
+00613 
+00614         // set Rot.
+00615         StateBit|= MAT_ROT;
+00616 }
+00617 // ======================================================================================================
+00618 void            CMatrix::rotate(const CVector &v, TRotOrder ro)
+00619 {
+00620         CMatrix         rot;
+00621         rot.identity();
+00622         switch(ro)
+00623         {
+00624                 case XYZ: rot.rotateX(v.x); rot.rotateY(v.y); rot.rotateZ(v.z); break;
+00625                 case XZY: rot.rotateX(v.x); rot.rotateZ(v.z); rot.rotateY(v.y); break;
+00626                 case YXZ: rot.rotateY(v.y); rot.rotateX(v.x); rot.rotateZ(v.z); break;
+00627                 case YZX: rot.rotateY(v.y); rot.rotateZ(v.z); rot.rotateX(v.x); break;
+00628                 case ZXY: rot.rotateZ(v.z); rot.rotateX(v.x); rot.rotateY(v.y); break;
+00629                 case ZYX: rot.rotateZ(v.z); rot.rotateY(v.y); rot.rotateX(v.x); break;
+00630         }
+00631 
+00632         (*this)*= rot;
+00633 }
+00634 
+00635 // ======================================================================================================
+00636 void            CMatrix::rotate(const CQuat &quat)
+00637 {
+00638         CMatrix         rot;
+00639         rot.setRot(quat);
+00640         (*this)*= rot;
+00641 }
+00642 
+00643 // ======================================================================================================
+00644 void            CMatrix::scale(float f)
+00645 {
+00646 
+00647         if(f==1.0f) return;
+00648         if(StateBit & MAT_SCALEANY)
+00649         {
+00650                 scale(CVector(f,f,f));
+00651         }
+00652         else
+00653         {
+00654                 testExpandRot();
+00655                 StateBit|= MAT_SCALEUNI;
+00656                 Scale33*=f;
+00657                 a11*= f; a12*=f; a13*=f;
+00658                 a21*= f; a22*=f; a23*=f;
+00659                 a31*= f; a32*=f; a33*=f;
+00660 
+00661                 // SetProj.
+00662                 if( hasProj() )
+00663                 {
+00664                         a41*=f; a42*=f; a43*=f;
+00665                 }
+00666         }
+00667 }
+00668 // ======================================================================================================
+00669 void            CMatrix::scale(const CVector &v)
+00670 {
+00671 
+00672         if( v==CVector(1,1,1) ) return;
+00673         if( !(StateBit & MAT_SCALEANY) && v.x==v.y && v.x==v.z)
+00674         {
+00675                 scale(v.x);
+00676         }
+00677         else
+00678         {
+00679                 testExpandRot();
+00680                 StateBit|=MAT_SCALEANY;
+00681                 a11*= v.x; a12*=v.y; a13*=v.z;
+00682                 a21*= v.x; a22*=v.y; a23*=v.z;
+00683                 a31*= v.x; a32*=v.y; a33*=v.z;
+00684 
+00685                 // SetProj.
+00686                 if( hasProj() )
+00687                 {
+00688                         a41*=v.x;
+00689                         a42*=v.y;
+00690                         a43*=v.z;
+00691                 }
+00692         }
+00693 }
+00694 
+00695 
+00696 // ======================================================================================================
+00697 // ======================================================================================================
+00698 // ======================================================================================================
+00699 
+00700 
+00701 // ***************************************************************************
+00702 void            CMatrix::setMulMatrixNoProj(const CMatrix &m1, const CMatrix &m2)
+00703 {
+00704         // Do *this= m1*m2
+00705         identity();
+00706         StateBit= (m1.StateBit | m2.StateBit) & (~MAT_PROJ);
+00707         StateBit&= ~MAT_VALIDALL;
+00708 
+00709 
+00710         // Build Rot part.
+00711         //===============
+00712         bool    M1Identity= ! m1.hasRot();
+00713         bool    M2Identity= ! m2.hasRot();
+00714         bool    M1ScaleOnly= ! (m1.StateBit & MAT_ROT);
+00715         bool    M2ScaleOnly= ! (m2.StateBit & MAT_ROT);
+00716         bool    MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly;
+00717 
+00718 
+00719         // Manage the most common general case first (optim the if ): blending of two rotations.
+00720         if( MGeneralCase )
+00721         {
+00722                 a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
+00723                 a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
+00724                 a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
+00725 
+00726                 a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
+00727                 a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
+00728                 a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
+00729 
+00730                 a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
+00731                 a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
+00732                 a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
+00733         }
+00734         // If one of the 3x3 matrix is an identity, just do a copy
+00735         else if( M1Identity || M2Identity )
+00736         {
+00737                 // If both identity, then me too.
+00738                 if( M1Identity && M2Identity )
+00739                 {
+00740                         // just expand me (important because validated below)
+00741                         testExpandRot();
+00742                 }
+00743                 else
+00744                 {
+00745                         // Copy the non identity matrix.
+00746                         const CMatrix   *c= M2Identity? &m1 : &m2;
+00747                         a11= c->a11; a12= c->a12; a13= c->a13;
+00748                         a21= c->a21; a22= c->a22; a23= c->a23;
+00749                         a31= c->a31; a32= c->a32; a33= c->a33;
+00750                 }
+00751         }
+00752         // If two 3x3 matrix are just scaleOnly matrix, do a scaleFact.
+00753         else if( M1ScaleOnly && M2ScaleOnly )
+00754         {
+00755                 // same process for scaleUni or scaleAny.
+00756                 a11= m1.a11*m2.a11; a12= 0; a13= 0; 
+00757                 a21= 0; a22= m1.a22*m2.a22; a23= 0; 
+00758                 a31= 0; a32= 0; a33= m1.a33*m2.a33;
+00759         }
+00760         // If one of the matrix is a scaleOnly matrix, do a scale*Rot.
+00761         else if( M1ScaleOnly && !M2ScaleOnly )
+00762         {
+00763                 a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13;
+00764                 a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23;
+00765                 a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33;
+00766         }
+00767         else
+00768         {
+00769                 // This must be this case
+00770                 nlassert(!M1ScaleOnly && M2ScaleOnly);
+00771                 a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33;
+00772                 a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33;
+00773                 a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33;
+00774         }
+00775 
+00776         // Modify Scale.
+00777         if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
+00778         {
+00779                 // Must have correct Scale33
+00780                 m1.testExpandRot();
+00781                 m2.testExpandRot();
+00782                 Scale33= m1.Scale33*m2.Scale33;
+00783         }
+00784         else
+00785                 Scale33=1;
+00786 
+00787         // In every case, I am valid now!
+00788         StateBit|=MAT_VALIDROT;
+00789 
+00790 
+00791         // Build Trans part.
+00792         //=================
+00793         if( StateBit & MAT_TRANS )
+00794         {
+00795                 // Compose M2 part. NB: always suppose MAT_TRANS, for optim consideration.
+00796                 // NB: the translation part is always valid, even if identity()
+00797                 if( M1Identity )
+00798                 {
+00799                         a14= m2.a14 + m1.a14;
+00800                         a24= m2.a24 + m1.a24;
+00801                         a34= m2.a34 + m1.a34;
+00802                 }
+00803                 else if (M1ScaleOnly )
+00804                 {
+00805                         a14= m1.a11*m2.a14 + m1.a14;
+00806                         a24= m1.a22*m2.a24 + m1.a24;
+00807                         a34= m1.a33*m2.a34 + m1.a34;
+00808                 }
+00809                 else
+00810                 {
+00811                         a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34 + m1.a14;
+00812                         a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34 + m1.a24;
+00813                         a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34 + m1.a34;
+00814                 }
+00815         }
+00816 
+00817 }
+00818 
+00819 
+00820 // ***************************************************************************
+00821 void            CMatrix::setMulMatrix(const CMatrix &m1, const CMatrix &m2)
+00822 {
+00823         // Do *this= m1*m2
+00824         identity();
+00825         StateBit= m1.StateBit | m2.StateBit;
+00826         StateBit&= ~MAT_VALIDALL;
+00827 
+00828         // Build Rot part.
+00829         //===============
+00830         bool    M1Identity= ! m1.hasRot();
+00831         bool    M2Identity= ! m2.hasRot();
+00832         bool    M1ScaleOnly= ! (m1.StateBit & MAT_ROT);
+00833         bool    M2ScaleOnly= ! (m2.StateBit & MAT_ROT);
+00834         bool    MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly;
+00835 
+00836 
+00837         // Manage the most common general case first (optim the if ): blending of two rotations.
+00838         if( MGeneralCase )
+00839         {
+00840                 a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
+00841                 a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
+00842                 a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
+00843 
+00844                 a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
+00845                 a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
+00846                 a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
+00847 
+00848                 a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
+00849                 a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
+00850                 a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
+00851         }
+00852         // If one of the 3x3 matrix is an identity, just do a copy
+00853         else if( M1Identity || M2Identity )
+00854         {
+00855                 // If both identity, then me too.
+00856                 if( M1Identity && M2Identity )
+00857                 {
+00858                         // just expand me (important because validated below)
+00859                         testExpandRot();
+00860                 }
+00861                 else
+00862                 {
+00863                         // Copy the non identity matrix.
+00864                         const CMatrix   *c= M2Identity? &m1 : &m2;
+00865                         a11= c->a11; a12= c->a12; a13= c->a13;
+00866                         a21= c->a21; a22= c->a22; a23= c->a23;
+00867                         a31= c->a31; a32= c->a32; a33= c->a33;
+00868                 }
+00869         }
+00870         // If two 3x3 matrix are just scaleOnly matrix, do a scaleFact.
+00871         else if( M1ScaleOnly && M2ScaleOnly )
+00872         {
+00873                 // same process for scaleUni or scaleAny.
+00874                 a11= m1.a11*m2.a11; a12= 0; a13= 0; 
+00875                 a21= 0; a22= m1.a22*m2.a22; a23= 0; 
+00876                 a31= 0; a32= 0; a33= m1.a33*m2.a33;
+00877         }
+00878         // If one of the matrix is a scaleOnly matrix, do a scale*Rot.
+00879         else if( M1ScaleOnly && !M2ScaleOnly )
+00880         {
+00881                 a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13;
+00882                 a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23;
+00883                 a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33;
+00884         }
+00885         else
+00886         {
+00887                 // This must be this case
+00888                 nlassert(!M1ScaleOnly && M2ScaleOnly);
+00889                 a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33;
+00890                 a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33;
+00891                 a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33;
+00892         }
+00893 
+00894         // If M1 has translate and M2 has projective, rotation is modified.
+00895         if( m1.hasTrans() && m2.hasProj())
+00896         {
+00897                 StateBit|= MAT_ROT|MAT_SCALEANY;
+00898 
+00899                 a11+= m1.a14*m2.a41;
+00900                 a12+= m1.a14*m2.a42;
+00901                 a13+= m1.a14*m2.a43;
+00902 
+00903                 a21+= m1.a24*m2.a41;
+00904                 a22+= m1.a24*m2.a42;
+00905                 a23+= m1.a24*m2.a43;
+00906 
+00907                 a31+= m1.a34*m2.a41;
+00908                 a32+= m1.a34*m2.a42;
+00909                 a33+= m1.a34*m2.a43;
+00910         }
+00911 
+00912         // Modify Scale.
+00913         if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
+00914         {
+00915                 // Must have correct Scale33
+00916                 m1.testExpandRot();
+00917                 m2.testExpandRot();
+00918                 Scale33= m1.Scale33*m2.Scale33;
+00919         }
+00920         else
+00921                 Scale33=1;
+00922 
+00923         // In every case, I am valid now!
+00924         StateBit|=MAT_VALIDROT;
+00925 
+00926 
+00927         // Build Trans part.
+00928         //=================
+00929         if( StateBit & MAT_TRANS )
+00930         {
+00931                 // Compose M2 part.
+00932                 if( M1Identity )
+00933                 {
+00934                         a14= m2.a14;
+00935                         a24= m2.a24;
+00936                         a34= m2.a34;
+00937                 }
+00938                 else if (M1ScaleOnly )
+00939                 {
+00940                         a14= m1.a11*m2.a14;
+00941                         a24= m1.a22*m2.a24;
+00942                         a34= m1.a33*m2.a34;
+00943                 }
+00944                 else
+00945                 {
+00946                         a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34;
+00947                         a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34;
+00948                         a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34;
+00949                 }
+00950                 // Compose M1 part.
+00951                 if(m1.StateBit & MAT_TRANS)
+00952                 {
+00953                         if(m2.StateBit & MAT_PROJ)
+00954                         {
+00955                                 a14+= m1.a14*m2.a44;
+00956                                 a24+= m1.a24*m2.a44;
+00957                                 a34+= m1.a34*m2.a44;
+00958                         }
+00959                         else
+00960                         {
+00961                                 a14+= m1.a14;
+00962                                 a24+= m1.a24;
+00963                                 a34+= m1.a34;
+00964                         }
+00965                 }
+00966         }
+00967 
+00968 
+00969         // Build Proj part.
+00970         //=================
+00971         if( StateBit & MAT_PROJ )
+00972         {
+00973                 // optimise nothing... (projection matrix are rare).
+00974                 m1.testExpandRot();
+00975                 m1.testExpandProj();
+00976                 m2.testExpandRot();
+00977                 m2.testExpandProj();
+00978                 a41= m1.a41*m2.a11 + m1.a42*m2.a21 + m1.a43*m2.a31 + m1.a44*m2.a41;
+00979                 a42= m1.a41*m2.a12 + m1.a42*m2.a22 + m1.a43*m2.a32 + m1.a44*m2.a42;
+00980                 a43= m1.a41*m2.a13 + m1.a42*m2.a23 + m1.a43*m2.a33 + m1.a44*m2.a43;
+00981                 a44= m1.a41*m2.a14 + m1.a42*m2.a24 + m1.a43*m2.a34 + m1.a44*m2.a44;
+00982                 // The proj is valid now
+00983                 StateBit|= MAT_VALIDPROJ;
+00984         }
+00985         else
+00986         {
+00987                 // Don't copy proj part, and leave MAT_VALIDPROJ not set
+00988         }
+00989 }
+00990 // ======================================================================================================
+00991 void            CMatrix::invert()
+00992 {
+00993 
+00994         *this= inverted();
+00995 }
+00996 
+00997 
+00998 // ======================================================================================================
+00999 void            CMatrix::transpose3x3()
+01000 {
+01001         if(hasRot())
+01002         {
+01003                 // swap values.
+01004                 swap(a12, a21);
+01005                 swap(a13, a31);
+01006                 swap(a32, a23);
+01007                 // Scale mode (none, uni, or any) is conserved. Scale33 too...
+01008         }
+01009 }
+01010 
+01011 // ======================================================================================================
+01012 void            CMatrix::transpose()
+01013 {
+01014         transpose3x3();
+01015         if(hasTrans() || hasProj())
+01016         {
+01017                 // if necessary, Get valid 0 on proj part.
+01018                 testExpandProj();
+01019                 // swap values
+01020                 swap(a41, a14);
+01021                 swap(a42, a24);
+01022                 swap(a43, a34);
+01023                 // swap StateBit flags, if not both were sets...
+01024                 if(!hasTrans() || !hasProj())
+01025                 {
+01026                         // swap StateBit flags (swap trans with proj).
+01027                         if(hasTrans())
+01028                         {
+01029                                 StateBit&= ~MAT_TRANS;
+01030                                 StateBit|= MAT_PROJ;
+01031                         }
+01032                         else
+01033                         {
+01034                                 StateBit&= ~MAT_PROJ;
+01035                                 StateBit|= MAT_TRANS;
+01036                         }
+01037                 }
+01038                 // reset validity. NB, maybe not usefull, but simpler, and bugfree.
+01039                 StateBit&= ~(MAT_VALIDPROJ);
+01040         }
+01041         // NB: if no Trans or no Proj, do nothing, so don't need to modify VALIDTRANS and VALIDPROJ too.
+01042 }
+01043 
+01044 
+01045 // ======================================================================================================
+01046 void    CMatrix::fastInvert33(CMatrix &ret) const
+01047 {
+01048         // Fast invert of 3x3 rot matrix.
+01049         // Work if no scale and if MAT_SCALEUNI. doesn't work if MAT_SCALEANY.
+01050 
+01051         if(StateBit & MAT_SCALEUNI)
+01052         {
+01053                 double  s,S;    // important for precision.
+01054                 // Must divide the matrix by 1/Scale 2 times, to set unit, and to have a Scale=1/Scale.
+01055                 S=1.0/Scale33;
+01056                 ret.Scale33= (float)S;
+01057                 s=S*S;
+01058                 // The matrix is a base, so just transpose it.
+01059                 ret.a11= (float)(a11*s); ret.a12= (float)(a21*s); ret.a13= (float)(a31*s);
+01060                 ret.a21= (float)(a12*s); ret.a22= (float)(a22*s); ret.a23= (float)(a32*s);
+01061                 ret.a31= (float)(a13*s); ret.a32= (float)(a23*s); ret.a33= (float)(a33*s);
+01062         }
+01063         else
+01064         {
+01065                 ret.Scale33=1;
+01066                 // The matrix is a base, so just transpose it.
+01067                 ret.a11= a11; ret.a12= a21; ret.a13=a31;
+01068                 ret.a21= a12; ret.a22= a22; ret.a23=a32;
+01069                 ret.a31= a13; ret.a32= a23; ret.a33=a33;
+01070         }
+01071 
+01072         // 15 cycles if no scale.
+01073         // 35 cycles if scale.
+01074 }
+01075 // ======================================================================================================
+01076 bool    CMatrix::slowInvert33(CMatrix &ret) const
+01077 {
+01078         CVector invi,invj,invk;
+01079         CVector i,j,k;
+01080         double  s;
+01081 
+01082         i= getI();
+01083         j= getJ();
+01084         k= getK();
+01085         // Compute cofactors (minors *(-1)^(i+j)).
+01086         invi.x= j.y*k.z - k.y*j.z;
+01087         invi.y= j.z*k.x - k.z*j.x;
+01088         invi.z= j.x*k.y - k.x*j.y;
+01089         invj.x= k.y*i.z - i.y*k.z;
+01090         invj.y= k.z*i.x - i.z*k.x;
+01091         invj.z= k.x*i.y - i.x*k.y;
+01092         invk.x= i.y*j.z - j.y*i.z;
+01093         invk.y= i.z*j.x - j.z*i.x;
+01094         invk.z= i.x*j.y - j.x*i.y;
+01095         // compute determinant.
+01096         s= invi.x*i.x + invj.x*j.x + invk.x*k.x;
+01097         if(s==0)
+01098                 return false;
+01099         // Transpose the Comatrice, and divide by determinant.
+01100         s=1.0/s;
+01101         ret.a11= (float)(invi.x*s); ret.a12= (float)(invi.y*s); ret.a13= (float)(invi.z*s);
+01102         ret.a21= (float)(invj.x*s); ret.a22= (float)(invj.y*s); ret.a23= (float)(invj.z*s);
+01103         ret.a31= (float)(invk.x*s); ret.a32= (float)(invk.y*s); ret.a33= (float)(invk.z*s);
+01104 
+01105         return true;
+01106         // Roundly 82 cycles. (1Div=10 cycles).
+01107 }
+01108 // ======================================================================================================
+01109 bool    CMatrix::slowInvert44(CMatrix &ret) const
+01110 {
+01111         sint    i,j;
+01112         double  s;
+01113 
+01114         // Compute Cofactors
+01115         //==================
+01116         for(i=0;i<=3;i++)
+01117         {
+01118                 for(j=0;j<=3;j++)
+01119                 {
+01120                         sint    l1,l2,l3;
+01121                         sint    c1,c2,c3;
+01122                         getCofactIndex(i,l1,l2,l3);
+01123                         getCofactIndex(j,c1,c2,c3);
+01124 
+01125                         ret.mat(i,j)= 0;
+01126                         ret.mat(i,j)+= mat(l1,c1) * mat(l2,c2) * mat(l3,c3);
+01127                         ret.mat(i,j)+= mat(l1,c2) * mat(l2,c3) * mat(l3,c1);
+01128                         ret.mat(i,j)+= mat(l1,c3) * mat(l2,c1) * mat(l3,c2);
+01129 
+01130                         ret.mat(i,j)-= mat(l1,c1) * mat(l2,c3) * mat(l3,c2);
+01131                         ret.mat(i,j)-= mat(l1,c2) * mat(l2,c1) * mat(l3,c3);
+01132                         ret.mat(i,j)-= mat(l1,c3) * mat(l2,c2) * mat(l3,c1);
+01133 
+01134                         if( (i+j)&1 )
+01135                                 ret.mat(i,j)=-ret.mat(i,j);
+01136                 }
+01137         }
+01138 
+01139         // Compute determinant.
+01140         //=====================
+01141         s= ret.mat(0,0) * mat(0,0) + ret.mat(0,1) * mat(0,1) + ret.mat(0,2) * mat(0,2) + ret.mat(0,3) * mat(0,3);
+01142         if(s==0)
+01143                 return false;
+01144 
+01145         // Divide by determinant.
+01146         //=======================
+01147         s=1.0/s;
+01148         for(i=0;i<=3;i++)
+01149         {
+01150                 for(j=0;j<=3;j++)
+01151                         ret.mat(i,j)= (float)(ret.mat(i,j)*s);
+01152         }
+01153 
+01154         // Transpose the comatrice.
+01155         //=========================
+01156         for(i=0;i<=3;i++)
+01157         {
+01158                 for(j=i+1;j<=3;j++)
+01159                 {
+01160                         swap(ret.mat(i,j), ret.mat(j,i));
+01161                 }
+01162         }
+01163 
+01164         return true;
+01165 }
+01166 // ======================================================================================================
+01167 CMatrix         CMatrix::inverted() const
+01168 {
+01169 
+01170         CMatrix ret;
+01171 
+01172         // \todo yoyo: TODO_OPTIMIZE it...
+01173         testExpandRot();
+01174         testExpandProj();
+01175 
+01176         // Do a conventionnal 44 inversion.
+01177         //=================================
+01178         if(StateBit & MAT_PROJ)
+01179         {
+01180                 if(!slowInvert44(ret))
+01181                 {
+01182                         ret.identity();
+01183                         return ret;
+01184                 }
+01185 
+01186                 // Well, don't know what happens to matrix, so set all StateBit :).
+01187                 ret.StateBit= MAT_TRANS|MAT_ROT|MAT_SCALEANY|MAT_PROJ;
+01188 
+01189                 // Check Trans state.
+01190                 if(ret.a14!=0 || ret.a24!=0 || ret.a34!=0)
+01191                         ret.StateBit|= MAT_TRANS;
+01192                 else
+01193                         ret.StateBit&= ~MAT_TRANS;
+01194 
+01195                 // Check Proj state.
+01196                 if(ret.a41!=0 || ret.a42!=0 || ret.a43!=0 || ret.a44!=1)
+01197                         ret.StateBit|= MAT_PROJ;
+01198                 else
+01199                         ret.StateBit&= ~MAT_PROJ;
+01200         }
+01201 
+01202         // Do a speed 34 inversion.
+01203         //=========================
+01204         else
+01205         {
+01206                 // Invert the rotation part.
+01207                 if(StateBit & MAT_SCALEANY)
+01208                 {
+01209                         if(!slowInvert33(ret))
+01210                         {
+01211                                 ret.identity();
+01212                                 return ret;
+01213                         }
+01214                 }
+01215                 else
+01216                         fastInvert33(ret);
+01217                 // Scale33 is updated in fastInvert33().
+01218 
+01219                 // Invert the translation part.
+01220                 if(StateBit & MAT_TRANS)
+01221                 {
+01222                         // Invert the translation part.
+01223                         // This can only work if 4th line is 0 0 0 1.
+01224                         // formula: InvVp= InvVi*(-Vp.x) + InvVj*(-Vp.y) + InvVk*(-Vp.z)
+01225                         ret.a14= ret.a11*(-a14) + ret.a12*(-a24) + ret.a13*(-a34);
+01226                         ret.a24= ret.a21*(-a14) + ret.a22*(-a24) + ret.a23*(-a34);
+01227                         ret.a34= ret.a31*(-a14) + ret.a32*(-a24) + ret.a33*(-a34);
+01228                 }
+01229                 else
+01230                 {
+01231                         ret.a14= 0;
+01232                         ret.a24= 0;
+01233                         ret.a34= 0;
+01234                 }
+01235 
+01236                 // The projection part is unmodified.
+01237                 ret.a41= 0; ret.a42= 0; ret.a43= 0; ret.a44= 1;
+01238 
+01239                 // The matrix inverted keep the same state bits.
+01240                 ret.StateBit= StateBit;
+01241         }
+01242         
+01243         
+01244         return ret;
+01245 }
+01246 // ======================================================================================================
+01247 bool            CMatrix::normalize(TRotOrder ro)
+01248 {
+01249 
+01250         CVector ti,tj,tk;
+01251         ti= getI();
+01252         tj= getJ();
+01253         tk= getK();
+01254 
+01255         // \todo yoyo: TODO_OPTIMIZE it...
+01256         testExpandRot();
+01257 
+01258         // Normalize with help of ro
+01259         switch(ro)
+01260         {
+01261                 case XYZ:
+01262                         ti.normalize();
+01263                         tk= ti^tj;
+01264                         tk.normalize();
+01265                         tj= tk^ti;
+01266                         break;
+01267                 case XZY:
+01268                         ti.normalize();
+01269                         tj= tk^ti;
+01270                         tj.normalize();
+01271                         tk= ti^tj;
+01272                         break;
+01273                 case YXZ:
+01274                         tj.normalize();
+01275                         tk= ti^tj;
+01276                         tk.normalize();
+01277                         ti= tj^tk;
+01278                         break;
+01279                 case YZX:
+01280                         tj.normalize();
+01281                         ti= tj^tk;
+01282                         ti.normalize();
+01283                         tk= ti^tj;
+01284                         break;
+01285                 case ZXY:
+01286                         tk.normalize();
+01287                         tj= tk^ti;
+01288                         tj.normalize();
+01289                         ti= tj^tk;
+01290                         break;
+01291                 case ZYX:
+01292                         tk.normalize();
+01293                         ti= tj^tk;
+01294                         ti.normalize();
+01295                         tj= tk^ti;
+01296                         break;
+01297         }
+01298 
+01299         // Check, and set result.
+01300         if( ti.isNull() || tj.isNull() || tk.isNull() )
+01301                 return false;
+01302         a11= ti.x; a12= tj.x; a13= tk.x; 
+01303         a21= ti.y; a22= tj.y; a23= tk.y; 
+01304         a31= ti.z; a32= tj.z; a33= tk.z; 
+01305         // Scale is reseted.
+01306         StateBit&= ~(MAT_SCALEUNI|MAT_SCALEANY);
+01307         // Rot is setup...
+01308         StateBit|= MAT_ROT;
+01309         Scale33=1;
+01310 
+01311         return true;
+01312 }
+01313 
+01314 
+01315 // ======================================================================================================
+01316 // ======================================================================================================
+01317 // ======================================================================================================
+01318 
+01319 
+01320 // ======================================================================================================
+01321 CVector         CMatrix::mulVector(const CVector &v) const
+01322 {
+01323 
+01324         CVector ret;
+01325 
+01326         if( hasRot() )
+01327         {
+01328                 ret.x= a11*v.x + a12*v.y + a13*v.z;
+01329                 ret.y= a21*v.x + a22*v.y + a23*v.z;
+01330                 ret.z= a31*v.x + a32*v.y + a33*v.z;
+01331                 return ret;
+01332         }
+01333         else
+01334                 return v;
+01335 }
+01336 
+01337 // ======================================================================================================
+01338 CVector         CMatrix::mulPoint(const CVector &v) const
+01339 {
+01340 
+01341         CVector ret;
+01342 
+01343         if( hasRot() )
+01344         {
+01345                 ret.x= a11*v.x + a12*v.y + a13*v.z;
+01346                 ret.y= a21*v.x + a22*v.y + a23*v.z;
+01347                 ret.z= a31*v.x + a32*v.y + a33*v.z;
+01348         }
+01349         else
+01350         {
+01351                 ret= v;
+01352         }
+01353         if( hasTrans() )
+01354         {
+01355                 ret.x+= a14;
+01356                 ret.y+= a24;
+01357                 ret.z+= a34;
+01358         }
+01359 
+01360         return ret;
+01361 }
+01362 
+01363 
+01364 /*
+01365  * Multiply
+01366  */
+01367 CVectorH        CMatrix::operator*(const CVectorH& v) const
+01368 {
+01369 
+01370         CVectorH ret;
+01371 
+01372         // \todo yoyo: TODO_OPTIMIZE it...
+01373         testExpandRot();
+01374         testExpandProj();
+01375 
+01376         ret.x= a11*v.x + a12*v.y + a13*v.z + a14*v.w;
+01377         ret.y= a21*v.x + a22*v.y + a23*v.z + a24*v.w;
+01378         ret.z= a31*v.x + a32*v.y + a33*v.z + a34*v.w;
+01379         ret.w= a41*v.x + a42*v.y + a43*v.z + a44*v.w;
+01380         return ret;
+01381 }
+01382 
+01383 
+01384 // ======================================================================================================
+01385 CPlane          operator*(const CPlane &p, const CMatrix &m)
+01386 {
+01387         // \todo yoyo: TODO_OPTIMIZE it...
+01388         m.testExpandRot();
+01389         m.testExpandProj();
+01390 
+01391 
+01392         CPlane  ret;
+01393 
+01394         if( m.StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY|MAT_PROJ) )
+01395         {
+01396                 // Compose with translation too.
+01397                 ret.a= p.a*m.a11 + p.b*m.a21 + p.c*m.a31 + p.d*m.a41;
+01398                 ret.b= p.a*m.a12 + p.b*m.a22 + p.c*m.a32 + p.d*m.a42;
+01399                 ret.c= p.a*m.a13 + p.b*m.a23 + p.c*m.a33 + p.d*m.a43;
+01400                 ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
+01401                 return ret;
+01402         }
+01403         else if( m.StateBit & MAT_TRANS )
+01404         {
+01405 
+01406                 // Compose just with a translation.
+01407                 ret.a= p.a;
+01408                 ret.b= p.b;
+01409                 ret.c= p.c;
+01410                 ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
+01411                 return ret;
+01412         }
+01413         else    // Identity!!
+01414                 return p;
+01415 }
+01416 
+01417 
+01418 // ======================================================================================================
+01419 // ======================================================================================================
+01420 // ======================================================================================================
+01421 
+01422 
+01423 // ======================================================================================================
+01424 void            CMatrix::setRot(const CQuat &quat)
+01425 {
+01426         // A quaternion do not have scale.
+01427         StateBit&= ~(MAT_ROT | MAT_SCALEANY|MAT_SCALEUNI);
+01428         Scale33= 1.0f;
+01429         if(quat.isIdentity())
+01430         {
+01431                 a11= 1; a12= 0; a13= 0; 
+01432                 a21= 0; a22= 1; a23= 0; 
+01433                 a31= 0; a32= 0; a33= 1; 
+01434         }
+01435         else
+01436         {
+01437                 StateBit|= MAT_ROT;
+01438                 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
+01439 
+01440                 // calculate coefficients
+01441                 x2 = quat.x + quat.x; y2 = quat.y + quat.y; 
+01442                 z2 = quat.z + quat.z;
+01443                 xx = quat.x * x2;   xy = quat.x * y2;   xz = quat.x * z2;
+01444                 yy = quat.y * y2;   yz = quat.y * z2;   zz = quat.z * z2;
+01445                 wx = quat.w * x2;   wy = quat.w * y2;   wz = quat.w * z2;
+01446 
+01447                 a11 = 1.0f - (yy + zz);
+01448                 a12 = xy - wz;
+01449                 a13 = xz + wy;
+01450 
+01451                 a21 = xy + wz;
+01452                 a22 = 1.0f - (xx + zz);
+01453                 a23 = yz - wx;
+01454 
+01455                 a31 = xz - wy;
+01456                 a32 = yz + wx;
+01457                 a33 = 1.0f - (xx + yy);
+01458         }
+01459 }
+01460 
+01461 
+01462 // ======================================================================================================
+01463 void            CMatrix::getRot(CQuat &quat) const
+01464 {
+01465         const CMatrix   *pmat= this;
+01466         CMatrix                 MatNormed;
+01467 
+01468 
+01469         // Rot Indentity?
+01470         if(! (StateBit & MAT_ROT))
+01471         {
+01472                 quat= CQuat::Identity;
+01473                 return;
+01474         }
+01475 
+01476         // Must normalize the matrix??
+01477         if(StateBit & (MAT_SCALEUNI | MAT_SCALEANY) )
+01478         {
+01479                 MatNormed= *this;
+01480                 MatNormed.normalize(ZYX);
+01481                 pmat= &MatNormed;
+01482         }
+01483 
+01484         // Compute quaternion.
+01485         float  tr, s, q[4];
+01486 
+01487         tr = pmat->a11 + pmat->a22 + pmat->a33;
+01488 
+01489         // check the diagonal
+01490         if (tr > 0.0)
+01491         {
+01492                 s = (float)sqrt (tr + 1.0f);
+01493                 quat.w = s / 2.0f;
+01494                 s = 0.5f / s;
+01495                 quat.x = (pmat->a32 - pmat->a23) * s;
+01496                 quat.y = (pmat->a13 - pmat->a31) * s;
+01497                 quat.z = (pmat->a21 - pmat->a12) * s;
+01498         }
+01499         else
+01500         {               
+01501                 sint    i, j, k;
+01502                 sint    nxt[3] = {1, 2, 0};
+01503 
+01504                 // diagonal is negative
+01505                 i = 0;
+01506                 if (pmat->a22 > pmat->a11) i = 1;
+01507                 if (pmat->a33 > pmat->mat(i,i) ) i = 2;
+01508                 j = nxt[i];
+01509                 k = nxt[j];
+01510 
+01511                 s = (float) sqrt (  (pmat->mat(i,i) - (pmat->mat(j,j) + pmat->mat(k,k)) )   + 1.0);
+01512 
+01513                 q[i] = s * 0.5f;
+01514 
+01515                 if (s != 0.0f) s = 0.5f / s;
+01516 
+01517                 q[j] = (pmat->mat(j,i) + pmat->mat(i,j)) * s;
+01518                 q[k] = (pmat->mat(k,i) + pmat->mat(i,k)) * s;
+01519                 q[3] =   (pmat->mat(k,j) - pmat->mat(j,k)) * s;
+01520 
+01521                 quat.x = q[0];
+01522                 quat.y = q[1];
+01523                 quat.z = q[2];
+01524                 quat.w = q[3];
+01525         }
+01526 
+01527 }
+01528 
+01529 
+01530 // ======================================================================================================
+01531 // ======================================================================================================
+01532 // ======================================================================================================
+01533 
+01534 
+01535 // ======================================================================================================
+01536 void            CMatrix::serial(IStream &f)
+01537 {
+01538         // Use versionning, maybe for futur improvement.
+01539         (void)f.serialVersion(0);
+01540 
+01541         if(f.isReading())
+01542                 identity();
+01543         f.serial(StateBit);
+01544         f.serial(Scale33);
+01545         if( hasRot() )
+01546         {
+01547                 f.serial(a11, a12, a13);
+01548                 f.serial(a21, a22, a23);
+01549                 f.serial(a31, a32, a33);
+01550         }
+01551         if( hasTrans() )
+01552         {
+01553                 f.serial(a14, a24, a34);
+01554         }
+01555         else if(f.isReading())
+01556         {
+01557                 // must reset because Pos must always be valid
+01558                 a14= a24= a34= 0;
+01559         }
+01560         if( hasProj() )
+01561         {
+01562                 f.serial(a41, a42, a43, a44);
+01563         }
+01564 }
+01565 
+01566 
+01567 
+01568 
+01569 }
+01570 
+
matrix.cpp
