nel/src/misc/matrix.cpp

   1 // NeL - MMORPG Framework <http://dev.ryzom.com/projects/nel/>
   2 // Copyright (C) 2010  Winch Gate Property Limited
   3 //
   4 // This source file has been modified by the following contributors:
   5 // Copyright (C) 2014  Jan BOON (Kaetemi) <jan.boon@kaetemi.be>
   6 //
   7 // This program is free software: you can redistribute it and/or modify
   8 // it under the terms of the GNU Affero General Public License as
   9 // published by the Free Software Foundation, either version 3 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful,
  13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 // GNU Affero General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU Affero General Public License
  18 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
  19
  20 #include "stdmisc.h"
  21
  22 #include "nel/misc/matrix.h"
  23 #include "nel/misc/plane.h"
  24 #include "nel/misc/debug.h"
  25
  26 using namespace std;
  27
  28
  29 #ifdef DEBUG_NEW
  30         #define new DEBUG_NEW
  31 #endif
  32
  33 namespace       NLMISC
  34 {
  35
  36
  37 // ======================================================================================================
  38 const CMatrix   CMatrix::Identity;
  39
  40
  41 // ======================================================================================================
  42 // ======================================================================================================
  43 // ======================================================================================================
  44
  45
  46 // State Bits.
  47 #define MAT_TRANS               1
  48 #define MAT_ROT                 2
  49 #define MAT_SCALEUNI    4
  50 #define MAT_SCALEANY    8
  51 #define MAT_PROJ                16
  52 // Validity bits. These means that the part may be yet identity, but is valid in the floats.
  53 // NB: MAT_VALIDTRANS no more used for faster Pos access
  54 #define MAT_VALIDROT    64
  55 #define MAT_VALIDPROJ   128
  56 #define MAT_VALIDALL    (MAT_VALIDROT | MAT_VALIDPROJ)
  57 // The identity is nothing.
  58 #define MAT_IDENTITY    0
  59
  60
  61
  62 // Matrix elements.
  63 #define a11             M[0]
  64 #define a21             M[1]
  65 #define a31             M[2]
  66 #define a41             M[3]
  67 #define a12             M[4]
  68 #define a22             M[5]
  69 #define a32             M[6]
  70 #define a42             M[7]
  71 #define a13             M[8]
  72 #define a23             M[9]
  73 #define a33             M[10]
  74 #define a43             M[11]
  75 #define a14             M[12]
  76 #define a24             M[13]
  77 #define a34             M[14]
  78 #define a44             M[15]
  79
  80
  81
  82 // ======================================================================================================
  83 // ======================================================================================================
  84 // ======================================================================================================
  85
  86
  87
  88 // ======================================================================================================
  89 bool            CMatrix::hasScalePart() const
  90 {
  91         return (StateBit&(MAT_SCALEUNI|MAT_SCALEANY))!=0;
  92 }
  93 bool            CMatrix::hasProjectionPart() const
  94 {
  95         return (StateBit&MAT_PROJ)!=0;
  96 }
  97
  98
  99 bool            CMatrix::hasScaleUniform() const
 100 {
 101         return (StateBit & (MAT_SCALEUNI|MAT_SCALEANY))== MAT_SCALEUNI;
 102 }
 103 float           CMatrix::getScaleUniform() const
 104 {
 105         if(hasScaleUniform())
 106                 return Scale33;
 107         else
 108                 return 1;
 109 }
 110
 111
 112
 113 // ======================================================================================================
 114 inline bool     CMatrix::hasRot() const
 115 {
 116         return (StateBit&(MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY))!=0;
 117 }
 118 inline bool     CMatrix::hasTrans() const
 119 {
 120         return (StateBit&MAT_TRANS)!=0;
 121 }
 122 inline bool     CMatrix::hasProj() const
 123 {
 124         return (StateBit&MAT_PROJ)!=0;
 125 }
 126 inline bool     CMatrix::hasAll() const
 127 {
 128         return (hasRot() && hasTrans() && hasProj());
 129 }
 130
 131
 132 inline void CMatrix::testExpandRot() const
 133 {
 134         if(hasRot())
 135                 return;
 136         if(!(StateBit&MAT_VALIDROT))
 137         {
 138                 CMatrix *self= const_cast<CMatrix*>(this);
 139                 self->StateBit|=MAT_VALIDROT;
 140                 self->a11= 1; self->a12=0; self->a13=0;
 141                 self->a21= 0; self->a22=1; self->a23=0;
 142                 self->a31= 0; self->a32=0; self->a33=1;
 143                 self->Scale33= 1;
 144         }
 145 }
 146
 147 inline void CMatrix::testExpandProj() const
 148 {
 149         if(hasProj())
 150                 return;
 151         if(!(StateBit&MAT_VALIDPROJ))
 152         {
 153                 CMatrix *self= const_cast<CMatrix*>(this);
 154                 self->StateBit|=MAT_VALIDPROJ;
 155                 self->a41=0; self->a42=0; self->a43=0; self->a44=1;
 156         }
 157 }
 158
 159
 160 // ======================================================================================================
 161 CMatrix::CMatrix(const CMatrix &m)
 162 {
 163         (*this)= m;
 164 }
 165 // ======================================================================================================
 166 CMatrix         &CMatrix::operator=(const CMatrix &m)
 167 {
 168         StateBit= m.StateBit & ~MAT_VALIDALL;
 169         if(hasAll())
 170         {
 171                 memcpy(M, m.M, 16*sizeof(float));
 172                 Scale33= m.Scale33;
 173         }
 174         else
 175         {
 176                 if(hasRot())
 177                 {
 178                         memcpy(&a11, &m.a11, 3*sizeof(float));
 179                         memcpy(&a12, &m.a12, 3*sizeof(float));
 180                         memcpy(&a13, &m.a13, 3*sizeof(float));
 181                         Scale33= m.Scale33;
 182                 }
 183                 if(hasProj())
 184                 {
 185                         a41= m.a41;
 186                         a42= m.a42;
 187                         a43= m.a43;
 188                         a44= m.a44;
 189                 }
 190                 // Must always copy Trans part.
 191                 memcpy(&a14, &m.a14, 3*sizeof(float));
 192         }
 193         return *this;
 194 }
 195
 196
 197 // ======================================================================================================
 198 void            CMatrix::identity()
 199 {
 200         StateBit= MAT_IDENTITY;
 201         // Reset just Pos because must always be valid for faster getPos()
 202         a14= a24= a34= 0;
 203         // For optimisation it would be useful to keep MAT_VALID states.
 204         // But this slows identity(), and this may not be interesting...
 205 }
 206 // ======================================================================================================
 207 void            CMatrix::setRot(const CVector &i, const CVector &j, const CVector &k, bool hintNoScale)
 208 {
 209         StateBit|= MAT_ROT | MAT_SCALEANY;
 210         if(hintNoScale)
 211                 StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
 212         a11= i.x; a12= j.x; a13= k.x;
 213         a21= i.y; a22= j.y; a23= k.y;
 214         a31= i.z; a32= j.z; a33= k.z;
 215         Scale33= 1.0f;
 216 }
 217 // ======================================================================================================
 218 void            CMatrix::setRot(const float m33[9], bool hintNoScale)
 219 {
 220         StateBit|= MAT_ROT | MAT_SCALEANY;
 221         if(hintNoScale)
 222                 StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI);
 223         a11= m33[0]; a12= m33[3]; a13= m33[6];
 224         a21= m33[1]; a22= m33[4]; a23= m33[7];
 225         a31= m33[2]; a32= m33[5]; a33= m33[8];
 226         Scale33= 1.0f;
 227 }
 228 // ======================================================================================================
 229 void            CMatrix::setRot(const CVector &v, TRotOrder ro)
 230 {
 231         CMatrix         rot;
 232         rot.identity();
 233         rot.rotate(v, ro);
 234         float   m33[9];
 235         rot.getRot(m33);
 236         setRot(m33, true);
 237 }
 238
 239
 240 // ======================================================================================================
 241 void            CMatrix::setRot(const CMatrix &matrix)
 242 {
 243         // copy rotpart statebit from other.
 244         StateBit&= ~(MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
 245         StateBit|= matrix.StateBit & (MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY);
 246         // copy values.
 247         if(hasRot())
 248         {
 249                 a11= matrix.a11; a12= matrix.a12; a13= matrix.a13;
 250                 a21= matrix.a21; a22= matrix.a22; a23= matrix.a23;
 251                 a31= matrix.a31; a32= matrix.a32; a33= matrix.a33;
 252                 // if has scale uniform, copy from matrix.
 253                 if(hasScaleUniform())
 254                         Scale33= matrix.Scale33;
 255         }
 256         else
 257         {
 258                 // we are rot identity, with undefined values.
 259                 StateBit&= ~MAT_VALIDROT;
 260         }
 261 }
 262
 263
 264 // ======================================================================================================
 265 void            CMatrix::setPos(const CVector &v)
 266 {
 267         a14= v.x;
 268         a24= v.y;
 269         a34= v.z;
 270         if(a14!=0 || a24!=0 || a34!=0)
 271                 StateBit|= MAT_TRANS;
 272         else
 273                 // The trans is identity
 274                 StateBit&= ~MAT_TRANS;
 275 }
 276 // ======================================================================================================
 277 void            CMatrix::movePos(const CVector &v)
 278 {
 279         a14+= v.x;
 280         a24+= v.y;
 281         a34+= v.z;
 282         if(a14!=0 || a24!=0 || a34!=0)
 283                 StateBit|= MAT_TRANS;
 284         else
 285                 // The trans is identity
 286                 StateBit&= ~MAT_TRANS;
 287 }
 288 // ======================================================================================================
 289 void            CMatrix::setProj(const float proj[4])
 290 {
 291         a41= proj[0];
 292         a42= proj[1];
 293         a43= proj[2];
 294         a44= proj[3];
 295
 296         // Check Proj state.
 297         if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
 298                 StateBit|= MAT_PROJ;
 299         else
 300         {
 301                 // The proj is identity, and is correcly setup!
 302                 StateBit&= ~MAT_PROJ;
 303                 StateBit|= MAT_VALIDPROJ;
 304         }
 305 }
 306 // ======================================================================================================
 307 void            CMatrix::resetProj()
 308 {
 309         a41= 0;
 310         a42= 0;
 311         a43= 0;
 312         a44= 1;
 313         // The proj is identity, and is correcly setup!
 314         StateBit&= ~MAT_PROJ;
 315         StateBit|= MAT_VALIDPROJ;
 316 }
 317 // ======================================================================================================
 318 void            CMatrix::set(const float m44[16])
 319 {
 320         StateBit= MAT_IDENTITY;
 321
 322         StateBit|= MAT_ROT | MAT_SCALEANY;
 323         memcpy(M, m44, 16*sizeof(float));
 324         Scale33= 1.0f;
 325
 326         // Check Trans state.
 327         if(a14!=0 || a24!=0 || a34!=0)
 328                 StateBit|= MAT_TRANS;
 329         else
 330                 // The trans is identity
 331                 StateBit&= ~MAT_TRANS;
 332
 333         // Check Proj state.
 334         if(a41!=0 || a42!=0 || a43!=0 || a44!=1)
 335                 StateBit|= MAT_PROJ;
 336         else
 337         {
 338                 // The proj is identity, and is correcly setup!
 339                 StateBit&= ~MAT_PROJ;
 340                 StateBit|= MAT_VALIDPROJ;
 341         }
 342 }
 343
 344
 345 // ======================================================================================================
 346 // ======================================================================================================
 347 // ======================================================================================================
 348
 349
 350 // ======================================================================================================
 351 void            CMatrix::getRot(CVector &i, CVector &j, CVector &k) const
 352 {
 353         if(hasRot())
 354         {
 355                 i.set(a11, a21, a31);
 356                 j.set(a12, a22, a32);
 357                 k.set(a13, a23, a33);
 358         }
 359         else
 360         {
 361                 i.set(1, 0, 0);
 362                 j.set(0, 1, 0);
 363                 k.set(0, 0, 1);
 364         }
 365 }
 366 // ======================================================================================================
 367 void            CMatrix::getRot(float m33[9]) const
 368 {
 369         if(hasRot())
 370         {
 371                 m33[0]= a11;
 372                 m33[1]= a21;
 373                 m33[2]= a31;
 374
 375                 m33[3]= a12;
 376                 m33[4]= a22;
 377                 m33[5]= a32;
 378
 379                 m33[6]= a13;
 380                 m33[7]= a23;
 381                 m33[8]= a33;
 382         }
 383         else
 384         {
 385                 m33[0]= 1;
 386                 m33[1]= 0;
 387                 m33[2]= 0;
 388
 389                 m33[3]= 0;
 390                 m33[4]= 1;
 391                 m33[5]= 0;
 392
 393                 m33[6]= 0;
 394                 m33[7]= 0;
 395                 m33[8]= 1;
 396         }
 397 }
 398 // ======================================================================================================
 399 void            CMatrix::getProj(float proj[4]) const
 400 {
 401         if(hasProj())
 402         {
 403                 proj[0]= a41;
 404                 proj[1]= a42;
 405                 proj[2]= a43;
 406                 proj[3]= a44;
 407         }
 408         else
 409         {
 410                 proj[0]= 0;
 411                 proj[1]= 0;
 412                 proj[2]= 0;
 413                 proj[3]= 1;
 414         }
 415 }
 416 // ======================================================================================================
 417 CVector         CMatrix::getI() const
 418 {
 419         if(hasRot())
 420                 return CVector(a11, a21, a31);
 421         else
 422                 return CVector(1, 0, 0);
 423 }
 424 // ======================================================================================================
 425 CVector         CMatrix::getJ() const
 426 {
 427         if(hasRot())
 428                 return CVector(a12, a22, a32);
 429         else
 430                 return CVector(0, 1, 0);
 431 }
 432 // ======================================================================================================
 433 CVector         CMatrix::getK() const
 434 {
 435         if(hasRot())
 436                 return CVector(a13, a23, a33);
 437         else
 438                 return CVector(0, 0, 1);
 439 }
 440 // ======================================================================================================
 441 void            CMatrix::get(float m44[16]) const
 442 {
 443         testExpandRot();
 444         testExpandProj();
 445         memcpy(m44, M, 16*sizeof(float));
 446 }
 447 // ======================================================================================================
 448 const float *CMatrix::get() const
 449 {
 450         testExpandRot();
 451         testExpandProj();
 452         return M;
 453 }
 454 /*// ======================================================================================================
 455 CVector         CMatrix::toEuler(TRotOrder ro) const
 456 {
 457
 458 }*/
 459
 460
 461 // ======================================================================================================
 462 // ======================================================================================================
 463 // ======================================================================================================
 464
 465
 466 // ======================================================================================================
 467 void            CMatrix::translate(const CVector &v)
 468 {
 469         // SetTrans.
 470         if( hasRot() )
 471         {
 472                 a14+= a11*v.x + a12*v.y + a13*v.z;
 473                 a24+= a21*v.x + a22*v.y + a23*v.z;
 474                 a34+= a31*v.x + a32*v.y + a33*v.z;
 475         }
 476         else
 477         {
 478                 a14+= v.x;
 479                 a24+= v.y;
 480                 a34+= v.z;
 481         }
 482
 483         // SetProj.
 484         if( hasProj() )
 485                 a44+= a41*v.x + a42*v.y + a43*v.z;
 486
 487         // Check Trans.
 488         if(a14!=0 || a24!=0 || a34!=0)
 489                 StateBit|= MAT_TRANS;
 490         else
 491                 // The trans is identity, and is correcly setup!
 492                 StateBit&= ~MAT_TRANS;
 493 }
 494 // ======================================================================================================
 495 void            CMatrix::rotateX(float a)
 496 {
 497
 498         if(a==0)
 499                 return;
 500         double  ca,sa;
 501         ca=cos(a);
 502         sa=sin(a);
 503
 504         // SetRot.
 505         if( hasRot() )
 506         {
 507                 float   b12=a12, b22=a22, b32=a32;
 508                 float   b13=a13, b23=a23, b33=a33;
 509                 a12= (float)(b12*ca + b13*sa);
 510                 a22= (float)(b22*ca + b23*sa);
 511                 a32= (float)(b32*ca + b33*sa);
 512                 a13= (float)(b13*ca - b12*sa);
 513                 a23= (float)(b23*ca - b22*sa);
 514                 a33= (float)(b33*ca - b32*sa);
 515         }
 516         else
 517         {
 518                 testExpandRot();
 519                 a12= 0.0f; a22= (float)ca; a32= (float)sa;
 520                 a13= 0.0f; a23= (float)-sa; a33= (float)ca;
 521         }
 522
 523         // SetProj.
 524         if( hasProj() )
 525         {
 526                 float   b42=a42, b43=a43;
 527                 a42= (float)(b42*ca + b43*sa);
 528                 a43= (float)(b43*ca - b42*sa);
 529         }
 530
 531         // set Rot.
 532         StateBit|= MAT_ROT;
 533 }
 534 // ======================================================================================================
 535 void            CMatrix::rotateY(float a)
 536 {
 537
 538         if(a==0)
 539                 return;
 540         double  ca,sa;
 541         ca=cos(a);
 542         sa=sin(a);
 543
 544         // SetRot.
 545         if( hasRot() )
 546         {
 547                 float   b11=a11, b21=a21, b31=a31;
 548                 float   b13=a13, b23=a23, b33=a33;
 549                 a11= (float)(b11*ca - b13*sa);
 550                 a21= (float)(b21*ca - b23*sa);
 551                 a31= (float)(b31*ca - b33*sa);
 552                 a13= (float)(b13*ca + b11*sa);
 553                 a23= (float)(b23*ca + b21*sa);
 554                 a33= (float)(b33*ca + b31*sa);
 555         }
 556         else
 557         {
 558                 testExpandRot();
 559                 a11= (float)ca; a21=0.0f; a31= (float)-sa;
 560                 a13= (float)sa; a23=0.0f; a33= (float)ca;
 561         }
 562
 563         // SetProj.
 564         if( hasProj() )
 565         {
 566                 float   b41=a41, b43=a43;
 567                 a41= (float)(b41*ca - b43*sa);
 568                 a43= (float)(b43*ca + b41*sa);
 569         }
 570
 571         // set Rot.
 572         StateBit|= MAT_ROT;
 573 }
 574 // ======================================================================================================
 575 void            CMatrix::rotateZ(float a)
 576 {
 577
 578         if(a==0)
 579                 return;
 580         double  ca,sa;
 581         ca=cos(a);
 582         sa=sin(a);
 583
 584         // SetRot.
 585         if( StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY) )
 586         {
 587                 float   b11=a11, b21=a21, b31=a31;
 588                 float   b12=a12, b22=a22, b32=a32;
 589                 a11= (float)(b11*ca + b12*sa);
 590                 a21= (float)(b21*ca + b22*sa);
 591                 a31= (float)(b31*ca + b32*sa);
 592                 a12= (float)(b12*ca - b11*sa);
 593                 a22= (float)(b22*ca - b21*sa);
 594                 a32= (float)(b32*ca - b31*sa);
 595         }
 596         else
 597         {
 598                 testExpandRot();
 599                 a11= (float)ca; a21= (float)sa; a31=0.0f;
 600                 a12= (float)-sa; a22= (float)ca; a32=0.0f;
 601         }
 602
 603         // SetProj.
 604         if( hasProj() )
 605         {
 606                 float   b41=a41, b42=a42;
 607                 a41= (float)(b41*ca + b42*sa);
 608                 a42= (float)(b42*ca - b41*sa);
 609         }
 610
 611         // set Rot.
 612         StateBit|= MAT_ROT;
 613 }
 614 // ======================================================================================================
 615 void            CMatrix::rotate(const CVector &v, TRotOrder ro)
 616 {
 617         CMatrix         rot;
 618         rot.identity();
 619         switch(ro)
 620         {
 621                 case XYZ: rot.rotateX(v.x); rot.rotateY(v.y); rot.rotateZ(v.z); break;
 622                 case XZY: rot.rotateX(v.x); rot.rotateZ(v.z); rot.rotateY(v.y); break;
 623                 case YXZ: rot.rotateY(v.y); rot.rotateX(v.x); rot.rotateZ(v.z); break;
 624                 case YZX: rot.rotateY(v.y); rot.rotateZ(v.z); rot.rotateX(v.x); break;
 625                 case ZXY: rot.rotateZ(v.z); rot.rotateX(v.x); rot.rotateY(v.y); break;
 626                 case ZYX: rot.rotateZ(v.z); rot.rotateY(v.y); rot.rotateX(v.x); break;
 627         }
 628
 629         (*this)*= rot;
 630 }
 631
 632 // ======================================================================================================
 633 void            CMatrix::rotate(const CQuat &quat)
 634 {
 635         CMatrix         rot;
 636         rot.setRot(quat);
 637         (*this)*= rot;
 638 }
 639
 640 // ======================================================================================================
 641 void            CMatrix::scale(float f)
 642 {
 643
 644         if(f==1.0f) return;
 645         if(StateBit & MAT_SCALEANY)
 646         {
 647                 scale(CVector(f,f,f));
 648         }
 649         else
 650         {
 651                 testExpandRot();
 652                 StateBit|= MAT_SCALEUNI;
 653                 Scale33*=f;
 654                 a11*= f; a12*=f; a13*=f;
 655                 a21*= f; a22*=f; a23*=f;
 656                 a31*= f; a32*=f; a33*=f;
 657
 658                 // SetProj.
 659                 if( hasProj() )
 660                 {
 661                         a41*=f; a42*=f; a43*=f;
 662                 }
 663         }
 664 }
 665 // ======================================================================================================
 666 void            CMatrix::scale(const CVector &v)
 667 {
 668
 669         if( v==CVector(1,1,1) ) return;
 670         if( !(StateBit & MAT_SCALEANY) && v.x==v.y && v.x==v.z)
 671         {
 672                 scale(v.x);
 673         }
 674         else
 675         {
 676                 testExpandRot();
 677                 StateBit|=MAT_SCALEANY;
 678                 a11*= v.x; a12*=v.y; a13*=v.z;
 679                 a21*= v.x; a22*=v.y; a23*=v.z;
 680                 a31*= v.x; a32*=v.y; a33*=v.z;
 681
 682                 // SetProj.
 683                 if( hasProj() )
 684                 {
 685                         a41*=v.x;
 686                         a42*=v.y;
 687                         a43*=v.z;
 688                 }
 689         }
 690 }
 691
 692
 693 // ======================================================================================================
 694 // ======================================================================================================
 695 // ======================================================================================================
 696
 697
 698 // ***************************************************************************
 699 void            CMatrix::setMulMatrixNoProj(const CMatrix &m1, const CMatrix &m2)
 700 {
 701         /*
 702         For a fast MulMatrix, it appears to be better to not take State bits into account (no test/if() overhead)
 703         Just do heavy mul all the time (common case, and not so slow)
 704         */
 705
 706         // Ensure the src matrix have correct values in rot part
 707         m1.testExpandRot();
 708         m2.testExpandRot();
 709
 710         // Rot Mul
 711         a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
 712         a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
 713         a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
 714
 715         a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
 716         a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
 717         a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
 718
 719         a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
 720         a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
 721         a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
 722
 723         // Trans mul
 724         a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34 + m1.a14;
 725         a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34 + m1.a24;
 726         a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34 + m1.a34;
 727
 728         // Setup no proj at all, and force valid rot (still may be identity, but 0/1 are filled)
 729         StateBit= (m1.StateBit | m2.StateBit | MAT_VALIDROT) & ~(MAT_PROJ|MAT_VALIDPROJ);
 730
 731         // Modify Scale. This test is important because Scale33 may be a #NAN if SCALEANY => avoid very slow mul.
 732         if( hasScaleUniform() )
 733                 Scale33= m1.Scale33*m2.Scale33;
 734         else
 735                 Scale33=1;
 736
 737 }
 738
 739
 740 // ***************************************************************************
 741 void            CMatrix::setMulMatrix(const CMatrix &m1, const CMatrix &m2)
 742 {
 743         // Do *this= m1*m2
 744         identity();
 745         StateBit= m1.StateBit | m2.StateBit;
 746         StateBit&= ~MAT_VALIDALL;
 747
 748         // Build Rot part.
 749         //===============
 750         bool    M1Identity= ! m1.hasRot();
 751         bool    M2Identity= ! m2.hasRot();
 752         bool    M1ScaleOnly= ! (m1.StateBit & MAT_ROT);
 753         bool    M2ScaleOnly= ! (m2.StateBit & MAT_ROT);
 754         bool    MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly;
 755
 756
 757         // Manage the most common general case first (optim the if ): blending of two rotations.
 758         if( MGeneralCase )
 759         {
 760                 a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31;
 761                 a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32;
 762                 a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33;
 763
 764                 a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31;
 765                 a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32;
 766                 a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33;
 767
 768                 a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31;
 769                 a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32;
 770                 a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33;
 771         }
 772         // If one of the 3x3 matrix is an identity, just do a copy
 773         else if( M1Identity || M2Identity )
 774         {
 775                 // If both identity, then me too.
 776                 if( M1Identity && M2Identity )
 777                 {
 778                         // just expand me (important because validated below)
 779                         testExpandRot();
 780                 }
 781                 else
 782                 {
 783                         // Copy the non identity matrix.
 784                         const CMatrix   *c= M2Identity? &m1 : &m2;
 785                         a11= c->a11; a12= c->a12; a13= c->a13;
 786                         a21= c->a21; a22= c->a22; a23= c->a23;
 787                         a31= c->a31; a32= c->a32; a33= c->a33;
 788                 }
 789         }
 790         // If two 3x3 matrix are just scaleOnly matrix, do a scaleFact.
 791         else if( M1ScaleOnly && M2ScaleOnly )
 792         {
 793                 // same process for scaleUni or scaleAny.
 794                 a11= m1.a11*m2.a11; a12= 0; a13= 0;
 795                 a21= 0; a22= m1.a22*m2.a22; a23= 0;
 796                 a31= 0; a32= 0; a33= m1.a33*m2.a33;
 797         }
 798         // If one of the matrix is a scaleOnly matrix, do a scale*Rot.
 799         else if( M1ScaleOnly && !M2ScaleOnly )
 800         {
 801                 a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13;
 802                 a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23;
 803                 a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33;
 804         }
 805         else
 806         {
 807                 // This must be this case
 808                 nlassert(!M1ScaleOnly && M2ScaleOnly);
 809                 a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33;
 810                 a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33;
 811                 a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33;
 812         }
 813
 814         // If M1 has translate and M2 has projective, rotation is modified.
 815         if( m1.hasTrans() && m2.hasProj())
 816         {
 817                 StateBit|= MAT_ROT|MAT_SCALEANY;
 818
 819                 a11+= m1.a14*m2.a41;
 820                 a12+= m1.a14*m2.a42;
 821                 a13+= m1.a14*m2.a43;
 822
 823                 a21+= m1.a24*m2.a41;
 824                 a22+= m1.a24*m2.a42;
 825                 a23+= m1.a24*m2.a43;
 826
 827                 a31+= m1.a34*m2.a41;
 828                 a32+= m1.a34*m2.a42;
 829                 a33+= m1.a34*m2.a43;
 830         }
 831
 832         // Modify Scale.
 833         if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) )
 834         {
 835                 // Must have correct Scale33
 836                 m1.testExpandRot();
 837                 m2.testExpandRot();
 838                 Scale33= m1.Scale33*m2.Scale33;
 839         }
 840         else
 841                 Scale33=1;
 842
 843         // In every case, I am valid now!
 844         StateBit|=MAT_VALIDROT;
 845
 846
 847         // Build Trans part.
 848         //=================
 849         if( StateBit & MAT_TRANS )
 850         {
 851                 // Compose M2 part.
 852                 if( M1Identity )
 853                 {
 854                         a14= m2.a14;
 855                         a24= m2.a24;
 856                         a34= m2.a34;
 857                 }
 858                 else if (M1ScaleOnly )
 859                 {
 860                         a14= m1.a11*m2.a14;
 861                         a24= m1.a22*m2.a24;
 862                         a34= m1.a33*m2.a34;
 863                 }
 864                 else
 865                 {
 866                         a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34;
 867                         a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34;
 868                         a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34;
 869                 }
 870                 // Compose M1 part.
 871                 if(m1.StateBit & MAT_TRANS)
 872                 {
 873                         if(m2.StateBit & MAT_PROJ)
 874                         {
 875                                 a14+= m1.a14*m2.a44;
 876                                 a24+= m1.a24*m2.a44;
 877                                 a34+= m1.a34*m2.a44;
 878                         }
 879                         else
 880                         {
 881                                 a14+= m1.a14;
 882                                 a24+= m1.a24;
 883                                 a34+= m1.a34;
 884                         }
 885                 }
 886         }
 887
 888
 889         // Build Proj part.
 890         //=================
 891         if( StateBit & MAT_PROJ )
 892         {
 893                 // optimize nothing... (projection matrix are rare).
 894                 m1.testExpandRot();
 895                 m1.testExpandProj();
 896                 m2.testExpandRot();
 897                 m2.testExpandProj();
 898                 a41= m1.a41*m2.a11 + m1.a42*m2.a21 + m1.a43*m2.a31 + m1.a44*m2.a41;
 899                 a42= m1.a41*m2.a12 + m1.a42*m2.a22 + m1.a43*m2.a32 + m1.a44*m2.a42;
 900                 a43= m1.a41*m2.a13 + m1.a42*m2.a23 + m1.a43*m2.a33 + m1.a44*m2.a43;
 901                 a44= m1.a41*m2.a14 + m1.a42*m2.a24 + m1.a43*m2.a34 + m1.a44*m2.a44;
 902                 // The proj is valid now
 903                 StateBit|= MAT_VALIDPROJ;
 904         }
 905         else
 906         {
 907                 // Don't copy proj part, and leave MAT_VALIDPROJ not set
 908         }
 909 }
 910 // ======================================================================================================
 911 void            CMatrix::invert()
 912 {
 913
 914         *this= inverted();
 915 }
 916
 917
 918 // ======================================================================================================
 919 void            CMatrix::transpose3x3()
 920 {
 921         if(hasRot())
 922         {
 923                 // swap values.
 924                 swap(a12, a21);
 925                 swap(a13, a31);
 926                 swap(a32, a23);
 927                 // Scale mode (none, uni, or any) is conserved. Scale33 too...
 928         }
 929 }
 930
 931 // ======================================================================================================
 932 void            CMatrix::transpose()
 933 {
 934         transpose3x3();
 935         if(hasTrans() || hasProj())
 936         {
 937                 // if necessary, Get valid 0 on proj part.
 938                 testExpandProj();
 939                 // swap values
 940                 swap(a41, a14);
 941                 swap(a42, a24);
 942                 swap(a43, a34);
 943                 // swap StateBit flags, if not both were sets...
 944                 if(!hasTrans() || !hasProj())
 945                 {
 946                         // swap StateBit flags (swap trans with proj).
 947                         if(hasTrans())
 948                         {
 949                                 StateBit&= ~MAT_TRANS;
 950                                 StateBit|= MAT_PROJ;
 951                         }
 952                         else
 953                         {
 954                                 StateBit&= ~MAT_PROJ;
 955                                 StateBit|= MAT_TRANS;
 956                         }
 957                 }
 958                 // reset validity. NB, maybe not useful, but simpler, and bugfree.
 959                 StateBit&= ~(MAT_VALIDPROJ);
 960         }
 961         // NB: if no Trans or no Proj, do nothing, so don't need to modify VALIDTRANS and VALIDPROJ too.
 962 }
 963
 964
 965 // ======================================================================================================
 966 bool    CMatrix::fastInvert33(CMatrix &ret) const
 967 {
 968         // Fast invert of 3x3 rot matrix.
 969         // Work if no scale and if MAT_SCALEUNI. doesn't work if MAT_SCALEANY.
 970
 971         if(StateBit & MAT_SCALEUNI)
 972         {
 973                 if (Scale33 == 0.f) return false;
 974                 double  s,S;    // important for precision.
 975                 // Must divide the matrix by 1/Scale 2 times, to set unit, and to have a Scale=1/Scale.
 976                 S=1.0/Scale33;
 977                 ret.Scale33= (float)S;
 978                 s=S*S;
 979                 // The matrix is a base, so just transpose it.
 980                 ret.a11= (float)(a11*s); ret.a12= (float)(a21*s); ret.a13= (float)(a31*s);
 981                 ret.a21= (float)(a12*s); ret.a22= (float)(a22*s); ret.a23= (float)(a32*s);
 982                 ret.a31= (float)(a13*s); ret.a32= (float)(a23*s); ret.a33= (float)(a33*s);
 983         }
 984         else
 985         {
 986                 ret.Scale33=1;
 987                 // The matrix is a base, so just transpose it.
 988                 ret.a11= a11; ret.a12= a21; ret.a13=a31;
 989                 ret.a21= a12; ret.a22= a22; ret.a23=a32;
 990                 ret.a31= a13; ret.a32= a23; ret.a33=a33;
 991         }
 992         return true;
 993         // 15 cycles if no scale.
 994         // 35 cycles if scale.
 995 }
 996 // ======================================================================================================
 997 bool    CMatrix::slowInvert33(CMatrix &ret) const
 998 {
 999         CVector invi,invj,invk;
1000         CVector i,j,k;
1001         double  s;
1002
1003         i= getI();
1004         j= getJ();
1005         k= getK();
1006         // Compute cofactors (minors *(-1)^(i+j)).
1007         invi.x= j.y*k.z - k.y*j.z;
1008         invi.y= j.z*k.x - k.z*j.x;
1009         invi.z= j.x*k.y - k.x*j.y;
1010         invj.x= k.y*i.z - i.y*k.z;
1011         invj.y= k.z*i.x - i.z*k.x;
1012         invj.z= k.x*i.y - i.x*k.y;
1013         invk.x= i.y*j.z - j.y*i.z;
1014         invk.y= i.z*j.x - j.z*i.x;
1015         invk.z= i.x*j.y - j.x*i.y;
1016         // compute determinant.
1017         s= invi.x*i.x + invj.x*j.x + invk.x*k.x;
1018         if(s==0)
1019                 return false;
1020         // Transpose the Comatrice, and divide by determinant.
1021         s=1.0/s;
1022         ret.a11= (float)(invi.x*s); ret.a12= (float)(invi.y*s); ret.a13= (float)(invi.z*s);
1023         ret.a21= (float)(invj.x*s); ret.a22= (float)(invj.y*s); ret.a23= (float)(invj.z*s);
1024         ret.a31= (float)(invk.x*s); ret.a32= (float)(invk.y*s); ret.a33= (float)(invk.z*s);
1025
1026         return true;
1027         // Roundly 82 cycles. (1Div=10 cycles).
1028 }
1029 // ======================================================================================================
1030 bool    CMatrix::slowInvert44(CMatrix &ret) const
1031 {
1032         sint    i,j;
1033         double  s;
1034
1035         // Compute Cofactors
1036         //==================
1037         for(i=0;i<=3;i++)
1038         {
1039                 for(j=0;j<=3;j++)
1040                 {
1041                         sint    l1=0,l2=0,l3=0;
1042                         sint    c1,c2,c3;
1043                         getCofactIndex(i,l1,l2,l3);
1044                         getCofactIndex(j,c1,c2,c3);
1045
1046                         ret.mat(i,j)= 0;
1047                         ret.mat(i,j)+= mat(l1,c1) * mat(l2,c2) * mat(l3,c3);
1048                         ret.mat(i,j)+= mat(l1,c2) * mat(l2,c3) * mat(l3,c1);
1049                         ret.mat(i,j)+= mat(l1,c3) * mat(l2,c1) * mat(l3,c2);
1050
1051                         ret.mat(i,j)-= mat(l1,c1) * mat(l2,c3) * mat(l3,c2);
1052                         ret.mat(i,j)-= mat(l1,c2) * mat(l2,c1) * mat(l3,c3);
1053                         ret.mat(i,j)-= mat(l1,c3) * mat(l2,c2) * mat(l3,c1);
1054
1055                         if( (i+j)&1 )
1056                                 ret.mat(i,j)=-ret.mat(i,j);
1057                 }
1058         }
1059
1060         // Compute determinant.
1061         //=====================
1062         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);
1063         if(s==0)
1064                 return false;
1065
1066         // Divide by determinant.
1067         //=======================
1068         s=1.0/s;
1069         for(i=0;i<=3;i++)
1070         {
1071                 for(j=0;j<=3;j++)
1072                         ret.mat(i,j)= (float)(ret.mat(i,j)*s);
1073         }
1074
1075         // Transpose the comatrice.
1076         //=========================
1077         for(i=0;i<=3;i++)
1078         {
1079                 for(j=i+1;j<=3;j++)
1080                 {
1081                         swap(ret.mat(i,j), ret.mat(j,i));
1082                 }
1083         }
1084
1085         return true;
1086 }
1087 // ======================================================================================================
1088 CMatrix         CMatrix::inverted() const
1089 {
1090
1091         CMatrix ret;
1092
1093         testExpandRot();
1094         testExpandProj();
1095
1096         // Do a conventionnal 44 inversion.
1097         //=================================
1098         if(StateBit & MAT_PROJ)
1099         {
1100                 if(!slowInvert44(ret))
1101                 {
1102                         ret.identity();
1103                         return ret;
1104                 }
1105
1106                 // Well, don't know what happens to matrix, so set all StateBit :).
1107                 ret.StateBit= MAT_TRANS|MAT_ROT|MAT_SCALEANY|MAT_PROJ;
1108
1109                 // Check Trans state.
1110                 if(ret.a14!=0 || ret.a24!=0 || ret.a34!=0)
1111                         ret.StateBit|= MAT_TRANS;
1112                 else
1113                         ret.StateBit&= ~MAT_TRANS;
1114
1115                 // Check Proj state.
1116                 if(ret.a41!=0 || ret.a42!=0 || ret.a43!=0 || ret.a44!=1)
1117                         ret.StateBit|= MAT_PROJ;
1118                 else
1119                         ret.StateBit&= ~MAT_PROJ;
1120         }
1121
1122         // Do a speed 34 inversion.
1123         //=========================
1124         else
1125         {
1126                 // Invert the rotation part.
1127                 if(StateBit & MAT_SCALEANY)
1128                 {
1129                         if(!slowInvert33(ret))
1130                         {
1131                                 ret.identity();
1132                                 return ret;
1133                         }
1134                 }
1135                 else
1136                 {
1137                         if (!fastInvert33(ret))
1138                         {
1139                                 ret.identity();
1140                                 return ret;
1141                         }
1142                 }
1143                 // Scale33 is updated in fastInvert33().
1144
1145                 // Invert the translation part.
1146                 if(StateBit & MAT_TRANS)
1147                 {
1148                         // Invert the translation part.
1149                         // This can only work if 4th line is 0 0 0 1.
1150                         // formula: InvVp= InvVi*(-Vp.x) + InvVj*(-Vp.y) + InvVk*(-Vp.z)
1151                         ret.a14= ret.a11*(-a14) + ret.a12*(-a24) + ret.a13*(-a34);
1152                         ret.a24= ret.a21*(-a14) + ret.a22*(-a24) + ret.a23*(-a34);
1153                         ret.a34= ret.a31*(-a14) + ret.a32*(-a24) + ret.a33*(-a34);
1154                 }
1155                 else
1156                 {
1157                         ret.a14= 0;
1158                         ret.a24= 0;
1159                         ret.a34= 0;
1160                 }
1161
1162                 // The projection part is unmodified.
1163                 ret.a41= 0; ret.a42= 0; ret.a43= 0; ret.a44= 1;
1164
1165                 // The matrix inverted keep the same state bits.
1166                 ret.StateBit= StateBit;
1167         }
1168
1169
1170         return ret;
1171 }
1172 // ======================================================================================================
1173 bool            CMatrix::normalize(TRotOrder ro)
1174 {
1175
1176         CVector ti,tj,tk;
1177         ti= getI();
1178         tj= getJ();
1179         tk= getK();
1180
1181         testExpandRot();
1182
1183         // Normalize with help of ro
1184         switch(ro)
1185         {
1186                 case XYZ:
1187                         ti.normalize();
1188                         tk= ti^tj;
1189                         tk.normalize();
1190                         tj= tk^ti;
1191                         break;
1192                 case XZY:
1193                         ti.normalize();
1194                         tj= tk^ti;
1195                         tj.normalize();
1196                         tk= ti^tj;
1197                         break;
1198                 case YXZ:
1199                         tj.normalize();
1200                         tk= ti^tj;
1201                         tk.normalize();
1202                         ti= tj^tk;
1203                         break;
1204                 case YZX:
1205                         tj.normalize();
1206                         ti= tj^tk;
1207                         ti.normalize();
1208                         tk= ti^tj;
1209                         break;
1210                 case ZXY:
1211                         tk.normalize();
1212                         tj= tk^ti;
1213                         tj.normalize();
1214                         ti= tj^tk;
1215                         break;
1216                 case ZYX:
1217                         tk.normalize();
1218                         ti= tj^tk;
1219                         ti.normalize();
1220                         tj= tk^ti;
1221                         break;
1222         }
1223
1224         // Check, and set result.
1225         if( ti.isNull() || tj.isNull() || tk.isNull() )
1226                 return false;
1227         a11= ti.x; a12= tj.x; a13= tk.x;
1228         a21= ti.y; a22= tj.y; a23= tk.y;
1229         a31= ti.z; a32= tj.z; a33= tk.z;
1230         // Scale is reseted.
1231         StateBit&= ~(MAT_SCALEUNI|MAT_SCALEANY);
1232         // Rot is setup...
1233         StateBit|= MAT_ROT;
1234         Scale33=1;
1235
1236         return true;
1237 }
1238
1239
1240 // ======================================================================================================
1241 // ======================================================================================================
1242 // ======================================================================================================
1243
1244
1245 // ======================================================================================================
1246 CVector         CMatrix::mulVector(const CVector &v) const
1247 {
1248
1249         CVector ret;
1250
1251         if( hasRot() )
1252         {
1253                 ret.x= a11*v.x + a12*v.y + a13*v.z;
1254                 ret.y= a21*v.x + a22*v.y + a23*v.z;
1255                 ret.z= a31*v.x + a32*v.y + a33*v.z;
1256                 return ret;
1257         }
1258         else
1259                 return v;
1260 }
1261
1262 // ======================================================================================================
1263 CVector         CMatrix::mulPoint(const CVector &v) const
1264 {
1265
1266         CVector ret;
1267
1268         if( hasRot() )
1269         {
1270                 ret.x= a11*v.x + a12*v.y + a13*v.z;
1271                 ret.y= a21*v.x + a22*v.y + a23*v.z;
1272                 ret.z= a31*v.x + a32*v.y + a33*v.z;
1273         }
1274         else
1275         {
1276                 ret= v;
1277         }
1278         if( hasTrans() )
1279         {
1280                 ret.x+= a14;
1281                 ret.y+= a24;
1282                 ret.z+= a34;
1283         }
1284
1285         return ret;
1286 }
1287
1288
1289 /*
1290  * Multiply
1291  */
1292 CVectorH        CMatrix::operator*(const CVectorH& v) const
1293 {
1294
1295         CVectorH ret;
1296
1297         testExpandRot();
1298         testExpandProj();
1299
1300         ret.x= a11*v.x + a12*v.y + a13*v.z + a14*v.w;
1301         ret.y= a21*v.x + a22*v.y + a23*v.z + a24*v.w;
1302         ret.z= a31*v.x + a32*v.y + a33*v.z + a34*v.w;
1303         ret.w= a41*v.x + a42*v.y + a43*v.z + a44*v.w;
1304         return ret;
1305 }
1306
1307
1308 // ======================================================================================================
1309 CPlane          operator*(const CPlane &p, const CMatrix &m)
1310 {
1311         m.testExpandRot();
1312         m.testExpandProj();
1313
1314         CPlane  ret;
1315
1316         if( m.StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY|MAT_PROJ) )
1317         {
1318                 // Compose with translation too.
1319                 ret.a= p.a*m.a11 + p.b*m.a21 + p.c*m.a31 + p.d*m.a41;
1320                 ret.b= p.a*m.a12 + p.b*m.a22 + p.c*m.a32 + p.d*m.a42;
1321                 ret.c= p.a*m.a13 + p.b*m.a23 + p.c*m.a33 + p.d*m.a43;
1322                 ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
1323                 return ret;
1324         }
1325         else if( m.StateBit & MAT_TRANS )
1326         {
1327
1328                 // Compose just with a translation.
1329                 ret.a= p.a;
1330                 ret.b= p.b;
1331                 ret.c= p.c;
1332                 ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44;
1333                 return ret;
1334         }
1335         else    // Identity!!
1336                 return p;
1337 }
1338
1339
1340 // ======================================================================================================
1341 // ======================================================================================================
1342 // ======================================================================================================
1343
1344
1345 // ======================================================================================================
1346 void            CMatrix::setRot(const CQuat &quat)
1347 {
1348         // A quaternion do not have scale.
1349         StateBit&= ~(MAT_ROT | MAT_SCALEANY|MAT_SCALEUNI);
1350         Scale33= 1.0f;
1351         if(quat.isIdentity())
1352         {
1353                 a11= 1; a12= 0; a13= 0;
1354                 a21= 0; a22= 1; a23= 0;
1355                 a31= 0; a32= 0; a33= 1;
1356         }
1357         else
1358         {
1359                 StateBit|= MAT_ROT;
1360                 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
1361
1362                 // calculate coefficients
1363                 x2 = quat.x + quat.x; y2 = quat.y + quat.y;
1364                 z2 = quat.z + quat.z;
1365                 xx = quat.x * x2;   xy = quat.x * y2;   xz = quat.x * z2;
1366                 yy = quat.y * y2;   yz = quat.y * z2;   zz = quat.z * z2;
1367                 wx = quat.w * x2;   wy = quat.w * y2;   wz = quat.w * z2;
1368
1369                 a11 = 1.0f - (yy + zz);
1370                 a12 = xy - wz;
1371                 a13 = xz + wy;
1372
1373                 a21 = xy + wz;
1374                 a22 = 1.0f - (xx + zz);
1375                 a23 = yz - wx;
1376
1377                 a31 = xz - wy;
1378                 a32 = yz + wx;
1379                 a33 = 1.0f - (xx + yy);
1380         }
1381 }
1382
1383
1384 // ======================================================================================================
1385 void            CMatrix::getRot(CQuat &quat) const
1386 {
1387         const CMatrix   *pmat= this;
1388         CMatrix                 MatNormed;
1389
1390
1391         // Rot Indentity?
1392         if(! (StateBit & MAT_ROT))
1393         {
1394                 quat= CQuat::Identity;
1395                 return;
1396         }
1397
1398         // Must normalize the matrix??
1399         if(StateBit & (MAT_SCALEUNI | MAT_SCALEANY) )
1400         {
1401                 MatNormed= *this;
1402                 MatNormed.normalize(ZYX);
1403                 pmat= &MatNormed;
1404         }
1405
1406         // Compute quaternion.
1407         float  tr, s, q[4];
1408
1409         tr = pmat->a11 + pmat->a22 + pmat->a33;
1410
1411         // check the diagonal
1412         if (tr > 0.0)
1413         {
1414                 s = (float)sqrt (tr + 1.0f);
1415                 quat.w = s / 2.0f;
1416                 s = 0.5f / s;
1417                 quat.x = (pmat->a32 - pmat->a23) * s;
1418                 quat.y = (pmat->a13 - pmat->a31) * s;
1419                 quat.z = (pmat->a21 - pmat->a12) * s;
1420         }
1421         else
1422         {
1423                 sint    i, j, k;
1424                 sint    nxt[3] = {1, 2, 0};
1425
1426                 // diagonal is negative
1427                 i = 0;
1428                 if (pmat->a22 > pmat->a11) i = 1;
1429                 if (pmat->a33 > pmat->mat(i,i) ) i = 2;
1430                 j = nxt[i];
1431                 k = nxt[j];
1432
1433                 s = (float) sqrt (  (pmat->mat(i,i) - (pmat->mat(j,j) + pmat->mat(k,k)) )   + 1.0);
1434
1435                 q[i] = s * 0.5f;
1436
1437                 if (s != 0.0f) s = 0.5f / s;
1438
1439                 q[j] = (pmat->mat(j,i) + pmat->mat(i,j)) * s;
1440                 q[k] = (pmat->mat(k,i) + pmat->mat(i,k)) * s;
1441                 q[3] =   (pmat->mat(k,j) - pmat->mat(j,k)) * s;
1442
1443                 quat.x = q[0];
1444                 quat.y = q[1];
1445                 quat.z = q[2];
1446                 quat.w = q[3];
1447         }
1448
1449 }
1450
1451
1452 // ======================================================================================================
1453 // ======================================================================================================
1454 // ======================================================================================================
1455
1456
1457 // ======================================================================================================
1458 inline  void    CMatrix::setScaleUni(float scale)
1459 {
1460         // A Scale matrix do not have rotation.
1461         StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
1462         StateBit|= MAT_SCALEUNI | MAT_VALIDROT;
1463         Scale33= scale;
1464         a11= scale; a12= 0; a13= 0;
1465         a21= 0; a22= scale; a23= 0;
1466         a31= 0; a32= 0; a33= scale;
1467 }
1468
1469 // ======================================================================================================
1470 void            CMatrix::setScale(float scale)
1471 {
1472         setScaleUni(scale);
1473 }
1474
1475
1476 // ======================================================================================================
1477 void            CMatrix::setScale(const CVector &v)
1478 {
1479         // actually a scale uniform?
1480         if(v.x==v.y && v.x==v.z)
1481                 setScaleUni(v.x);
1482
1483         // A Scale matrix do not have rotation.
1484         StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
1485         StateBit|= MAT_SCALEANY | MAT_VALIDROT;
1486         a11= v.x; a12= 0; a13= 0;
1487         a21= 0; a22= v.y; a23= 0;
1488         a31= 0; a32= 0; a33= v.z;
1489
1490 }
1491
1492
1493 // ======================================================================================================
1494 // ======================================================================================================
1495 // ======================================================================================================
1496
1497
1498 // ======================================================================================================
1499 void            CMatrix::serial(IStream &f)
1500 {
1501         // Use versionning, maybe for futur improvement.
1502         (void)f.serialVersion(0);
1503
1504         if(f.isReading())
1505                 identity();
1506         f.serial(StateBit);
1507         // avoid serial of random data
1508         if(!f.isReading() && !hasScaleUniform())
1509         {
1510                 float   fs= 1.f;
1511                 f.serial(fs);
1512         }
1513         else
1514                 f.serial(Scale33);
1515         if( hasRot() )
1516         {
1517                 f.serial(a11, a12, a13);
1518                 f.serial(a21, a22, a23);
1519                 f.serial(a31, a32, a33);
1520         }
1521         if( hasTrans() )
1522         {
1523                 f.serial(a14, a24, a34);
1524         }
1525         else if(f.isReading())
1526         {
1527                 // must reset because Pos must always be valid
1528                 a14= a24= a34= 0;
1529         }
1530         if( hasProj() )
1531         {
1532                 f.serial(a41, a42, a43, a44);
1533         }
1534 }
1535
1536
1537 // ======================================================================================================
1538 void            CMatrix::setArbitraryRotI(const CVector &idir)
1539 {
1540         // avoid gimbal lock. if idir == nearly K, use another second lead vector
1541         if( fabs(idir.z)<0.9f )
1542                 setRot(idir, CVector::J, CVector::K);
1543         else
1544                 setRot(idir, CVector::J, CVector::I);
1545         normalize(CMatrix::XZY);
1546 }
1547
1548 void            CMatrix::setArbitraryRotJ(const CVector &jdir)
1549 {
1550         // avoid gimbal lock. if jdir == nearly K, use another second lead vector
1551         if(fabs(jdir.z)<0.9f)
1552                 setRot(CVector::I, jdir, CVector::K);
1553         else
1554                 setRot(CVector::I, jdir, CVector::J);
1555         normalize(CMatrix::YZX);
1556 }
1557
1558 void            CMatrix::setArbitraryRotK(const CVector &kdir)
1559 {
1560         // avoid gimbal lock. if kdir == nearly I, use another second lead vector
1561         if( fabs(kdir.y)<0.9f )
1562                 setRot(CVector::I, CVector::J, kdir);
1563         else
1564                 setRot(CVector::I, CVector::K, kdir);
1565         normalize(CMatrix::ZYX);
1566 }
1567
1568
1569
1570
1571 }
1572