glc_matrix4x4.h

Go to the documentation of this file.

/****************************************************************************

 This file is part of the GLC-lib library.
 Copyright (C) 2005-2008 Laurent Ribon (laumaya@users.sourceforge.net)
 http://glc-lib.sourceforge.net

 GLC-lib is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 3 of the License, or
 (at your option) any later version.

 GLC-lib 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 Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with GLC-lib; if not, write to the Free Software
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA

*****************************************************************************/


#ifndef GLC_MATRIX4X4_H_
#define GLC_MATRIX4X4_H_

#include <QVector>
#include "glc_vector3d.h"

#include "../glc_config.h"



class GLC_LIB_EXPORT GLC_Matrix4x4
{
        friend class GLC_Vector3d;
public:
        enum
        {
                General= 0x0000,
                Direct= 0x0001,
                Indirect= 0x0002,
                Identity= 0x0003
        };

// Constructor
public:


        inline GLC_Matrix4x4();

        inline GLC_Matrix4x4(const GLC_Matrix4x4 &matrix)
        :m_Type(matrix.m_Type)
        {
                memcpy(m_Matrix, matrix.m_Matrix, sizeof(double) * 16);
        }

        inline GLC_Matrix4x4(const double *pArray)
        : m_Type(General)
        {
                memcpy(m_Matrix, pArray, sizeof(double) * 16);
        }

        inline GLC_Matrix4x4(const float *);

        inline GLC_Matrix4x4(const GLC_Vector3d &Vect, const double &dAngleRad);

        inline GLC_Matrix4x4(const GLC_Vector3d &Vect1, const GLC_Vector3d &Vect2);

        inline GLC_Matrix4x4(const GLC_Vector3d &Vect)
        {setMatTranslate(Vect);}

        inline GLC_Matrix4x4(const double Tx, const double Ty, const double Tz)
        {setMatTranslate(Tx, Ty, Tz);}



public:
        inline GLC_Matrix4x4& operator = (const GLC_Matrix4x4 &matrix);

        inline GLC_Matrix4x4 operator * (const GLC_Matrix4x4 &Mat) const;

        inline GLC_Vector3d operator * (const GLC_Vector3d &Vect) const;

        inline bool operator==(const GLC_Matrix4x4& mat) const;

        inline bool operator!=(const GLC_Matrix4x4& mat) const
        {return !operator==(mat);}




public:
        inline double determinant(void) const;


        inline const double* getData(void)
        {return m_Matrix;}

        inline const double* getData(void) const
        {return m_Matrix;}

        inline double* setData(void)
        {
                m_Type= General;
                return m_Matrix;
        }

        QVector<double> toEuler(void) const;

        QString toString() const;

        inline GLC_Matrix4x4 rotationMatrix() const;

        inline GLC_Matrix4x4 isometricMatrix() const;

        inline double scalingX() const
        {return GLC_Vector3d(m_Matrix[0], m_Matrix[1], m_Matrix[2]).length();}

        inline double scalingY() const
        {return GLC_Vector3d(m_Matrix[4], m_Matrix[5], m_Matrix[6]).length();}

        inline double scalingZ() const
        {return GLC_Vector3d(m_Matrix[8], m_Matrix[9], m_Matrix[10]).length();}

        inline GLC_Matrix4x4 inverted() const
        {return GLC_Matrix4x4(*this).invert();}

        inline int type() const
        {
                return m_Type;
        }

        inline bool isDirect() const
        {return (m_Type & Direct);}




public:
        inline GLC_Matrix4x4& setMatRot(const GLC_Vector3d &, const double &);

        inline GLC_Matrix4x4& setMatRot(const GLC_Vector3d &, const GLC_Vector3d &);

        inline GLC_Matrix4x4& setMatTranslate(const GLC_Vector3d &);

        inline GLC_Matrix4x4& setMatTranslate(const double, const double, const double);

        inline GLC_Matrix4x4& setMatScaling(const double, const double, const double);

        inline GLC_Matrix4x4& invert(void);

        inline GLC_Matrix4x4& setToIdentity();

        inline GLC_Matrix4x4& transpose(void);

        GLC_Matrix4x4& fromEuler(const double, const double, const double);

        GLC_Matrix4x4& setColumn(int index, const GLC_Vector3d& vector);

        inline GLC_Matrix4x4& optimise(bool force= false);


private:

        inline bool isInDiagonal(const int index) const
        {
                if ((index == 0) || (index == 5) || (index == 10) || (index == 15))
                        return true;
                else
                        return false;
        }

        inline double getDeterminantLC(const int, const int) const;

        inline void getSubMat(const int, const int, double *) const;

        inline GLC_Matrix4x4 getTranspose(void) const;

        inline GLC_Matrix4x4 getCoMat4x4(void) const;



// Private members
private:

        enum {TAILLEMAT4X4 = 16};

        enum {DIMMAT4X4 = 4};

        double m_Matrix[TAILLEMAT4X4];

        int m_Type;

/*
the matrix :
                                        a[00] a[04] a[08] a[12]

                                        a[01] a[05] a[09] a[13]

                                        a[02] a[06] a[10] a[14]

                                        a[03] a[07] a[11] a[15]
 */
//                                      Tx = 12,        Ty = 13,        Tz = 14

};

inline double getDeterminant3x3(const double *Mat3x3)
{
        double Determinant;

        Determinant= Mat3x3[0] * ( Mat3x3[4] * Mat3x3[8] - Mat3x3[7] * Mat3x3[5]);
        Determinant+= - Mat3x3[3] * ( Mat3x3[1] * Mat3x3[8] - Mat3x3[7] * Mat3x3[2]);
        Determinant+= Mat3x3[6] * ( Mat3x3[1] * Mat3x3[5] - Mat3x3[4] * Mat3x3[2]);

        return Determinant;
}

// Constructor/Destructor

GLC_Matrix4x4::GLC_Matrix4x4()
: m_Type(Identity)
{
        setToIdentity();
}

GLC_Matrix4x4::GLC_Matrix4x4(const float *Tableau)
: m_Type(General)
{

        for (int i=0; i < TAILLEMAT4X4; i++)
        {
                m_Matrix[i]= static_cast<double>(Tableau[i]);
        }
}
GLC_Matrix4x4::GLC_Matrix4x4(const GLC_Vector3d &Vect, const double &dAngleRad)
: m_Type(Direct)
{
        setToIdentity();
        setMatRot(Vect, dAngleRad);
}

GLC_Matrix4x4::GLC_Matrix4x4(const GLC_Vector3d &Vect1, const GLC_Vector3d &Vect2)
: m_Type(Direct)
{
        setToIdentity();
        setMatRot(Vect1, Vect2);
}

GLC_Matrix4x4 GLC_Matrix4x4::operator * (const GLC_Matrix4x4 &Mat) const
{
        if (m_Type == Identity)
        {
                return Mat;
        }
        else if (Mat.m_Type == Identity)
        {
                return *this;
        }

        int Colonne;
        int Ligne;
        int i;
        double ValInt;

        int IndexInt;

        GLC_Matrix4x4 MatResult;
        for (Ligne= 0; Ligne < DIMMAT4X4; Ligne++)
        {
                for (Colonne=0; Colonne < DIMMAT4X4; Colonne++)
                {
                        ValInt= 0.0;
                        IndexInt= Colonne * DIMMAT4X4;

                        for (i= 0; i < DIMMAT4X4; i++)
                        {
                                ValInt+= m_Matrix[ (i * DIMMAT4X4) + Ligne] * Mat.m_Matrix[ IndexInt + i];
                        }
                        MatResult.m_Matrix[ IndexInt + Ligne]= ValInt;
                }
        }
        if ((m_Type == Indirect) || (Mat.m_Type == Indirect))
        {
                MatResult.m_Type= Indirect;
        }
        else
        {
                MatResult.m_Type= m_Type & Mat.m_Type;
        }

        return MatResult;
}

GLC_Matrix4x4& GLC_Matrix4x4::operator = (const GLC_Matrix4x4 &matrix)
{
        m_Type= matrix.m_Type;
        memcpy(m_Matrix, matrix.m_Matrix, sizeof(double) * 16);

        return *this;
}

GLC_Vector3d GLC_Matrix4x4::operator * (const GLC_Vector3d &Vect) const
{
        double ValInt;
        int i;
        GLC_Vector3d VectResult;
        double mat[4];

        for (int Index= 0; Index < DIMMAT4X4; Index++)
        {
                ValInt= 0.0;
                for (i= 0; i < DIMMAT4X4 - 1; i++)
                {
                        ValInt+= m_Matrix[(i * DIMMAT4X4) + Index] * Vect.m_Vector[i];
                }
                ValInt+= m_Matrix[(3 * DIMMAT4X4) + Index];
                mat[Index]= ValInt;
        }

        double invW= 1.0;
        if (fabs(mat[3]) > 0.00001)
        {
                invW/= mat[3];
        }
        VectResult.m_Vector[0]= mat[0] * invW;
        VectResult.m_Vector[1]= mat[1] * invW;
        VectResult.m_Vector[2]= mat[2] * invW;


        return VectResult;
}

bool GLC_Matrix4x4::operator==(const GLC_Matrix4x4& mat) const
{
        bool result= true;
        int i= 0;
        while (result && (i < TAILLEMAT4X4))
        {
                result= (qFuzzyCompare(m_Matrix[i], mat.m_Matrix[i]));
                ++i;
        }
        return result;
}

GLC_Matrix4x4 GLC_Matrix4x4::rotationMatrix() const
{
        GLC_Matrix4x4 result(*this);
        const double invScaleX= 1.0 / scalingX();
        const double invScaleY= 1.0 / scalingY();
        const double invScaleZ= 1.0 / scalingZ();
        result.m_Matrix[0]= result.m_Matrix[0] * invScaleX;
        result.m_Matrix[1]= result.m_Matrix[1] * invScaleX;
        result.m_Matrix[2]= result.m_Matrix[2] * invScaleX;

        result.m_Matrix[4]= result.m_Matrix[4] * invScaleY;
        result.m_Matrix[5]= result.m_Matrix[5] * invScaleY;
        result.m_Matrix[6]= result.m_Matrix[6] * invScaleY;

        result.m_Matrix[8]= result.m_Matrix[8] * invScaleZ;
        result.m_Matrix[9]= result.m_Matrix[9] * invScaleZ;
        result.m_Matrix[10]= result.m_Matrix[10] * invScaleZ;

        result.m_Matrix[12]= 0.0; result.m_Matrix[13]= 0.0; result.m_Matrix[14]= 0.0;
        result.m_Matrix[3]= 0.0; result.m_Matrix[7]= 0.0; result.m_Matrix[11]= 0.0;
        result.m_Matrix[15]= 1.0;

        result.m_Type= General;

        return result;
}

GLC_Matrix4x4 GLC_Matrix4x4::isometricMatrix() const
{
        GLC_Matrix4x4 result(*this);
        const double invScaleX= 1.0 / scalingX();
        const double invScaleY= 1.0 / scalingY();
        const double invScaleZ= 1.0 / scalingZ();
        result.m_Matrix[0]= result.m_Matrix[0] * invScaleX;
        result.m_Matrix[1]= result.m_Matrix[1] * invScaleX;
        result.m_Matrix[2]= result.m_Matrix[2] * invScaleX;

        result.m_Matrix[4]= result.m_Matrix[4] * invScaleY;
        result.m_Matrix[5]= result.m_Matrix[5] * invScaleY;
        result.m_Matrix[6]= result.m_Matrix[6] * invScaleY;

        result.m_Matrix[8]= result.m_Matrix[8] * invScaleZ;
        result.m_Matrix[9]= result.m_Matrix[9] * invScaleZ;
        result.m_Matrix[10]= result.m_Matrix[10] * invScaleZ;

        result.m_Type= General;

        return result;
}

GLC_Matrix4x4& GLC_Matrix4x4::setMatRot(const GLC_Vector3d &Vect, const double &dAngleRad)
{
        // Normalize the vector
        GLC_Vector3d VectRot(Vect);
        VectRot.normalize();

        // Code optimisation
        const double SinAngleSur2= sin(dAngleRad / 2.0);

        // Quaternion computation
        const double q0= cos(dAngleRad / 2);
        const double q1= VectRot.m_Vector[0] * SinAngleSur2;
        const double q2= VectRot.m_Vector[1] * SinAngleSur2;
        const double q3= VectRot.m_Vector[2] * SinAngleSur2;

        // Code optimisation
        const double q0Carre= (q0 * q0);
        const double q1Carre= (q1 * q1);
        const double q2Carre= (q2 * q2);
        const double q3Carre= (q3 * q3);

        m_Matrix[0]= q0Carre + q1Carre - q2Carre - q3Carre;
        m_Matrix[1]= 2.0 * (q1 *q2 + q0 * q3);
        m_Matrix[2]= 2.0 * (q1 * q3 - q0 * q2);
        m_Matrix[3]= 0.0;
        m_Matrix[4]= 2.0 * (q1 * q2 - q0 * q3);
        m_Matrix[5]= q0Carre + q2Carre - q3Carre - q1Carre;
        m_Matrix[6]= 2.0 * (q2 * q3 + q0 * q1);
        m_Matrix[7]= 0.0;
        m_Matrix[8]= 2.0 * (q1 * q3 + q0 * q2);
        m_Matrix[9]= 2.0 * (q2 * q3 - q0 * q1);
        m_Matrix[10]= q0Carre + q3Carre - q1Carre - q2Carre;
        m_Matrix[11]= 0.0;

        m_Matrix[12]= 0.0;      //TX
        m_Matrix[13]= 0.0;      //TY
        m_Matrix[14]= 0.0;      //TZ
        m_Matrix[15]= 1.0;

        m_Type= Direct;

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::setMatRot(const GLC_Vector3d &Vect1, const GLC_Vector3d &Vect2)
{

        // Compute rotation matrix
        const GLC_Vector3d VectAxeRot(Vect1 ^ Vect2);
        // Check if rotation vector axis is not null
        if (!VectAxeRot.isNull())
        {  // Ok, vector not null
                const double Angle= acos(Vect1 * Vect2);
                setMatRot(VectAxeRot, Angle);
        }

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::setMatTranslate(const GLC_Vector3d &Vect)
{
        m_Matrix[0]= 1.0; m_Matrix[4]= 0.0; m_Matrix[8]=  0.0; m_Matrix[12]= Vect.m_Vector[0];
        m_Matrix[1]= 0.0; m_Matrix[5]= 1.0; m_Matrix[9]=  0.0; m_Matrix[13]= Vect.m_Vector[1];
        m_Matrix[2]= 0.0; m_Matrix[6]= 0.0; m_Matrix[10]= 1.0; m_Matrix[14]= Vect.m_Vector[2];
        m_Matrix[3]= 0.0; m_Matrix[7]= 0.0; m_Matrix[11]= 0.0; m_Matrix[15]= 1.0;

        m_Type= Direct;

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::setMatTranslate(const double Tx, const double Ty, const double Tz)
{
        m_Matrix[0]= 1.0; m_Matrix[4]= 0.0; m_Matrix[8]=  0.0; m_Matrix[12]= Tx;
        m_Matrix[1]= 0.0; m_Matrix[5]= 1.0; m_Matrix[9]=  0.0; m_Matrix[13]= Ty;
        m_Matrix[2]= 0.0; m_Matrix[6]= 0.0; m_Matrix[10]= 1.0; m_Matrix[14]= Tz;
        m_Matrix[3]= 0.0; m_Matrix[7]= 0.0; m_Matrix[11]= 0.0; m_Matrix[15]= 1.0;

        m_Type= Direct;

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::setMatScaling(const double sX, const double sY, const double sZ)
{
        m_Matrix[0]= sX; m_Matrix[4]= 0.0; m_Matrix[8]=  0.0; m_Matrix[12]= 0.0;
        m_Matrix[1]= 0.0; m_Matrix[5]= sY; m_Matrix[9]=  0.0; m_Matrix[13]= 0.0;
        m_Matrix[2]= 0.0; m_Matrix[6]= 0.0; m_Matrix[10]= sZ; m_Matrix[14]= 0.0;
        m_Matrix[3]= 0.0; m_Matrix[7]= 0.0; m_Matrix[11]= 0.0; m_Matrix[15]= 1.0;

        m_Type= General;

        return *this;
}


GLC_Matrix4x4& GLC_Matrix4x4::invert(void)
{
        const double det= determinant();

        // Test if the inverion is possible
        if (det == 0.0f) return *this;

        const double invDet = 1.0 / det;
        GLC_Matrix4x4 TCoMat= getCoMat4x4().getTranspose();

        for (int i= 0; i < TAILLEMAT4X4; i++)
        {
                m_Matrix[i]= TCoMat.m_Matrix[i] * invDet;
        }

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::setToIdentity()
{
        m_Matrix[0]= 1.0; m_Matrix[4]= 0.0; m_Matrix[8]=  0.0; m_Matrix[12]= 0.0;
        m_Matrix[1]= 0.0; m_Matrix[5]= 1.0; m_Matrix[9]=  0.0; m_Matrix[13]= 0.0;
        m_Matrix[2]= 0.0; m_Matrix[6]= 0.0; m_Matrix[10]= 1.0; m_Matrix[14]= 0.0;
        m_Matrix[3]= 0.0; m_Matrix[7]= 0.0; m_Matrix[11]= 0.0; m_Matrix[15]= 1.0;

        m_Type= Identity;

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::transpose(void)
{
        GLC_Matrix4x4 MatT(m_Matrix);
        int IndexOrigine;
        int IndexTrans;
        for (int Colonne= 0; Colonne < DIMMAT4X4; Colonne++)
        {
                for (int Ligne=0 ; Ligne < DIMMAT4X4; Ligne++)
                {
                        IndexOrigine= (Colonne * DIMMAT4X4) + Ligne;
                        IndexTrans= (Ligne * DIMMAT4X4) + Colonne;

                        MatT.m_Matrix[IndexTrans]= m_Matrix[IndexOrigine];
                }
        }

        // Load the transposed matrix in this matrix
        memcpy(m_Matrix, MatT.m_Matrix, sizeof(double) * 16);

        return *this;
}

GLC_Matrix4x4& GLC_Matrix4x4::optimise(bool force)
{
        if (force || (m_Type == General))
        {
                bool identityVal= (m_Matrix[0] == 1.0f) && (m_Matrix[4] == 0.0f) && (m_Matrix[8] ==  0.0f) && (m_Matrix[12] == 0.0f);
                identityVal= identityVal && (m_Matrix[1] == 0.0f) && (m_Matrix[5] == 1.0f) && (m_Matrix[9] ==  0.0f) && (m_Matrix[13] == 0.0);
                identityVal= identityVal && (m_Matrix[2] == 0.0f) && (m_Matrix[6] == 0.0f) && (m_Matrix[10] == 1.0f) && (m_Matrix[14] == 0.0);
                identityVal= identityVal && (m_Matrix[3] == 0.0f) && (m_Matrix[7] == 0.0f) && (m_Matrix[11] == 0.0f) && (m_Matrix[15] == 1.0f);
                if (identityVal)
                {
                        m_Type= Identity;
                }
                else
                {
                        if (determinant() > 0)
                        {
                                m_Type= Direct;
                        }
                        else
                        {
                                m_Type= Indirect;
                        }
                }
        }
        return *this;
}

double GLC_Matrix4x4::determinant(void) const
{
        double Determinant= 0.0;
        double SubMat3x3[9];
        int Signe= 1;

        for (int Colonne= 0; Colonne < DIMMAT4X4; Colonne++, Signe*= -1)
        {
                getSubMat(0, Colonne, SubMat3x3);
                Determinant+= Signe * m_Matrix[Colonne * DIMMAT4X4] * getDeterminant3x3(SubMat3x3);
        }

        return Determinant;

}

double GLC_Matrix4x4::getDeterminantLC(const int Ligne, const int Colonne) const
{
        double Mat3x3[9];
        double Determinant;

        getSubMat(Ligne, Colonne, Mat3x3);

        if ( 0 == ((Ligne + Colonne) % 2)) // Even number
                Determinant= m_Matrix[(Colonne + DIMMAT4X4) + Ligne] * getDeterminant3x3(Mat3x3);
        else
                Determinant= - m_Matrix[(Colonne + DIMMAT4X4) + Ligne] * getDeterminant3x3(Mat3x3);

        return Determinant;
}

void GLC_Matrix4x4::getSubMat(const int Ligne, const int Colonne, double *ResultMat) const
{

        int LigneResult;
        int ColonneResult;
        int IndexOrigine;
        int IndexResult;

        for (int ColonneOrigine= 0; ColonneOrigine < DIMMAT4X4; ColonneOrigine++)
        {
                if (ColonneOrigine != Colonne)
                {
                        if (ColonneOrigine < Colonne)
                                ColonneResult= ColonneOrigine;
                        else
                                ColonneResult= ColonneOrigine - 1;

                        for (int LigneOrigine= 0; LigneOrigine < DIMMAT4X4; LigneOrigine++)
                        {
                                if (LigneOrigine != Ligne)
                                {
                                        if (LigneOrigine < Ligne)
                                                LigneResult= LigneOrigine;
                                        else
                                                LigneResult= LigneOrigine - 1;
                                        IndexOrigine= (ColonneOrigine * DIMMAT4X4) + LigneOrigine;
                                        IndexResult= (ColonneResult * (DIMMAT4X4 - 1)) + LigneResult;

                                        ResultMat[IndexResult]= m_Matrix[IndexOrigine];
                                }
                        }
                }
        }
}

GLC_Matrix4x4 GLC_Matrix4x4::getTranspose(void) const
{
        GLC_Matrix4x4 MatT(m_Matrix);
        int IndexOrigine;
        int IndexTrans;
        for (int Colonne= 0; Colonne < DIMMAT4X4; Colonne++)
        {
                for (int Ligne=0 ; Ligne < DIMMAT4X4; Ligne++)
                {
                        IndexOrigine= (Colonne * DIMMAT4X4) + Ligne;
                        IndexTrans= (Ligne * DIMMAT4X4) + Colonne;

                        MatT.m_Matrix[IndexTrans]= m_Matrix[IndexOrigine];
                }
        }

        MatT.m_Type= m_Type;
        return MatT;
}

GLC_Matrix4x4 GLC_Matrix4x4::getCoMat4x4(void) const
{
        GLC_Matrix4x4 CoMat(m_Matrix);
        double SubMat3x3[9];
        int Index;

        for (int Colonne= 0; Colonne < DIMMAT4X4; Colonne++)
        {
                for (int Ligne=0 ; Ligne < DIMMAT4X4; Ligne++)
                {
                        getSubMat(Ligne, Colonne, SubMat3x3);
                        Index= (Colonne * DIMMAT4X4) + Ligne;
                        if (((Colonne + Ligne + 2) % 2) == 0) // Even Number
                                CoMat.m_Matrix[Index]= getDeterminant3x3(SubMat3x3);
                        else
                                CoMat.m_Matrix[Index]= -getDeterminant3x3(SubMat3x3);
                }
        }


        CoMat.m_Type= General;
        return CoMat;
}


#endif /*GLC_MATRIX4X4_H_*/