# Home    # nevrax.com   
Nevrax
Nevrax.org
#News
#Mailing-list
#Documentation
#CVS
#Bugs
#License
Docs
 
Documentation  
Main Page   Namespace List   Class Hierarchy   Alphabetical List   Compound List   File List   Namespace Members   Compound Members   File Members   Related Pages   Search  

matrix.cpp

Go to the documentation of this file.
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