matrix4.hpp

// Copyright (C) 2002-2012 Nikolaus Gebhardt
// This file is part of the "Irrlicht Engine".
// For conditions of distribution and use, see copyright notice in nirtcpp/nirtcpp.hpp
 
#ifndef NIRT_MATRIX_HPP_INCLUDED
#define NIRT_MATRIX_HPP_INCLUDED
 
#include <nirtcpp/core/engine/irrMath.hpp>
#include <nirtcpp/core/engine/vector3d.hpp>
#include <nirtcpp/core/engine/vector2d.hpp>
#include <nirtcpp/core/engine/plane3d.hpp>
#include <nirtcpp/core/engine/aabbox3d.hpp>
#include <nirtcpp/core/engine/rect.hpp>
#include <nirtcpp/core/engine/irrString.hpp>
#include <algorithm>
#include <math.h>
 
// enable this to keep track of changes to the matrix
// and make simpler identity check for seldom changing matrices
// otherwise identity check will always compare the elements
//#define USE_MATRIX_TEST
 
// this is only for debugging purposes
//#define USE_MATRIX_TEST_DEBUG
 
#if defined( USE_MATRIX_TEST_DEBUG )
 
class MatrixTest
{
public:
    MatrixTest () : ID(0), Calls(0) {}
    char buf[256];
    int Calls;
    int ID;
};
static MatrixTest MTest;
 
#endif
 
namespace nirt
{
namespace core
{
 
 
    template <typename T>
    class CMatrix4
    {
        public:
 
            enum eConstructor
            {
                EM4CONST_NOTHING = 0,
                EM4CONST_COPY,
                EM4CONST_IDENTITY,
                EM4CONST_TRANSPOSED,
                EM4CONST_INVERSE,
                EM4CONST_INVERSE_TRANSPOSED
            };
 
 
            CMatrix4( eConstructor constructor = EM4CONST_IDENTITY );
 
            CMatrix4(const T& r0c0, const T& r0c1, const T& r0c2, const T& r0c3,
                     const T& r1c0, const T& r1c1, const T& r1c2, const T& r1c3,
                     const T& r2c0, const T& r2c1, const T& r2c2, const T& r2c3,
                     const T& r3c0, const T& r3c1, const T& r3c2, const T& r3c3)
            {
                M[0]  = r0c0; M[1]  = r0c1; M[2]  = r0c2; M[3]  = r0c3;
                M[4]  = r1c0; M[5]  = r1c1; M[6]  = r1c2; M[7]  = r1c3;
                M[8]  = r2c0; M[9]  = r2c1; M[10] = r2c2; M[11] = r2c3;
                M[12] = r3c0; M[13] = r3c1; M[14] = r3c2; M[15] = r3c3;
            }
 
 
            CMatrix4(const CMatrix4<T>& other, eConstructor constructor = EM4CONST_COPY);
 
            T& operator()(const s32 row, const s32 col)
            {
#if defined ( USE_MATRIX_TEST )
                definitelyIdentityMatrix=false;
#endif
                return M[ row * 4 + col ];
            }
 
            const T& operator()(const s32 row, const s32 col) const { return M[row * 4 + col]; }
 
            T& operator[](u32 index)
            {
#if defined ( USE_MATRIX_TEST )
                definitelyIdentityMatrix=false;
#endif
                return M[index];
            }
 
            const T& operator[](u32 index) const { return M[index]; }
            
            T & operator[](nirt::u32 row, nirt::u32 col)
            {
#if defined ( USE_MATRIX_TEST )
                definitelyIdentityMatrix=false;
#endif
                return M[row*4+col];
            }
            
            const T & operator[](nirt::u32 row, nirt::u32 col) const
            {
                return M[row*4+col];
            }
 
            inline CMatrix4<T>& operator=(const CMatrix4<T> &other);
 
            inline CMatrix4<T>& operator=(const T& scalar);
 
            const T* pointer() const { return M; }
            T* pointer()
            {
#if defined ( USE_MATRIX_TEST )
                definitelyIdentityMatrix=false;
#endif
                return M;
            }
 
            bool operator==(const CMatrix4<T> &other) const;
 
            bool operator!=(const CMatrix4<T> &other) const;
 
            CMatrix4<T> operator+(const CMatrix4<T>& other) const;
 
            CMatrix4<T>& operator+=(const CMatrix4<T>& other);
 
            CMatrix4<T> operator-(const CMatrix4<T>& other) const;
 
            CMatrix4<T>& operator-=(const CMatrix4<T>& other);
 
 
            inline CMatrix4<T>& setbyproduct(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b );
 
 
            CMatrix4<T>& setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b );
 
 
            CMatrix4<T> operator*(const CMatrix4<T>& other) const;
 
 
            CMatrix4<T>& operator*=(const CMatrix4<T>& other);
 
            CMatrix4<T> operator*(const T& scalar) const;
 
            CMatrix4<T>& operator*=(const T& scalar);
 
            inline CMatrix4<T>& makeIdentity();
 
            inline bool isIdentity() const;
 
            inline bool isOrthogonal() const;
 
            bool isIdentity_integer_base () const;
 
            CMatrix4<T>& setTranslation( const vector3d<T>& translation );
 
            vector3d<T> getTranslation() const;
 
            CMatrix4<T>& setInverseTranslation( const vector3d<T>& translation );
 
            inline CMatrix4<T>& setRotationRadians( const vector3d<T>& rotation );
 
            CMatrix4<T>& setRotationDegrees( const vector3d<T>& rotation );
 
 
            core::vector3d<T> getRotationDegrees(const vector3d<T>& scale) const;
 
 
            core::vector3d<T> getRotationDegrees() const;
 
 
            inline CMatrix4<T>& setInverseRotationRadians( const vector3d<T>& rotation );
 
 
            inline CMatrix4<T>& setInverseRotationDegrees( const vector3d<T>& rotation );
 
 
            inline CMatrix4<T>& setRotationAxisRadians(const T& angle, const vector3d<T>& axis);
 
            CMatrix4<T>& setScale( const vector3d<T>& scale );
 
            CMatrix4<T>& setScale( const T scale ) { return setScale(core::vector3d<T>(scale,scale,scale)); }
 
            core::vector3d<T> getScale() const;
 
            void inverseTranslateVect( vector3df& vect ) const;
 
            void inverseRotateVect( vector3df& vect ) const;
 
            void rotateVect( vector3df& vect ) const;
 
            void rotateVect(core::vector3df& out, const core::vector3df& in) const;
 
            void rotateVect(T *out,const core::vector3df &in) const;
 
 
            void transformVect( vector3df& vect) const;
 
 
            void transformVect( vector3df& out, const vector3df& in ) const;
 
 
            void transformVect(T *out,const core::vector3df &in) const;
 
 
            void transformVec3(T *out, const T * in) const;
 
            void transformVec4(T *out, const T * in) const;
 
 
            void translateVect( vector3df& vect ) const;
 
            void transformPlane( core::plane3d<f32> &plane) const;
 
            void transformPlane( const core::plane3d<f32> &in, core::plane3d<f32> &out) const;
 
 
            void transformBox(core::aabbox3d<f32>& box) const;
 
 
            void transformBoxEx(core::aabbox3d<f32>& box) const;
 
            void multiplyWith1x4Matrix(T* matrix) const;
 
 
            bool makeInverse();
 
 
 
            bool getInversePrimitive ( CMatrix4<T>& out ) const;
 
 
            bool getInverse(CMatrix4<T>& out) const;
 
            //\param zClipFromZero: Clipping of z can be projected from 0 to w when true (D3D style) and from -w to w when false (OGL style).
            CMatrix4<T>& buildProjectionMatrixPerspectiveFovRH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            CMatrix4<T>& buildProjectionMatrixPerspectiveFovLH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            CMatrix4<T>& buildProjectionMatrixPerspectiveFovInfinityLH(f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 epsilon=0);
 
            CMatrix4<T>& buildProjectionMatrixPerspectiveRH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            CMatrix4<T>& buildProjectionMatrixPerspectiveLH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            //\param zClipFromZero: Clipping of z can be projected from 0 to 1 when true (D3D style) and from -1 to 1 when false (OGL style).
            CMatrix4<T>& buildProjectionMatrixOrthoLH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            CMatrix4<T>& buildProjectionMatrixOrthoRH(f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero=true);
 
            CMatrix4<T>& buildCameraLookAtMatrixLH(
                    const vector3df& position,
                    const vector3df& target,
                    const vector3df& upVector);
 
            CMatrix4<T>& buildCameraLookAtMatrixRH(
                    const vector3df& position,
                    const vector3df& target,
                    const vector3df& upVector);
 
 
            CMatrix4<T>& buildShadowMatrix(const core::vector3df& light, core::plane3df plane, f32 point=1.0f);
 
 
            CMatrix4<T>& buildNDCToDCMatrix( const core::rect<s32>& area, f32 zScale);
 
 
            CMatrix4<T> interpolate(const core::CMatrix4<T>& b, f32 time) const;
 
            CMatrix4<T> getTransposed() const;
 
            inline void getTransposed( CMatrix4<T>& dest ) const;
 
 
            CMatrix4<T>& buildRotateFromTo(const core::vector3df& from, const core::vector3df& to);
 
 
            void setRotationCenter(const core::vector3df& center, const core::vector3df& translate);
 
 
            void buildAxisAlignedBillboard(const core::vector3df& camPos,
                        const core::vector3df& center,
                        const core::vector3df& translation,
                        const core::vector3df& axis,
                        const core::vector3df& from);
 
            /*
                construct 2D Texture transformations
                rotate about center, scale, and transform.
            */
            CMatrix4<T>& buildTextureTransform( f32 rotateRad,
                    const core::vector2df &rotatecenter,
                    const core::vector2df &translate,
                    const core::vector2df &scale);
 
 
            CMatrix4<T>& setTextureRotationCenter( f32 radAngle );
 
 
            CMatrix4<T>& setTextureTranslate( f32 x, f32 y );
 
 
            void getTextureTranslate( f32& x, f32& y ) const;
 
 
            CMatrix4<T>& setTextureTranslateTransposed( f32 x, f32 y );
 
 
            CMatrix4<T>& setTextureScale( f32 sx, f32 sy );
 
 
            void getTextureScale( f32& sx, f32& sy ) const;
 
 
            CMatrix4<T>& setTextureScaleCenter( f32 sx, f32 sy );
 
            CMatrix4<T>& setM(const T* data);
 
            void setDefinitelyIdentityMatrix( bool isDefinitelyIdentityMatrix);
 
            bool getDefinitelyIdentityMatrix() const;
 
            bool equals(const core::CMatrix4<T>& other, const T tolerance=(T)ROUNDING_ERROR_f64) const;
 
        private:
            T M[16];
#if defined ( USE_MATRIX_TEST )
            mutable u32 definitelyIdentityMatrix;
#endif
#if defined ( USE_MATRIX_TEST_DEBUG )
            u32 id;
            mutable u32 calls;
#endif
 
    };  // class CMatrix4
 
    // Default constructor
    template <class T>
    inline CMatrix4<T>::CMatrix4( eConstructor constructor )
#if defined ( USE_MATRIX_TEST )
        : definitelyIdentityMatrix(BIT_UNTESTED)
#endif
#if defined ( USE_MATRIX_TEST_DEBUG )
        ,id ( MTest.ID++), calls ( 0 )
#endif
    {
        switch ( constructor )
        {
            case EM4CONST_NOTHING:
            case EM4CONST_COPY:
                break;
            case EM4CONST_IDENTITY:
            case EM4CONST_INVERSE:
            default:
                makeIdentity();
                break;
        }
    }
 
    // Copy constructor
    template <class T>
    inline CMatrix4<T>::CMatrix4( const CMatrix4<T>& other, eConstructor constructor)
#if defined ( USE_MATRIX_TEST )
        : definitelyIdentityMatrix(BIT_UNTESTED)
#endif
#if defined ( USE_MATRIX_TEST_DEBUG )
        ,id ( MTest.ID++), calls ( 0 )
#endif
    {
        switch ( constructor )
        {
            case EM4CONST_IDENTITY:
                makeIdentity();
                break;
            case EM4CONST_NOTHING:
                break;
            case EM4CONST_COPY:
                *this = other;
                break;
            case EM4CONST_TRANSPOSED:
                other.getTransposed(*this);
                break;
            case EM4CONST_INVERSE:
                if (!other.getInverse(*this))
                    std::fill_n(M, 16, 0);
                break;
            case EM4CONST_INVERSE_TRANSPOSED:
                if (!other.getInverse(*this))
                    std::fill_n(M, 16, 0);
                else
                    *this=getTransposed();
                break;
        }
    }
 
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::operator+(const CMatrix4<T>& other) const
    {
        CMatrix4<T> temp ( EM4CONST_NOTHING );
 
        temp[0] = M[0]+other[0];
        temp[1] = M[1]+other[1];
        temp[2] = M[2]+other[2];
        temp[3] = M[3]+other[3];
        temp[4] = M[4]+other[4];
        temp[5] = M[5]+other[5];
        temp[6] = M[6]+other[6];
        temp[7] = M[7]+other[7];
        temp[8] = M[8]+other[8];
        temp[9] = M[9]+other[9];
        temp[10] = M[10]+other[10];
        temp[11] = M[11]+other[11];
        temp[12] = M[12]+other[12];
        temp[13] = M[13]+other[13];
        temp[14] = M[14]+other[14];
        temp[15] = M[15]+other[15];
 
        return temp;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator+=(const CMatrix4<T>& other)
    {
        M[0]+=other[0];
        M[1]+=other[1];
        M[2]+=other[2];
        M[3]+=other[3];
        M[4]+=other[4];
        M[5]+=other[5];
        M[6]+=other[6];
        M[7]+=other[7];
        M[8]+=other[8];
        M[9]+=other[9];
        M[10]+=other[10];
        M[11]+=other[11];
        M[12]+=other[12];
        M[13]+=other[13];
        M[14]+=other[14];
        M[15]+=other[15];
 
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::operator-(const CMatrix4<T>& other) const
    {
        CMatrix4<T> temp ( EM4CONST_NOTHING );
 
        temp[0] = M[0]-other[0];
        temp[1] = M[1]-other[1];
        temp[2] = M[2]-other[2];
        temp[3] = M[3]-other[3];
        temp[4] = M[4]-other[4];
        temp[5] = M[5]-other[5];
        temp[6] = M[6]-other[6];
        temp[7] = M[7]-other[7];
        temp[8] = M[8]-other[8];
        temp[9] = M[9]-other[9];
        temp[10] = M[10]-other[10];
        temp[11] = M[11]-other[11];
        temp[12] = M[12]-other[12];
        temp[13] = M[13]-other[13];
        temp[14] = M[14]-other[14];
        temp[15] = M[15]-other[15];
 
        return temp;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator-=(const CMatrix4<T>& other)
    {
        M[0]-=other[0];
        M[1]-=other[1];
        M[2]-=other[2];
        M[3]-=other[3];
        M[4]-=other[4];
        M[5]-=other[5];
        M[6]-=other[6];
        M[7]-=other[7];
        M[8]-=other[8];
        M[9]-=other[9];
        M[10]-=other[10];
        M[11]-=other[11];
        M[12]-=other[12];
        M[13]-=other[13];
        M[14]-=other[14];
        M[15]-=other[15];
 
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::operator*(const T& scalar) const
    {
        CMatrix4<T> temp ( EM4CONST_NOTHING );
 
        temp[0] = M[0]*scalar;
        temp[1] = M[1]*scalar;
        temp[2] = M[2]*scalar;
        temp[3] = M[3]*scalar;
        temp[4] = M[4]*scalar;
        temp[5] = M[5]*scalar;
        temp[6] = M[6]*scalar;
        temp[7] = M[7]*scalar;
        temp[8] = M[8]*scalar;
        temp[9] = M[9]*scalar;
        temp[10] = M[10]*scalar;
        temp[11] = M[11]*scalar;
        temp[12] = M[12]*scalar;
        temp[13] = M[13]*scalar;
        temp[14] = M[14]*scalar;
        temp[15] = M[15]*scalar;
 
        return temp;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator*=(const T& scalar)
    {
        M[0]*=scalar;
        M[1]*=scalar;
        M[2]*=scalar;
        M[3]*=scalar;
        M[4]*=scalar;
        M[5]*=scalar;
        M[6]*=scalar;
        M[7]*=scalar;
        M[8]*=scalar;
        M[9]*=scalar;
        M[10]*=scalar;
        M[11]*=scalar;
        M[12]*=scalar;
        M[13]*=scalar;
        M[14]*=scalar;
        M[15]*=scalar;
 
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator*=(const CMatrix4<T>& other)
    {
#if defined ( USE_MATRIX_TEST )
        // do checks on your own in order to avoid copy creation
        if ( !other.isIdentity() )
        {
            if ( this->isIdentity() )
            {
                return (*this = other);
            }
            else
            {
                CMatrix4<T> temp ( *this );
                return setbyproduct_nocheck( temp, other );
            }
        }
        return *this;
#else
        CMatrix4<T> temp ( *this );
        return setbyproduct_nocheck( temp, other );
#endif
    }
 
    // set this matrix to the product of two other matrices
    // goal is to reduce stack use and copy
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setbyproduct_nocheck(const CMatrix4<T>& other_a,const CMatrix4<T>& other_b )
    {
        const T *m1 = other_a.M;
        const T *m2 = other_b.M;
 
        M[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
        M[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
        M[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
        M[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];
 
        M[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
        M[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
        M[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
        M[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];
 
        M[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
        M[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
        M[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
        M[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];
 
        M[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
        M[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
        M[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
        M[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // set this matrix to the product of two other matrices
    // goal is to reduce stack use and copy
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setbyproduct(const CMatrix4<T>& other_a, const CMatrix4<T>& other_b )
    {
#if defined ( USE_MATRIX_TEST )
        if ( other_a.isIdentity () )
            return (*this = other_b);
        else
        if ( other_b.isIdentity () )
            return (*this = other_a);
        else
            return setbyproduct_nocheck(other_a,other_b);
#else
        return setbyproduct_nocheck(other_a,other_b);
#endif
    }
 
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::operator*(const CMatrix4<T>& m2) const
    {
#if defined ( USE_MATRIX_TEST )
        // Testing purpose..
        if ( this->isIdentity() )
            return m2;
        if ( m2.isIdentity() )
            return *this;
#endif
 
        CMatrix4<T> m3 ( EM4CONST_NOTHING );
 
        const T *m1 = M;
 
        m3[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
        m3[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
        m3[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
        m3[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];
 
        m3[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
        m3[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
        m3[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
        m3[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];
 
        m3[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
        m3[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
        m3[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
        m3[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];
 
        m3[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
        m3[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
        m3[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
        m3[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
        return m3;
    }
 
 
 
    template <class T>
    inline vector3d<T> CMatrix4<T>::getTranslation() const
    {
        return vector3d<T>(M[12], M[13], M[14]);
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTranslation( const vector3d<T>& translation )
    {
        M[12] = translation.X;
        M[13] = translation.Y;
        M[14] = translation.Z;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setInverseTranslation( const vector3d<T>& translation )
    {
        M[12] = -translation.X;
        M[13] = -translation.Y;
        M[14] = -translation.Z;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setScale( const vector3d<T>& scale )
    {
        M[0] = scale.X;
        M[5] = scale.Y;
        M[10] = scale.Z;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    template <class T>
    inline vector3d<T> CMatrix4<T>::getScale() const
    {
        // See http://www.robertblum.com/articles/2005/02/14/decomposing-matrices
 
        // Deal with the 0 rotation case first
        // Prior to Nirtcpp 1.6, we always returned this value.
        if(core::iszero(M[1]) && core::iszero(M[2]) &&
            core::iszero(M[4]) && core::iszero(M[6]) &&
            core::iszero(M[8]) && core::iszero(M[9]))
            return vector3d<T>(M[0], M[5], M[10]);
 
        // We have to do the full calculation.
        return vector3d<T>(sqrtf(M[0] * M[0] + M[1] * M[1] + M[2] * M[2]),
                            sqrtf(M[4] * M[4] + M[5] * M[5] + M[6] * M[6]),
                            sqrtf(M[8] * M[8] + M[9] * M[9] + M[10] * M[10]));
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setRotationDegrees( const vector3d<T>& rotation )
    {
        return setRotationRadians( rotation * core::DEGTORAD );
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setInverseRotationDegrees( const vector3d<T>& rotation )
    {
        return setInverseRotationRadians( rotation * core::DEGTORAD );
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setRotationRadians( const vector3d<T>& rotation )
    {
        const f64 cr = cos( rotation.X );
        const f64 sr = sin( rotation.X );
        const f64 cp = cos( rotation.Y );
        const f64 sp = sin( rotation.Y );
        const f64 cy = cos( rotation.Z );
        const f64 sy = sin( rotation.Z );
 
        M[0] = (T)( cp*cy );
        M[1] = (T)( cp*sy );
        M[2] = (T)( -sp );
 
        const f64 srsp = sr*sp;
        const f64 crsp = cr*sp;
 
        M[4] = (T)( srsp*cy-cr*sy );
        M[5] = (T)( srsp*sy+cr*cy );
        M[6] = (T)( sr*cp );
 
        M[8] = (T)( crsp*cy+sr*sy );
        M[9] = (T)( crsp*sy-sr*cy );
        M[10] = (T)( cr*cp );
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
 
    template <class T>
    inline core::vector3d<T> CMatrix4<T>::getRotationDegrees(const vector3d<T>& scale_) const
    {
        const CMatrix4<T> &mat = *this;
        const core::vector3d<f64> scale(core::iszero(scale_.X) ? FLT_MAX : scale_.X , core::iszero(scale_.Y) ? FLT_MAX : scale_.Y, core::iszero(scale_.Z) ? FLT_MAX : scale_.Z);
        const core::vector3d<f64> invScale(core::reciprocal(scale.X),core::reciprocal(scale.Y),core::reciprocal(scale.Z));
 
        f64 Y = -asin(core::clamp(mat[2]*invScale.X, -1.0, 1.0));
        const f64 C = cos(Y);
        Y *= RADTODEG64;
 
        f64 rotx, roty, X, Z;
 
        if (!core::iszero((T)C))
        {
            const f64 invC = core::reciprocal(C);
            rotx = mat[10] * invC * invScale.Z;
            roty = mat[6] * invC * invScale.Y;
            X = atan2( roty, rotx ) * RADTODEG64;
            rotx = mat[0] * invC * invScale.X;
            roty = mat[1] * invC * invScale.X;
            Z = atan2( roty, rotx ) * RADTODEG64;
        }
        else
        {
            X = 0.0;
            rotx = mat[5] * invScale.Y;
            roty = -mat[4] * invScale.Y;
            Z = atan2( roty, rotx ) * RADTODEG64;
        }
 
        // fix values that get below zero
        if (X < 0.0) X += 360.0;
        if (Y < 0.0) Y += 360.0;
        if (Z < 0.0) Z += 360.0;
 
        return vector3d<T>((T)X,(T)Y,(T)Z);
    }
 
    template <class T>
    inline core::vector3d<T> CMatrix4<T>::getRotationDegrees() const
    {
        // Note: Using getScale() here make it look like it could do matrix decomposition. 
        // It can't! It works (or should work) as long as rotation doesn't flip the handedness 
        // aka scale swapping 1 or 3 axes. (I think we could catch that as well by comparing 
        // crossproduct of first 2 axes to direction of third axis, but TODO)
        // And maybe it should also offer the solution for the simple calculation 
        // without regarding scaling as Nirtcpp did before 1.7
        core::vector3d<T> scale(getScale());
 
        // We assume the matrix uses rotations instead of negative scaling 2 axes.
        // Otherwise it fails even for some simple cases, like rotating around 
        // 2 axes by 180° which getScale thinks is a negative scaling.
        if (scale.Y<0 && scale.Z<0)
        {
            scale.Y =-scale.Y;
            scale.Z =-scale.Z;
        }
        else if (scale.X<0 && scale.Z<0)
        {
            scale.X =-scale.X;
            scale.Z =-scale.Z;
        }
        else if (scale.X<0 && scale.Y<0)
        {
            scale.X =-scale.X;
            scale.Y =-scale.Y;
        }
 
        return getRotationDegrees(scale);
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setInverseRotationRadians( const vector3d<T>& rotation )
    {
        f64 cr = cos( rotation.X );
        f64 sr = sin( rotation.X );
        f64 cp = cos( rotation.Y );
        f64 sp = sin( rotation.Y );
        f64 cy = cos( rotation.Z );
        f64 sy = sin( rotation.Z );
 
        M[0] = (T)( cp*cy );
        M[4] = (T)( cp*sy );
        M[8] = (T)( -sp );
 
        f64 srsp = sr*sp;
        f64 crsp = cr*sp;
 
        M[1] = (T)( srsp*cy-cr*sy );
        M[5] = (T)( srsp*sy+cr*cy );
        M[9] = (T)( sr*cp );
 
        M[2] = (T)( crsp*cy+sr*sy );
        M[6] = (T)( crsp*sy-sr*cy );
        M[10] = (T)( cr*cp );
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setRotationAxisRadians( const T& angle, const vector3d<T>& axis )
    {
        const f64 c = cos(angle);
        const f64 s = sin(angle);
        const f64 t = 1.0 - c;
 
        const f64 tx  = t * axis.X;
        const f64 ty  = t * axis.Y;
        const f64 tz  = t * axis.Z;
 
        const f64 sx  = s * axis.X;
        const f64 sy  = s * axis.Y;
        const f64 sz  = s * axis.Z;
 
        M[0] = (T)(tx * axis.X + c);
        M[1] = (T)(tx * axis.Y + sz);
        M[2] = (T)(tx * axis.Z - sy);
 
        M[4] = (T)(ty * axis.X - sz);
        M[5] = (T)(ty * axis.Y + c);
        M[6] = (T)(ty * axis.Z + sx);
 
        M[8]  = (T)(tz * axis.X + sy);
        M[9]  = (T)(tz * axis.Y - sx);
        M[10] = (T)(tz * axis.Z + c);
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::makeIdentity()
    {
        std::fill_n(M, 16, 0);
        M[0] = M[5] = M[10] = M[15] = (T)1;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=true;
#endif
        return *this;
    }
 
 
    /*
        check identity with epsilon
        solve floating range problems..
    */
    template <class T>
    inline bool CMatrix4<T>::isIdentity() const
    {
#if defined ( USE_MATRIX_TEST )
        if (definitelyIdentityMatrix)
            return true;
#endif
        if (!core::equals( M[12], (T)0 ) || !core::equals( M[13], (T)0 ) || !core::equals( M[14], (T)0 ) || !core::equals( M[15], (T)1 ))
            return false;
 
        if (!core::equals( M[ 0], (T)1 ) || !core::equals( M[ 1], (T)0 ) || !core::equals( M[ 2], (T)0 ) || !core::equals( M[ 3], (T)0 ))
            return false;
 
        if (!core::equals( M[ 4], (T)0 ) || !core::equals( M[ 5], (T)1 ) || !core::equals( M[ 6], (T)0 ) || !core::equals( M[ 7], (T)0 ))
            return false;
 
        if (!core::equals( M[ 8], (T)0 ) || !core::equals( M[ 9], (T)0 ) || !core::equals( M[10], (T)1 ) || !core::equals( M[11], (T)0 ))
            return false;
/*
        if (!core::equals( M[ 0], (T)1 ) ||
            !core::equals( M[ 5], (T)1 ) ||
            !core::equals( M[10], (T)1 ) ||
            !core::equals( M[15], (T)1 ))
            return false;
 
        for (s32 i=0; i<4; ++i)
            for (s32 j=0; j<4; ++j)
                if ((j != i) && (!iszero((*this)(i,j))))
                    return false;
*/
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=true;
#endif
        return true;
    }
 
 
    /* Check orthogonality of matrix. */
    template <class T>
    inline bool CMatrix4<T>::isOrthogonal() const
    {
        T dp=M[0] * M[4 ] + M[1] * M[5 ] + M[2 ] * M[6 ] + M[3 ] * M[7 ];
        if (!iszero(dp))
            return false;
        dp = M[0] * M[8 ] + M[1] * M[9 ] + M[2 ] * M[10] + M[3 ] * M[11];
        if (!iszero(dp))
            return false;
        dp = M[0] * M[12] + M[1] * M[13] + M[2 ] * M[14] + M[3 ] * M[15];
        if (!iszero(dp))
            return false;
        dp = M[4] * M[8 ] + M[5] * M[9 ] + M[6 ] * M[10] + M[7 ] * M[11];
        if (!iszero(dp))
            return false;
        dp = M[4] * M[12] + M[5] * M[13] + M[6 ] * M[14] + M[7 ] * M[15];
        if (!iszero(dp))
            return false;
        dp = M[8] * M[12] + M[9] * M[13] + M[10] * M[14] + M[11] * M[15];
        return (iszero(dp));
    }
 
 
    /*
        doesn't solve floating range problems..
        but takes care on +/- 0 on translation because we are changing it..
        reducing floating point branches
        but it needs the floats in memory..
    */
    template <class T>
    inline bool CMatrix4<T>::isIdentity_integer_base() const
    {
#if defined ( USE_MATRIX_TEST )
        if (definitelyIdentityMatrix)
            return true;
#endif
        if(IR(M[0])!=F32_VALUE_1)   return false;
        if(IR(M[1])!=0)         return false;
        if(IR(M[2])!=0)         return false;
        if(IR(M[3])!=0)         return false;
 
        if(IR(M[4])!=0)         return false;
        if(IR(M[5])!=F32_VALUE_1)   return false;
        if(IR(M[6])!=0)         return false;
        if(IR(M[7])!=0)         return false;
 
        if(IR(M[8])!=0)         return false;
        if(IR(M[9])!=0)         return false;
        if(IR(M[10])!=F32_VALUE_1)  return false;
        if(IR(M[11])!=0)        return false;
 
        if(IR(M[12])!=0)        return false;
        if(IR(M[13])!=0)        return false;
        if(IR(M[13])!=0)        return false;
        if(IR(M[15])!=F32_VALUE_1)  return false;
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=true;
#endif
        return true;
    }
 
 
    template <class T>
    inline void CMatrix4<T>::rotateVect( vector3df& vect ) const
    {
        vector3d<T> tmp(static_cast<T>(vect.X), static_cast<T>(vect.Y), static_cast<T>(vect.Z));
        vect.X = static_cast<f32>(tmp.X*M[0] + tmp.Y*M[4] + tmp.Z*M[8]);
        vect.Y = static_cast<f32>(tmp.X*M[1] + tmp.Y*M[5] + tmp.Z*M[9]);
        vect.Z = static_cast<f32>(tmp.X*M[2] + tmp.Y*M[6] + tmp.Z*M[10]);
    }
 
    template <class T>
    inline void CMatrix4<T>::rotateVect(core::vector3df& out, const core::vector3df& in) const
    {
        out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8];
        out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9];
        out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10];
    }
 
    template <class T>
    inline void CMatrix4<T>::rotateVect(T *out, const core::vector3df& in) const
    {
        out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8];
        out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9];
        out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10];
    }
 
    template <class T>
    inline void CMatrix4<T>::inverseRotateVect( vector3df& vect ) const
    {
        vector3d<T> tmp(static_cast<T>(vect.X), static_cast<T>(vect.Y), static_cast<T>(vect.Z));
        vect.X = static_cast<f32>(tmp.X*M[0] + tmp.Y*M[1] + tmp.Z*M[2]);
        vect.Y = static_cast<f32>(tmp.X*M[4] + tmp.Y*M[5] + tmp.Z*M[6]);
        vect.Z = static_cast<f32>(tmp.X*M[8] + tmp.Y*M[9] + tmp.Z*M[10]);
    }
 
    template <class T>
    inline void CMatrix4<T>::transformVect( vector3df& vect) const
    {
        T vector[3];
 
        vector[0] = vect.X*M[0] + vect.Y*M[4] + vect.Z*M[8] + M[12];
        vector[1] = vect.X*M[1] + vect.Y*M[5] + vect.Z*M[9] + M[13];
        vector[2] = vect.X*M[2] + vect.Y*M[6] + vect.Z*M[10] + M[14];
 
        vect.X = static_cast<f32>(vector[0]);
        vect.Y = static_cast<f32>(vector[1]);
        vect.Z = static_cast<f32>(vector[2]);
    }
 
    template <class T>
    inline void CMatrix4<T>::transformVect( vector3df& out, const vector3df& in) const
    {
        out.X = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];
        out.Y = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];
        out.Z = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];
    }
 
 
    template <class T>
    inline void CMatrix4<T>::transformVect(T *out, const core::vector3df &in) const
    {
        out[0] = in.X*M[0] + in.Y*M[4] + in.Z*M[8] + M[12];
        out[1] = in.X*M[1] + in.Y*M[5] + in.Z*M[9] + M[13];
        out[2] = in.X*M[2] + in.Y*M[6] + in.Z*M[10] + M[14];
        out[3] = in.X*M[3] + in.Y*M[7] + in.Z*M[11] + M[15];
    }
 
    template <class T>
    inline void CMatrix4<T>::transformVec3(T *out, const T * in) const
    {
        out[0] = in[0]*M[0] + in[1]*M[4] + in[2]*M[8] + M[12];
        out[1] = in[0]*M[1] + in[1]*M[5] + in[2]*M[9] + M[13];
        out[2] = in[0]*M[2] + in[1]*M[6] + in[2]*M[10] + M[14];
    }
 
    template <class T>
    inline void CMatrix4<T>::transformVec4(T *out, const T * in) const
    {
        out[0] = in[0]*M[0] + in[1]*M[4] + in[2]*M[8] + in[3]*M[12];
        out[1] = in[0]*M[1] + in[1]*M[5] + in[2]*M[9] + in[3]*M[13];
        out[2] = in[0]*M[2] + in[1]*M[6] + in[2]*M[10] + in[3]*M[14];
        out[3] = in[0]*M[3] + in[1]*M[7] + in[2]*M[11] + in[3]*M[15];
    }
 
 
    template <class T>
    inline void CMatrix4<T>::transformPlane( core::plane3d<f32> &plane) const
    {
        vector3df member;
        // Transform the plane member point, i.e. rotate, translate and scale it.
        transformVect(member, plane.getMemberPoint());
 
        // Transform the normal by the transposed inverse of the matrix
        CMatrix4<T> transposedInverse(*this, EM4CONST_INVERSE_TRANSPOSED);
        vector3df normal = plane.Normal;
        transposedInverse.rotateVect(normal);
        plane.setPlane(member, normal.normalize());
    }
 
    template <class T>
    inline void CMatrix4<T>::transformPlane( const core::plane3d<f32> &in, core::plane3d<f32> &out) const
    {
        out = in;
        transformPlane( out );
    }
 
    template <class T>
    NIRT_DEPRECATED inline void CMatrix4<T>::transformBox(core::aabbox3d<f32>& box) const
    {
#if defined ( USE_MATRIX_TEST )
        if (isIdentity())
            return;
#endif
 
        transformVect(box.MinEdge);
        transformVect(box.MaxEdge);
        box.repair();
    }
 
    template <class T>
    inline void CMatrix4<T>::transformBoxEx(core::aabbox3d<f32>& box) const
    {
#if defined ( USE_MATRIX_TEST )
        if (isIdentity())
            return;
#endif
 
        const f32 Amin[3] = {box.MinEdge.X, box.MinEdge.Y, box.MinEdge.Z};
        const f32 Amax[3] = {box.MaxEdge.X, box.MaxEdge.Y, box.MaxEdge.Z};
 
        f32 Bmin[3];
        f32 Bmax[3];
 
        Bmin[0] = Bmax[0] = M[12];
        Bmin[1] = Bmax[1] = M[13];
        Bmin[2] = Bmax[2] = M[14];
 
        const CMatrix4<T> &m = *this;
 
        for (u32 i = 0; i < 3; ++i)
        {
            for (u32 j = 0; j < 3; ++j)
            {
                const f32 a = m(j,i) * Amin[j];
                const f32 b = m(j,i) * Amax[j];
 
                if (a < b)
                {
                    Bmin[i] += a;
                    Bmax[i] += b;
                }
                else
                {
                    Bmin[i] += b;
                    Bmax[i] += a;
                }
            }
        }
 
        box.MinEdge.X = Bmin[0];
        box.MinEdge.Y = Bmin[1];
        box.MinEdge.Z = Bmin[2];
 
        box.MaxEdge.X = Bmax[0];
        box.MaxEdge.Y = Bmax[1];
        box.MaxEdge.Z = Bmax[2];
    }
 
 
    template <class T>
    inline void CMatrix4<T>::multiplyWith1x4Matrix(T* matrix) const
    {
        /*
        0  1  2  3
        4  5  6  7
        8  9  10 11
        12 13 14 15
        */
 
        T mat[4];
        mat[0] = matrix[0];
        mat[1] = matrix[1];
        mat[2] = matrix[2];
        mat[3] = matrix[3];
 
        matrix[0] = M[0]*mat[0] + M[4]*mat[1] + M[8]*mat[2] + M[12]*mat[3];
        matrix[1] = M[1]*mat[0] + M[5]*mat[1] + M[9]*mat[2] + M[13]*mat[3];
        matrix[2] = M[2]*mat[0] + M[6]*mat[1] + M[10]*mat[2] + M[14]*mat[3];
        matrix[3] = M[3]*mat[0] + M[7]*mat[1] + M[11]*mat[2] + M[15]*mat[3];
    }
 
    template <class T>
    inline void CMatrix4<T>::inverseTranslateVect( vector3df& vect ) const
    {
        vect.X = vect.X-M[12];
        vect.Y = vect.Y-M[13];
        vect.Z = vect.Z-M[14];
    }
 
    template <class T>
    inline void CMatrix4<T>::translateVect( vector3df& vect ) const
    {
        vect.X = vect.X+M[12];
        vect.Y = vect.Y+M[13];
        vect.Z = vect.Z+M[14];
    }
 
 
    template <class T>
    inline bool CMatrix4<T>::getInverse(CMatrix4<T>& out) const
    {
 
#if defined ( USE_MATRIX_TEST )
        if ( this->isIdentity() )
        {
            out=*this;
            return true;
        }
#endif
        const CMatrix4<T> &m = *this;
 
        f32 d = (m[0] * m[5] - m[1] * m[4]) * (m[10] * m[15] - m[11] * m[14]) -
            (m[0] * m[6] - m[2] * m[4]) * (m[9] * m[15] - m[11] * m[13]) +
            (m[0] * m[7] - m[3] * m[4]) * (m[9] * m[14] - m[10] * m[13]) +
            (m[1] * m[6] - m[2] * m[5]) * (m[8] * m[15] - m[11] * m[12]) -
            (m[1] * m[7] - m[3] * m[5]) * (m[8] * m[14] - m[10] * m[12]) +
            (m[2] * m[7] - m[3] * m[6]) * (m[8] * m[13] - m[9] * m[12]);
 
        if( core::iszero ( d, FLT_MIN ) )
            return false;
 
        d = core::reciprocal ( d );
 
        out[0] = d * (m[5] * (m[10] * m[15] - m[11] * m[14]) +
                m[6] * (m[11] * m[13] - m[9] * m[15]) +
                m[7] * (m[9] * m[14] - m[10] * m[13]));
        out[1] = d * (m[9] * (m[2] * m[15] - m[3] * m[14]) +
                m[10] * (m[3] * m[13] - m[1] * m[15]) +
                m[11] * (m[1] * m[14] - m[2] * m[13]));
        out[2] = d * (m[13] * (m[2] * m[7] - m[3] * m[6]) +
                m[14] * (m[3] * m[5] - m[1] * m[7]) +
                m[15] * (m[1] * m[6] - m[2] * m[5]));
        out[3] = d * (m[1] * (m[7] * m[10] - m[6] * m[11]) +
                m[2] * (m[5] * m[11] - m[7] * m[9]) +
                m[3] * (m[6] * m[9] - m[5] * m[10]));
        out[4] = d * (m[6] * (m[8] * m[15] - m[11] * m[12]) +
                m[7] * (m[10] * m[12] - m[8] * m[14]) +
                m[4] * (m[11] * m[14] - m[10] * m[15]));
        out[5] = d * (m[10] * (m[0] * m[15] - m[3] * m[12]) +
                m[11] * (m[2] * m[12] - m[0] * m[14]) +
                m[8] * (m[3] * m[14] - m[2] * m[15]));
        out[6] = d * (m[14] * (m[0] * m[7] - m[3] * m[4]) +
                m[15] * (m[2] * m[4] - m[0] * m[6]) +
                m[12] * (m[3] * m[6] - m[2] * m[7]));
        out[7] = d * (m[2] * (m[7] * m[8] - m[4] * m[11]) +
                m[3] * (m[4] * m[10] - m[6] * m[8]) +
                m[0] * (m[6] * m[11] - m[7] * m[10]));
        out[8] = d * (m[7] * (m[8] * m[13] - m[9] * m[12]) +
                m[4] * (m[9] * m[15] - m[11] * m[13]) +
                m[5] * (m[11] * m[12] - m[8] * m[15]));
        out[9] = d * (m[11] * (m[0] * m[13] - m[1] * m[12]) +
                m[8] * (m[1] * m[15] - m[3] * m[13]) +
                m[9] * (m[3] * m[12] - m[0] * m[15]));
        out[10] = d * (m[15] * (m[0] * m[5] - m[1] * m[4]) +
                m[12] * (m[1] * m[7] - m[3] * m[5]) +
                m[13] * (m[3] * m[4] - m[0] * m[7]));
        out[11] = d * (m[3] * (m[5] * m[8] - m[4] * m[9]) +
                m[0] * (m[7] * m[9] - m[5] * m[11]) +
                m[1] * (m[4] * m[11] - m[7] * m[8]));
        out[12] = d * (m[4] * (m[10] * m[13] - m[9] * m[14]) +
                m[5] * (m[8] * m[14] - m[10] * m[12]) +
                m[6] * (m[9] * m[12] - m[8] * m[13]));
        out[13] = d * (m[8] * (m[2] * m[13] - m[1] * m[14]) +
                m[9] * (m[0] * m[14] - m[2] * m[12]) +
                m[10] * (m[1] * m[12] - m[0] * m[13]));
        out[14] = d * (m[12] * (m[2] * m[5] - m[1] * m[6]) +
                m[13] * (m[0] * m[6] - m[2] * m[4]) +
                m[14] * (m[1] * m[4] - m[0] * m[5]));
        out[15] = d * (m[0] * (m[5] * m[10] - m[6] * m[9]) +
                m[1] * (m[6] * m[8] - m[4] * m[10]) +
                m[2] * (m[4] * m[9] - m[5] * m[8]));
 
#if defined ( USE_MATRIX_TEST )
        out.definitelyIdentityMatrix = definitelyIdentityMatrix;
#endif
        return true;
    }
 
 
    template <class T>
    inline bool CMatrix4<T>::getInversePrimitive ( CMatrix4<T>& out ) const
    {
        out.M[0 ] = M[0];
        out.M[1 ] = M[4];
        out.M[2 ] = M[8];
        out.M[3 ] = 0;
 
        out.M[4 ] = M[1];
        out.M[5 ] = M[5];
        out.M[6 ] = M[9];
        out.M[7 ] = 0;
 
        out.M[8 ] = M[2];
        out.M[9 ] = M[6];
        out.M[10] = M[10];
        out.M[11] = 0;
 
        out.M[12] = (T)-(M[12]*M[0] + M[13]*M[1] + M[14]*M[2]);
        out.M[13] = (T)-(M[12]*M[4] + M[13]*M[5] + M[14]*M[6]);
        out.M[14] = (T)-(M[12]*M[8] + M[13]*M[9] + M[14]*M[10]);
        out.M[15] = 1;
 
#if defined ( USE_MATRIX_TEST )
        out.definitelyIdentityMatrix = definitelyIdentityMatrix;
#endif
        return true;
    }
 
    template <class T>
    inline bool CMatrix4<T>::makeInverse()
    {
#if defined ( USE_MATRIX_TEST )
        if (definitelyIdentityMatrix)
            return true;
#endif
        CMatrix4<T> temp ( EM4CONST_NOTHING );
 
        if (getInverse(temp))
        {
            *this = temp;
            return true;
        }
 
        return false;
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator=(const CMatrix4<T> &other)
    {
        if (this==&other)
            return *this;
        std::copy_n(other.M, 16, M);
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=other.definitelyIdentityMatrix;
#endif
        return *this;
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::operator=(const T& scalar)
    {
        for (s32 i = 0; i < 16; ++i)
            M[i]=scalar;
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    template <class T>
    inline bool CMatrix4<T>::operator==(const CMatrix4<T> &other) const
    {
#if defined ( USE_MATRIX_TEST )
        if (definitelyIdentityMatrix && other.definitelyIdentityMatrix)
            return true;
#endif
        for (s32 i = 0; i < 16; ++i)
            if (M[i] != other.M[i])
                return false;
 
        return true;
    }
 
 
    template <class T>
    inline bool CMatrix4<T>::operator!=(const CMatrix4<T> &other) const
    {
        return !(*this == other);
    }
 
 
    // Builds a right-handed perspective projection matrix based on a field of view
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovRH(
            f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));
        NIRT_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero
        const T w = static_cast<T>(h / aspectRatio);
 
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = w;
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)h;
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        //M[10]
        M[11] = -1;
 
        M[12] = 0;
        M[13] = 0;
        //M[14]
        M[15] = 0;
 
        if ( zClipFromZero ) // DirectX version
        {
            M[10] = (T)(zFar/(zNear-zFar));
            M[14] = (T)(zNear*zFar/(zNear-zFar));
        }
        else    // OpenGL version
        {
            M[10] = (T)((zFar+zNear)/(zNear-zFar));
            M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a left-handed perspective projection matrix based on a field of view
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovLH(
            f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));
        NIRT_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero
        const T w = static_cast<T>(h / aspectRatio);
 
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = w;
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)h;
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        //M[10]
        M[11] = 1;
 
        M[12] = 0;
        M[13] = 0;
        //M[14]
        M[15] = 0;
 
        if ( zClipFromZero ) // DirectX version
        {
            M[10] = (T)(zFar/(zFar-zNear));
            M[14] = (T)(-zNear*zFar/(zFar-zNear));
        }
        else    // OpenGL version
        {
            M[10] = (T)((zFar+zNear)/(zFar-zNear));
            M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a left-handed perspective projection matrix based on a field of view, with far plane culling at infinity
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveFovInfinityLH(
            f32 fieldOfViewRadians, f32 aspectRatio, f32 zNear, f32 epsilon)
    {
        const f64 h = reciprocal(tan(fieldOfViewRadians*0.5));
        NIRT_DEBUG_BREAK_IF(aspectRatio==0.f); //divide by zero
        const T w = static_cast<T>(h / aspectRatio);
 
        M[0] = w;
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)h;
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        M[10] = (T)(1.f-epsilon);
        M[11] = 1;
 
        M[12] = 0;
        M[13] = 0;
        M[14] = (T)(zNear*(epsilon-1.f));
        M[15] = 0;
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a left-handed orthogonal projection matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoLH(
            f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        NIRT_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = (T)(2/widthOfViewVolume);
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)(2/heightOfViewVolume);
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        // M[10]
        M[11] = 0;
 
        M[12] = 0;
        M[13] = 0;
        // M[14]
        M[15] = 1;
 
        if ( zClipFromZero )
        {
            M[10] = (T)(1/(zFar-zNear));
            M[14] = (T)(zNear/(zNear-zFar));
        }
        else
        {
            M[10] = (T)(2/(zFar-zNear));
            M[14] = (T)-(zFar+zNear)/(zFar-zNear);
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a right-handed orthogonal projection matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixOrthoRH(
            f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        NIRT_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = (T)(2/widthOfViewVolume);
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)(2/heightOfViewVolume);
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        // M[10]
        M[11] = 0;
 
        M[12] = 0;
        M[13] = 0;
        // M[14]
        M[15] = 1;
 
        if ( zClipFromZero )
        {
            M[10] = (T)(1/(zNear-zFar));
            M[14] = (T)(zNear/(zNear-zFar));
        }
        else
        {
            M[10] = (T)(2/(zNear-zFar));
            M[14] = (T)-(zFar+zNear)/(zFar-zNear);
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a right-handed perspective projection matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveRH(
            f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        NIRT_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = (T)(2*zNear/widthOfViewVolume);
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)(2*zNear/heightOfViewVolume);
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        //M[10]
        M[11] = -1;
 
        M[12] = 0;
        M[13] = 0;
        //M[14]
        M[15] = 0;
 
        if ( zClipFromZero ) // DirectX version
        {
            M[10] = (T)(zFar/(zNear-zFar));
            M[14] = (T)(zNear*zFar/(zNear-zFar));
        }
        else    // OpenGL version
        {
            M[10] = (T)((zFar+zNear)/(zNear-zFar));
            M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a left-handed perspective projection matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildProjectionMatrixPerspectiveLH(
            f32 widthOfViewVolume, f32 heightOfViewVolume, f32 zNear, f32 zFar, bool zClipFromZero)
    {
        NIRT_DEBUG_BREAK_IF(widthOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(heightOfViewVolume==0.f); //divide by zero
        NIRT_DEBUG_BREAK_IF(zNear==zFar); //divide by zero
        M[0] = (T)(2*zNear/widthOfViewVolume);
        M[1] = 0;
        M[2] = 0;
        M[3] = 0;
 
        M[4] = 0;
        M[5] = (T)(2*zNear/heightOfViewVolume);
        M[6] = 0;
        M[7] = 0;
 
        M[8] = 0;
        M[9] = 0;
        //M[10]
        M[11] = 1;
 
        M[12] = 0;
        M[13] = 0;
        //M[14] = (T)(zNear*zFar/(zNear-zFar));
        M[15] = 0;
 
        if ( zClipFromZero ) // DirectX version
        {
            M[10] = (T)(zFar/(zFar-zNear));
            M[14] = (T)(zNear*zFar/(zNear-zFar));
        }
        else    // OpenGL version
        {
            M[10] = (T)((zFar+zNear)/(zFar-zNear));
            M[14] = (T)(2.0f*zNear*zFar/(zNear-zFar));
        }
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a matrix that flattens geometry into a plane.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildShadowMatrix(const core::vector3df& light, core::plane3df plane, f32 point)
    {
        plane.Normal.normalize();
        const f32 d = plane.Normal.dotProduct(light);
 
        M[ 0] = (T)(-plane.Normal.X * light.X + d);
        M[ 1] = (T)(-plane.Normal.X * light.Y);
        M[ 2] = (T)(-plane.Normal.X * light.Z);
        M[ 3] = (T)(-plane.Normal.X * point);
 
        M[ 4] = (T)(-plane.Normal.Y * light.X);
        M[ 5] = (T)(-plane.Normal.Y * light.Y + d);
        M[ 6] = (T)(-plane.Normal.Y * light.Z);
        M[ 7] = (T)(-plane.Normal.Y * point);
 
        M[ 8] = (T)(-plane.Normal.Z * light.X);
        M[ 9] = (T)(-plane.Normal.Z * light.Y);
        M[10] = (T)(-plane.Normal.Z * light.Z + d);
        M[11] = (T)(-plane.Normal.Z * point);
 
        M[12] = (T)(-plane.D * light.X);
        M[13] = (T)(-plane.D * light.Y);
        M[14] = (T)(-plane.D * light.Z);
        M[15] = (T)(-plane.D * point + d);
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
    // Builds a left-handed look-at matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixLH(
                const vector3df& position,
                const vector3df& target,
                const vector3df& upVector)
    {
        vector3df zaxis = target - position;
        zaxis.normalize_z();
 
        vector3df xaxis = normalize_y(upVector).crossProduct(zaxis);
        xaxis.normalize_x();
 
        vector3df yaxis = zaxis.crossProduct(xaxis);
 
        M[0] = (T)xaxis.X;
        M[1] = (T)yaxis.X;
        M[2] = (T)zaxis.X;
        M[3] = 0;
 
        M[4] = (T)xaxis.Y;
        M[5] = (T)yaxis.Y;
        M[6] = (T)zaxis.Y;
        M[7] = 0;
 
        M[8] = (T)xaxis.Z;
        M[9] = (T)yaxis.Z;
        M[10] = (T)zaxis.Z;
        M[11] = 0;
 
        M[12] = (T)-xaxis.dotProduct(position);
        M[13] = (T)-yaxis.dotProduct(position);
        M[14] = (T)-zaxis.dotProduct(position);
        M[15] = 1;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // Builds a right-handed look-at matrix.
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildCameraLookAtMatrixRH(
                const vector3df& position,
                const vector3df& target,
                const vector3df& upVector)
    {
        vector3df zaxis = position - target;
        zaxis.normalize();
 
        vector3df xaxis = upVector.crossProduct(zaxis);
        xaxis.normalize();
 
        vector3df yaxis = zaxis.crossProduct(xaxis);
 
        M[0] = (T)xaxis.X;
        M[1] = (T)yaxis.X;
        M[2] = (T)zaxis.X;
        M[3] = 0;
 
        M[4] = (T)xaxis.Y;
        M[5] = (T)yaxis.Y;
        M[6] = (T)zaxis.Y;
        M[7] = 0;
 
        M[8] = (T)xaxis.Z;
        M[9] = (T)yaxis.Z;
        M[10] = (T)zaxis.Z;
        M[11] = 0;
 
        M[12] = (T)-xaxis.dotProduct(position);
        M[13] = (T)-yaxis.dotProduct(position);
        M[14] = (T)-zaxis.dotProduct(position);
        M[15] = 1;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // creates a new matrix as interpolated matrix from this and the passed one.
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::interpolate(const core::CMatrix4<T>& b, f32 time) const
    {
        CMatrix4<T> mat ( EM4CONST_NOTHING );
 
        for (u32 i=0; i < 16; i += 4)
        {
            mat.M[i+0] = (T)(M[i+0] + ( b.M[i+0] - M[i+0] ) * time);
            mat.M[i+1] = (T)(M[i+1] + ( b.M[i+1] - M[i+1] ) * time);
            mat.M[i+2] = (T)(M[i+2] + ( b.M[i+2] - M[i+2] ) * time);
            mat.M[i+3] = (T)(M[i+3] + ( b.M[i+3] - M[i+3] ) * time);
        }
        return mat;
    }
 
 
    // returns transposed matrix
    template <class T>
    inline CMatrix4<T> CMatrix4<T>::getTransposed() const
    {
        CMatrix4<T> t ( EM4CONST_NOTHING );
        getTransposed ( t );
        return t;
    }
 
 
    // returns transposed matrix
    template <class T>
    inline void CMatrix4<T>::getTransposed( CMatrix4<T>& o ) const
    {
        o[ 0] = M[ 0];
        o[ 1] = M[ 4];
        o[ 2] = M[ 8];
        o[ 3] = M[12];
 
        o[ 4] = M[ 1];
        o[ 5] = M[ 5];
        o[ 6] = M[ 9];
        o[ 7] = M[13];
 
        o[ 8] = M[ 2];
        o[ 9] = M[ 6];
        o[10] = M[10];
        o[11] = M[14];
 
        o[12] = M[ 3];
        o[13] = M[ 7];
        o[14] = M[11];
        o[15] = M[15];
#if defined ( USE_MATRIX_TEST )
        o.definitelyIdentityMatrix=definitelyIdentityMatrix;
#endif
    }
 
 
    // used to scale <-1,-1><1,1> to viewport
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildNDCToDCMatrix( const core::rect<s32>& viewport, f32 zScale)
    {
        const f32 scaleX = (viewport.getWidth() - 0.75f ) * 0.5f;
        const f32 scaleY = -(viewport.getHeight() - 0.75f ) * 0.5f;
 
        const f32 dx = -0.5f + ( (viewport.UpperLeftCorner.X + viewport.LowerRightCorner.X ) * 0.5f );
        const f32 dy = -0.5f + ( (viewport.UpperLeftCorner.Y + viewport.LowerRightCorner.Y ) * 0.5f );
 
        makeIdentity();
        M[12] = (T)dx;
        M[13] = (T)dy;
        return setScale(core::vector3d<T>((T)scaleX, (T)scaleY, (T)zScale));
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildRotateFromTo(const core::vector3df& from, const core::vector3df& to)
    {
        // unit vectors
        core::vector3df f(from);
        core::vector3df t(to);
        f.normalize();
        t.normalize();
 
        // axis multiplication by sin
        core::vector3df vs(t.crossProduct(f));
 
        // axis of rotation
        core::vector3df v(vs);
        v.normalize();
 
        // cosinus angle
        T ca = f.dotProduct(t);
 
        core::vector3df vt(v * (1 - ca));
 
        M[0] = vt.X * v.X + ca;
        M[5] = vt.Y * v.Y + ca;
        M[10] = vt.Z * v.Z + ca;
 
        vt.X *= v.Y;
        vt.Z *= v.X;
        vt.Y *= v.Z;
 
        M[1] = vt.X - vs.Z;
        M[2] = vt.Z + vs.Y;
        M[3] = 0;
 
        M[4] = vt.X + vs.Z;
        M[6] = vt.Y - vs.X;
        M[7] = 0;
 
        M[8] = vt.Z - vs.Y;
        M[9] = vt.Y + vs.X;
        M[11] = 0;
 
        M[12] = 0;
        M[13] = 0;
        M[14] = 0;
        M[15] = 1;
 
        return *this;
    }
 
 
    template <class T>
    inline void CMatrix4<T>::buildAxisAlignedBillboard(
                const core::vector3df& camPos,
                const core::vector3df& center,
                const core::vector3df& translation,
                const core::vector3df& axis,
                const core::vector3df& from)
    {
        // axis of rotation
        core::vector3df up = axis;
        up.normalize();
        const core::vector3df forward = (camPos - center).normalize();
        const core::vector3df right = up.crossProduct(forward).normalize();
 
        // correct look vector
        const core::vector3df look = right.crossProduct(up);
 
        // rotate from to
        // axis multiplication by sin
        const core::vector3df vs = look.crossProduct(from);
 
        // cosinus angle
        const f32 ca = from.dotProduct(look);
 
        core::vector3df vt(up * (1.f - ca));
 
        M[0] = static_cast<T>(vt.X * up.X + ca);
        M[5] = static_cast<T>(vt.Y * up.Y + ca);
        M[10] = static_cast<T>(vt.Z * up.Z + ca);
 
        vt.X *= up.Y;
        vt.Z *= up.X;
        vt.Y *= up.Z;
 
        M[1] = static_cast<T>(vt.X - vs.Z);
        M[2] = static_cast<T>(vt.Z + vs.Y);
        M[3] = 0;
 
        M[4] = static_cast<T>(vt.X + vs.Z);
        M[6] = static_cast<T>(vt.Y - vs.X);
        M[7] = 0;
 
        M[8] = static_cast<T>(vt.Z - vs.Y);
        M[9] = static_cast<T>(vt.Y + vs.X);
        M[11] = 0;
 
        setRotationCenter(center, translation);
    }
 
 
    template <class T>
    inline void CMatrix4<T>::setRotationCenter(const core::vector3df& center, const core::vector3df& translation)
    {
        M[12] = -M[0]*center.X - M[4]*center.Y - M[8]*center.Z + (center.X - translation.X );
        M[13] = -M[1]*center.X - M[5]*center.Y - M[9]*center.Z + (center.Y - translation.Y );
        M[14] = -M[2]*center.X - M[6]*center.Y - M[10]*center.Z + (center.Z - translation.Z );
        M[15] = (T) 1.0;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::buildTextureTransform( f32 rotateRad,
            const core::vector2df &rotatecenter,
            const core::vector2df &translate,
            const core::vector2df &scale)
    {
        const f32 c = cosf(rotateRad);
        const f32 s = sinf(rotateRad);
 
        M[0] = (T)(c * scale.X);
        M[1] = (T)(s * scale.Y);
        M[2] = 0;
        M[3] = 0;
 
        M[4] = (T)(-s * scale.X);
        M[5] = (T)(c * scale.Y);
        M[6] = 0;
        M[7] = 0;
 
        M[8] = (T)(c * scale.X * rotatecenter.X + -s * rotatecenter.Y + translate.X);
        M[9] = (T)(s * scale.Y * rotatecenter.X +  c * rotatecenter.Y + translate.Y);
        M[10] = 1;
        M[11] = 0;
 
        M[12] = 0;
        M[13] = 0;
        M[14] = 0;
        M[15] = 1;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // rotate about z axis, center ( 0.5, 0.5 )
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTextureRotationCenter( f32 rotateRad )
    {
        const f32 c = cosf(rotateRad);
        const f32 s = sinf(rotateRad);
        M[0] = (T)c;
        M[1] = (T)s;
 
        M[4] = (T)-s;
        M[5] = (T)c;
 
        M[8] = (T)(0.5f * ( s - c) + 0.5f);
        M[9] = (T)(-0.5f * ( s + c) + 0.5f);
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = definitelyIdentityMatrix && (rotateRad==0.0f);
#endif
        return *this;
    }
 
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTextureTranslate ( f32 x, f32 y )
    {
        M[8] = (T)x;
        M[9] = (T)y;
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = definitelyIdentityMatrix && (x==0.0f) && (y==0.0f);
#endif
        return *this;
    }
 
    template <class T>
    inline void CMatrix4<T>::getTextureTranslate(f32& x, f32& y) const
    {
        x = (f32)M[8];
        y = (f32)M[9];
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTextureTranslateTransposed ( f32 x, f32 y )
    {
        M[2] = (T)x;
        M[6] = (T)y;
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = definitelyIdentityMatrix && (x==0.0f) && (y==0.0f);
#endif
        return *this;
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTextureScale ( f32 sx, f32 sy )
    {
        M[0] = (T)sx;
        M[5] = (T)sy;
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = definitelyIdentityMatrix && (sx==1.0f) && (sy==1.0f);
#endif
        return *this;
    }
 
    template <class T>
    inline void CMatrix4<T>::getTextureScale ( f32& sx, f32& sy ) const
    {
        sx = (f32)M[0];
        sy = (f32)M[5];
    }
 
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setTextureScaleCenter( f32 sx, f32 sy )
    {
        M[0] = (T)sx;
        M[5] = (T)sy;
        M[8] = (T)(0.5f - 0.5f * sx);
        M[9] = (T)(0.5f - 0.5f * sy);
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = definitelyIdentityMatrix && (sx==1.0f) && (sy==1.0f);
#endif
        return *this;
    }
 
 
    // sets all matrix data members at once
    template <class T>
    inline CMatrix4<T>& CMatrix4<T>::setM(const T* data)
    {
        std::copy_n(data, 16, M);
 
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix=false;
#endif
        return *this;
    }
 
 
    // sets if the matrix is definitely identity matrix
    template <class T>
    inline void CMatrix4<T>::setDefinitelyIdentityMatrix( bool isDefinitelyIdentityMatrix)
    {
#if defined ( USE_MATRIX_TEST )
        definitelyIdentityMatrix = isDefinitelyIdentityMatrix;
#else
        (void)isDefinitelyIdentityMatrix; // prevent compiler warning
#endif
    }
 
 
    // gets if the matrix is definitely identity matrix
    template <class T>
    inline bool CMatrix4<T>::getDefinitelyIdentityMatrix() const
    {
#if defined ( USE_MATRIX_TEST )
        return definitelyIdentityMatrix;
#else
        return false;
#endif
    }
 
 
    template <class T>
    inline bool CMatrix4<T>::equals(const core::CMatrix4<T>& other, const T tolerance) const
    {
#if defined ( USE_MATRIX_TEST )
        if (definitelyIdentityMatrix && other.definitelyIdentityMatrix)
            return true;
#endif
        for (s32 i = 0; i < 16; ++i)
            if (!core::equals(M[i],other.M[i], tolerance))
                return false;
 
        return true;
    }
 
 
    // Multiply by scalar.
    template <class T>
    inline CMatrix4<T> operator*(const T scalar, const CMatrix4<T>& mat)
    {
        return mat*scalar;
    }
 
 
    using matrix4 = CMatrix4<f32>;
 
    NIRTCPP_API extern const matrix4 IdentityMatrix;
 
} // end namespace core
} // end namespace nirt
 
#endif