Home

nevrax.com

Nevrax.org News Mailing-list Documentation CVS Bugs License

Documentation                       Search   matrix.cpp Go to the documentation of this file. /* Copyright, 2000 Nevrax Ltd. * * This file is part of NEVRAX NEL. * NEVRAX NEL is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2, or (at your option) * any later version. * NEVRAX NEL is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * General Public License for more details. * You should have received a copy of the GNU General Public License * along with NEVRAX NEL; see the file COPYING. If not, write to the * Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, * MA 02111-1307, USA. */ #include "stdmisc.h" #include "nel/misc/matrix.h" #include "nel/misc/plane.h" #include "nel/misc/debug.h" using namespace std; namespace NLMISC { // ====================================================================================================== const CMatrix CMatrix::Identity; // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // State Bits. #define MAT_TRANS 1 #define MAT_ROT 2 #define MAT_SCALEUNI 4 #define MAT_SCALEANY 8 #define MAT_PROJ 16 // Validity bits. These means that the part may be yet identity, but is valid in the floats. // NB: MAT_VALIDTRANS no more used for faster Pos access #define MAT_VALIDROT 64 #define MAT_VALIDPROJ 128 #define MAT_VALIDALL (MAT_VALIDROT | MAT_VALIDPROJ) // The identity is nothing. #define MAT_IDENTITY 0 // Matrix elements. #define a11 M[0] #define a21 M[1] #define a31 M[2] #define a41 M[3] #define a12 M[4] #define a22 M[5] #define a32 M[6] #define a42 M[7] #define a13 M[8] #define a23 M[9] #define a33 M[10] #define a43 M[11] #define a14 M[12] #define a24 M[13] #define a34 M[14] #define a44 M[15] // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== bool CMatrix::hasScalePart() const { return (StateBit&(MAT_SCALEUNI|MAT_SCALEANY))!=0; } bool CMatrix::hasProjectionPart() const { return (StateBit&MAT_PROJ)!=0; } bool CMatrix::hasScaleUniform() const { return (StateBit&MAT_SCALEUNI)!=0 && (StateBit&MAT_SCALEANY)==0; } float CMatrix::getScaleUniform() const { if(hasScaleUniform()) return Scale33; else return 1; } // ====================================================================================================== inline bool CMatrix::hasRot() const { return (StateBit&(MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY))!=0; } inline bool CMatrix::hasTrans() const { return (StateBit&MAT_TRANS)!=0; } inline bool CMatrix::hasProj() const { return (StateBit&MAT_PROJ)!=0; } inline bool CMatrix::hasAll() const { return (hasRot() && hasTrans() && hasProj()); } inline void CMatrix::testExpandRot() const { if(hasRot()) return; if(!(StateBit&MAT_VALIDROT)) { CMatrix *self= const_cast<CMatrix*>(this); self->StateBit|=MAT_VALIDROT; self->a11= 1; self->a12=0; self->a13=0; self->a21= 0; self->a22=1; self->a23=0; self->a31= 0; self->a32=0; self->a33=1; self->Scale33= 1; } } inline void CMatrix::testExpandProj() const { if(hasProj()) return; if(!(StateBit&MAT_VALIDPROJ)) { CMatrix *self= const_cast<CMatrix*>(this); self->StateBit|=MAT_VALIDPROJ; self->a41=0; self->a42=0; self->a43=0; self->a44=1; } } // ====================================================================================================== CMatrix::CMatrix(const CMatrix &m) { (*this)= m; } // ====================================================================================================== CMatrix &CMatrix::operator=(const CMatrix &m) { StateBit= m.StateBit & ~MAT_VALIDALL; if(hasAll()) { memcpy(M, m.M, 16*sizeof(float)); Scale33= m.Scale33; } else { if(hasRot()) { memcpy(&a11, &m.a11, 3*sizeof(float)); memcpy(&a12, &m.a12, 3*sizeof(float)); memcpy(&a13, &m.a13, 3*sizeof(float)); Scale33= m.Scale33; } if(hasProj()) { a41= m.a41; a42= m.a42; a43= m.a43; a44= m.a44; } // Must always copy Trans part. memcpy(&a14, &m.a14, 3*sizeof(float)); } return *this; } // ====================================================================================================== void CMatrix::identity() { StateBit= MAT_IDENTITY; // Reset just Pos because must always be valid for faster getPos() a14= a24= a34= 0; // For optimisation it would be usefull to keep MAT_VALID states. // But this slows identity(), and this may not be interesting... } // ====================================================================================================== void CMatrix::setRot(const CVector &i, const CVector &j, const CVector &k, bool hintNoScale) { StateBit|= MAT_ROT | MAT_SCALEANY; if(hintNoScale) StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI); a11= i.x; a12= j.x; a13= k.x; a21= i.y; a22= j.y; a23= k.y; a31= i.z; a32= j.z; a33= k.z; Scale33= 1.0f; } // ====================================================================================================== void CMatrix::setRot(const float m33[9], bool hintNoScale) { StateBit|= MAT_ROT | MAT_SCALEANY; if(hintNoScale) StateBit&= ~(MAT_SCALEANY|MAT_SCALEUNI); a11= m33[0]; a12= m33[3]; a13= m33[6]; a21= m33[1]; a22= m33[4]; a23= m33[7]; a31= m33[2]; a32= m33[5]; a33= m33[8]; Scale33= 1.0f; } // ====================================================================================================== void CMatrix::setRot(const CVector &v, TRotOrder ro) { CMatrix rot; rot.identity(); rot.rotate(v, ro); float m33[9]; rot.getRot(m33); setRot(m33, true); } // ====================================================================================================== void CMatrix::setRot(const CMatrix &matrix) { // copy rotpart statebit from other. StateBit&= ~(MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY); StateBit|= matrix.StateBit & (MAT_ROT | MAT_SCALEUNI | MAT_SCALEANY); // copy values. if(hasRot()) { a11= matrix.a11; a12= matrix.a12; a13= matrix.a13; a21= matrix.a21; a22= matrix.a22; a23= matrix.a23; a31= matrix.a31; a32= matrix.a32; a33= matrix.a33; // if has scale, copy from matrix. if(StateBit & MAT_SCALEUNI) Scale33= matrix.Scale33; } else { // we are rot identity, with undefined values. StateBit&= ~MAT_VALIDROT; } } // ====================================================================================================== void CMatrix::setPos(const CVector &v) { a14= v.x; a24= v.y; a34= v.z; if(a14!=0 || a24!=0 || a34!=0) StateBit|= MAT_TRANS; else // The trans is identity StateBit&= ~MAT_TRANS; } // ====================================================================================================== void CMatrix::movePos(const CVector &v) { a14+= v.x; a24+= v.y; a34+= v.z; if(a14!=0 || a24!=0 || a34!=0) StateBit|= MAT_TRANS; else // The trans is identity StateBit&= ~MAT_TRANS; } // ====================================================================================================== void CMatrix::setProj(const float proj[4]) { a41= proj[0]; a42= proj[1]; a43= proj[2]; a44= proj[3]; // Check Proj state. if(a41!=0 || a42!=0 || a43!=0 || a44!=1) StateBit|= MAT_PROJ; else { // The proj is identity, and is correcly setup! StateBit&= ~MAT_PROJ; StateBit|= MAT_VALIDPROJ; } } // ====================================================================================================== void CMatrix::resetProj() { a41= 0; a42= 0; a43= 0; a44= 1; // The proj is identity, and is correcly setup! StateBit&= ~MAT_PROJ; StateBit|= MAT_VALIDPROJ; } // ====================================================================================================== void CMatrix::set(const float m44[16]) { StateBit= MAT_IDENTITY; StateBit|= MAT_ROT | MAT_SCALEANY; memcpy(M, m44, 16*sizeof(float)); Scale33= 1.0f; // Check Trans state. if(a14!=0 || a24!=0 || a34!=0) StateBit|= MAT_TRANS; else // The trans is identity StateBit&= ~MAT_TRANS; // Check Proj state. if(a41!=0 || a42!=0 || a43!=0 || a44!=1) StateBit|= MAT_PROJ; else { // The proj is identity, and is correcly setup! StateBit&= ~MAT_PROJ; StateBit|= MAT_VALIDPROJ; } } // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== void CMatrix::getRot(CVector &i, CVector &j, CVector &k) const { if(hasRot()) { i.set(a11, a21, a31); j.set(a12, a22, a32); k.set(a13, a23, a33); } else { i.set(1, 0, 0); j.set(0, 1, 0); k.set(0, 0, 1); } } // ====================================================================================================== void CMatrix::getRot(float m33[9]) const { if(hasRot()) { m33[0]= a11; m33[1]= a21; m33[2]= a31; m33[3]= a12; m33[4]= a22; m33[5]= a32; m33[6]= a13; m33[7]= a23; m33[8]= a33; } else { m33[0]= 1; m33[1]= 0; m33[2]= 0; m33[3]= 0; m33[4]= 1; m33[5]= 0; m33[6]= 0; m33[7]= 0; m33[8]= 1; } } // ====================================================================================================== void CMatrix::getProj(float proj[4]) const { if(hasProj()) { proj[0]= a41; proj[1]= a42; proj[2]= a43; proj[3]= a44; } else { proj[0]= 0; proj[1]= 0; proj[2]= 0; proj[3]= 1; } } // ====================================================================================================== CVector CMatrix::getI() const { if(hasRot()) return CVector(a11, a21, a31); else return CVector(1, 0, 0); } // ====================================================================================================== CVector CMatrix::getJ() const { if(hasRot()) return CVector(a12, a22, a32); else return CVector(0, 1, 0); } // ====================================================================================================== CVector CMatrix::getK() const { if(hasRot()) return CVector(a13, a23, a33); else return CVector(0, 0, 1); } // ====================================================================================================== void CMatrix::get(float m44[16]) const { // \todo yoyo: TODO_OPTIMIZE_it. testExpandRot(); testExpandProj(); memcpy(m44, M, 16*sizeof(float)); } // ====================================================================================================== const float *CMatrix::get() const { testExpandRot(); testExpandProj(); return M; } /*// ====================================================================================================== CVector CMatrix::toEuler(TRotOrder ro) const { }*/ // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== void CMatrix::translate(const CVector &v) { // SetTrans. if( hasRot() ) { a14+= a11*v.x + a12*v.y + a13*v.z; a24+= a21*v.x + a22*v.y + a23*v.z; a34+= a31*v.x + a32*v.y + a33*v.z; } else { a14+= v.x; a24+= v.y; a34+= v.z; } // SetProj. if( hasProj() ) a44+= a41*v.x + a42*v.y + a43*v.z; // Check Trans. if(a14!=0 || a24!=0 || a34!=0) StateBit|= MAT_TRANS; else // The trans is identity, and is correcly setup! StateBit&= ~MAT_TRANS; } // ====================================================================================================== void CMatrix::rotateX(float a) { if(a==0) return; double ca,sa; ca=cos(a); sa=sin(a); // SetRot. if( hasRot() ) { float b12=a12, b22=a22, b32=a32; float b13=a13, b23=a23, b33=a33; a12= (float)(b12*ca + b13*sa); a22= (float)(b22*ca + b23*sa); a32= (float)(b32*ca + b33*sa); a13= (float)(b13*ca - b12*sa); a23= (float)(b23*ca - b22*sa); a33= (float)(b33*ca - b32*sa); } else { testExpandRot(); a12= 0.0f; a22= (float)ca; a32= (float)sa; a13= 0.0f; a23= (float)-sa; a33= (float)ca; } // SetProj. if( hasProj() ) { float b42=a42, b43=a43; a42= (float)(b42*ca + b43*sa); a43= (float)(b43*ca - b42*sa); } // set Rot. StateBit|= MAT_ROT; } // ====================================================================================================== void CMatrix::rotateY(float a) { if(a==0) return; double ca,sa; ca=cos(a); sa=sin(a); // SetRot. if( hasRot() ) { float b11=a11, b21=a21, b31=a31; float b13=a13, b23=a23, b33=a33; a11= (float)(b11*ca - b13*sa); a21= (float)(b21*ca - b23*sa); a31= (float)(b31*ca - b33*sa); a13= (float)(b13*ca + b11*sa); a23= (float)(b23*ca + b21*sa); a33= (float)(b33*ca + b31*sa); } else { testExpandRot(); a11= (float)ca; a21=0.0f; a31= (float)-sa; a13= (float)sa; a23=0.0f; a33= (float)ca; } // SetProj. if( hasProj() ) { float b41=a41, b43=a43; a41= (float)(b41*ca - b43*sa); a43= (float)(b43*ca + b41*sa); } // set Rot. StateBit|= MAT_ROT; } // ====================================================================================================== void CMatrix::rotateZ(float a) { if(a==0) return; double ca,sa; ca=cos(a); sa=sin(a); // SetRot. if( StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY) ) { float b11=a11, b21=a21, b31=a31; float b12=a12, b22=a22, b32=a32; a11= (float)(b11*ca + b12*sa); a21= (float)(b21*ca + b22*sa); a31= (float)(b31*ca + b32*sa); a12= (float)(b12*ca - b11*sa); a22= (float)(b22*ca - b21*sa); a32= (float)(b32*ca - b31*sa); } else { testExpandRot(); a11= (float)ca; a21= (float)sa; a31=0.0f; a12= (float)-sa; a22= (float)ca; a32=0.0f; } // SetProj. if( hasProj() ) { float b41=a41, b42=a42; a41= (float)(b41*ca + b42*sa); a42= (float)(b42*ca - b41*sa); } // set Rot. StateBit|= MAT_ROT; } // ====================================================================================================== void CMatrix::rotate(const CVector &v, TRotOrder ro) { CMatrix rot; rot.identity(); switch(ro) { case XYZ: rot.rotateX(v.x); rot.rotateY(v.y); rot.rotateZ(v.z); break; case XZY: rot.rotateX(v.x); rot.rotateZ(v.z); rot.rotateY(v.y); break; case YXZ: rot.rotateY(v.y); rot.rotateX(v.x); rot.rotateZ(v.z); break; case YZX: rot.rotateY(v.y); rot.rotateZ(v.z); rot.rotateX(v.x); break; case ZXY: rot.rotateZ(v.z); rot.rotateX(v.x); rot.rotateY(v.y); break; case ZYX: rot.rotateZ(v.z); rot.rotateY(v.y); rot.rotateX(v.x); break; } (*this)*= rot; } // ====================================================================================================== void CMatrix::rotate(const CQuat &quat) { CMatrix rot; rot.setRot(quat); (*this)*= rot; } // ====================================================================================================== void CMatrix::scale(float f) { if(f==1.0f) return; if(StateBit & MAT_SCALEANY) { scale(CVector(f,f,f)); } else { testExpandRot(); StateBit|= MAT_SCALEUNI; Scale33*=f; a11*= f; a12*=f; a13*=f; a21*= f; a22*=f; a23*=f; a31*= f; a32*=f; a33*=f; // SetProj. if( hasProj() ) { a41*=f; a42*=f; a43*=f; } } } // ====================================================================================================== void CMatrix::scale(const CVector &v) { if( v==CVector(1,1,1) ) return; if( !(StateBit & MAT_SCALEANY) && v.x==v.y && v.x==v.z) { scale(v.x); } else { testExpandRot(); StateBit|=MAT_SCALEANY; a11*= v.x; a12*=v.y; a13*=v.z; a21*= v.x; a22*=v.y; a23*=v.z; a31*= v.x; a32*=v.y; a33*=v.z; // SetProj. if( hasProj() ) { a41*=v.x; a42*=v.y; a43*=v.z; } } } // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // *************************************************************************** void CMatrix::setMulMatrixNoProj(const CMatrix &m1, const CMatrix &m2) { // Do *this= m1*m2 identity(); StateBit= (m1.StateBit | m2.StateBit) & (~MAT_PROJ); StateBit&= ~MAT_VALIDALL; // Build Rot part. //=============== bool M1Identity= ! m1.hasRot(); bool M2Identity= ! m2.hasRot(); bool M1ScaleOnly= ! (m1.StateBit & MAT_ROT); bool M2ScaleOnly= ! (m2.StateBit & MAT_ROT); bool MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly; // Manage the most common general case first (optim the if ): blending of two rotations. if( MGeneralCase ) { a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31; a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32; a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33; a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31; a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32; a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33; a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31; a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32; a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33; } // If one of the 3x3 matrix is an identity, just do a copy else if( M1Identity || M2Identity ) { // If both identity, then me too. if( M1Identity && M2Identity ) { // just expand me (important because validated below) testExpandRot(); } else { // Copy the non identity matrix. const CMatrix *c= M2Identity? &m1 : &m2; a11= c->a11; a12= c->a12; a13= c->a13; a21= c->a21; a22= c->a22; a23= c->a23; a31= c->a31; a32= c->a32; a33= c->a33; } } // If two 3x3 matrix are just scaleOnly matrix, do a scaleFact. else if( M1ScaleOnly && M2ScaleOnly ) { // same process for scaleUni or scaleAny. a11= m1.a11*m2.a11; a12= 0; a13= 0; a21= 0; a22= m1.a22*m2.a22; a23= 0; a31= 0; a32= 0; a33= m1.a33*m2.a33; } // If one of the matrix is a scaleOnly matrix, do a scale*Rot. else if( M1ScaleOnly && !M2ScaleOnly ) { a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13; a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23; a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33; } else { // This must be this case nlassert(!M1ScaleOnly && M2ScaleOnly); a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33; a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33; a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33; } // Modify Scale. if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) ) { // Must have correct Scale33 m1.testExpandRot(); m2.testExpandRot(); Scale33= m1.Scale33*m2.Scale33; } else Scale33=1; // In every case, I am valid now! StateBit|=MAT_VALIDROT; // Build Trans part. //================= if( StateBit & MAT_TRANS ) { // Compose M2 part. NB: always suppose MAT_TRANS, for optim consideration. // NB: the translation part is always valid, even if identity() if( M1Identity ) { a14= m2.a14 + m1.a14; a24= m2.a24 + m1.a24; a34= m2.a34 + m1.a34; } else if (M1ScaleOnly ) { a14= m1.a11*m2.a14 + m1.a14; a24= m1.a22*m2.a24 + m1.a24; a34= m1.a33*m2.a34 + m1.a34; } else { a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34 + m1.a14; a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34 + m1.a24; a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34 + m1.a34; } } } // *************************************************************************** void CMatrix::setMulMatrix(const CMatrix &m1, const CMatrix &m2) { // Do *this= m1*m2 identity(); StateBit= m1.StateBit | m2.StateBit; StateBit&= ~MAT_VALIDALL; // Build Rot part. //=============== bool M1Identity= ! m1.hasRot(); bool M2Identity= ! m2.hasRot(); bool M1ScaleOnly= ! (m1.StateBit & MAT_ROT); bool M2ScaleOnly= ! (m2.StateBit & MAT_ROT); bool MGeneralCase= !M1Identity && !M2Identity && !M1ScaleOnly && !M2ScaleOnly; // Manage the most common general case first (optim the if ): blending of two rotations. if( MGeneralCase ) { a11= m1.a11*m2.a11 + m1.a12*m2.a21 + m1.a13*m2.a31; a12= m1.a11*m2.a12 + m1.a12*m2.a22 + m1.a13*m2.a32; a13= m1.a11*m2.a13 + m1.a12*m2.a23 + m1.a13*m2.a33; a21= m1.a21*m2.a11 + m1.a22*m2.a21 + m1.a23*m2.a31; a22= m1.a21*m2.a12 + m1.a22*m2.a22 + m1.a23*m2.a32; a23= m1.a21*m2.a13 + m1.a22*m2.a23 + m1.a23*m2.a33; a31= m1.a31*m2.a11 + m1.a32*m2.a21 + m1.a33*m2.a31; a32= m1.a31*m2.a12 + m1.a32*m2.a22 + m1.a33*m2.a32; a33= m1.a31*m2.a13 + m1.a32*m2.a23 + m1.a33*m2.a33; } // If one of the 3x3 matrix is an identity, just do a copy else if( M1Identity || M2Identity ) { // If both identity, then me too. if( M1Identity && M2Identity ) { // just expand me (important because validated below) testExpandRot(); } else { // Copy the non identity matrix. const CMatrix *c= M2Identity? &m1 : &m2; a11= c->a11; a12= c->a12; a13= c->a13; a21= c->a21; a22= c->a22; a23= c->a23; a31= c->a31; a32= c->a32; a33= c->a33; } } // If two 3x3 matrix are just scaleOnly matrix, do a scaleFact. else if( M1ScaleOnly && M2ScaleOnly ) { // same process for scaleUni or scaleAny. a11= m1.a11*m2.a11; a12= 0; a13= 0; a21= 0; a22= m1.a22*m2.a22; a23= 0; a31= 0; a32= 0; a33= m1.a33*m2.a33; } // If one of the matrix is a scaleOnly matrix, do a scale*Rot. else if( M1ScaleOnly && !M2ScaleOnly ) { a11= m1.a11*m2.a11; a12= m1.a11*m2.a12; a13= m1.a11*m2.a13; a21= m1.a22*m2.a21; a22= m1.a22*m2.a22; a23= m1.a22*m2.a23; a31= m1.a33*m2.a31; a32= m1.a33*m2.a32; a33= m1.a33*m2.a33; } else { // This must be this case nlassert(!M1ScaleOnly && M2ScaleOnly); a11= m1.a11*m2.a11; a12= m1.a12*m2.a22; a13= m1.a13*m2.a33; a21= m1.a21*m2.a11; a22= m1.a22*m2.a22; a23= m1.a23*m2.a33; a31= m1.a31*m2.a11; a32= m1.a32*m2.a22; a33= m1.a33*m2.a33; } // If M1 has translate and M2 has projective, rotation is modified. if( m1.hasTrans() && m2.hasProj()) { StateBit|= MAT_ROT|MAT_SCALEANY; a11+= m1.a14*m2.a41; a12+= m1.a14*m2.a42; a13+= m1.a14*m2.a43; a21+= m1.a24*m2.a41; a22+= m1.a24*m2.a42; a23+= m1.a24*m2.a43; a31+= m1.a34*m2.a41; a32+= m1.a34*m2.a42; a33+= m1.a34*m2.a43; } // Modify Scale. if( (StateBit & MAT_SCALEUNI) && !(StateBit & MAT_SCALEANY) ) { // Must have correct Scale33 m1.testExpandRot(); m2.testExpandRot(); Scale33= m1.Scale33*m2.Scale33; } else Scale33=1; // In every case, I am valid now! StateBit|=MAT_VALIDROT; // Build Trans part. //================= if( StateBit & MAT_TRANS ) { // Compose M2 part. if( M1Identity ) { a14= m2.a14; a24= m2.a24; a34= m2.a34; } else if (M1ScaleOnly ) { a14= m1.a11*m2.a14; a24= m1.a22*m2.a24; a34= m1.a33*m2.a34; } else { a14= m1.a11*m2.a14 + m1.a12*m2.a24 + m1.a13*m2.a34; a24= m1.a21*m2.a14 + m1.a22*m2.a24 + m1.a23*m2.a34; a34= m1.a31*m2.a14 + m1.a32*m2.a24 + m1.a33*m2.a34; } // Compose M1 part. if(m1.StateBit & MAT_TRANS) { if(m2.StateBit & MAT_PROJ) { a14+= m1.a14*m2.a44; a24+= m1.a24*m2.a44; a34+= m1.a34*m2.a44; } else { a14+= m1.a14; a24+= m1.a24; a34+= m1.a34; } } } // Build Proj part. //================= if( StateBit & MAT_PROJ ) { // optimise nothing... (projection matrix are rare). m1.testExpandRot(); m1.testExpandProj(); m2.testExpandRot(); m2.testExpandProj(); a41= m1.a41*m2.a11 + m1.a42*m2.a21 + m1.a43*m2.a31 + m1.a44*m2.a41; a42= m1.a41*m2.a12 + m1.a42*m2.a22 + m1.a43*m2.a32 + m1.a44*m2.a42; a43= m1.a41*m2.a13 + m1.a42*m2.a23 + m1.a43*m2.a33 + m1.a44*m2.a43; a44= m1.a41*m2.a14 + m1.a42*m2.a24 + m1.a43*m2.a34 + m1.a44*m2.a44; // The proj is valid now StateBit|= MAT_VALIDPROJ; } else { // Don't copy proj part, and leave MAT_VALIDPROJ not set } } // ====================================================================================================== void CMatrix::invert() { *this= inverted(); } // ====================================================================================================== void CMatrix::transpose3x3() { if(hasRot()) { // swap values. swap(a12, a21); swap(a13, a31); swap(a32, a23); // Scale mode (none, uni, or any) is conserved. Scale33 too... } } // ====================================================================================================== void CMatrix::transpose() { transpose3x3(); if(hasTrans() || hasProj()) { // if necessary, Get valid 0 on proj part. testExpandProj(); // swap values swap(a41, a14); swap(a42, a24); swap(a43, a34); // swap StateBit flags, if not both were sets... if(!hasTrans() || !hasProj()) { // swap StateBit flags (swap trans with proj). if(hasTrans()) { StateBit&= ~MAT_TRANS; StateBit|= MAT_PROJ; } else { StateBit&= ~MAT_PROJ; StateBit|= MAT_TRANS; } } // reset validity. NB, maybe not usefull, but simpler, and bugfree. StateBit&= ~(MAT_VALIDPROJ); } // NB: if no Trans or no Proj, do nothing, so don't need to modify VALIDTRANS and VALIDPROJ too. } // ====================================================================================================== void CMatrix::fastInvert33(CMatrix &ret) const { // Fast invert of 3x3 rot matrix. // Work if no scale and if MAT_SCALEUNI. doesn't work if MAT_SCALEANY. if(StateBit & MAT_SCALEUNI) { double s,S; // important for precision. // Must divide the matrix by 1/Scale 2 times, to set unit, and to have a Scale=1/Scale. S=1.0/Scale33; ret.Scale33= (float)S; s=S*S; // The matrix is a base, so just transpose it. ret.a11= (float)(a11*s); ret.a12= (float)(a21*s); ret.a13= (float)(a31*s); ret.a21= (float)(a12*s); ret.a22= (float)(a22*s); ret.a23= (float)(a32*s); ret.a31= (float)(a13*s); ret.a32= (float)(a23*s); ret.a33= (float)(a33*s); } else { ret.Scale33=1; // The matrix is a base, so just transpose it. ret.a11= a11; ret.a12= a21; ret.a13=a31; ret.a21= a12; ret.a22= a22; ret.a23=a32; ret.a31= a13; ret.a32= a23; ret.a33=a33; } // 15 cycles if no scale. // 35 cycles if scale. } // ====================================================================================================== bool CMatrix::slowInvert33(CMatrix &ret) const { CVector invi,invj,invk; CVector i,j,k; double s; i= getI(); j= getJ(); k= getK(); // Compute cofactors (minors *(-1)^(i+j)). invi.x= j.y*k.z - k.y*j.z; invi.y= j.z*k.x - k.z*j.x; invi.z= j.x*k.y - k.x*j.y; invj.x= k.y*i.z - i.y*k.z; invj.y= k.z*i.x - i.z*k.x; invj.z= k.x*i.y - i.x*k.y; invk.x= i.y*j.z - j.y*i.z; invk.y= i.z*j.x - j.z*i.x; invk.z= i.x*j.y - j.x*i.y; // compute determinant. s= invi.x*i.x + invj.x*j.x + invk.x*k.x; if(s==0) return false; // Transpose the Comatrice, and divide by determinant. s=1.0/s; ret.a11= (float)(invi.x*s); ret.a12= (float)(invi.y*s); ret.a13= (float)(invi.z*s); ret.a21= (float)(invj.x*s); ret.a22= (float)(invj.y*s); ret.a23= (float)(invj.z*s); ret.a31= (float)(invk.x*s); ret.a32= (float)(invk.y*s); ret.a33= (float)(invk.z*s); return true; // Roundly 82 cycles. (1Div=10 cycles). } // ====================================================================================================== bool CMatrix::slowInvert44(CMatrix &ret) const { sint i,j; double s; // Compute Cofactors //================== for(i=0;i<=3;i++) { for(j=0;j<=3;j++) { sint l1,l2,l3; sint c1,c2,c3; getCofactIndex(i,l1,l2,l3); getCofactIndex(j,c1,c2,c3); ret.mat(i,j)= 0; ret.mat(i,j)+= mat(l1,c1) * mat(l2,c2) * mat(l3,c3); ret.mat(i,j)+= mat(l1,c2) * mat(l2,c3) * mat(l3,c1); ret.mat(i,j)+= mat(l1,c3) * mat(l2,c1) * mat(l3,c2); ret.mat(i,j)-= mat(l1,c1) * mat(l2,c3) * mat(l3,c2); ret.mat(i,j)-= mat(l1,c2) * mat(l2,c1) * mat(l3,c3); ret.mat(i,j)-= mat(l1,c3) * mat(l2,c2) * mat(l3,c1); if( (i+j)&1 ) ret.mat(i,j)=-ret.mat(i,j); } } // Compute determinant. //===================== 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); if(s==0) return false; // Divide by determinant. //======================= s=1.0/s; for(i=0;i<=3;i++) { for(j=0;j<=3;j++) ret.mat(i,j)= (float)(ret.mat(i,j)*s); } // Transpose the comatrice. //========================= for(i=0;i<=3;i++) { for(j=i+1;j<=3;j++) { swap(ret.mat(i,j), ret.mat(j,i)); } } return true; } // ====================================================================================================== CMatrix CMatrix::inverted() const { CMatrix ret; // \todo yoyo: TODO_OPTIMIZE it... testExpandRot(); testExpandProj(); // Do a conventionnal 44 inversion. //================================= if(StateBit & MAT_PROJ) { if(!slowInvert44(ret)) { ret.identity(); return ret; } // Well, don't know what happens to matrix, so set all StateBit :). ret.StateBit= MAT_TRANS|MAT_ROT|MAT_SCALEANY|MAT_PROJ; // Check Trans state. if(ret.a14!=0 || ret.a24!=0 || ret.a34!=0) ret.StateBit|= MAT_TRANS; else ret.StateBit&= ~MAT_TRANS; // Check Proj state. if(ret.a41!=0 || ret.a42!=0 || ret.a43!=0 || ret.a44!=1) ret.StateBit|= MAT_PROJ; else ret.StateBit&= ~MAT_PROJ; } // Do a speed 34 inversion. //========================= else { // Invert the rotation part. if(StateBit & MAT_SCALEANY) { if(!slowInvert33(ret)) { ret.identity(); return ret; } } else fastInvert33(ret); // Scale33 is updated in fastInvert33(). // Invert the translation part. if(StateBit & MAT_TRANS) { // Invert the translation part. // This can only work if 4th line is 0 0 0 1. // formula: InvVp= InvVi*(-Vp.x) + InvVj*(-Vp.y) + InvVk*(-Vp.z) ret.a14= ret.a11*(-a14) + ret.a12*(-a24) + ret.a13*(-a34); ret.a24= ret.a21*(-a14) + ret.a22*(-a24) + ret.a23*(-a34); ret.a34= ret.a31*(-a14) + ret.a32*(-a24) + ret.a33*(-a34); } else { ret.a14= 0; ret.a24= 0; ret.a34= 0; } // The projection part is unmodified. ret.a41= 0; ret.a42= 0; ret.a43= 0; ret.a44= 1; // The matrix inverted keep the same state bits. ret.StateBit= StateBit; } return ret; } // ====================================================================================================== bool CMatrix::normalize(TRotOrder ro) { CVector ti,tj,tk; ti= getI(); tj= getJ(); tk= getK(); // \todo yoyo: TODO_OPTIMIZE it... testExpandRot(); // Normalize with help of ro switch(ro) { case XYZ: ti.normalize(); tk= ti^tj; tk.normalize(); tj= tk^ti; break; case XZY: ti.normalize(); tj= tk^ti; tj.normalize(); tk= ti^tj; break; case YXZ: tj.normalize(); tk= ti^tj; tk.normalize(); ti= tj^tk; break; case YZX: tj.normalize(); ti= tj^tk; ti.normalize(); tk= ti^tj; break; case ZXY: tk.normalize(); tj= tk^ti; tj.normalize(); ti= tj^tk; break; case ZYX: tk.normalize(); ti= tj^tk; ti.normalize(); tj= tk^ti; break; } // Check, and set result. if( ti.isNull() || tj.isNull() || tk.isNull() ) return false; a11= ti.x; a12= tj.x; a13= tk.x; a21= ti.y; a22= tj.y; a23= tk.y; a31= ti.z; a32= tj.z; a33= tk.z; // Scale is reseted. StateBit&= ~(MAT_SCALEUNI|MAT_SCALEANY); // Rot is setup... StateBit|= MAT_ROT; Scale33=1; return true; } // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== CVector CMatrix::mulVector(const CVector &v) const { CVector ret; if( hasRot() ) { ret.x= a11*v.x + a12*v.y + a13*v.z; ret.y= a21*v.x + a22*v.y + a23*v.z; ret.z= a31*v.x + a32*v.y + a33*v.z; return ret; } else return v; } // ====================================================================================================== CVector CMatrix::mulPoint(const CVector &v) const { CVector ret; if( hasRot() ) { ret.x= a11*v.x + a12*v.y + a13*v.z; ret.y= a21*v.x + a22*v.y + a23*v.z; ret.z= a31*v.x + a32*v.y + a33*v.z; } else { ret= v; } if( hasTrans() ) { ret.x+= a14; ret.y+= a24; ret.z+= a34; } return ret; } /* * Multiply */ CVectorH CMatrix::operator*(const CVectorH& v) const { CVectorH ret; // \todo yoyo: TODO_OPTIMIZE it... testExpandRot(); testExpandProj(); ret.x= a11*v.x + a12*v.y + a13*v.z + a14*v.w; ret.y= a21*v.x + a22*v.y + a23*v.z + a24*v.w; ret.z= a31*v.x + a32*v.y + a33*v.z + a34*v.w; ret.w= a41*v.x + a42*v.y + a43*v.z + a44*v.w; return ret; } // ====================================================================================================== CPlane operator*(const CPlane &p, const CMatrix &m) { // \todo yoyo: TODO_OPTIMIZE it... m.testExpandRot(); m.testExpandProj(); CPlane ret; if( m.StateBit & (MAT_ROT|MAT_SCALEUNI|MAT_SCALEANY|MAT_PROJ) ) { // Compose with translation too. ret.a= p.a*m.a11 + p.b*m.a21 + p.c*m.a31 + p.d*m.a41; ret.b= p.a*m.a12 + p.b*m.a22 + p.c*m.a32 + p.d*m.a42; ret.c= p.a*m.a13 + p.b*m.a23 + p.c*m.a33 + p.d*m.a43; ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44; return ret; } else if( m.StateBit & MAT_TRANS ) { // Compose just with a translation. ret.a= p.a; ret.b= p.b; ret.c= p.c; ret.d= p.a*m.a14 + p.b*m.a24 + p.c*m.a34 + p.d*m.a44; return ret; } else // Identity!! return p; } // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== void CMatrix::setRot(const CQuat &quat) { // A quaternion do not have scale. StateBit&= ~(MAT_ROT | MAT_SCALEANY|MAT_SCALEUNI); Scale33= 1.0f; if(quat.isIdentity()) { a11= 1; a12= 0; a13= 0; a21= 0; a22= 1; a23= 0; a31= 0; a32= 0; a33= 1; } else { StateBit|= MAT_ROT; float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2; // calculate coefficients x2 = quat.x + quat.x; y2 = quat.y + quat.y; z2 = quat.z + quat.z; xx = quat.x * x2; xy = quat.x * y2; xz = quat.x * z2; yy = quat.y * y2; yz = quat.y * z2; zz = quat.z * z2; wx = quat.w * x2; wy = quat.w * y2; wz = quat.w * z2; a11 = 1.0f - (yy + zz); a12 = xy - wz; a13 = xz + wy; a21 = xy + wz; a22 = 1.0f - (xx + zz); a23 = yz - wx; a31 = xz - wy; a32 = yz + wx; a33 = 1.0f - (xx + yy); } } // ====================================================================================================== void CMatrix::getRot(CQuat &quat) const { const CMatrix *pmat= this; CMatrix MatNormed; // Rot Indentity? if(! (StateBit & MAT_ROT)) { quat= CQuat::Identity; return; } // Must normalize the matrix?? if(StateBit & (MAT_SCALEUNI | MAT_SCALEANY) ) { MatNormed= *this; MatNormed.normalize(ZYX); pmat= &MatNormed; } // Compute quaternion. float tr, s, q[4]; tr = pmat->a11 + pmat->a22 + pmat->a33; // check the diagonal if (tr > 0.0) { s = (float)sqrt (tr + 1.0f); quat.w = s / 2.0f; s = 0.5f / s; quat.x = (pmat->a32 - pmat->a23) * s; quat.y = (pmat->a13 - pmat->a31) * s; quat.z = (pmat->a21 - pmat->a12) * s; } else { sint i, j, k; sint nxt[3] = {1, 2, 0}; // diagonal is negative i = 0; if (pmat->a22 > pmat->a11) i = 1; if (pmat->a33 > pmat->mat(i,i) ) i = 2; j = nxt[i]; k = nxt[j]; s = (float) sqrt ( (pmat->mat(i,i) - (pmat->mat(j,j) + pmat->mat(k,k)) ) + 1.0); q[i] = s * 0.5f; if (s != 0.0f) s = 0.5f / s; q[j] = (pmat->mat(j,i) + pmat->mat(i,j)) * s; q[k] = (pmat->mat(k,i) + pmat->mat(i,k)) * s; q[3] = (pmat->mat(k,j) - pmat->mat(j,k)) * s; quat.x = q[0]; quat.y = q[1]; quat.z = q[2]; quat.w = q[3]; } } // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== // ====================================================================================================== void CMatrix::serial(IStream &f) { // Use versionning, maybe for futur improvement. (void)f.serialVersion(0); if(f.isReading()) identity(); f.serial(StateBit); f.serial(Scale33); if( hasRot() ) { f.serial(a11, a12, a13); f.serial(a21, a22, a23); f.serial(a31, a32, a33); } if( hasTrans() ) { f.serial(a14, a24, a34); } else if(f.isReading()) { // must reset because Pos must always be valid a14= a24= a34= 0; } if( hasProj() ) { f.serial(a41, a42, a43, a44); } } }