D:/Zythum/DinoKod/Common/Matrix.h

#ifndef __MATRIX_H__
#define __MATRIX_H__

#include "Common/CommonDll.h"
#include <math.h>
#include <memory.h>
#include "Common/Types.h"

//---------------------------------------------------------------------------------------------------------------------
class COMMON_API KMatrix
{
protected:
        inline void lubksb( KMatrix& a, int *indx, float *b)
        {
                int                     i, j, ii = -1, ip;
                float           sum;
                
                for (i=0; i<4; i++)
                {
                        ip = indx[i];
                        sum = b[ip];
                        b[ip] = b[i];
                        if (ii>=0)
                        {
                                for (j=ii; j<=i-1; j++)
                                {
                                        sum -= a.m_Matrix[i][j] * b[j];
                                }
                        }
                        else
                        if (sum != 0.0)
                        {
                                ii = i;
                        }

                        b[i] = sum;
                }
                
                for (i=3; i>=0; i--)
                {
                        sum = b[i];
                        
                        for (j=i+1; j<4; j++)
                        {
                                sum -= a.m_Matrix[i][j] * b[j];
                        }
                        
                        b[i] = sum/a.m_Matrix[i][i];
                }
        }
        
        inline void ludcmp( KMatrix &a, int *indx, float *d)
        {
                float           vv[4];
                float           big, dum, sum, tmp;
                int                     i, imax, j, k;
                
                *d = 1.0f;
                
                for (i=0; i<4; i++)
                {
                        big = 0.0f;
                        for (j=0; j<4; j++)
                        {
                                if ((tmp = (float) fabs(a.m_Matrix[i][j])) > big)
                                {
                                        big = tmp;
                                }
                        }
                        /* if (big == 0.0f) { printf("ludcmp(): singular matrix found...\n"); exit(1); } */
                        
                        vv[i] = 1.0f/big;
                }
                
                for (j=0; j<4; j++)
                {
                        for (i=0; i<j; i++)
                        {
                                sum = a.m_Matrix[i][j];
                                for (k=0; k<i; k++)
                                {
                                        sum -= a.m_Matrix[i][k] * a.m_Matrix[k][j];
                                } a.m_Matrix[i][j] = sum;
                        }
                        
                        big = 0.0f;
                        for (i=j; i<4; i++)
                        {
                                sum = a.m_Matrix[i][j];
                                for (k=0; k<j; k++)
                                {
                                        sum -= a.m_Matrix[i][k]*a.m_Matrix[k][j];
                                }
                                
                                a.m_Matrix[i][j] = sum;
                                
                                if ((dum = vv[i] * (float)fabs(sum)) >= big)
                                {
                                        big = dum; imax = i;
                                }
                        }
                        
                        if (j != imax)
                        {
                                for (k=0; k<4; k++)
                                {
                                        dum = a.m_Matrix[imax][k];
                                        a.m_Matrix[imax][k] = a.m_Matrix[j][k];
                                        a.m_Matrix[j][k] = dum;
                                }
                                
                                *d = -(*d);
                                vv[imax] = vv[j];
                        }
                        
                        indx[j] = imax;
                        
                        if (a.m_Matrix[j][j] == 0.0f)
                        {
                                a.m_Matrix[j][j] = 1.0e-20f;      /* can be 0.0 also... */
                        }
                        
                        if (j != 3)
                        {
                                dum = 1.0f/a.m_Matrix[j][j];
                                for (i=j+1; i<4; i++)
                                {
                                        a.m_Matrix[i][j] *= dum;
                                }
                        }
                }
        }



public:
        union
        {
                struct
                {
                        float   _11, _12, _13, _14;
                        float   _21, _22, _23, _24;
                        float   _31, _32, _33, _34;
                        float   _41, _42, _43, _44;
                };
                struct
                {
                        float   m_Matrix[4][4];
                };
        };
        // Constructeur
        inline KMatrix()
        {
                _11 = _12 = _13 = _14 = 0.0f;
                _21 = _22 = _23 = _24 = 0.0f;
                _31 = _32 = _33 = _34 = 0.0f;
                _41 = _42 = _43 = _44 = 0.0f;
        }

        inline KMatrix( float _11, float _12, float _13, float _14,
                                        float _21, float _22, float _23, float _24,
                                        float _31, float _32, float _33, float _34,
                                        float _41, float _42, float _43, float _44 )
        {
                this->_11 = _11, this->_12 = _12, this->_13 = _13, this->_14 = _14;
                this->_21 = _21, this->_22 = _22, this->_23 = _23, this->_24 = _24;
                this->_31 = _31, this->_32 = _32, this->_33 = _33, this->_34 = _34;
                this->_41 = _41, this->_42 = _42, this->_43 = _43, this->_44 = _44;
        }

        // Operateur
        inline KMatrix operator * ( KMatrix& matrix )
        {
                KMatrix result;

                result._11 = _11 * matrix._11 + _12 * matrix._21 + _13 * matrix._31 + _14 * matrix._41;
                result._12 = _11 * matrix._12 + _12 * matrix._22 + _13 * matrix._32 + _14 * matrix._42;
                result._13 = _11 * matrix._13 + _12 * matrix._23 + _13 * matrix._33 + _14 * matrix._43;
                result._14 = _11 * matrix._14 + _12 * matrix._24 + _13 * matrix._34 + _14 * matrix._44;
        
                result._21 = _21 * matrix._11 + _22 * matrix._21 + _23 * matrix._31 + _24 * matrix._41;
                result._22 = _21 * matrix._12 + _22 * matrix._22 + _23 * matrix._32 + _24 * matrix._42;
                result._23 = _21 * matrix._13 + _22 * matrix._23 + _23 * matrix._33 + _24 * matrix._43;
                result._24 = _21 * matrix._14 + _22 * matrix._24 + _23 * matrix._34 + _24 * matrix._44;

                result._31 = _31 * matrix._11 + _32 * matrix._21 + _33 * matrix._31 + _34 * matrix._41;
                result._32 = _31 * matrix._12 + _32 * matrix._22 + _33 * matrix._32 + _34 * matrix._42;
                result._33 = _31 * matrix._13 + _32 * matrix._23 + _33 * matrix._33 + _34 * matrix._43;
                result._34 = _31 * matrix._14 + _32 * matrix._24 + _33 * matrix._34 + _34 * matrix._44;

                result._41 = _41 * matrix._11 + _42 * matrix._21 + _43 * matrix._31 + _44 * matrix._41;
                result._42 = _41 * matrix._12 + _42 * matrix._22 + _43 * matrix._32 + _44 * matrix._42;
                result._43 = _41 * matrix._13 + _42 * matrix._23 + _43 * matrix._33 + _44 * matrix._43;
                result._44 = _41 * matrix._14 + _42 * matrix._24 + _43 * matrix._34 + _44 * matrix._44;

                return result;
        }

        inline void operator *= ( KMatrix& matrix )
        {
                KMatrix result;
                
                result._11 = _11 * matrix._11 + _12 * matrix._21 + _13 * matrix._31 + _14 * matrix._41;
                result._12 = _11 * matrix._12 + _12 * matrix._22 + _13 * matrix._32 + _14 * matrix._42;
                result._13 = _11 * matrix._13 + _12 * matrix._23 + _13 * matrix._33 + _14 * matrix._43;
                result._14 = _11 * matrix._14 + _12 * matrix._24 + _13 * matrix._34 + _14 * matrix._44;
        
                result._21 = _21 * matrix._11 + _22 * matrix._21 + _23 * matrix._31 + _24 * matrix._41;
                result._22 = _21 * matrix._12 + _22 * matrix._22 + _23 * matrix._32 + _24 * matrix._42;
                result._23 = _21 * matrix._13 + _22 * matrix._23 + _23 * matrix._33 + _24 * matrix._43;
                result._24 = _21 * matrix._14 + _22 * matrix._24 + _23 * matrix._34 + _24 * matrix._44;

                result._31 = _31 * matrix._11 + _32 * matrix._21 + _33 * matrix._31 + _34 * matrix._41;
                result._32 = _31 * matrix._12 + _32 * matrix._22 + _33 * matrix._32 + _34 * matrix._42;
                result._33 = _31 * matrix._13 + _32 * matrix._23 + _33 * matrix._33 + _34 * matrix._43;
                result._34 = _31 * matrix._14 + _32 * matrix._24 + _33 * matrix._34 + _34 * matrix._44;

                result._41 = _41 * matrix._11 + _42 * matrix._21 + _43 * matrix._31 + _44 * matrix._41;
                result._42 = _41 * matrix._12 + _42 * matrix._22 + _43 * matrix._32 + _44 * matrix._42;
                result._43 = _41 * matrix._13 + _42 * matrix._23 + _43 * matrix._33 + _44 * matrix._43;
                result._44 = _41 * matrix._14 + _42 * matrix._24 + _43 * matrix._34 + _44 * matrix._44;

                *this = result;
        }

        bool operator == ( KMatrix& matrix )
        {
                return( memcmp( &matrix, this, sizeof( KMatrix ) ) == 0 );
        }

        bool operator != ( KMatrix& matrix )
        {
                return( memcmp( &matrix, this, sizeof( KMatrix ) ) != 0 );
        }

        inline KVector MultiplyAdd( KVector& v )
        {
                KVector Result;

                Result.x        = _11 * v.x + _21 * v.y + _31 * v.z + _41;
                Result.y        = _12 * v.x + _22 * v.y + _32 * v.z + _42;
                Result.z        = _13 * v.x + _23 * v.y + _33 * v.z + _43;
                
                return Result;
        }

        inline KVector Transform( KVector& v, float* pRhw = NULL )
        {
                KVector Result;
                float   rhw;

                Result.x        = _11 * v.x + _21 * v.y + _31 * v.z + _41;
                Result.y        = _12 * v.x + _22 * v.y + _32 * v.z + _42;
                Result.z        = _13 * v.x + _23 * v.y + _33 * v.z + _43;
                rhw                     = _14 * v.x + _24 * v.y + _34 * v.z + _44;
                
                if( rhw )
                {
                        rhw = 1.0f / rhw;
                        Result.x *= rhw;
                        Result.y *= rhw;
                        Result.z *= rhw;
                }

                if( pRhw )
                        *pRhw = rhw;

                return Result;
        }

        // Fonctions matricielles
        inline void LoadIdentity()
        {
                _11 = _22 = _33 = _44 = 1.0f;
                _12 = _13 = _14 = 0.0f;
                _21 = _23 = _24 = 0.0f;
                _31 = _32 = _34 = 0.0f;
                _41 = _42 = _43 = 0.0f;
        }

        inline void Inverse2()
        {
                int                     i, j, indx[4];
                float           d, col[4];
                KMatrix         n;

                n = *this;
                ludcmp( n, indx, &d );

                for(j = 0;j < 4;j ++)
                {
                        for(i = 0;i < 4;i ++)
                        {
                                col[i] = 0.0f;
                        }
                        col[j] = 1.0f;
                        
                        lubksb(n, indx, col);
                        for(i = 0;i < 4;i ++)
                        {
                                m_Matrix[i][j] = col[i];
                        }
                }
        }

        //------------------------------
        inline float det2x2( float a, float b, float c, float d )
        {
                return a*d - b*c;
        }

        inline float det3x3( float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3 )
        {
                return  a1 * det2x2(b2, b3, c2, c3)     - b1 * det2x2(a2, a3, c2, c3) + c1 * det2x2(a2, a3, b2, b3);
        }

        float Determinant ( const KMatrix& m )
        {
                float a1 = m._11;       float a2 = m._21;       float a3 = m._31;       float a4 = m._41;
                float b1 = m._12;       float b2 = m._22;       float b3 = m._32;       float b4 = m._42;
                float c1 = m._13;       float c2 = m._23;       float c3 = m._33;       float c4 = m._43;
                float d1 = m._14;       float d2 = m._24;       float d3 = m._34;       float d4 = m._44;

                return  a1 * det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4)
                        -       b1 * det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4)
                        +       c1 * det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4)
                        -       d1 * det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
        }

        KMatrix Adjoint( const KMatrix& m )
        {
                float a1 = m._11;       float a2 = m._12;       float a3 = m._13;       float a4 = m._14;
                float b1 = m._21;       float b2 = m._22;       float b3 = m._23;       float b4 = m._24;
                float c1 = m._31;       float c2 = m._32;       float c3 = m._33;       float c4 = m._34;
                float d1 = m._41;       float d2 = m._42;       float d3 = m._43;       float d4 = m._44;

                // Adjoint(x,y) = -1^(x+y) * a(y,x)
                // Where a(i,j) is the 3x3 determinant of m with row i and col j removed
                KMatrix retVal;
                retVal._11 = det3x3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
                retVal._12 = -det3x3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
                retVal._13 = det3x3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
                retVal._14 = -det3x3(a2, a3, a4, b2, b3, b4, c2, c3, c4);
                
                retVal._21 = -det3x3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
                retVal._22 = det3x3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
                retVal._23 = -det3x3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
                retVal._24 = det3x3(a1, a3, a4, b1, b3, b4, c1, c3, c4);
                
                retVal._31 = det3x3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
                retVal._32 = -det3x3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
                retVal._33 = det3x3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
                retVal._34 = -det3x3(a1, a2, a4, b1, b2, b4, c1, c2, c4);
                
                retVal._41 = -det3x3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
                retVal._42 = det3x3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
                retVal._43 = -det3x3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
                retVal._44 = det3x3(a1, a2, a3, b1, b2, b3, c1, c2, c3);
        
                return retVal;
        }

        inline void Inverse()
        {
                float det = Determinant( *this );
//              KASSERT( det );

                *this = Adjoint( *this );
                
                for( int i = 0; i < 4; ++i )
                {
                        for( int j = 0; j < 4; ++j )
                        {
                                m_Matrix[i][j] /= det;
                        }
                }
        }

        inline void Transpose()
        {
                float x;
                
                x = _21;
                _21     = _12;
                _12 = x;

                x = _31;
                _31 = _13;
                _13 = x;

                x = _41;
                _41 = _14;
                _14 = x;

                x = _32;
                _32 = _23;
                _23 = x;

                x = _42;
                _42 = _24;
                _24 = x;

                x = _43;
                _43 = _34;
                _34 = x;
        }

        // Fonctions transformation
        inline void RotX( float Angle )
        {
                float           cosine;
                float           sine;

                cosine  = (float)cos( Angle );
                sine    = (float)sin( Angle );

                KMatrix Mat;
                Mat.LoadIdentity();
                Mat._22 = cosine;
                Mat._23 = sine;
                Mat._32 = -sine;
                Mat._33 = cosine;

                *this *= Mat;
        }

        inline void RotY( float Angle )
        {
                float           cosine;
                float           sine;

                cosine  = (float)cos( Angle );
                sine    = (float)sin( Angle );

                KMatrix         Mat;
                Mat.LoadIdentity();
                Mat._11 = cosine;
                Mat._13 = sine;
                Mat._31 = -sine;
                Mat._33 = cosine;
                
                *this *= Mat;
        }

        inline void RotZ( float Angle )
        {
                float           cosine;
                float           sine;

                cosine  = (float)cos( Angle );
                sine    = (float)sin( Angle );

                KMatrix         Mat;
                Mat.LoadIdentity();
                Mat._11 = cosine;
                Mat._12 = -sine;
                Mat._21 = sine;
                Mat._22 = cosine;
                
                *this *= Mat;
        }

        inline void Trans( float x, float y, float z )
        {
                _41 += x;
                _42 += y;
                _43 += z;
        }
        
        inline void Scale( float x, float y, float z )
        {
                KMatrix         Mat;
                Mat.LoadIdentity();
                Mat._11 = x;
                Mat._22 = y;
                Mat._33 = z;
                
                *this *= Mat;
        }
};

#endif // __MATRIX_H__

---