00001
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
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
00056
00057 #define MAT_VALIDROT 64
00058 #define MAT_VALIDPROJ 128
00059 #define MAT_VALIDALL (MAT_VALIDROT | MAT_VALIDPROJ)
00060
00061 #define MAT_IDENTITY 0
00062
00063
00064
00065
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
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
00204 a14= a24= a34= 0;
00205
00206
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
00246 StateBit&= ~(MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
00247 StateBit|= matrix.StateBit & (MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
00248
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
00255 if(StateBit & MAT_SCALEUNI)
00256 Scale33= matrix.Scale33;
00257 }
00258 else
00259 {
00260
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
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
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
00299 if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
00300 StateBit|= MAT_PROJ;
00301 else
00302 {
00303
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
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
00329 if(a14!=0 || a24!=0 || a34!=0)
00330 StateBit|= MAT_TRANS;
00331 else
00332
00333 StateBit&= ~MAT_TRANS;
00334
00335
00336 if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
00337 StateBit|= MAT_PROJ;
00338 else
00339 {
00340
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
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
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468
00469
00470 void CMatrix::translate(const CVector &v)
00471 {
00472
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
00487 if( hasProj() )
00488 a44+= a41*v.x + a42*v.y + a43*v.z;
00489
00490
00491 if(a14!=0 || a24!=0 || a34!=0)
00492 StateBit|= MAT_TRANS;
00493 else
00494
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
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
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
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
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
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
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
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
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
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
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
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
00705 identity();
00706 StateBit= (m1.StateBit | m2.StateBit) & (~MAT_PROJ);
00707 StateBit&= ~MAT_VALIDALL;
00708
00709
00710
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
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
00735 else if( M1Identity || M2Identity )
00736 {
00737
00738 if( M1Identity && M2Identity )
00739 {
00740
00741 testExpandRot();
00742 }
00743 else
00744 {
00745
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
00753 else if( M1ScaleOnly && M2ScaleOnly )
00754 {
00755
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
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
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
00777 if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
00778 {
00779
00780 m1.testExpandRot();
00781 m2.testExpandRot();
00782 Scale33= m1.Scale33*m2.Scale33;
00783 }
00784 else
00785 Scale33=1;
00786
00787
00788 StateBit|=MAT_VALIDROT;
00789
00790
00791
00792
00793 if( StateBit & MAT_TRANS )
00794 {
00795
00796
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
00824 identity();
00825 StateBit= m1.StateBit | m2.StateBit;
00826 StateBit&= ~MAT_VALIDALL;
00827
00828
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
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
00853 else if( M1Identity || M2Identity )
00854 {
00855
00856 if( M1Identity && M2Identity )
00857 {
00858
00859 testExpandRot();
00860 }
00861 else
00862 {
00863
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
00871 else if( M1ScaleOnly && M2ScaleOnly )
00872 {
00873
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
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
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
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
00913 if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
00914 {
00915
00916 m1.testExpandRot();
00917 m2.testExpandRot();
00918 Scale33= m1.Scale33*m2.Scale33;
00919 }
00920 else
00921 Scale33=1;
00922
00923
00924 StateBit|=MAT_VALIDROT;
00925
00926
00927
00928
00929 if( StateBit & MAT_TRANS )
00930 {
00931
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
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
00970
00971 if( StateBit & MAT_PROJ )
00972 {
00973
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
00983 StateBit|= MAT_VALIDPROJ;
00984 }
00985 else
00986 {
00987
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
01004 swap(a12, a21);
01005 swap(a13, a31);
01006 swap(a32, a23);
01007
01008 }
01009 }
01010
01011
01012 void CMatrix::transpose()
01013 {
01014 transpose3x3();
01015 if(hasTrans() || hasProj())
01016 {
01017
01018 testExpandProj();
01019
01020 swap(a41, a14);
01021 swap(a42, a24);
01022 swap(a43, a34);
01023
01024 if(!hasTrans() || !hasProj())
01025 {
01026
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
01039 StateBit&= ~(MAT_VALIDPROJ);
01040 }
01041
01042 }
01043
01044
01045
01046 void CMatrix::fastInvert33(CMatrix &ret) const
01047 {
01048
01049
01050
01051 if(StateBit & MAT_SCALEUNI)
01052 {
01053 double s,S;
01054
01055 S=1.0/Scale33;
01056 ret.Scale33= (float)S;
01057 s=S*S;
01058
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
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
01073
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
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
01096 s= invi.x*i.x + invj.x*j.x + invk.x*k.x;
01097 if(s==0)
01098 return false;
01099
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
01107 }
01108
01109 bool CMatrix::slowInvert44(CMatrix &ret) const
01110 {
01111 sint i,j;
01112 double s;
01113
01114
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
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
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
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
01173 testExpandRot();
01174 testExpandProj();
01175
01176
01177
01178 if(StateBit & MAT_PROJ)
01179 {
01180 if(!slowInvert44(ret))
01181 {
01182 ret.identity();
01183 return ret;
01184 }
01185
01186
01187 ret.StateBit= MAT_TRANS|MAT_ROT|MAT_SCALEANY|MAT_PROJ;
01188
01189
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
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
01203
01204 else
01205 {
01206
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
01218
01219
01220 if(StateBit & MAT_TRANS)
01221 {
01222
01223
01224
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
01237 ret.a41= 0; ret.a42= 0; ret.a43= 0; ret.a44= 1;
01238
01239
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
01256 testExpandRot();
01257
01258
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
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
01306 StateBit&= ~(MAT_SCALEUNI|MAT_SCALEANY);
01307
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
01366
01367 CVectorH CMatrix::operator*(const CVectorH& v) const
01368 {
01369
01370 CVectorH ret;
01371
01372
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
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
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
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
01414 return p;
01415 }
01416
01417
01418
01419
01420
01421
01422
01423
01424 void CMatrix::setRot(const CQuat &quat)
01425 {
01426
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
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
01470 if(! (StateBit & MAT_ROT))
01471 {
01472 quat= CQuat::Identity;
01473 return;
01474 }
01475
01476
01477 if(StateBit & (MAT_SCALEUNI | MAT_SCALEANY) )
01478 {
01479 MatNormed= *this;
01480 MatNormed.normalize(ZYX);
01481 pmat= &MatNormed;
01482 }
01483
01484
01485 float tr, s, q[4];
01486
01487 tr = pmat->a11 + pmat->a22 + pmat->a33;
01488
01489
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
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
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
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