K3dMatrix.h

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2007 by Jan Koci   *
 *   honza.koci@email.cz   *
 *   http://kengine.sourceforge.net/tutorial/
 *                                                                         *
 *   This program 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 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#pragma once

#include "K3dVector3.h"
#include "K3dVector4.h"


class K3dMatrix: public K3dString
{
                // 4x4 matrix array

                // Standard matrix:
                // |0,   1,  2  ,3|
                // |4,   5,  6  ,7|
                // |8,   9, 10 ,11|
                // |12, 13, 14 ,15|

                // OpenGL matrix:
                // |0, 4,  8  ,12|
                // |1, 5,  9  ,13|
                // |2, 6, 10  ,14|
                // |3, 7, 11  ,15|

                float m_afMatrix[16];           
                int m_iId;                              
        public:
                K3dMatrix();
                K3dMatrix ( const float afMatrix[16] ); // Constructor
                ~K3dMatrix();

                int &GetId()
                {
                        return m_iId;
                }
                // Reset matrix
                void LoadIdentity();
                // Reset roation matrix
                void ResetRotation();
                // Reset translation matrix
                void ResetTranslation();
                // Set matrix from array
                // Get or set current matrix array
                float *Matrix();
                const float *Matrix() const;

                // Get current matrix array
                float *GetMatrix()
                {
                        return m_afMatrix;
                }

                void SetMatrix ( const float *_pfMatrix, const int _iSize=16 );
                void SetMatrix ( const K3dMatrix &_rkMatrix, const int _iSize=16 );
                void SetRow ( const K3dVector4 &_rkRow, const int _iRow );
                void SetColumn ( const K3dVector4 &_rkColumn, const int _iRow );
                void GetRow ( K3dVector4 &_rkRow, const int _iRow );
                void GetColumn ( K3dVector4 &_rkColumn, const int _iColumn );
                // Overloaded operator[] allows set matrix arrays
                float &operator[] ( int index ) ;
                // Overloaded operator() allow set ant get row and col value
                float operator() ( int iRow, int iCol ) const;
                float& operator() ( int iRow, int iCol );

                // Assignment
                K3dMatrix& operator= ( const K3dMatrix &_rkM );

                // Comparison
                bool operator== ( K3dMatrix& rkM );
                bool operator!= ( K3dMatrix& rkM ) ;
                bool operator< ( K3dMatrix& rkM );
                bool operator<= ( K3dMatrix& rkM );
                bool operator> ( K3dMatrix& rkM );
                bool operator>= ( K3dMatrix& rkM );
                // Compare arrays for comparison operators
                int CompareArrays ( K3dMatrix& rkV );

                // Arithmetic operations
                K3dMatrix operator+ ( K3dMatrix& rkM );
                K3dMatrix operator- ( K3dMatrix& rkM );
                K3dMatrix operator* ( K3dMatrix &rkMatrix ) ;
                K3dMatrix operator* ( float fScalar );
                K3dMatrix operator* ( const float afMatrix[16] );
                K3dVector3 operator* ( K3dVector3& rkV );
                K3dMatrix operator- ();

                // Arithmetic updates
                K3dMatrix& operator+= ( K3dMatrix& rkM );
                K3dMatrix& operator-= ( K3dMatrix& rkM );
                K3dMatrix& operator*= ( float fScalar );
                K3dMatrix& operator/= ( float fScalar );


                // Functions

                void Multiply ( K3dMatrix& _rkMat );
                // Multiply transpse (OpenGL matrix)
                void MultiplyTranspose ( K3dMatrix& _rkMat );
                // Multiply row by scalar
                void MultiplyRow ( const float fScalar, const int iRow );
                // Multiply column by scalar
                void MultiplyColumn ( const float fScalar, const int iColumn );
                // Add row to another iRow1 = iRow1 + iRow2
                void AddRow ( const int iRow1, const int iRow2 );
                // Set translation matrix
                void Translation ( const float *translation );
                // Set translation matrix from vector
                void Translation ( const K3dVector3& _rkVector );
                // Set inverse translation matrix from vector
                void InverseTranslation ( const K3dVector3 &_rkVector );

                // Calculate rotation matrix
                void Rotation ( K3dVector3 &_rkAngle );
                // Calculate rotation matrix
                void Rotation ( const float *pfAngle );
                // Set inverse rotation matrix
                void InverseRotation ( K3dVector3 &_rkAngle );
                // Set inverse rotation matrix
                void InverseRotation ( const float *pfAngle );
                // Calculate X rotation matrix
                void RotationX ( const float fAngle );
                void RotationXGL ( const float _fAngle );
                // Calculate Y rotation matrix
                void RotationY ( const float fAngle );
                void RotationYGL ( const float _fAngle );
                // Calculate Z rotation matrix
                void RotationZ ( const float fAngle );
                void RotationZGL ( const float _fAngle );
                // Set scale matrix
                void Scale ( K3dVector3 &_rkVector );
                // Get vector from matrix
                void GetVector ( K3dVector3 &_rkVector );
                // Get vector from matrix
                //K3dVector3 *GetVector();
                // Get scalar index by row and column
                int GetScalarIndex ( const int iRow, const int iCol );
                //      Change vector by matrix only rotation
                void ChangeVector ( K3dVector3& _rkVec );
                //      Change vector by matrix with tranform
                void ChangeVectorTran ( K3dVector3& _rkVec );
                //      Translate vector by matrix
                void TranslateVector ( K3dVector3& _rkVec );
                //      Change GL vector by matrix
                void ChangeVectorGL ( K3dVector3& _rkVec );
                //      Change X vector coordination by matrix
                void ChangeVectorX ( K3dVector3& _rkVec );
                void ChangeVectorXGL ( K3dVector3& _rkVec );
                //      Change Y vector coordination by matrix
                void ChangeVectorY ( K3dVector3& _rkVec );
                void ChangeVectorYGL ( K3dVector3& _rkVec );
                //      Change Z vector coordination by matrix
                void ChangeVectorZ ( K3dVector3& _rkVec );
                void ChangeVectorZGL ( K3dVector3& _rkVec );
                // Calculate determinant
                float Determinant();
                // Inverse matrix
                void Inverse();
                // Inverse opengl matrix
                void InverseGL();
                // Flip rows to column (normal matrix to OpenGL or again)
                void FlipRowToColumn ( const K3dMatrix &_rkMatrix );
                // Rotation through an angle about an arbitrary axis
                void Rotation ( const K3dVector3 &_rkAxis  ,const float fAngle );
                // GL rotation through an angle about an arbitrary axis
                void RotationGL ( const K3dVector3 &_rkAxis  ,const float fAngle );
                // Calculate linear system of three equations
                K3dVector3 LinearSystem ( const K3dVector4 &_rkEq1,const K3dVector4 &_rkEq2, const K3dVector4 &_rkEq3 );
                // Change standrard matrix to the OpenGL matrix
                void ToOrFromGL();
                // Multiply matrix and vector
                // dst = M * src
                void MultMatrixVec ( const K3dVector3 &_rkSrc, K3dVector3 &rkDst );
                void MultMatrixVec ( K3dVector3 & rkSrcAndDst );
                // Multiply vector and matrix
                // dst = src * M
                void MultVecMatrix ( const K3dVector3 &rkSrc, K3dVector3 &rkDst );
                void MultVecMatrix ( K3dVector3 & rkSrcAndDst );
                // Multiply directional matrix and vector
                // dst = M * src
                void MultMatrixDir ( const K3dVector3 &rkSrc, K3dVector3 &rkDst );
                void MultMatrixDir ( K3dVector3 & rkSrcAndDst );
                // Multiply vector and directional matrix
                // dst = src * M
                void MultDirMatrix ( const K3dVector3 &rkSrc, K3dVector3 &rkDst );
                void MultDirMatrix ( K3dVector3 & rkSrcAndDst );
                void GetTranslation ( K3dVector3 &_rkTranslation )
                {
                        _rkTranslation[0] = m_afMatrix[12];
                        _rkTranslation[1] = m_afMatrix[13];
                        _rkTranslation[2] = m_afMatrix[14];
                }
};



// Get or set current matrix array
inline float *K3dMatrix::Matrix()
{
        return m_afMatrix;
}
inline const float *K3dMatrix::Matrix() const
{
        return m_afMatrix;
}



// Overloaded operator[] allows set matrix arrays
inline float &K3dMatrix::operator[] ( int i )
{
        if ( ! ( 0 <= i ) && ( i < 16 ) )
        {
                cerr  << "Error :: K3dMatrix::operator[] -- wrong index " << i << endl;
        }
        return m_afMatrix[i];
}



// Overloaded operator() allow set ant get row and col value
inline float K3dMatrix::operator() ( int iRow, int iCol ) const
{
        if ( ( iRow==0 ) && ( iCol==0 ) ) return m_afMatrix[0];
        if ( ( iRow==0 ) && ( iCol==1 ) ) return m_afMatrix[1];
        if ( ( iRow==0 ) && ( iCol==2 ) ) return m_afMatrix[2];
        if ( ( iRow==0 ) && ( iCol==3 ) ) return m_afMatrix[3];
        if ( ( iRow==1 ) && ( iCol==0 ) ) return m_afMatrix[4];
        if ( ( iRow==1 ) && ( iCol==1 ) ) return m_afMatrix[5];
        if ( ( iRow==1 ) && ( iCol==2 ) ) return m_afMatrix[6];
        if ( ( iRow==1 ) && ( iCol==3 ) ) return m_afMatrix[7];
        if ( ( iRow==2 ) && ( iCol==0 ) ) return m_afMatrix[8];
        if ( ( iRow==2 ) && ( iCol==1 ) ) return m_afMatrix[9];
        if ( ( iRow==2 ) && ( iCol==2 ) ) return m_afMatrix[10];
        if ( ( iRow==2 ) && ( iCol==3 ) ) return m_afMatrix[11];
        if ( ( iRow==3 ) && ( iCol==0 ) ) return m_afMatrix[12];
        if ( ( iRow==3 ) && ( iCol==1 ) ) return m_afMatrix[13];
        if ( ( iRow==3 ) && ( iCol==2 ) ) return m_afMatrix[14];
        if ( ( iRow==3 ) && ( iCol==3 ) ) return m_afMatrix[15];

        return m_afMatrix[0];
}



inline float &K3dMatrix::operator() ( int iRow, int iCol )
{
        int iIndex = GetScalarIndex ( iRow, iCol );
        return m_afMatrix[iIndex];
}



// Reset matrix
inline void K3dMatrix::LoadIdentity()
{
        m_afMatrix[0] = 1;      m_afMatrix[4] = 0;      m_afMatrix[8]  = 0;     m_afMatrix[12] = 0;
        m_afMatrix[1] = 0;      m_afMatrix[5] = 1;      m_afMatrix[9]  = 0;     m_afMatrix[13] = 0;
        m_afMatrix[2] = 0;      m_afMatrix[6] = 0;      m_afMatrix[10] = 1;     m_afMatrix[14] = 0;
        m_afMatrix[3] = 0;      m_afMatrix[7] = 0;      m_afMatrix[11] = 0;     m_afMatrix[15] = 1;
}



// Reset roation matrix
inline void K3dMatrix::ResetRotation()
{
        m_afMatrix[0] = 1;      m_afMatrix[4] = 0;      m_afMatrix[8]  = 0;
        m_afMatrix[1] = 0;      m_afMatrix[5] = 1;      m_afMatrix[9]  = 0;
        m_afMatrix[2] = 0;      m_afMatrix[6] = 0;      m_afMatrix[10] = 1;
        m_afMatrix[3] = 0;      m_afMatrix[7] = 0;      m_afMatrix[11] = 0;
}



// Reset translation matrix
inline void K3dMatrix::ResetTranslation()
{
        m_afMatrix[12] = 0;
        m_afMatrix[13] = 0;
        m_afMatrix[14] = 0;
        m_afMatrix[15] = 1;
}



inline bool K3dMatrix::operator== ( K3dMatrix& rkM )
{
        return memcmp ( m_afMatrix, rkM.m_afMatrix, 16*sizeof ( float ) ) == 0;
}



inline bool K3dMatrix::operator!= ( K3dMatrix& rkM )
{
        return memcmp ( m_afMatrix, rkM.m_afMatrix, 16*sizeof ( float ) ) != 0;
}



inline bool K3dMatrix::operator< ( K3dMatrix& rkM )
{
        return CompareArrays ( rkM ) < 0;
}



inline bool K3dMatrix::operator<= ( K3dMatrix& rkM )
{
        return CompareArrays ( rkM ) <= 0;
}



inline bool K3dMatrix::operator> ( K3dMatrix& rkM )
{
        return CompareArrays ( rkM ) > 0;
}



inline bool K3dMatrix::operator>= ( K3dMatrix& rkM )
{
        return CompareArrays ( rkM ) >= 0;
}



inline K3dMatrix K3dMatrix::operator* ( K3dMatrix &rkMatrix )
{

        float afMatrix[16];
        const float *m1 = m_afMatrix, *m2 = rkMatrix.m_afMatrix;

        afMatrix[0] = ( float ) ( m1[0] * m2[0] )  + ( m1[4] * m2[1] )  + ( m1[8]  * m2[2] );
        afMatrix[1] = ( float ) ( m1[1] * m2[0] )  + ( m1[5] * m2[1] )  + ( m1[9]  * m2[2] );
        afMatrix[2] = ( float ) ( m1[2] * m2[0] )  + ( m1[6] * m2[1] )  + ( m1[10] * m2[2] );
        afMatrix[3] = 0;

        afMatrix[4] = ( float ) ( m1[0] * m2[4] )  + ( m1[4] * m2[5] )  + ( m1[8]  * m2[6] );
        afMatrix[5] = ( float ) ( m1[1] * m2[4] )  + ( m1[5] * m2[5] )  + ( m1[9]  * m2[6] );
        afMatrix[6] = ( float ) ( m1[2] * m2[4] )  + ( m1[6] * m2[5] )  + ( m1[10] * m2[6] );
        afMatrix[7] = 0;

        afMatrix[8] = ( float ) ( m1[0] * m2[8] )  + ( m1[4] * m2[9] )  + ( m1[8]  * m2[10] );
        afMatrix[9] = ( float ) ( m1[1] * m2[8] )  + ( m1[5] * m2[9] )  + ( m1[9]  * m2[10] );
        afMatrix[10] = ( float ) ( m1[2] * m2[8] )  + ( m1[6] * m2[9] )  + ( m1[10] * m2[10] );
        afMatrix[11] = 0;

        afMatrix[12] = ( float ) ( m1[0] * m2[12] ) + ( m1[4] * m2[13] ) + ( m1[8]  * m2[14] ) + m1[12];
        afMatrix[13] = ( float ) ( m1[1] * m2[12] ) + ( m1[5] * m2[13] ) + ( m1[9]  * m2[14] ) + m1[13];
        afMatrix[14] = ( float ) ( m1[2] * m2[12] ) + ( m1[6] * m2[13] ) + ( m1[10] * m2[14] ) + m1[14];
        afMatrix[15] = 1;

        SetMatrix ( afMatrix );
        return *this;
}



inline K3dMatrix K3dMatrix::operator* ( const float afMatrix[16] )
{
        float afNewMatrix[16];
        const float *m1 = m_afMatrix, *m2 = afMatrix;

        afNewMatrix[0] = ( float ) ( m1[0] * m2[0] )  + ( m1[4] * m2[1] )  + ( m1[8]  * m2[2] );
        afNewMatrix[1] = ( float ) ( m1[1] * m2[0] )  + ( m1[5] * m2[1] )  + ( m1[9]  * m2[2] );
        afNewMatrix[2] = ( float ) ( m1[2] * m2[0] )  + ( m1[6] * m2[1] )  + ( m1[10] * m2[2] );
        afNewMatrix[3] = 0;

        afNewMatrix[4] = ( float ) ( m1[0] * m2[4] )  + ( m1[4] * m2[5] )  + ( m1[8]  * m2[6] );
        afNewMatrix[5] = ( float ) ( m1[1] * m2[4] )  + ( m1[5] * m2[5] )  + ( m1[9]  * m2[6] );
        afNewMatrix[6] = ( float ) ( m1[2] * m2[4] )  + ( m1[6] * m2[5] )  + ( m1[10] * m2[6] );
        afNewMatrix[7] = 0;

        afNewMatrix[8] = ( float ) ( m1[0] * m2[8] )  + ( m1[4] * m2[9] )  + ( m1[8]  * m2[10] );
        afNewMatrix[9] = ( float ) ( m1[1] * m2[8] )  + ( m1[5] * m2[9] )  + ( m1[9]  * m2[10] );
        afNewMatrix[10] = ( float ) ( m1[2] * m2[8] )  + ( m1[6] * m2[9] )  + ( m1[10] * m2[10] );
        afNewMatrix[11] = 0;

        afNewMatrix[12] = ( float ) ( m1[0] * m2[12] ) + ( m1[4] * m2[13] ) + ( m1[8]  * m2[14] ) + m1[12];
        afNewMatrix[13] = ( float ) ( m1[1] * m2[12] ) + ( m1[5] * m2[13] ) + ( m1[9]  * m2[14] ) + m1[13];
        afNewMatrix[14] = ( float ) ( m1[2] * m2[12] ) + ( m1[6] * m2[13] ) + ( m1[10] * m2[14] ) + m1[14];
        afNewMatrix[15] = 1;



        SetMatrix ( afNewMatrix );

        return *this;
}



inline K3dVector3 K3dMatrix::operator* ( K3dVector3& rkV )
{
        K3dVector3 kVec = rkV;
        this->ChangeVector ( kVec );
        return kVec;
}



inline void K3dMatrix::Multiply ( K3dMatrix& _rkMat )
{
        float afMatrix[16];
        const float *m1 = m_afMatrix, *m2 = _rkMat.m_afMatrix;

        afMatrix[0] = ( float ) ( m1[0] * m2[0] )  + ( m1[4] * m2[1] )  + ( m1[8]  * m2[2] );
        afMatrix[1] = ( float ) ( m1[1] * m2[0] )  + ( m1[5] * m2[1] )  + ( m1[9]  * m2[2] );
        afMatrix[2] = ( float ) ( m1[2] * m2[0] )  + ( m1[6] * m2[1] )  + ( m1[10] * m2[2] );
        afMatrix[3] = 0;

        afMatrix[4] = ( float ) ( m1[0] * m2[4] )  + ( m1[4] * m2[5] )  + ( m1[8]  * m2[6] );
        afMatrix[5] = ( float ) ( m1[1] * m2[4] )  + ( m1[5] * m2[5] )  + ( m1[9]  * m2[6] );
        afMatrix[6] = ( float ) ( m1[2] * m2[4] )  + ( m1[6] * m2[5] )  + ( m1[10] * m2[6] );
        afMatrix[7] = 0;

        afMatrix[8] = ( float ) ( m1[0] * m2[8] )  + ( m1[4] * m2[9] )  + ( m1[8]  * m2[10] );
        afMatrix[9] = ( float ) ( m1[1] * m2[8] )  + ( m1[5] * m2[9] )  + ( m1[9]  * m2[10] );
        afMatrix[10] = ( float ) ( m1[2] * m2[8] )  + ( m1[6] * m2[9] )  + ( m1[10] * m2[10] );
        afMatrix[11] = 0;

        afMatrix[12] = ( float ) ( m1[0] * m2[12] ) + ( m1[4] * m2[13] ) + ( m1[8]  * m2[14] ) + m1[12];
        afMatrix[13] = ( float ) ( m1[1] * m2[12] ) + ( m1[5] * m2[13] ) + ( m1[9]  * m2[14] ) + m1[13];
        afMatrix[14] = ( float ) ( m1[2] * m2[12] ) + ( m1[6] * m2[13] ) + ( m1[10] * m2[14] ) + m1[14];
        afMatrix[15] = 1;

        SetMatrix ( afMatrix );
}



inline void K3dMatrix::MultiplyTranspose ( K3dMatrix& _rkMat )
{
        float afMatrix[16];
        const float *m1 = m_afMatrix, *m2 = _rkMat.m_afMatrix;

        afMatrix[0]  = ( float ) ( m1[0] * m2[0] ) + ( m1[1] * m2[4] ) + ( m1[2] * m2[8] );
        afMatrix[1]  = ( float ) ( m1[0] * m2[1] ) + ( m1[1] * m2[5] ) + ( m1[2] * m2[9] );
        afMatrix[2]  = ( float ) ( m1[0] * m2[2] ) + ( m1[1] * m2[6] ) + ( m1[2] * m2[10] );
        afMatrix[3]  = 0;

        afMatrix[4]  = ( float ) ( m1[4] * m2[0] ) + ( m1[5] * m2[4] ) + ( m1[6] * m2[8] );
        afMatrix[5]  = ( float ) ( m1[4] * m2[1] ) + ( m1[5] * m2[5] ) + ( m1[6] * m2[9] );
        afMatrix[6]  = ( float ) ( m1[4] * m2[2] ) + ( m1[5] * m2[6] ) + ( m1[6] * m2[10] );
        afMatrix[7]  = 0;

        afMatrix[8]  = ( float ) ( m1[8] * m2[0] ) + ( m1[9] * m2[4] ) + ( m1[10] * m2[8] );
        afMatrix[9]  = ( float ) ( m1[8] * m2[1] ) + ( m1[9] * m2[5] ) + ( m1[10] * m2[9] );
        afMatrix[10] = ( float ) ( m1[8] * m2[2] ) + ( m1[9] * m2[6] ) + ( m1[10] * m2[10] );
        afMatrix[11] = 0;

        afMatrix[12] = ( float ) ( m1[12] * m2[0] ) + ( m1[13] * m2[4] ) + ( m1[14] * m2[8] )  + m1[12];
        afMatrix[13] = ( float ) ( m1[12] * m2[1] ) + ( m1[13] * m2[5] ) + ( m1[14] * m2[9] )  + m1[13];
        afMatrix[14] = ( float ) ( m1[12] * m2[2] ) + ( m1[13] * m2[6] ) + ( m1[14] * m2[10] ) + m1[14];
        afMatrix[15] = 1;

        SetMatrix ( afMatrix );
}



// Add row to another iRow1 = iRow1 + iRow2
inline void K3dMatrix::AddRow ( const int iRow1, const int iRow2 )
{
        K3dVector4 kRow1;
        K3dVector4 kRow2;

        if ( ! ( 0 <= iRow1 ) && ( iRow1 < 4 ) )
        {
                cerr  << "Error :: K3dMatrix::AddRow() -- wrong index " << iRow1 << endl;
        }
        if ( ! ( 0 <= iRow2 ) && ( iRow2 < 4 ) )
        {
                cerr  << "Error :: K3dMatrix::AddRow() -- wrong index " << iRow2 << endl;
        }

        // Get row 1
        this->GetRow ( kRow1, iRow1 );
        // Get row 2
        this->GetRow ( kRow2, iRow2 );

        // Add row2 to row1
        kRow1 += kRow2;

        // Set row 1
        this->SetRow ( kRow1, iRow1 );

}



// Set translation matrix from array
inline void K3dMatrix::Translation ( const float *translation )
{
        m_afMatrix[12] = translation[0];
        m_afMatrix[13] = translation[1];
        m_afMatrix[14] = translation[2];
}



// Set translation matrix from vector
inline void K3dMatrix::Translation ( const K3dVector3 &_rkVector )
{
        m_afMatrix[12] = _rkVector[0];
        m_afMatrix[13] = _rkVector[1];
        m_afMatrix[14] = _rkVector[2];
}



inline void K3dMatrix::InverseTranslation ( const K3dVector3 &_rkVector )
{
        m_afMatrix[12] = -_rkVector[0];
        m_afMatrix[13] = -_rkVector[1];
        m_afMatrix[14] = -_rkVector[2];
}



// Set scale matrix
inline void K3dMatrix::Scale ( K3dVector3 &_rkVector )
{
        m_afMatrix[0] = _rkVector[0];
        m_afMatrix[1] = 0;
        m_afMatrix[2] = 0;
        m_afMatrix[3] = 0;

        m_afMatrix[4] = 0;
        m_afMatrix[5] = _rkVector[1];
        m_afMatrix[6] = 0;
        m_afMatrix[7] = 0;

        m_afMatrix[8] = 0;
        m_afMatrix[9] = 0;
        m_afMatrix[10] = _rkVector[2];
        m_afMatrix[11] = 0;

        m_afMatrix[12] = 0;
        m_afMatrix[13] = 0;
        m_afMatrix[14] = 0;
        m_afMatrix[15] = 1;
}





// Calculate rotation matrix
inline void K3dMatrix::Rotation ( K3dVector3 &_rkAngle )
{
        double cr = cos ( _rkAngle[0] );
        double sr = sin ( _rkAngle[0] );
        double cp = cos ( _rkAngle[1] );
        double sp = sin ( _rkAngle[1] );
        double cy = cos ( _rkAngle[2] );
        double sy = sin ( _rkAngle[2] );

        m_afMatrix[0] = ( float ) ( cp * cy );
        m_afMatrix[1] = ( float ) ( cp * sy );
        m_afMatrix[2] = ( float ) ( -sp );

        double srsp = sr * sp;
        double crsp = cr * sp;

        m_afMatrix[4] = ( float ) ( srsp * cy - cr  *sy );
        m_afMatrix[5] = ( float ) ( srsp * sy + cr * cy );
        m_afMatrix[6] = ( float ) ( sr * cp );

        m_afMatrix[8] = ( float ) ( crsp * cy + sr * sy );
        m_afMatrix[9] = ( float ) ( crsp * sy - sr * cy );
        m_afMatrix[10] = ( float ) ( cr * cp );
}



// Calculate rotation matrix
inline void K3dMatrix::Rotation ( const float *pfAngle )
{

        double cr = cos ( pfAngle[0] );
        double sr = sin ( pfAngle[0] );
        double cp = cos ( pfAngle[1] );
        double sp = sin ( pfAngle[1] );
        double cy = cos ( pfAngle[2] );
        double sy = sin ( pfAngle[2] );

        m_afMatrix[0] = ( float ) ( cp * cy );
        m_afMatrix[1] = ( float ) ( cp * sy );
        m_afMatrix[2] = ( float ) ( -sp );

        double srsp = sr * sp;
        double crsp = cr * sp;

        m_afMatrix[4] = ( float ) ( srsp * cy - cr  *sy );
        m_afMatrix[5] = ( float ) ( srsp * sy + cr * cy );
        m_afMatrix[6] = ( float ) ( sr * cp );

        m_afMatrix[8] = ( float ) ( crsp * cy + sr * sy );
        m_afMatrix[9] = ( float ) ( crsp * sy - sr * cy );
        m_afMatrix[10] = ( float ) ( cr * cp );
}




// Set inverse rotation matrix
inline void K3dMatrix::InverseRotation ( K3dVector3 &_rkAngle )
{
        double cr = cos ( _rkAngle[0] );
        double sr = sin ( _rkAngle[0] );
        double cp = cos ( _rkAngle[1] );
        double sp = sin ( _rkAngle[1] );
        double cy = cos ( _rkAngle[2] );
        double sy = sin ( _rkAngle[2] );

        m_afMatrix[0] = ( float ) ( cp * cy );
        m_afMatrix[4] = ( float ) ( cp * sy );
        m_afMatrix[8] = ( float ) ( -sp );

        double srsp = sr * sp;
        double crsp = cr * sp;

        m_afMatrix[1] = ( float ) ( srsp * cy - cr * sy );
        m_afMatrix[5] = ( float ) ( srsp * sy + cr * cy );
        m_afMatrix[9] = ( float ) ( sr * cp );

        m_afMatrix[2] = ( float ) ( crsp * cy + sr * sy );
        m_afMatrix[6] = ( float ) ( crsp * sy - sr * cy );
        m_afMatrix[10] = ( float ) ( cr * cp );
}



// Set inverse rotation matrix
inline void K3dMatrix::InverseRotation ( const float *pfAngle )
{
        double cr = cos ( pfAngle[0] );
        double sr = sin ( pfAngle[0] );
        double cp = cos ( pfAngle[1] );
        double sp = sin ( pfAngle[1] );
        double cy = cos ( pfAngle[2] );
        double sy = sin ( pfAngle[2] );


        m_afMatrix[0] = ( float ) ( cp * cy );
        m_afMatrix[4] = ( float ) ( cp * sy );
        m_afMatrix[8] = ( float ) ( -sp );

        double srsp = sr * sp;
        double crsp = cr * sp;

        m_afMatrix[1] = ( float ) ( srsp * cy - cr * sy );
        m_afMatrix[5] = ( float ) ( srsp * sy + cr * cy );
        m_afMatrix[9] = ( float ) ( sr * cp );

        m_afMatrix[2] = ( float ) ( crsp * cy + sr * sy );
        m_afMatrix[6] = ( float ) ( crsp * sy - sr * cy );
        m_afMatrix[10] = ( float ) ( cr * cp );
}



// Calculate X rotation matrix
inline void K3dMatrix::RotationX ( const float fAngle )
{
        const float fS = sin ( fAngle );
        const float fC = cos ( fAngle );

        m_afMatrix[5] = ( float ) fC;
        m_afMatrix[6] = ( float )-fS;
        m_afMatrix[9] = ( float ) fS;
        m_afMatrix[10] = ( float ) fC;
}



inline void K3dMatrix::RotationXGL ( const float _fAngle )
{
        const float fS = sin ( _fAngle );
        const float fC = cos ( _fAngle );

        m_afMatrix[5] = ( float ) fC;
        m_afMatrix[9] = ( float )-fS;
        m_afMatrix[6] = ( float ) fS;
        m_afMatrix[10] = ( float ) fC;
}



// Calculate Y rotation matrix
inline void K3dMatrix::RotationY ( const float fAngle )
{
        const float fS = sin ( fAngle );
        const float fC = cos ( fAngle );

        m_afMatrix[0] = ( float ) fC;
        m_afMatrix[2] = ( float ) fS;
        m_afMatrix[8] = ( float )-fS;
        m_afMatrix[10] = ( float ) fC;
}



inline void K3dMatrix::RotationYGL ( const float _fAngle )
{
        const float fS = sin ( _fAngle );
        const float fC = cos ( _fAngle );

        m_afMatrix[0] = ( float ) fC;
        m_afMatrix[8] = ( float ) fS;
        m_afMatrix[2] = ( float )-fS;
        m_afMatrix[10] = ( float ) fC;
}



// Calculate Z rotation matrix
inline void K3dMatrix::RotationZ ( const float fAngle )
{
        const float fS = sin ( fAngle );
        const float fC = cos ( fAngle );

        m_afMatrix[0] = ( float ) fC;
        m_afMatrix[1] = ( float )-fS;
        m_afMatrix[4] = ( float ) fS;
        m_afMatrix[5] = ( float ) fC;
}



inline void K3dMatrix::RotationZGL ( const float _fAngle )
{
        const float fS = sin ( _fAngle );
        const float fC = cos ( _fAngle );

        m_afMatrix[0] = ( float ) fC;
        m_afMatrix[4] = ( float )-fS;
        m_afMatrix[1] = ( float ) fS;
        m_afMatrix[5] = ( float ) fC;
}



// Get translate vector from matrix
inline void K3dMatrix::GetVector ( K3dVector3 &rkVector )
{
        K3dVector3 kVector;
        kVector[0] = m_afMatrix[12];
        kVector[1] = m_afMatrix[13];
        kVector[2] = m_afMatrix[14];

        rkVector.Set ( kVector );
}


/*
        // Get vector from matrix
        inline K3dVector3 *K3dMatrix::GetVector()
        {
                K3dVector3 kVector;
                kVector[0] = m_afMatrix[12];
                kVector[1] = m_afMatrix[13];
                kVector[2] = m_afMatrix[14];

                return &kVector;
        }


*/
// Get scalar index by row and column
inline int K3dMatrix::GetScalarIndex ( const int iRow, const int iCol )
{
        if ( ! ( 0 <= iRow && iRow <= 3 && 0 <= iCol && iCol <= 3 ) )
        {
                cerr  << "Error :: K3dMatrix::GetScalarIndex() -- iRow and iCol indexes is must be from 0 to 3 " << " iRow " << iRow << " iCol " << iCol << endl;
        }
        if ( ( iRow==0 ) && ( iCol==0 ) ) return 0;
        if ( ( iRow==0 ) && ( iCol==1 ) ) return 1;
        if ( ( iRow==0 ) && ( iCol==2 ) ) return 2;
        if ( ( iRow==0 ) && ( iCol==3 ) ) return 3;
        if ( ( iRow==1 ) && ( iCol==0 ) ) return 4;
        if ( ( iRow==1 ) && ( iCol==1 ) ) return 5;
        if ( ( iRow==1 ) && ( iCol==2 ) ) return 6;
        if ( ( iRow==1 ) && ( iCol==3 ) ) return 7;
        if ( ( iRow==2 ) && ( iCol==0 ) ) return 8;
        if ( ( iRow==2 ) && ( iCol==1 ) ) return 9;
        if ( ( iRow==2 ) && ( iCol==2 ) ) return 10;
        if ( ( iRow==2 ) && ( iCol==3 ) ) return 11;
        if ( ( iRow==3 ) && ( iCol==0 ) ) return 12;
        if ( ( iRow==3 ) && ( iCol==1 ) ) return 13;
        if ( ( iRow==3 ) && ( iCol==2 ) ) return 14;
        if ( ( iRow==3 ) && ( iCol==3 ) ) return 15;
        return 0;
}



//      Change vector by matrix only rotation
inline void K3dMatrix::ChangeVector ( K3dVector3& rkVec )
{
        K3dVector3 kResult;


        // Rotate
        kResult[0] = ( rkVec[0] * m_afMatrix[0] ) + ( rkVec[1] * m_afMatrix[1] ) + ( rkVec[2] * m_afMatrix[2] ) /* + m_afMatrix[12]*/;
        kResult[1] = ( rkVec[0] * m_afMatrix[4] ) + ( rkVec[1] * m_afMatrix[5] ) + ( rkVec[2] * m_afMatrix[6] ) /* + m_afMatrix[13]*/;
        kResult[2] = ( rkVec[0] * m_afMatrix[8] ) + ( rkVec[1] * m_afMatrix[9] ) + ( rkVec[2] * m_afMatrix[10] ) /* + m_afMatrix[14]*/;


        rkVec = kResult;
}



//      Change vector by matrix with tranform
inline void K3dMatrix::ChangeVectorTran ( K3dVector3& rkVec )
{
        K3dVector3 kResult;


        // Rotate
        kResult[0] = ( rkVec[0] * m_afMatrix[0] ) + ( rkVec[1] * m_afMatrix[1] ) + ( rkVec[2] * m_afMatrix[2] ) + m_afMatrix[12];
        kResult[1] = ( rkVec[0] * m_afMatrix[4] ) + ( rkVec[1] * m_afMatrix[5] ) + ( rkVec[2] * m_afMatrix[6] ) + m_afMatrix[13];
        kResult[2] = ( rkVec[0] * m_afMatrix[8] ) + ( rkVec[1] * m_afMatrix[9] ) + ( rkVec[2] * m_afMatrix[10] ) + m_afMatrix[14];


        rkVec = kResult;
}



//      Translate vector by matrix
inline void K3dMatrix::TranslateVector ( K3dVector3& rkVec )
{
        K3dVector3 kResult;


        // Rotate
        kResult[0] = rkVec[0] +  m_afMatrix[12];
        kResult[1] = rkVec[1] +  m_afMatrix[13];
        kResult[2] = rkVec[2] +  m_afMatrix[14];


        rkVec = kResult;
}



inline void K3dMatrix::ChangeVectorGL ( K3dVector3& _rkVec )
{
        K3dVector3 kResult;

        kResult[0] = ( _rkVec[0] * m_afMatrix[0] ) + ( _rkVec[1] * m_afMatrix[4] ) + ( _rkVec[2] * m_afMatrix[8] );
        kResult[1] = ( _rkVec[0] * m_afMatrix[1] ) + ( _rkVec[1] * m_afMatrix[5] ) + ( _rkVec[2] * m_afMatrix[9] );
        kResult[2] = ( _rkVec[0] * m_afMatrix[2] ) + ( _rkVec[1] * m_afMatrix[6] ) + ( _rkVec[2] * m_afMatrix[10] );

        _rkVec = kResult;
}



//      Change X vector coordination by matrix
inline void K3dMatrix::ChangeVectorX ( K3dVector3& rkVec )
{
        float fVec;

        fVec = ( rkVec[0] * m_afMatrix[0] ) + ( rkVec[1] * m_afMatrix[1] ) + ( rkVec[2] * m_afMatrix[2] );
        rkVec[0] = fVec;

}



inline void K3dMatrix::ChangeVectorXGL ( K3dVector3& _rkVec )
{
        float fVec = ( _rkVec[0] * m_afMatrix[0] ) + ( _rkVec[1] * m_afMatrix[4] ) + ( _rkVec[2] * m_afMatrix[8] );
        _rkVec[0] = fVec;
}



//      Change Y vector coordination by matrix
inline void K3dMatrix::ChangeVectorY ( K3dVector3& _rkVec )
{
        float fVec;

        fVec = _rkVec[0] * m_afMatrix[4] + _rkVec[1] * m_afMatrix[5] + _rkVec[2] * m_afMatrix[6];
        _rkVec[1] = fVec;
}



inline void K3dMatrix::ChangeVectorYGL ( K3dVector3& _rkVec )
{
        float fVec = _rkVec[0] * m_afMatrix[1] + _rkVec[1] * m_afMatrix[5] + _rkVec[2] * m_afMatrix[9];
        _rkVec[1] = fVec;
}



//      Change Z vector coordination by matrix
inline void K3dMatrix::ChangeVectorZ ( K3dVector3& rkVec )
{
        float fVec;

        fVec = rkVec[0] * m_afMatrix[8] + rkVec[1] * m_afMatrix[9] + rkVec[2] * m_afMatrix[10];
        rkVec[2] = fVec;
}



inline void K3dMatrix::ChangeVectorZGL ( K3dVector3& _rkVec )
{
        float fVec = _rkVec[0] * m_afMatrix[2] + _rkVec[1] * m_afMatrix[6] + _rkVec[2] * m_afMatrix[10];
        _rkVec[2] = fVec;
}




// Calculate determinant
inline float K3dMatrix::Determinant ()
{
        float fA0 = m_afMatrix[ 0]*m_afMatrix[ 5] - m_afMatrix[ 1]*m_afMatrix[ 4];
        float fA1 = m_afMatrix[ 0]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 4];
        float fA2 = m_afMatrix[ 0]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 4];
        float fA3 = m_afMatrix[ 1]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 5];
        float fA4 = m_afMatrix[ 1]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 5];
        float fA5 = m_afMatrix[ 2]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 6];
        float fB0 = m_afMatrix[ 8]*m_afMatrix[13] - m_afMatrix[ 9]*m_afMatrix[12];
        float fB1 = m_afMatrix[ 8]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[12];
        float fB2 = m_afMatrix[ 8]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[12];
        float fB3 = m_afMatrix[ 9]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[13];
        float fB4 = m_afMatrix[ 9]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[13];
        float fB5 = m_afMatrix[10]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[14];
        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
        return fDet;
}



inline void K3dMatrix::Inverse()
{
        float fA0 = m_afMatrix[ 0]*m_afMatrix[ 5] - m_afMatrix[ 1]*m_afMatrix[ 4];
        float fA1 = m_afMatrix[ 0]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 4];
        float fA2 = m_afMatrix[ 0]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 4];
        float fA3 = m_afMatrix[ 1]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 5];
        float fA4 = m_afMatrix[ 1]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 5];
        float fA5 = m_afMatrix[ 2]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 6];
        float fB0 = m_afMatrix[ 8]*m_afMatrix[13] - m_afMatrix[ 9]*m_afMatrix[12];
        float fB1 = m_afMatrix[ 8]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[12];
        float fB2 = m_afMatrix[ 8]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[12];
        float fB3 = m_afMatrix[ 9]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[13];
        float fB4 = m_afMatrix[ 9]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[13];
        float fB5 = m_afMatrix[10]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[14];

        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;

        float kInv[16];
        kInv[0] = + m_afMatrix[ 5]*fB5 - m_afMatrix[ 6]*fB4 + m_afMatrix[ 7]*fB3;
        kInv[1] = - m_afMatrix[ 1]*fB5 + m_afMatrix[ 2]*fB4 - m_afMatrix[ 3]*fB3;
        kInv[2] = + m_afMatrix[13]*fA5 - m_afMatrix[14]*fA4 + m_afMatrix[15]*fA3;
        kInv[3] = - m_afMatrix[ 9]*fA5 + m_afMatrix[10]*fA4 - m_afMatrix[11]*fA3;
        kInv[4] = - m_afMatrix[ 4]*fB5 + m_afMatrix[ 6]*fB2 - m_afMatrix[ 7]*fB1;
        kInv[5] = + m_afMatrix[ 0]*fB5 - m_afMatrix[ 2]*fB2 + m_afMatrix[ 3]*fB1;
        kInv[6] = - m_afMatrix[12]*fA5 + m_afMatrix[14]*fA2 - m_afMatrix[15]*fA1;
        kInv[7] = + m_afMatrix[ 8]*fA5 - m_afMatrix[10]*fA2 + m_afMatrix[11]*fA1;
        kInv[8] = + m_afMatrix[ 4]*fB4 - m_afMatrix[ 5]*fB2 + m_afMatrix[ 7]*fB0;
        kInv[9] = - m_afMatrix[ 0]*fB4 + m_afMatrix[ 1]*fB2 - m_afMatrix[ 3]*fB0;
        kInv[10] = + m_afMatrix[12]*fA4 - m_afMatrix[13]*fA2 + m_afMatrix[15]*fA0;
        kInv[11] = - m_afMatrix[ 8]*fA4 + m_afMatrix[ 9]*fA2 - m_afMatrix[11]*fA0;
        kInv[12] = - m_afMatrix[ 4]*fB3 + m_afMatrix[ 5]*fB1 - m_afMatrix[ 6]*fB0;
        kInv[13] = + m_afMatrix[ 0]*fB3 - m_afMatrix[ 1]*fB1 + m_afMatrix[ 2]*fB0;
        kInv[14] = - m_afMatrix[12]*fA3 + m_afMatrix[13]*fA1 - m_afMatrix[14]*fA0;
        kInv[15] = + m_afMatrix[ 8]*fA3 - m_afMatrix[ 9]*fA1 + m_afMatrix[10]*fA0;

        float fInvDet = ( ( float ) 1.0 ) /fDet;

        for ( int i=0 ; i<16; i++ )
        {
                kInv[i] *= fInvDet;
                m_afMatrix[i] = kInv[i];
        }
}



// Inverse opengl matrix
inline void K3dMatrix::InverseGL()
{
        ToOrFromGL();

        float fA0 = m_afMatrix[ 0]*m_afMatrix[ 5] - m_afMatrix[ 1]*m_afMatrix[ 4];
        float fA1 = m_afMatrix[ 0]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 4];
        float fA2 = m_afMatrix[ 0]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 4];
        float fA3 = m_afMatrix[ 1]*m_afMatrix[ 6] - m_afMatrix[ 2]*m_afMatrix[ 5];
        float fA4 = m_afMatrix[ 1]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 5];
        float fA5 = m_afMatrix[ 2]*m_afMatrix[ 7] - m_afMatrix[ 3]*m_afMatrix[ 6];
        float fB0 = m_afMatrix[ 8]*m_afMatrix[13] - m_afMatrix[ 9]*m_afMatrix[12];
        float fB1 = m_afMatrix[ 8]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[12];
        float fB2 = m_afMatrix[ 8]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[12];
        float fB3 = m_afMatrix[ 9]*m_afMatrix[14] - m_afMatrix[10]*m_afMatrix[13];
        float fB4 = m_afMatrix[ 9]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[13];
        float fB5 = m_afMatrix[10]*m_afMatrix[15] - m_afMatrix[11]*m_afMatrix[14];

        float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;

        float kInv[16];
        kInv[0] = + m_afMatrix[ 5]*fB5 - m_afMatrix[ 6]*fB4 + m_afMatrix[ 7]*fB3;
        kInv[1] = - m_afMatrix[ 1]*fB5 + m_afMatrix[ 2]*fB4 - m_afMatrix[ 3]*fB3;
        kInv[2] = + m_afMatrix[13]*fA5 - m_afMatrix[14]*fA4 + m_afMatrix[15]*fA3;
        kInv[3] = - m_afMatrix[ 9]*fA5 + m_afMatrix[10]*fA4 - m_afMatrix[11]*fA3;
        kInv[4] = - m_afMatrix[ 4]*fB5 + m_afMatrix[ 6]*fB2 - m_afMatrix[ 7]*fB1;
        kInv[5] = + m_afMatrix[ 0]*fB5 - m_afMatrix[ 2]*fB2 + m_afMatrix[ 3]*fB1;
        kInv[6] = - m_afMatrix[12]*fA5 + m_afMatrix[14]*fA2 - m_afMatrix[15]*fA1;
        kInv[7] = + m_afMatrix[ 8]*fA5 - m_afMatrix[10]*fA2 + m_afMatrix[11]*fA1;
        kInv[8] = + m_afMatrix[ 4]*fB4 - m_afMatrix[ 5]*fB2 + m_afMatrix[ 7]*fB0;
        kInv[9] = - m_afMatrix[ 0]*fB4 + m_afMatrix[ 1]*fB2 - m_afMatrix[ 3]*fB0;
        kInv[10] = + m_afMatrix[12]*fA4 - m_afMatrix[13]*fA2 + m_afMatrix[15]*fA0;
        kInv[11] = - m_afMatrix[ 8]*fA4 + m_afMatrix[ 9]*fA2 - m_afMatrix[11]*fA0;
        kInv[12] = - m_afMatrix[ 4]*fB3 + m_afMatrix[ 5]*fB1 - m_afMatrix[ 6]*fB0;
        kInv[13] = + m_afMatrix[ 0]*fB3 - m_afMatrix[ 1]*fB1 + m_afMatrix[ 2]*fB0;
        kInv[14] = - m_afMatrix[12]*fA3 + m_afMatrix[13]*fA1 - m_afMatrix[14]*fA0;
        kInv[15] = + m_afMatrix[ 8]*fA3 - m_afMatrix[ 9]*fA1 + m_afMatrix[10]*fA0;

        float fInvDet = ( ( float ) 1.0 ) /fDet;

        for ( int i=0 ; i<16; i++ )
        {
                kInv[i] *= fInvDet;
                m_afMatrix[i] = kInv[i];
        }
}



// Flip rows to column (normal matrix to OpenGL or again)
inline void K3dMatrix::FlipRowToColumn ( const K3dMatrix &_rkMatrix )
{
        m_afMatrix[ 0] = _rkMatrix.m_afMatrix[ 0];
        m_afMatrix[ 1] = _rkMatrix.m_afMatrix[ 4];
        m_afMatrix[ 2] = _rkMatrix.m_afMatrix[ 8];
        m_afMatrix[ 3] = _rkMatrix.m_afMatrix[12];
        m_afMatrix[ 4] = _rkMatrix.m_afMatrix[ 1];
        m_afMatrix[ 5] = _rkMatrix.m_afMatrix[ 5];
        m_afMatrix[ 6] = _rkMatrix.m_afMatrix[ 9];
        m_afMatrix[ 7] = _rkMatrix.m_afMatrix[13];
        m_afMatrix[ 8] = _rkMatrix.m_afMatrix[ 2];
        m_afMatrix[ 9] = _rkMatrix.m_afMatrix[ 6];
        m_afMatrix[10] = _rkMatrix.m_afMatrix[10];
        m_afMatrix[11] = _rkMatrix.m_afMatrix[14];
        m_afMatrix[12] = _rkMatrix.m_afMatrix[ 3];
        m_afMatrix[13] = _rkMatrix.m_afMatrix[ 7];
        m_afMatrix[14] = _rkMatrix.m_afMatrix[11];
        m_afMatrix[15] = _rkMatrix.m_afMatrix[15];
}



// Rotation through an angle about an arbitrary axis
inline void K3dMatrix::Rotation ( const K3dVector3 &_rkAxis  ,const float fAngle )
{
        //                        | C+Ax^2(1-C)   , AxAy(1-C)-AzS , AxAz(1-C)+AyS |
        //RA(angle) = | AxAy(1-C)+AzS , C+Ay^2(1-C)   , AyAz(1-C)-AxS |
        //                        | AxAz(1-C)-AyS , AyAz(1-C)+AxS , C+Az^2(1-C)   |

        // S = sin, C = cos
        K3dVector3 kA = _rkAxis;
        float fS = ( float ) sin ( fAngle );
        float fC = ( float ) cos ( fAngle );
        float f1C = 1.0f - fC;
        float fAxAy = _rkAxis.GetX() *_rkAxis.GetY();
        float fAxAz = _rkAxis.GetX() *_rkAxis.GetZ();
        float fAyAz = _rkAxis.GetY() *_rkAxis.GetZ();
        float fAxS = _rkAxis.GetX() *fS;
        float fAyS = _rkAxis.GetY() *fS;
        float fAzS = _rkAxis.GetZ() *fS;
        float fAx2 = _rkAxis.GetX() *_rkAxis.GetX();
        float fAy2 = _rkAxis.GetY() *_rkAxis.GetY();
        float fAz2 = _rkAxis.GetZ() *_rkAxis.GetZ();
        // Set matrix
        m_afMatrix[0]=fC+ ( fAx2*f1C );     m_afMatrix[1]= ( fAxAy*f1C )-fAzS;  m_afMatrix[2]= ( fAxAz*f1C ) +fAyS;
        m_afMatrix[4]= ( fAxAy*f1C ) +fAzS;     m_afMatrix[5]=fC+ ( fAy2*f1C );         m_afMatrix[6]= ( fAyAz*f1C )-fAxS;
        m_afMatrix[8]= ( fAxAz*f1C )-fAyS;      m_afMatrix[9]= ( fAyAz*f1C ) +fAxS;     m_afMatrix[10]=fC+ ( fAz2*f1C );
}





// GL rotation through an angle about an arbitrary axis
inline void K3dMatrix::RotationGL ( const K3dVector3 &_rkAxis  ,const float fAngle )
{
        //                        | C+Ax^2(1-C)   , AxAy(1-C)-AzS , AxAz(1-C)+AyS |
        //RA(angle) = | AxAy(1-C)+AzS , C+Ay^2(1-C)   , AyAz(1-C)-AxS |
        //                        | AxAz(1-C)-AyS , AyAz(1-C)+AxS , C+Az^2(1-C)   |

        // S = sin, C = cos
        K3dVector3 kA = _rkAxis;
        float fS = ( float ) sin ( fAngle );
        float fC = ( float ) cos ( fAngle );
        float f1C = 1.0f - fC;
        float fAxAy = _rkAxis.GetX() *_rkAxis.GetY();
        float fAxAz = _rkAxis.GetX() *_rkAxis.GetZ();
        float fAyAz = _rkAxis.GetY() *_rkAxis.GetZ();
        float fAxS = _rkAxis.GetX() *fS;
        float fAyS = _rkAxis.GetY() *fS;
        float fAzS = _rkAxis.GetZ() *fS;
        float fAx2 = _rkAxis.GetX() *_rkAxis.GetX();
        float fAy2 = _rkAxis.GetY() *_rkAxis.GetY();
        float fAz2 = _rkAxis.GetZ() *_rkAxis.GetZ();
        // Set matrix
        m_afMatrix[0]=fC+ ( fAx2*f1C );     m_afMatrix[4]= ( fAxAy*f1C )-fAzS;  m_afMatrix[8]= ( fAxAz*f1C ) +fAyS;
        m_afMatrix[1]= ( fAxAy*f1C ) +fAzS;     m_afMatrix[5]=fC+ ( fAy2*f1C );         m_afMatrix[9]= ( fAyAz*f1C )-fAxS;
        m_afMatrix[2]= ( fAxAz*f1C )-fAyS;      m_afMatrix[6]= ( fAyAz*f1C ) +fAxS;     m_afMatrix[10]=fC+ ( fAz2*f1C );
}

// Calculate linear system of three equations
inline K3dVector3 K3dMatrix::LinearSystem ( const K3dVector4 &_rkEq1,const K3dVector4 &_rkEq2,const K3dVector4 &_rkEq3 )
{
        K3dVector3 kV;
        K3dVector4 kV4;
        K3dMatrix kM;
        float fScalar;
        //float *pM;

        // Set equations to the matrix
        kM.SetRow ( _rkEq1, 0 );
        kM.SetRow ( _rkEq2, 1 );
        kM.SetRow ( _rkEq3, 2 );

        // Set scalar for multiplication row 1
        // Column 1 of row 1 must equal to 1

        fScalar = kM[0];
        if ( fScalar == 0 ) return kV;
        fScalar = 1/fScalar;
        // Multiply row 1 by scalar
        kM.MultiplyRow ( fScalar, 0 );

        // Set scalar for add a multiple row 1 to row 2
        // Column 1 of row 2 must equal to 0
        fScalar = -kM[4];
        // Get row 1 to 4D vector
        kM.GetRow ( kV4, 0 );
        // Multiply row 1 by scalar
        kM.MultiplyRow ( fScalar, 0 );
        // Add row 1 to row 2
        kM.AddRow ( 1, 0 );
        // Set previous row 1
        kM.SetRow ( kV4, 0 );

        // Set scalar for add a multiple row 1 to row 3
        // Column 1 of row 3 must equal to 0
        fScalar = -kM[8];
        // Multiply row 1 by scalar
        kM.MultiplyRow ( fScalar, 0 );
        // Add row 1 to row 3
        kM.AddRow ( 2, 0 );
        // Set previous row 1
        kM.SetRow ( kV4, 0 );



        // Set scalar for multiplication row 2
        // Column 2 of row 2 must equal to 1

        fScalar = kM[5];
        if ( fScalar == 0 ) return kV;
        fScalar = 1/fScalar;
        // Multiply row 2 by scalar
        kM.MultiplyRow ( fScalar, 1 );

        // Set scalar for add a multiple row 2 to row 1
        // Column 2 of row 2 must equal to 0
        fScalar = -kM[1];
        // Get row 2 to 4D vector
        kM.GetRow ( kV4, 1 );
        // Multiply row 2 by scalar
        kM.MultiplyRow ( fScalar, 1 );
        // Add row 2 to row 1
        kM.AddRow ( 0, 1 );
        // Set previous row 2
        kM.SetRow ( kV4, 1 );

        // Set scalar for add a multiple row 2 to row 3
        // Column 2 of row 3 must equal to 0
        fScalar = -kM[9];
        // Multiply row 2 by scalar
        kM.MultiplyRow ( fScalar, 1 );
        // Add row 2 to row 3
        kM.AddRow ( 2, 1 );
        // Set previous row 2
        kM.SetRow ( kV4, 1 );



        // Set scalar for multiplication row 3
        // Column 3 of row 3 must equal to 1

        fScalar = kM[10];
        //if(fScalar == 0) return kV;
        if ( fScalar == 0 ) return kV;
        fScalar = 1/fScalar;
        // Multiply row 3 by scalar
        kM.MultiplyRow ( fScalar, 2 );

        // Set scalar for add a multiple row 3 to row 1
        // Column 3 of row 1 must equal to 0
        fScalar = -kM[2];
        // Get row 3 to 4D vector
        kM.GetRow ( kV4, 2 );
        // Multiply row 3 by scalar
        kM.MultiplyRow ( fScalar, 2 );
        // Add row 3 to row 1
        kM.AddRow ( 0, 2 );
        // Set previous row 3
        kM.SetRow ( kV4, 2 );

        // Set scalar for add a multiple row 3 to row 2
        // Column 3 of row 2 must equal to 0
        fScalar = -kM[6];
        // Multiply row 3 by scalar
        kM.MultiplyRow ( fScalar, 2 );
        // Add row 3 to row 2
        kM.AddRow ( 1, 2 );
        // Set previous row 3
        kM.SetRow ( kV4, 2 );


        // Get results from matrix
        kV[0] = kM[3];
        kV[1] = kM[7];
        kV[2] = kM[11];

        return kV;
}



// Change standrard matrix to the OpenGL matrix
inline void K3dMatrix::ToOrFromGL()
{
        K3dMatrix kM = m_afMatrix;

        m_afMatrix[1] = kM[4];
        m_afMatrix[2] = kM[8];
        m_afMatrix[3] = kM[12];
        m_afMatrix[4] = kM[1];
        m_afMatrix[6] = kM[9];
        m_afMatrix[7] = kM[13];
        m_afMatrix[8] = kM[2];
        m_afMatrix[9] = kM[6];
        m_afMatrix[11] = kM[14];
        m_afMatrix[12] = kM[3];
        m_afMatrix[13] = kM[7];
        m_afMatrix[14] = kM[11];

}



// Multiply matrix and vector
// dst = M * src
inline void K3dMatrix::MultMatrixVec ( const K3dVector3 &rkSrc, K3dVector3 &rkDst )
{
        K3dMatrix kElement = m_afMatrix;

        float fW = (
                       rkSrc[0] * kElement ( 3,0 ) +
                       rkSrc[1] * kElement ( 3,1 ) +
                       rkSrc[2] * kElement ( 3,2 ) +
                       kElement ( 3,3 ) );

        if ( fW == ( float ) 0 )
        {
                cerr  << "Error :: K3dMatrix::MultMatrixVec() -- Divide by zero" << fW << endl;
        }
        fW = ( float ) 1;


        rkDst[0]  = (
                        rkSrc[0] * kElement ( 0,0 ) +
                        rkSrc[1] * kElement ( 0,1 ) +
                        rkSrc[2] * kElement ( 0,2 ) +
                        kElement ( 0,3 ) ) / fW;
        rkDst[1]  = (
                        rkSrc[0] * kElement ( 1,0 ) +
                        rkSrc[1] * kElement ( 1,1 ) +
                        rkSrc[2] * kElement ( 1,2 ) +
                        kElement ( 1,3 ) ) / fW;
        rkDst[2]  = (
                        rkSrc[0] * kElement ( 2,0 ) +
                        rkSrc[1] * kElement ( 2,1 ) +
                        rkSrc[2] * kElement ( 2,2 ) +
                        kElement ( 2,3 ) ) / fW;
}



// Multiply matrix and vector
// dst = M * src
inline void K3dMatrix::MultMatrixVec ( K3dVector3 & rkSrcAndDst )
{
        //MultMatrixVec(K3dVector3(rkSrcAndDst), rkSrcAndDst);
        MultMatrixVec ( rkSrcAndDst, rkSrcAndDst );
}




// Multiply vector and matrix
// dst = src * M
inline void K3dMatrix::MultVecMatrix ( const K3dVector3 &rkSrc, K3dVector3 &rkDst )
{
        K3dMatrix kElement = m_afMatrix;

        float fW = (
                       rkSrc[0] * kElement ( 0,3 ) +
                       rkSrc[1] * kElement ( 1,3 ) +
                       rkSrc[2] * kElement ( 2,3 ) +
                       kElement ( 3,3 ) );

        if ( fW == ( float ) 0 )
        {
                cerr  << "Error :: K3dMatrix::MultVecMatrix()  -- Divide by zero" << fW << endl;
        }

        rkDst[0]  = (
                        rkSrc[0] * kElement ( 0,0 ) +
                        rkSrc[1] * kElement ( 1,0 ) +
                        rkSrc[2] * kElement ( 2,0 ) +
                        kElement ( 3,0 ) ) / fW;
        rkDst[1]  = (
                        rkSrc[0] * kElement ( 0,1 ) +
                        rkSrc[1] * kElement ( 1,1 ) +
                        rkSrc[2] * kElement ( 2,1 ) +
                        kElement ( 3,1 ) ) / fW;
        rkDst[2]  = (
                        rkSrc[0] * kElement ( 0,2 ) +
                        rkSrc[1] * kElement ( 1,2 ) +
                        rkSrc[2] * kElement ( 2,2 ) +
                        kElement ( 3,2 ) ) / fW;
}




// Multiply vector and matrix
// dst = src * M
inline void K3dMatrix::MultVecMatrix ( K3dVector3 & rkSrcAndDst )
{
        //MultVecMatrix(K3dVector3(rkSrcAndDst), rkSrcAndDst);
        MultVecMatrix ( rkSrcAndDst, rkSrcAndDst );
}



// Multiply directional matrix and vector
// dst = M * src
inline void K3dMatrix::MultMatrixDir ( const  K3dVector3 &rkSrc, K3dVector3 &rkDst )
{
        K3dMatrix kElement = m_afMatrix;

        rkDst[0]  = (
                        rkSrc[0] * kElement ( 0,0 ) +
                        rkSrc[1] * kElement ( 0,1 ) +
                        rkSrc[2] * kElement ( 0,2 ) ) ;
        rkDst[1]  = (
                        rkSrc[0] * kElement ( 1,0 ) +
                        rkSrc[1] * kElement ( 1,1 ) +
                        rkSrc[2] * kElement ( 1,2 ) ) ;
        rkDst[2]  = (
                        rkSrc[0] * kElement ( 2,0 ) +
                        rkSrc[1] * kElement ( 2,1 ) +
                        rkSrc[2] * kElement ( 2,2 ) ) ;
}



// Multiply directional matrix and vector
// dst = src * M
inline void K3dMatrix::MultMatrixDir ( K3dVector3 & rkSrcAndDst )
{
        //MultMatrixDir(K3dVector3(rkSrcAndDst), rkSrcAndDst);
        MultMatrixDir ( rkSrcAndDst, rkSrcAndDst );
}



// Multiply vector and directional matrix
// dst = src * M
inline void K3dMatrix::MultDirMatrix ( const  K3dVector3 &rkSrc, K3dVector3 &rkDst )
{
        K3dMatrix kElement = m_afMatrix;

        rkDst[0]  = (
                        rkSrc[0] * kElement ( 0,0 ) +
                        rkSrc[1] * kElement ( 1,0 ) +
                        rkSrc[2] * kElement ( 2,0 ) ) ;
        rkDst[1]  = (
                        rkSrc[0] * kElement ( 0,1 ) +
                        rkSrc[1] * kElement ( 1,1 ) +
                        rkSrc[2] * kElement ( 2,1 ) ) ;
        rkDst[2]  = (
                        rkSrc[0] * kElement ( 0,2 ) +
                        rkSrc[1] * kElement ( 1,2 ) +
                        rkSrc[2] * kElement ( 2,2 ) ) ;
}



// Multiply vector and directional matrix
// dst = src * M
inline void K3dMatrix::MultDirMatrix ( K3dVector3 & rkSrcAndDst )
{
        //MultDirMatrix(K3dVector3(rkSrcAndDst), rkSrcAndDst);
        MultDirMatrix ( rkSrcAndDst, rkSrcAndDst );
}

---