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|MAT_SCALEANY))== MAT_SCALEUNI;
}
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 useful 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 uniform, copy from matrix.
        if(hasScaleUniform())
            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
{
    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)
{
    /*
    For a fast MulMatrix, it appears to be better to not take State bits into account (no test/if() overhead)
    Just do heavy mul all the time (common case, and not so slow)
    */

    // Ensure the src matrix have correct values in rot part
    m1.testExpandRot();
    m2.testExpandRot();

    // Rot Mul
    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;

    // Trans mul
    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;

    // Setup no proj at all, and force valid rot (still may be identity, but 0/1 are filled)
    StateBit= (m1.StateBit | m2.StateBit | MAT_VALIDROT) & ~(MAT_PROJ|MAT_VALIDPROJ);

    // Modify Scale. This test is important because Scale33 may be a #NAN if SCALEANY => avoid very slow mul.
    if( hasScaleUniform() )
        Scale33= m1.Scale33*m2.Scale33;
    else
        Scale33=1;

}


// ***************************************************************************
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 )
    {
        // optimize 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 useful, 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.
}


// ======================================================================================================
bool    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)
    {
        if (Scale33 == 0.f) return false;
        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;
    }
    return true;
    // 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=0,l2=0,l3=0;
            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;

    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
        {
            if (!fastInvert33(ret))
            {
                ret.identity();
                return 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();

    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;

    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)
{
    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];
    }

}


// ======================================================================================================
// ======================================================================================================
// ======================================================================================================


// ======================================================================================================
inline  void    CMatrix::setScaleUni(float scale)
{
    // A Scale matrix do not have rotation.
    StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
    StateBit|= MAT_SCALEUNI | MAT_VALIDROT;
    Scale33= scale;
    a11= scale; a12= 0; a13= 0;
    a21= 0; a22= scale; a23= 0;
    a31= 0; a32= 0; a33= scale;
}

// ======================================================================================================
void        CMatrix::setScale(float scale)
{
    setScaleUni(scale);
}


// ======================================================================================================
void        CMatrix::setScale(const CVector &v)
{
    // actually a scale uniform?
    if(v.x==v.y && v.x==v.z)
        setScaleUni(v.x);

    // A Scale matrix do not have rotation.
    StateBit&= ~(MAT_ROT | MAT_SCALEANY | MAT_SCALEUNI);
    StateBit|= MAT_SCALEANY | MAT_VALIDROT;
    a11= v.x; a12= 0; a13= 0;
    a21= 0; a22= v.y; a23= 0;
    a31= 0; a32= 0; a33= v.z;

}


// ======================================================================================================
// ======================================================================================================
// ======================================================================================================


// ======================================================================================================
void        CMatrix::serial(IStream &f)
{
    // Use versionning, maybe for futur improvement.
    (void)f.serialVersion(0);

    if(f.isReading())
        identity();
    f.serial(StateBit);
    // avoid serial of random data
    if(!f.isReading() && !hasScaleUniform())
    {
        float   fs= 1.f;
        f.serial(fs);
    }
    else
        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);
    }
}


// ======================================================================================================
void        CMatrix::setArbitraryRotI(const CVector &idir)
{
    // avoid gimbal lock. if idir == nearly K, use an other second lead vector
    if( fabs(idir.z)<0.9f )
        setRot(idir, CVector::J, CVector::K);
    else
        setRot(idir, CVector::J, CVector::I);
    normalize(CMatrix::XZY);
}

void        CMatrix::setArbitraryRotJ(const CVector &jdir)
{
    // avoid gimbal lock. if jdir == nearly K, use an other second lead vector
    if(fabs(jdir.z)<0.9f)
        setRot(CVector::I, jdir, CVector::K);
    else
        setRot(CVector::I, jdir, CVector::J);
    normalize(CMatrix::YZX);
}

void        CMatrix::setArbitraryRotK(const CVector &kdir)
{
    // avoid gimbal lock. if kdir == nearly I, use an other second lead vector
    if( fabs(kdir.y)<0.9f )
        setRot(CVector::I, CVector::J, kdir);
    else
        setRot(CVector::I, CVector::K, kdir);
    normalize(CMatrix::ZYX);
}




}