ParaMatrix4.h

#pragma once
namespace ParaEngine
{
    class QMatrix;

    class Matrix4
    {
    public:
        union {
            struct {
                float        _11, _12, _13, _14;
                float        _21, _22, _23, _24;
                float        _31, _32, _33, _34;
                float        _41, _42, _43, _44;
            };
            float m[4][4];
            float _m[16];
        };
    public:
        inline Matrix4()
        {
        }

        inline Matrix4(
            float m00, float m01, float m02, float m03,
            float m10, float m11, float m12, float m13,
            float m20, float m21, float m22, float m23,
            float m30, float m31, float m32, float m33 )
        {
            m[0][0] = m00;            m[0][1] = m01;            m[0][2] = m02;            m[0][3] = m03;
            m[1][0] = m10;            m[1][1] = m11;            m[1][2] = m12;            m[1][3] = m13;
            m[2][0] = m20;            m[2][1] = m21;            m[2][2] = m22;            m[2][3] = m23;
            m[3][0] = m30;            m[3][1] = m31;            m[3][2] = m32;            m[3][3] = m33;
        }

        inline Matrix4(const Matrix3& m3x3)
        {
          operator=(IDENTITY);
          operator=(m3x3);
        }

        inline Matrix4(const DeviceMatrix& mat)
        {
            operator=(reinterpret_cast<const Matrix4&>(mat));
        }

        Matrix4(const QMatrix& mat2DAffine);

        inline Matrix4(const Quaternion& rot)
        {
            rot.ToRotationMatrix(*this, Vector3::ZERO);
        }
        
        inline void identity() {
            *this = Matrix4::IDENTITY;
        };

        inline float* operator [] ( size_t iRow )
        {
            assert( iRow < 4 );
            return m[iRow];
        }

        inline const float *const operator [] ( size_t iRow ) const
        {
            assert( iRow < 4 );
            return m[iRow];
        }

        inline Matrix4 operator * ( const Matrix4 &m2 ) const
        {
            Matrix4 r;
            ParaMatrixMultiply(&r, this, &m2);
            return r;
        }

        Matrix4 Multiply4x3(const Matrix4 &m2) const;

        inline void operator *= (const Matrix4 &m2)
        {
            ParaMatrixMultiply(this, this, &m2);
        }

        inline Vector3 operator * ( const Vector3 &v ) const
        {
            Vector3 r;

            float fInvW = 1.0f / ( m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] );

            r.x = ( m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] ) * fInvW;
            r.y = ( m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] ) * fInvW;
            r.z = ( m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] ) * fInvW;

            return r;
        }
        inline Vector4 operator * (const Vector4& v) const
        {
            return Vector4(
                m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, 
                m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w,
                m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w,
                m[3][0] * v.x + m[3][1] * v.y + m[3][2] * v.z + m[3][3] * v.w
                );
        }
        inline Plane operator * (const Plane& p) const
        {
            Plane ret;
            Matrix4 invTrans = inverse().transpose();
            Vector4 v4( p.normal.x, p.normal.y, p.normal.z, p.d );
            v4 = invTrans * v4;
            ret.normal.x = v4.x; 
            ret.normal.y = v4.y; 
            ret.normal.z = v4.z;
            ret.d = v4.w / ret.normal.normalise();

            return ret;
        }


        inline Matrix4 operator + ( const Matrix4 &m2 ) const
        {
            Matrix4 r;

            r.m[0][0] = m[0][0] + m2.m[0][0];
            r.m[0][1] = m[0][1] + m2.m[0][1];
            r.m[0][2] = m[0][2] + m2.m[0][2];
            r.m[0][3] = m[0][3] + m2.m[0][3];

            r.m[1][0] = m[1][0] + m2.m[1][0];
            r.m[1][1] = m[1][1] + m2.m[1][1];
            r.m[1][2] = m[1][2] + m2.m[1][2];
            r.m[1][3] = m[1][3] + m2.m[1][3];

            r.m[2][0] = m[2][0] + m2.m[2][0];
            r.m[2][1] = m[2][1] + m2.m[2][1];
            r.m[2][2] = m[2][2] + m2.m[2][2];
            r.m[2][3] = m[2][3] + m2.m[2][3];

            r.m[3][0] = m[3][0] + m2.m[3][0];
            r.m[3][1] = m[3][1] + m2.m[3][1];
            r.m[3][2] = m[3][2] + m2.m[3][2];
            r.m[3][3] = m[3][3] + m2.m[3][3];

            return r;
        }
        inline Matrix4& operator += (const Matrix4 &m2)
        {
            m[0][0] += m2.m[0][0];          m[0][1] += m2.m[0][1];          m[0][2] += m2.m[0][2];          m[0][3] += m2.m[0][3];
            m[1][0] += m2.m[1][0];          m[1][1] += m2.m[1][1];          m[1][2] += m2.m[1][2];          m[1][3] += m2.m[1][3];
            m[2][0] += m2.m[2][0];          m[2][1] += m2.m[2][1];          m[2][2] += m2.m[2][2];          m[2][3] += m2.m[2][3];
            m[3][0] += m2.m[3][0];          m[3][1] += m2.m[3][1];          m[3][2] += m2.m[3][2];          m[3][3] += m2.m[3][3];
            return *this;
        }
        
        inline Matrix4 operator - ( const Matrix4 &m2 ) const
        {
            Matrix4 r;
            r.m[0][0] = m[0][0] - m2.m[0][0];
            r.m[0][1] = m[0][1] - m2.m[0][1];
            r.m[0][2] = m[0][2] - m2.m[0][2];
            r.m[0][3] = m[0][3] - m2.m[0][3];

            r.m[1][0] = m[1][0] - m2.m[1][0];
            r.m[1][1] = m[1][1] - m2.m[1][1];
            r.m[1][2] = m[1][2] - m2.m[1][2];
            r.m[1][3] = m[1][3] - m2.m[1][3];

            r.m[2][0] = m[2][0] - m2.m[2][0];
            r.m[2][1] = m[2][1] - m2.m[2][1];
            r.m[2][2] = m[2][2] - m2.m[2][2];
            r.m[2][3] = m[2][3] - m2.m[2][3];

            r.m[3][0] = m[3][0] - m2.m[3][0];
            r.m[3][1] = m[3][1] - m2.m[3][1];
            r.m[3][2] = m[3][2] - m2.m[3][2];
            r.m[3][3] = m[3][3] - m2.m[3][3];

            return r;
        }
        inline Matrix4& operator -= (const Matrix4 &m2)
        {
            m[0][0] -= m2.m[0][0];          m[0][1] -= m2.m[0][1];          m[0][2] -= m2.m[0][2];          m[0][3] -= m2.m[0][3];
            m[1][0] -= m2.m[1][0];          m[1][1] -= m2.m[1][1];          m[1][2] -= m2.m[1][2];          m[1][3] -= m2.m[1][3];
            m[2][0] -= m2.m[2][0];          m[2][1] -= m2.m[2][1];          m[2][2] -= m2.m[2][2];          m[2][3] -= m2.m[2][3];
            m[3][0] -= m2.m[3][0];          m[3][1] -= m2.m[3][1];          m[3][2] -= m2.m[3][2];          m[3][3] -= m2.m[3][3];
            return *this;
        }

        inline bool operator == ( const Matrix4& m2 ) const
        {
            if( 
                m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
                m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
                m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
                m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
                return false;
            return true;
        }

        inline bool operator != ( const Matrix4& m2 ) const
        {
            if( 
                m[0][0] != m2.m[0][0] || m[0][1] != m2.m[0][1] || m[0][2] != m2.m[0][2] || m[0][3] != m2.m[0][3] ||
                m[1][0] != m2.m[1][0] || m[1][1] != m2.m[1][1] || m[1][2] != m2.m[1][2] || m[1][3] != m2.m[1][3] ||
                m[2][0] != m2.m[2][0] || m[2][1] != m2.m[2][1] || m[2][2] != m2.m[2][2] || m[2][3] != m2.m[2][3] ||
                m[3][0] != m2.m[3][0] || m[3][1] != m2.m[3][1] || m[3][2] != m2.m[3][2] || m[3][3] != m2.m[3][3] )
                return true;
            return false;
        }

        inline void operator = ( const Matrix3& mat3 )
        {
            m[0][0] = mat3.m[0][0]; m[0][1] = mat3.m[0][1]; m[0][2] = mat3.m[0][2];
            m[1][0] = mat3.m[1][0]; m[1][1] = mat3.m[1][1]; m[1][2] = mat3.m[1][2];
            m[2][0] = mat3.m[2][0]; m[2][1] = mat3.m[2][1]; m[2][2] = mat3.m[2][2];
        }

        inline Matrix4 transpose(void) const
        {
            return Matrix4(m[0][0], m[1][0], m[2][0], m[3][0],
                           m[0][1], m[1][1], m[2][1], m[3][1],
                           m[0][2], m[1][2], m[2][2], m[3][2],
                           m[0][3], m[1][3], m[2][3], m[3][3]);
        }

        /*
        -----------------------------------------------------------------------
        Translation Transformation
        -----------------------------------------------------------------------
        */
        inline void offsetTrans(const Vector3& v)
        {
            m[3][0] += v.x;             m[3][1] += v.y;         m[3][2] += v.z;
        }

        inline void setTrans( const Vector3& v )
        {
            m[3][0] = v.x;
            m[3][1] = v.y;
            m[3][2] = v.z;
        }

        inline Vector3 getTrans() const
        {
          return Vector3(m[3][0], m[3][1], m[3][2]);
        }
        

        inline void makeTrans( const Vector3& v )
        {
            m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0;
            m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = 0.0;
            m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = 0.0;
            m[3][0] = v.x; m[3][1] = v.y; m[3][2] = v.z; m[3][3] = 1.0;
        }

        inline void makeTrans( float tx, float ty, float tz )
        {
            m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0; m[0][3] = 0.0;
            m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0; m[1][3] = 0.0;
            m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0; m[2][3] = 0.0;
            m[3][0] = tx;  m[3][1] = ty;  m[3][2] = tz;  m[3][3] = 1.0;
        }

        
        /*
        -----------------------------------------------------------------------
        Scale Transformation
        -----------------------------------------------------------------------
        */
        inline void setScale( const Vector3& v )
        {
            m[0][0] = v.x;
            m[1][1] = v.y;
            m[2][2] = v.z;
        }

        inline void makeScale( float s_x, float s_y, float s_z )
        {
            identity();
            m[0][0] = s_x; m[1][1] = s_y; m[2][2] = s_z; 
        }

        inline void makeScale(const Vector3& v)
        {
            identity();
            m[0][0] = v.x; m[1][1] = v.y; m[2][2] = v.z;
        }

        inline void extract3x3Matrix(Matrix3& m3x3) const
        {
            m3x3.m[0][0] = m[0][0];            m3x3.m[0][1] = m[0][1];            m3x3.m[0][2] = m[0][2];
            m3x3.m[1][0] = m[1][0];            m3x3.m[1][1] = m[1][1];            m3x3.m[1][2] = m[1][2];
            m3x3.m[2][0] = m[2][0];            m3x3.m[2][1] = m[2][1];            m3x3.m[2][2] = m[2][2];
        }

        bool hasScale() const;

        inline bool hasNegativeScale() const
        {
            return determinant() < 0;
        }

        inline Quaternion extractQuaternion() const
        {
          return Quaternion(*this);
        }

        static const Matrix4 ZERO;
        static const Matrix4 IDENTITY;
        static const Matrix4 CLIPSPACE2DTOIMAGESPACE;

        inline Matrix4 operator*(float scalar) const
        {
            return Matrix4(
                scalar*m[0][0], scalar*m[0][1], scalar*m[0][2], scalar*m[0][3],
                scalar*m[1][0], scalar*m[1][1], scalar*m[1][2], scalar*m[1][3],
                scalar*m[2][0], scalar*m[2][1], scalar*m[2][2], scalar*m[2][3],
                scalar*m[3][0], scalar*m[3][1], scalar*m[3][2], scalar*m[3][3]);
        }
        
        friend std::ostream& operator << ( std::ostream& o, const Matrix4& m )
        {
            o << "Matrix4(";
            for (size_t i = 0; i < 4; ++i)
            {
                o << " row" << (unsigned)i << "{";
                for(size_t j = 0; j < 4; ++j)
                {
                    o << m[i][j] << " ";
                }
                o << "}";
            }
            o << ")";
            return o;
        }
        
        Matrix4 adjoint() const;
        float determinant() const;
        Matrix4 inverse() const;
        inline void invert() {
            *this = inverse();
        };

        Matrix4 InvertPRMatrix() const;

        void makeTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation);

        void makeRot(const Quaternion& orientation, const Vector3& origin);

        void makeInverseTransform(const Vector3& position, const Vector3& scale, const Quaternion& orientation);

        inline bool isAffine(void) const
        {
            return m[0][3] == 0 && m[1][3] == 0 && m[2][3] == 0 && m[3][3] == 1;
        }
        inline bool isAffineColumnMajor(void) const
        {
            return m[3][0] == 0 && m[3][1] == 0 && m[3][2] == 0 && m[3][3] == 1;
        }


        inline Vector3 transformAffine(const Vector3& v) const
        {
            // assert(isAffineColumnMajor());

            return Vector3(
                    m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3], 
                    m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3],
                    m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3]);
        }

        inline Vector4 transformAffine(const Vector4& v) const
        {
            // assert(isAffineColumnMajor());

            return Vector4(
                m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z + m[0][3] * v.w, 
                m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z + m[1][3] * v.w,
                m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z + m[2][3] * v.w,
                v.w);
        }


        // NOTE: There is some compiler optimization issues with WIN64 that cause FORCEINLINE to cause a crash
        // Remove any scaling from this matrix (ie magnitude of each row is 1)
        void RemoveScaling(float Tolerance=SMALL_NUMBER);

        // Returns matrix without scale information
        Matrix4 GetMatrixWithoutScale(float Tolerance=SMALL_NUMBER) const;

        Vector3 ExtractScaling(float Tolerance=SMALL_NUMBER);

        Vector3 GetScaleVector(float Tolerance=SMALL_NUMBER) const;
        float GetScaleByAxis(int axis, float Tolerance = SMALL_NUMBER) const;
        // Remove any translation from this matrix
        Matrix4 RemoveTranslation() const;

        inline DeviceMatrix_ptr GetPointer() { return (DeviceMatrix_ptr)this; }
        inline const DeviceMatrix_ptr GetConstPointer() const { return (const DeviceMatrix_ptr)this; }
    };
}