// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: matrix44.h,v 1.55.14.1 2007/03/25 21:23:45 oliver Exp $
//

#ifndef BALL_MATHS_MATRIX44_H
#define BALL_MATHS_MATRIX44_H

#ifndef BALL_COMMON_EXCEPTION_H
# include <BALL/COMMON/exception.h>
#endif

#include <cmath>
#include <iostream>
#include <cstdlib>
#include <iomanip>

#ifndef BALL_MATHS_ANGLE_H
#	include <BALL/MATHS/angle.h>
#endif

#ifndef BALL_MATHS_VECTOR3_H
#	include <BALL/MATHS/vector3.h>
#endif

#ifndef BALL_MATHS_VECTOR4_H
#	include <BALL/MATHS/vector4.h>
#endif

namespace BALL
{
    /**	\defgroup Matrix44 4x4 Matrix
            Matrix representing transformations: class  \link TMatrix4x4 TMatrix4x4 \endlink  and class  \link Matrix4x4 Matrix4x4 \endlink

            \ingroup Primitives
    */
    //@{
    /// Default Type
    template <typename T>
    class TMatrix4x4;

    /** @name Storers
     */
    //@{
    /**	Input Operator.
            Read sixteen values of type <tt>T</tt> from an input stream.
            @param s	the input stream
            @param m the matrix to read
    */
    template <typename T>
    std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m)
        ;

    /**	Output Operator
            Writes sixteen values of type <tt>T</tt> to an output stream.
            @param s	the output stream
            @param m  the matrix to write
    */
    template <typename T>
    std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m)
        ;
    //@}

    /**	Generic 4x4 Matrix Class.
    */
    template <typename T>
    class TMatrix4x4
    {
        public:

        BALL_CREATE(TMatrix4x4)

        /**	@name	Constructors and Destructors
        */
        //@{

        /**	Default constructor.
                This method creates a new TMatrix4x4 object. The components
                are initialized to <tt>0</tt>.
        */
        TMatrix4x4()
            ;

        /**	Array constructor.
                This constructor creates a TMatrix4x4 object from the first
                sixteen elements pointed to by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        TMatrix4x4(const T* ptr);

        /**	Array constructor.
                This constructor creates a TMatrix4x4 object from the
                sixteen elements in the array assigned by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        TMatrix4x4(const T ptr[4][4]);

        /**	Copy constructor.
                Create a new TMatrix4x4 object from another.
                @param TMatrix4x4 the TMatrix4x4 object to be copied
                @param bool ignored (just for interface consistency)
        */
        TMatrix4x4(const TMatrix4x4& m)
            ;

        /**	Detailed constructor.
                Create a new TMatrix4x4 object from four TVector4.
                @param col1 assigned to the first column
                @param col2 assigned to the second column
                @param col3 assigned to the third column
                @param col4 assigned to the fourth column

        */
        TMatrix4x4
            (const TVector4<T>& col1, const TVector4<T>& col2,
             const TVector4<T>& col3, const TVector4<T>& col4)
            ;

        /**	Detailed constructor.
                Create a new TMatrix4x4 object from sixteen <tt>T</tt> values.
                @param m11 - m44 assigned to the components
        */
        TMatrix4x4
            (const T& m11, const T& m12, const T& m13, const T& m14,
             const T& m21, const T& m22, const T& m23, const T& m24,
             const T& m31, const T& m32, const T& m33, const T& m34,
             const T& m41, const T& m42, const T& m43, const T& m44)
            ;

        /**	Destructor.
                Destructs the TMatrix4x4 object. As there are no dynamic
                data structures, nothing happens.
        */
        virtual ~TMatrix4x4()

        {
        }

        /**	Clear method.
                The values are set to 0.
        */
        virtual void clear()
            ;

        //@}
        /**	@name	Assignment
        */
        //@{

        /**	Assign from array-ptr.
                Assign from the first	sixteen elements pointed to by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        void set(const T* ptr);

        /**	Assign from the first sixteen elements.
                pointed to by the array assigned by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        void set(const T ptr[4][4]);

        /**	Assign from another instance.
                @param TMatrix4x4	the TMatrix4x4 object to assign from
        */
        void set(const TMatrix4x4& m);

        /**	Assign from four TVector4.
                @param col1 assigned to the first column
                @param col2 assigned to the second column
                @param col3 assigned to the third column
                @param col4 assigned to the fourth column
        */
        void set
            (const TVector4<T>& col1, const TVector4<T>& col2,
             const TVector4<T>& col3, const TVector4<T>& col4);

        /**	Assign from sixteen values of type T.
                @param m11 - m44 assigned to the components
        */
        void set
            (const T& m11, const T& m12, const T& m13, const T& m14,
             const T& m21, const T& m22, const T& m23, const T& m24,
             const T& m31, const T& m32, const T& m33, const T& m34,
             const T& m41, const T& m42, const T& m43, const T& m44);

        /**	Assignment operator.
                Assign the components from the first 16 values assigned by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        **/
        TMatrix4x4& operator = (const T* ptr);

        /**	Assignment operator.
                Assign the components from the first 16 values assigned by <tt>ptr</tt>.
                @param ptr the array to construct from
                @exception NullPointer if <tt>ptr == 0</tt>
        **/
        TMatrix4x4& operator = (const T ptr[4][4]);

        /**	Assignment operator.
                Assign the components from another instance of TMatrix4x4.
                @param TMatrix4x4 the TMatrix4x4 to assign from
        **/
        TMatrix4x4& operator = (const TMatrix4x4& m);

        /**	Assign to an array.
                Assigns the components to a pointer of an array of sixteen values of type <tt>T</tt>.
                @param ptr the pointer to assign to
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        void get(T* ptr) const;

        /**	Assign to an array.
                Assigns the components to an array of sixteen values of type <tt>T</tt>.
                @param ptr the array to assign to
                @exception NullPointer if <tt>ptr == 0</tt>
        */
        void get(T ptr[4][4]) const;

        /**	Assign to another instance.
                Assigns the components to another TMatrix4x4.
                @param TMatrix4x4	the TMatrix4x4 to be assigned to
        */
        void get(TMatrix4x4& m) const;

        /**	Assign to four variables of type <b>  TVector4 </b>.
                @param col1 the TVector4 to obtain the values of the first column
                @param col2 the TVector4 to obtain the values of the second column
                @param col3 the TVector4 to obtain the values of the third column
                @param col4 the TVector4 to obtain the values of the fourth column
        */
        void get
            (TVector4<T>& col1, TVector4<T>& col2,
             TVector4<T>& col3, TVector4<T>& col4) const;

        /**	Assign to sixteen variables of type <tt>T</tt>.
                @param m11 - m44 the variables to assign to
        */
        void get
            (T& m11, T& m12, T& m13, T& m14,
             T& m21, T& m22, T& m23, T& m24,
             T& m31, T& m32, T& m33, T& m34,
             T& m41, T& m42, T& m43, T& m44) const;

        /**	Swap the contents of two instances of TMatrix4x4.
                @param TMatrix4x4 the TMatrix4x4 to swap contents with
        */
        void swap(TMatrix4x4& m);

        //@}
        /**	@name	Accessors
        */
        //@{

        /** Compute the trace.
                Get the sum of the diagonal elements (m11 + m22 + m33 + m44).
                @return T the trace
        */
        T getTrace() const;

        /** Create a zero matrix.
                A new matrix object is created and all elements set to 0.
        */
        static const TMatrix4x4& getZero();

        /** Create an identity matrix.
                A new matrix object is created and all elements but the diagonal are
                set to zero. The diagonal elements are set to 1.
        */
        static const TMatrix4x4& getIdentity();

        /** Set to an identity matrix.
                m11, m22, m33, m44 = 1;
                the other cells have the value 0;
        */
        void setIdentity();

        /** Set the diagonal elements to the given value.
                All other elements are set to 0.
                @param T the value to fill with (default: 1)
        */
        void set(const T& t = (T)1);

        /** Mirror the Matrix at the diagonal.
                All values are swaped by the mirrored value.
                (I.e. m12 <=> m21 , m13 <=> m31 , ...)
        */
        void transpose();

        /** Get a row of the matrix.
                @param row the number of the row (0-3)
                @return TVector4 the row
                @exception IndexOverflow if <tt>row > 3</tt>
        */
        TVector4<T> getRow(Position row) const;

        /** Get a column of the matrix.
                @param col the number of the column (0-3)
                @return TVector4 the column
                @exception IndexOverflow if <tt>col > 3</tt>
        */
        TVector4<T> getColumn(Position col) const;

        /** Set a row of the matrix.
                @param row the number of the row (0-3)
                @param row_value the new value of the row
                @exception IndexOverflow if <tt>row > 3</tt>
        */
        void setRow(Position row, const TVector4<T>& row_value);

        /** Set a column of the matrix.
                @param col the number of the column (0-3)
                @param col_value the new value of the col
                @exception IndexOverflow if <tt>col > 3</tt>
        */
        void setColumn(Position col, const TVector4<T>& col_value);

        /** Test whether two matrices are equal.
                Two matrices are considered equal, if
                \link Maths::isEqual Maths::isEqual \endlink  returns <b>true</b>
                for each pair of corresponding elements.
                @param m the matrix to compare with
                @return bool, <b>true</b> if all components are equal, <b>false</b> otherwise
        */
        bool isEqual(const TMatrix4x4& m) const;

        /** Get the diagonal of the matrix.
                @return TVector4 the diagonal
        */
        TVector4<T> getDiagonal() const;

        /** Access operator of a cell.
                @param row the number of the row (0-3)
                @param col the number of the column (0-3)
                @return T& a reference to the cell
                @exception IndexOverflow if <tt>col >3 || row > 3</tt>
        */
        T& operator () (Position row, Position col);

        /** Constant access operator of a cell.
                @param row the number of the row (0-3)
                @param col the number of the column (0-3)
                @return T& a const reference to the cell
                @exception IndexOverflow if <tt>col ||row > 3</tt>
        */
        const T& operator () (Position row, Position col) const;

        /**	Constant random access operator.
                Access single elements of the matrix. <tt>index</tt> may assume
                values in the range of 0 - 15. The elements of the matrix
                are returned rows first, i.e., in the following order: <tt>m11</tt>, <tt>m12</tt>, <tt>m13</tt>...
                @exception IndexOverflow if <tt>position > 15</tt>
        */
        const T& operator [] (Position position) const;

        /**	Mutable random access operator.
                @see operator[]
                @exception IndexOverflow if <tt>position > 15</tt>
        */
        T& operator [] (Position position);

        /**	Positive sign.
        */
        TMatrix4x4 operator + () const;

        /**	Negative sign.
        */
        TMatrix4x4 operator - () const;

        /** Addition operator.
                Adds another matrix to this matrix and return the result.
                @param m the matrix to add
                @return TMatrix4x4 the result
        */
        TMatrix4x4 operator + (const TMatrix4x4& m) const;

        /** Addition operator.
                Adds another matrix to this matrix.
                @param m the matrix to add
                @return TMatrix4x4&, {\em *this}
        */
        TMatrix4x4& operator += (const TMatrix4x4& m);

        /** Subtraction operator.
                Subtract another matrix from this matrix and return the result
                @param m the matrix to subtract
                @return TMatrix4x4 the result
        */
        TMatrix4x4 operator - (const TMatrix4x4& m) const;

        /** Subtraction operator.
                Subtract another matrix from this matrix.
                @param m the matrix to subtract
                @return TMatrix4x4&, {\em *this}
        */
        TMatrix4x4& operator -= (const TMatrix4x4& m);

        /**	Multiply by a scalar.
                Operator for multiplying every cell value with a scalar value.
                @return TMatrix4x4 the result
        */
        TMatrix4x4 operator * (const T& scalar) const;

        /**	Multiply by a scalar.
                Operator for multiplying every cell value with a scalar value.
                @return TMatrix4x4&, {\em *this}
        */
        TMatrix4x4& operator *= (const T& scalar);

        /**	Divide by a scalar.
                Operator for dividing every cell value by a scalar value.
                @return TMatrix4x4 the result
                @exception DivisionByZero if <tt>scalar == 0</tt>
        */
        TMatrix4x4 operator / (const T& scalar) const;

        /**	Divide by a scalar.
                Operator for dividing every cell value by a scalar value.
                @return TMatrix4x4&, {\em *this}
                @exception DivisionByZero if <tt>scalar == 0</tt>
        */
        TMatrix4x4& operator /= (const T& scalar);

        /**	Multiply two matrices.
                @return TMatrix4x4 the result
        */
        TMatrix4x4 operator * (const TMatrix4x4& m) const;

        /**	Multiply two matrices
                @return TMatrix4x4&, {\em *this}
        */
        TMatrix4x4& operator *= (const TMatrix4x4& m);

        /**	Multiplication by an instance of type <b>  TVector4 </b>.
                @return TMatrix4x4&, {\em *this}
        */
        TVector4<T> operator * (const TVector4<T>& vector) const;

        /**	Invert the matrix.
                Tests if the matrix can be inverted.
                If possible, the result will be inverted and the result returned in <b>  inverse </b>.
                @param inverse is assigned the inverse matrix
                @return bool true if the inverse matrix could be calculated, otherwise false.
        */
        bool invert(TMatrix4x4& inverse) const;

        /**	Invert the matrix.
                Tests if the matrix can be inverted.
                If this is possible, the result is stored in the matrix.
                @return bool true if the inverse matrix could be calculated, otherwise false.
        */
        bool invert();

        /**	Compute the determinant.
                @return T the determinant.
        */
        T getDeterminant() const;

        /**	Translate the matrix.
                @param x the x-component of the translation
                @param y the y-component of the translation
                @param z the z-component of the translation
        */
        void translate(const T &x, const T &y, const T &z);

        /**	Translate the matrix.
                @param v the vector to translate with
        */
        void translate(const TVector3<T>& v);

        /**	Set the matrix to a translation matrix.
                @param x the x-component of the translation
                @param y the y-component of the translation
                @param z the z-component of the translation
        */
        void setTranslation(const T& x, const T& y, const T& z);

        /**	Set the matrix to a translation matrix.
                @param v the vector to translate with
        */
        void setTranslation(const TVector3<T>& v);

        /**	Scale the matrix.
                @param x_scale the x scale factor
                @param y_scale the y scale factor
                @param z_scale the z scale factor
        */
        void scale(const T& x_scale, const T& y_scale, const T& z_scale);

        /**	Scale the matrix.
                @param scale the scale factor
        */
        void scale(const T& scale);

        /**	Scale the matrix.
                @param v the vector with the scale factor
        */
        void scale(const TVector3<T>& v);

        /**	Set the matrix to a scalation matrix.
                @param x_scale the x scale factor
                @param y_scale the y scale factor
                @param z_scale the z scale factor
        */
        void setScale(const T& x_scale, const T& y_scale, const T& z_scale);

        /**	Set the matrix to a scalation matrix.
                @param scale the scale factor
        */
        void setScale(const T& scale);

        /**	Set the matrix to a scalation matrix.
                @param v the vector with the scale factor
        */
        void setScale(const TVector3<T>& v);

        /**	Rotate the matrix around the x axis.
                @param phi the rotation angle
        */
        void rotateX(const TAngle<T>& phi);

        /**	Set the matrix to a x rotation matrix.
                @param phi the rotation angle
        */
        void setRotationX(const TAngle<T>& phi);

        /**	Rotate the matrix around the y axis.
                @param phi the rotation angle
        */
        void rotateY(const TAngle<T>& phi);

        /**	Set the matrix to a y rotation matrix.
                @param phi the rotation angle
        */
        void setRotationY(const TAngle<T>& phi);

        /**	Rotate the matrix around the z axis.
                @param phi the rotation angle
        */
        void rotateZ(const TAngle<T>& phi);

        /**	Set the matrix to a z rotation matrix.
                @param phi the rotation angle
        */
        void setRotationZ(const TAngle<T>& phi);

        /** Rotate the matrix around a given axis.
                @param phi the rotation angle
                @param axis_x the x component of the axis
                @param axis_y the y component of the axis
                @param axis_z the z component of the axis
        */
        void rotate(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z);

        /** Rotate the matrix around a given axis.
                @param phi the rotation angle
                @param axis the axis vector
        */
        void rotate(const TAngle<T>& phi, const TVector3<T>& axis);

        /** Rotate the matrix around a given axis.
                @param phi the rotation angle
                @param axis the axis vector, the fourth component of the vector is ignored
        */
        void rotate(const TAngle<T>& phi, const TVector4<T>& axis);

        /**	Set the matrix to a rotation matrix.
                @param phi the rotation angle
                @param axis_x the x component of the axis
                @param axis_y the y component of the axis
                @param axis_z the z component of the axis
        */
        void setRotation(const TAngle<T>& phi, const T& axis_x, const T& axis_y, const T& axis_z);

        /**	Set the matrix to a rotation matrix.
                @param phi the rotation angle
                @param axis the axis vector
        */
        void setRotation(const TAngle<T>& phi, const TVector3<T>& axis);

        /**	Set the matrix to a rotation matrix.
                @param phi the rotation angle
                @param axis the axis vector, the fourth component of the vector is ignored
        */
        void setRotation(const TAngle<T>& phi, const TVector4<T>& axis);
        //@}

        /**	@name	Predicates
        */
        //@{

        /**	Equality operator.
                Instead of this operator isEqual should be used.
                 \link isEqual isEqual \endlink
                @return bool, <b>true</b> if all components are equal, <b>false</b> otherwise
        */
        bool operator == (const TMatrix4x4& m) const;

        /**	Inequality operator.
                Instead of this operator isEqual should be used.
                 \link isEqual isEqual \endlink
                @return bool, <b>true</b> if the two TMatrix4x4 differ in at least one component, <b>false</b> otherwise
        */
        bool operator != (const TMatrix4x4& m) const;

        /** Test whether this matrix is an identity matrix.
                (I.e. m11, m22, m33, m44 = 1 and the other cells have the value 0)
                @return bool, <b>true</b> if identity matrix, <b>false</b> otherwise
        */
        bool isIdentity() const;

        /** Test whether this matrix is regular.
                @return bool, <b>true</b> if (Determinant != 0), <b>false</b> otherwise
        */
        bool isRegular() const;

        /** Test whether this matrix is singular.
                @return bool, <b>true</b> if (Determinant == 0), <b>false</b> otherwise
        */
        bool isSingular() const;

        /** Test whether this matrix is symmetric.
                (m12 = m21, m31 = m13, ...)
                @return bool, <b>true</b> if symmatric, <b>false</b> otherwise
        */
        bool isSymmetric() const;

        /** Test whether the lower triangular is zero.
                @return bool, <b>true</b> if (m12 = m13 = m14 = m23 = m24 = m34 = 0), <b>false</b> otherwise
        */
        bool isLowerTriangular() const;

        /** Test whether the upper triangular is zero.
                @return bool, <b>true</b> if (m21 = m31 = m32 = m41 = m42 = m43 = 0), <b>false</b> otherwise
        */
        bool isUpperTriangular() const;

        /** Test whether all cells but the diagonal are zero.
                @return bool, <b>true</b> or <b>false</b>
        */
        bool isDiagonal() const;
        //@}

        /**	@name	Debugging and Diagnostics
        */
        //@{

        /**	Test whether instance is valid.
                Always returns true.
                @return bool <b>true</b>
        */
        bool isValid() const;

        /** Internal state dump.
                Dump the current internal state of {\em *this} to
                the output ostream <b>  s </b> with dumping depth <b>  depth </b>.
                @param   s - output stream where to output the internal state of {\em *this}
                @param   depth - the dumping depth
        */
        void dump(std::ostream& s = std::cout, Size depth = 0) const;
        //@}

        /**	@name	Attributes
        */
        //@{

        ///	1st cell in the 1st row
        T m11;

        /// 2nd cell in the 1st row
        T m12;

        ///	3rd cell in the 1st row
        T m13;

        ///	4th cell in the 1st row
        T m14;

        ///	1st cell in the 2nd row
        T m21;

        ///	2nd cell in the 2nd row
        T m22;

        ///	3rd cell in the 2nd row
        T m23;

        ///	4th cell in the 2nd row
        T m24;

        ///	1st cell in the 3rd row
        T m31;

        ///	2nd cell in the 3rd row
        T m32;

        ///	3rd cell in the 3rd row
        T m33;

        ///	4th cell in the 3rd row
        T m34;

        ///	1st cell in the 4th row
        T m41;

        ///	2nd cell in the 4th row
        T m42;

        ///	3rd cell in the 4th row
        T m43;

        ///	4th cell in the 4th row
        T m44;
        //@}

        private:

        void initializeComponentPointers_()
        {
            T **ptr = (T **)comp_ptr_;

            *ptr++ = &m11; *ptr++ = &m12; *ptr++ = &m13; *ptr++ = &m14;
            *ptr++ = &m21; *ptr++ = &m22;	*ptr++ = &m23; *ptr++ = &m24;
            *ptr++ = &m31; *ptr++ = &m32; *ptr++ = &m33; *ptr++ = &m34;
            *ptr++ = &m41; *ptr++ = &m42;	*ptr++ = &m43; *ptr   = &m44;
        }

        // pointers to the components of the matrix
        T* comp_ptr_[16];
    };
    //@}

    template <typename T>
    TMatrix4x4<T>::TMatrix4x4()
        :	m11(0), m12(0), m13(0), m14(0),
            m21(0), m22(0), m23(0), m24(0),
            m31(0), m32(0), m33(0), m34(0),
            m41(0), m42(0), m43(0), m44(0)
    {
        initializeComponentPointers_();
    }

    template <typename T>
    TMatrix4x4<T>::TMatrix4x4( const T* ptr)
    {
        if (ptr == 0)
        {
            throw Exception::NullPointer(__FILE__, __LINE__);
        }

        m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
        m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
        m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
        m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;

        initializeComponentPointers_();
    }

    template <typename T>
    TMatrix4x4<T>::TMatrix4x4(const T array_ptr[4][4])
    {
        if (array_ptr == 0)
        {
            throw Exception::NullPointer(__FILE__, __LINE__);
        }

        const T *ptr = *array_ptr;

        m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
        m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
        m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
        m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;

        initializeComponentPointers_();
    }

    template <typename T>
    TMatrix4x4<T>::TMatrix4x4(const TMatrix4x4<T>& m)
        :	m11(m.m11), m12(m.m12), m13(m.m13), m14(m.m14),
            m21(m.m21), m22(m.m22), m23(m.m23), m24(m.m24),
            m31(m.m31), m32(m.m32), m33(m.m33), m34(m.m34),
            m41(m.m41), m42(m.m42), m43(m.m43), m44(m.m44)
    {
        initializeComponentPointers_();
    }


    template <typename T>
    TMatrix4x4<T>::TMatrix4x4
        (const TVector4<T>& col1, const TVector4<T>& col2,
         const TVector4<T>& col3,const TVector4<T>& col4)
        :	m11(col1.x), m12(col1.y), m13(col1.z), m14(col1.h),
            m21(col2.x), m22(col2.y), m23(col2.z), m24(col2.h),
            m31(col3.x), m32(col3.y), m33(col3.z), m34(col3.h),
            m41(col4.x), m42(col4.y), m43(col4.z), m44(col4.h)
    {
        initializeComponentPointers_();
    }

    template <typename T>
    TMatrix4x4<T>::TMatrix4x4
        (const T& m11, const T& m12, const T& m13, const T& m14,
         const T& m21, const T& m22, const T& m23, const T& m24,
         const T& m31, const T& m32, const T& m33, const T& m34,
         const T& m41, const T& m42, const T& m43, const T& m44)
        :	m11(m11), m12(m12), m13(m13), m14(m14),
            m21(m21), m22(m22), m23(m23), m24(m24),
            m31(m31), m32(m32), m33(m33), m34(m34),
            m41(m41), m42(m42), m43(m43), m44(m44)
    {
        initializeComponentPointers_();
    }

    template <typename T>
    void TMatrix4x4<T>::clear()
    {
        set((T)0);
    }

    template <typename T>
    void TMatrix4x4<T>::set(const T* ptr)
    {
        if (ptr == 0)
        {
            throw Exception::NullPointer(__FILE__, __LINE__);
        }

        m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
        m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
        m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
        m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
    }

    template <typename T>
    void TMatrix4x4<T>::set(const T array_ptr[4][4])
    {
    if (array_ptr == 0)
        {
      throw Exception::NullPointer(__FILE__, __LINE__);
        }

        const T *ptr = *array_ptr;

        m11 = *ptr++; m12 = *ptr++; m13 = *ptr++; m14 = *ptr++;
        m21 = *ptr++; m22 = *ptr++; m23 = *ptr++; m24 = *ptr++;
        m31 = *ptr++; m32 = *ptr++; m33 = *ptr++; m34 = *ptr++;
        m41 = *ptr++; m42 = *ptr++; m43 = *ptr++; m44 = *ptr;
    }

    template <typename T>
    void TMatrix4x4<T>::set(const TMatrix4x4<T>& m)
    {
        m11 = m.m11; m12 = m.m12; m13 = m.m13; m14 = m.m14;
        m21 = m.m21; m22 = m.m22; m23 = m.m23; m24 = m.m24;
        m31 = m.m31; m32 = m.m32; m33 = m.m33; m34 = m.m34;
        m41 = m.m41; m42 = m.m42; m43 = m.m43; m44 = m.m44;
    }

    template <typename T>
    void TMatrix4x4<T>::set
        (const TVector4<T>& col1, const TVector4<T>& col2,
         const TVector4<T>& col3, const TVector4<T>& col4)
    {
        m11 = col1.x; m12 = col1.y; m13 = col1.z; m14 = col1.h;
        m21 = col2.x; m22 = col2.y; m23 = col2.z; m24 = col2.h;
        m31 = col3.x; m32 = col3.y; m33 = col3.z; m34 = col3.h;
        m41 = col4.x; m42 = col4.y; m43 = col4.z; m44 = col4.h;
    }

    template <typename T>
    void TMatrix4x4<T>::set
        (const T& c11, const T& c12, const T& c13, const T& c14,
         const T& c21, const T& c22, const T& c23, const T& c24,
         const T& c31, const T& c32, const T& c33, const T& c34,
         const T& c41, const T& c42, const T& c43, const T& c44)
    {
        m11 = c11; m12 = c12;	m13 = c13; m14 = c14;
        m21 = c21; m22 = c22;	m23 = c23; m24 = c24;
        m31 = c31; m32 = c32; m33 = c33; m34 = c34;
        m41 = c41; m42 = c42; m43 = c43; m44 = c44;
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T* ptr)
    {
        set(ptr);
        return *this;
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T>& TMatrix4x4<T>::operator = (const T array_ptr[4][4])
    {
        set(array_ptr);
        return *this;
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T>& TMatrix4x4<T>::operator = (const TMatrix4x4<T>& m)
    {
        set(m);
        return *this;
    }

    template <typename T>
    void TMatrix4x4<T>::get(T* ptr) const
    {
    if (ptr == 0)
        {
      throw Exception::NullPointer(__FILE__, __LINE__);
        }

        *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14;
        *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24;
        *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34;
        *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr   = m44;
    }

    template <typename T>
    void TMatrix4x4<T>::get(T array_ptr[4][4]) const
    {
    if (array_ptr == 0)
        {
       throw Exception::NullPointer(__FILE__, __LINE__);
        }

        T *ptr = *array_ptr;

        *ptr++ = m11; *ptr++ = m12; *ptr++ = m13; *ptr++ = m14;
        *ptr++ = m21; *ptr++ = m22; *ptr++ = m23; *ptr++ = m24;
        *ptr++ = m31; *ptr++ = m32; *ptr++ = m33; *ptr++ = m34;
        *ptr++ = m41; *ptr++ = m42; *ptr++ = m43; *ptr   = m44;
    }

    template <typename T>
    void TMatrix4x4<T>::get(TMatrix4x4<T>& m) const
    {
        m.set(*this);
    }

    template <typename T>
    void TMatrix4x4<T>::get
        (TVector4<T>& col1, TVector4<T>& col2,
         TVector4<T>& col3, TVector4<T>& col4) const
    {
        col1.x = m11; col1.y = m12; col1.z = m13; col1.h = m14;
        col2.x = m21; col2.y = m22; col2.z = m23; col2.h = m24;
        col3.x = m31; col3.y = m32; col3.z = m33; col3.h = m34;
        col4.x = m41; col4.y = m42; col4.z = m43; col4.h = m44;
    }

    template <typename T>
    void TMatrix4x4<T>::get
        (T& c11, T& c12, T& c13, T& c14,
         T& c21, T& c22, T& c23, T& c24,
         T& c31, T& c32, T& c33, T& c34,
         T& c41, T& c42, T& c43, T& c44) const
    {
        c11 = m11; c12 = m12;	c13 = m13; c14 = m14;
        c21 = m21; c22 = m22;	c23 = m23; c24 = m24;
        c31 = m31; c32 = m32; c33 = m33; c34 = m34;
        c41 = m41; c42 = m42; c43 = m43; c44 = m44;
    }

    template <typename T>
    BALL_INLINE
    T TMatrix4x4<T>::getTrace() const
    {
        return (m11 + m22 + m33 + m44);
    }

    template <typename T>
    BALL_INLINE
    const TMatrix4x4<T>& TMatrix4x4<T>::getZero()
    {
        static TMatrix4x4<T> null_matrix
            (0, 0, 0, 0,
             0, 0, 0, 0,
             0, 0, 0, 0,
             0, 0, 0, 0);

        return null_matrix;
    }


    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::setIdentity()
    {
        m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
        m11 = m22 = m33 = m44 = (T)1;
    }
    template <typename T>
    BALL_INLINE
    const TMatrix4x4<T>& TMatrix4x4<T>::getIdentity()
    {
        static TMatrix4x4<T> identity
            (1, 0, 0, 0,
             0, 1, 0, 0,
             0, 0, 1, 0,
             0, 0, 0, 1);

        return identity;
    }

    template <typename T>
    void TMatrix4x4<T>::set(const T& t)
    {
            m11 = m12 = m13 = m14
        = m21 = m22 = m23 = m24
        = m31 = m32 = m33 = m34
        = m41 = m42 = m43 = m44
        = t;
    }

    template <typename T>
    void TMatrix4x4<T>::swap(TMatrix4x4<T>& m)
    {
        T tmp = m11; m11 = m.m11; m.m11 = tmp;
            tmp = m12; m12 = m.m12; m.m12 = tmp;
            tmp = m13; m13 = m.m13; m.m13 = tmp;
            tmp = m14; m14 = m.m14; m.m14 = tmp;
            tmp = m21; m21 = m.m21; m.m21 = tmp;
            tmp = m22; m22 = m.m22; m.m22 = tmp;
            tmp = m23; m23 = m.m23; m.m23 = tmp;
            tmp = m24; m24 = m.m24; m.m24 = tmp;
            tmp = m31; m31 = m.m31; m.m31 = tmp;
            tmp = m32; m32 = m.m32; m.m32 = tmp;
            tmp = m33; m33 = m.m33; m.m33 = tmp;
            tmp = m34; m34 = m.m34; m.m34 = tmp;
            tmp = m41; m41 = m.m41; m.m41 = tmp;
            tmp = m42; m42 = m.m42; m.m42 = tmp;
            tmp = m43; m43 = m.m43; m.m43 = tmp;
            tmp = m44; m44 = m.m44; m.m44 = tmp;
    }

    template <typename T>
    void TMatrix4x4<T>::transpose()
    {
        T tmp = m12;
        m12 = m21;
        m21 = tmp;

        tmp  = m13;
        m13 = m31;
        m31 = tmp;

        tmp  = m14;
        m14 = m41;
        m41 = tmp;

        tmp  = m23;
        m23 = m32;
        m32 = tmp;

        tmp  = m24;
        m24 = m42;
        m42 = tmp;

        tmp  = m34;
        m34 = m43;
        m43 = tmp;
    }

    template <typename T>
    TVector4<T> TMatrix4x4<T>::getRow(Position row) const
    {
        if (row > 3)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3);
        }

        // calculate the start of the row in the array
        const T* ptr = comp_ptr_[4 * row];
        return TVector4<T> (ptr[0], ptr[1], ptr[2], ptr[3]);
    }

    template <typename T>
    TVector4<T> TMatrix4x4<T>::getColumn(Position col) const
    {
        if (col > 3)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3);
        }

        const T* ptr = comp_ptr_[col];

        return TVector4<T> (ptr[0], ptr[4], ptr[8], ptr[12]);
    }


    template <typename T>
    void TMatrix4x4<T>::setRow(Position row, const TVector4<T>& row_value)
    {
        if (row > 3)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, row, 3);
        }

        // calculate a pointer to the start of the row
        T* ptr = comp_ptr_[4 * row];

        ptr[0] = row_value.x;
        ptr[1] = row_value.y;
        ptr[2] = row_value.z;
        ptr[3] = row_value.h;
    }

    template <typename T>
    void TMatrix4x4<T>::setColumn(Position col, const TVector4<T>& col_value)
    {
        if (col > 3)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, col, 3);
        }

        // calculate a pointer to the start of the column
        T* ptr = comp_ptr_[col];

        ptr[0] = col_value.x;
        ptr[4] = col_value.y;
        ptr[8] = col_value.z;
        ptr[12] = col_value.h;
    }

    template <typename T>
    bool TMatrix4x4<T>::isEqual(const TMatrix4x4<T>& m) const
    {
        // iterate over all component pointers
        // and compare the elements for approximate equality
        for (Position i = 0; i < 16; i++)
        {
            if (Maths::isEqual(*comp_ptr_[i], *m.comp_ptr_[i]) == false)
            {
                return false;
            }
        }

        return true;
    }

    template <typename T>
    TVector4<T>TMatrix4x4<T>::getDiagonal() const
    {
        return TVector4<T>(m11, m22, m33, m44);
    }

    template <typename T>
    BALL_INLINE
    T& TMatrix4x4<T>::operator () (Position row, Position col)
    {
    if ((row > 3) || (col > 3))
        {
      throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3);
        }

        return *comp_ptr_[4 * row + col];
    }

    template <typename T>
    BALL_INLINE
    const T& TMatrix4x4<T>::operator () (Position row, Position col) const
    {
    if ((row > 3) || (col > 3))
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, row + col, 3);
        }

        return *comp_ptr_[4 * row + col];
    }

    template <typename T>
    BALL_INLINE
    const T& TMatrix4x4<T>::operator [] (Position position) const
    {
        if (position > 15)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15);
        }
        return *comp_ptr_[position];
    }

    template <typename T>
    BALL_INLINE
    T& TMatrix4x4<T>::operator [] (Position position)
    {
        if (position > 15)
        {
            throw Exception::IndexOverflow(__FILE__, __LINE__, position, 15);
        }
        return *comp_ptr_[position];
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T> TMatrix4x4<T>::operator + () const
    {
        return *this;
    }

    template <typename T>
    BALL_INLINE TMatrix4x4<T>
    TMatrix4x4<T>::operator - () const
    {
        return TMatrix4x4<T>
            (-m11, -m12, -m13, -m14,
             -m21, -m22, -m23, -m24,
             -m31, -m32, -m33, -m34,
             -m41, -m42, -m43, -m44);
    }

    template <typename T>
    TMatrix4x4<T> TMatrix4x4<T>::operator + (const TMatrix4x4<T>& m) const
    {
        return TMatrix4x4<T>
            (m11 + m.m11, m12 + m.m12, m13 + m.m13, m14 + m.m14,
             m21 + m.m21, m22 + m.m22, m23 + m.m23, m24 + m.m24,
             m31 + m.m31, m32 + m.m32, m33 + m.m33, m34 + m.m34,
             m41 + m.m41, m42 + m.m42, m43 + m.m43, m44 + m.m44);
    }

    template <typename T>
    TMatrix4x4<T>& TMatrix4x4<T>::operator += (const TMatrix4x4<T>& m)
    {
        m11 += m.m11;
        m12 += m.m12;
        m13 += m.m13;
        m14 += m.m14;
        m21 += m.m21;
        m22 += m.m22;
        m23 += m.m23;
        m24 += m.m24;
        m31 += m.m31;
        m32 += m.m32;
        m33 += m.m33;
        m34 += m.m34;
        m41 += m.m41;
        m42 += m.m42;
        m43 += m.m43;
        m44 += m.m44;

        return *this;
    }

    template <typename T>
    TMatrix4x4<T> TMatrix4x4<T>::operator - (const TMatrix4x4<T>& m) const
    {
        return TMatrix4x4<T>
            (m11 - m.m11, m12 - m.m12, m13 - m.m13, m14 - m.m14,
             m21 - m.m21, m22 - m.m22, m23 - m.m23, m24 - m.m24,
             m31 - m.m31, m32 - m.m32, m33 - m.m33, m34 - m.m34,
             m41 - m.m41, m42 - m.m42, m43 - m.m43, m44 - m.m44);
    }

    template <typename T>
    TMatrix4x4<T>& TMatrix4x4<T>::operator -= (const TMatrix4x4<T>& m)
    {
        m11 -= m.m11;
        m12 -= m.m12;
        m13 -= m.m13;
        m14 -= m.m14;
        m21 -= m.m21;
        m22 -= m.m22;
        m23 -= m.m23;
        m24 -= m.m24;
        m31 -= m.m31;
        m32 -= m.m32;
        m33 -= m.m33;
        m34 -= m.m34;
        m41 -= m.m41;
        m42 -= m.m42;
        m43 -= m.m43;
        m44 -= m.m44;

        return *this;
    }

    template <typename T>
    TMatrix4x4<T> TMatrix4x4<T>::operator * (const T& scalar) const
    {
        return TMatrix4x4<T>
            (m11 * scalar, m12 * scalar, m13 * scalar, m14 * scalar,
             m21 * scalar, m22 * scalar, m23 * scalar, m24 * scalar,
             m31 * scalar, m32 * scalar, m33 * scalar, m34 * scalar,
             m41 * scalar, m42 * scalar, m43 * scalar, m44 * scalar);
    }

    template <typename T>
    TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m)
    {
        return TMatrix4x4<T>
            (scalar * m.m11, scalar * m.m12, scalar * m.m13, scalar * m.m14,
             scalar * m.m21, scalar * m.m22, scalar * m.m23, scalar * m.m24,
             scalar * m.m31, scalar * m.m32, scalar * m.m33, scalar * m.m34,
             scalar * m.m41, scalar * m.m42, scalar * m.m43, scalar * m.m44);
    }

    template <typename T>
    TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const T& scalar)
    {
        m11 *= scalar;
        m12 *= scalar;
        m13 *= scalar;
        m14 *= scalar;
        m21 *= scalar;
        m22 *= scalar;
        m23 *= scalar;
        m24 *= scalar;
        m31 *= scalar;
        m32 *= scalar;
        m33 *= scalar;
        m34 *= scalar;
        m41 *= scalar;
        m42 *= scalar;
        m43 *= scalar;
        m44 *= scalar;

        return *this;
    }

    template <typename T>
    TVector3<T> operator *(const TMatrix4x4<T>& matrix, const TVector3<T>& vector)
    {
        return TVector3<T>
            (matrix.m11 * vector.x + matrix.m12 * vector.y + matrix.m13 * vector.z + matrix.m14,
             matrix.m21 * vector.x + matrix.m22 * vector.y + matrix.m23 * vector.z + matrix.m24,
             matrix.m31 * vector.x + matrix.m32 * vector.y + matrix.m33 * vector.z + matrix.m34);
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T>TMatrix4x4<T>::operator / (const T& scalar) const
    {
        if (scalar == (T)0)
        {
            throw Exception::DivisionByZero(__FILE__, __LINE__);
        }

        return (*this * ((T)1 / scalar));
    }

    template <typename T>
    BALL_INLINE
    TMatrix4x4<T>& TMatrix4x4<T>::operator /= (const T& scalar)
    {
        if (scalar == (T)0)
        {
            throw Exception::DivisionByZero(__FILE__, __LINE__);
        }

        return (*this *= (T)1 / scalar);
    }

    template <typename T>
    TMatrix4x4<T> TMatrix4x4<T>::operator * (const TMatrix4x4<T>& m) const
    {
        return TMatrix4x4<T>
                (m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41,
                 m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42,
                 m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43,
                 m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44,

                 m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41,
                 m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42,
                 m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43,
                 m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44,

                 m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41,
                 m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42,
                 m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43,
                 m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44,

                 m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41,
                 m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42,
                 m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43,
                 m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44);
    }

    template <typename T>
    TMatrix4x4<T>& TMatrix4x4<T>::operator *= (const TMatrix4x4<T>& m)
    {
        set(m11 * m.m11 + m12 * m.m21 + m13 * m.m31 + m14 * m.m41,
                m11 * m.m12 + m12 * m.m22 + m13 * m.m32 + m14 * m.m42,
                m11 * m.m13 + m12 * m.m23 + m13 * m.m33 + m14 * m.m43,
                m11 * m.m14 + m12 * m.m24 + m13 * m.m34 + m14 * m.m44,

        m21 * m.m11 + m22 * m.m21 + m23 * m.m31 + m24 * m.m41,
        m21 * m.m12 + m22 * m.m22 + m23 * m.m32 + m24 * m.m42,
        m21 * m.m13 + m22 * m.m23 + m23 * m.m33 + m24 * m.m43,
        m21 * m.m14 + m22 * m.m24 + m23 * m.m34 + m24 * m.m44,

        m31 * m.m11 + m32 * m.m21 + m33 * m.m31 + m34 * m.m41,
        m31 * m.m12 + m32 * m.m22 + m33 * m.m32 + m34 * m.m42,
        m31 * m.m13 + m32 * m.m23 + m33 * m.m33 + m34 * m.m43,
        m31 * m.m14 + m32 * m.m24 + m33 * m.m34 + m34 * m.m44,

        m41 * m.m11 + m42 * m.m21 + m43 * m.m31 + m44 * m.m41,
        m41 * m.m12 + m42 * m.m22 + m43 * m.m32 + m44 * m.m42,
        m41 * m.m13 + m42 * m.m23 + m43 * m.m33 + m44 * m.m43,
        m41 * m.m14 + m42 * m.m24 + m43 * m.m34 + m44 * m.m44);

        return *this;
    }

    template <typename T>
    TVector4<T> TMatrix4x4<T>::operator * (const TVector4<T>& v) const
    {
        return TVector4<T>
            (m11 * v.x + m12 * v.y + m13 * v.z + m14 * v.h,
             m21 * v.x + m22 * v.y + m23 * v.z + m24 * v.h,
             m31 * v.x + m32 * v.y + m33 * v.z + m34 * v.h,
             m41 * v.x + m42 * v.y + m43 * v.z + m44 * v.h);
    }

    template <typename T>
    bool TMatrix4x4<T>::invert(TMatrix4x4<T>& inverse) const
    {
        /** First, we compute a QR decomposition, then we use it to solve
         *  the system A*A^-1 = I <=> R * A^-1 = Q^t, where R is upper
         *  triangular.
         *
         *  This is based on the Householder transform algorithm given in
         *  the Numerical Recipes.
         */
        Index i, j, k;

        T a[4][4] = // holds the matrix we want to invert
        {
            { m11, m12, m13, m14 },
            { m21, m22, m23, m24 },
            { m31, m32, m33, m34 },
            { m41, m42, m43, m44 }
        };

        // holds the maximum in the part of A we still have to work with
        T scale, sum_of_squares, sigma, tau;
        T c[4], d[4];
        for (k=0; k<3; k++)
        {
            scale = (T)0;
            // find the maximum in a
            for (i=k; i<4; i++)
                scale = Maths::max((T)fabs(a[i][k]), scale);

            // is the matrix singular?
            if (scale == (T)0)
                return false;

            // nope. we can normalize the remaining rows
            for (i=k; i<4; i++)
                a[i][k] /= scale;

            sum_of_squares = (T)0;
            for (i=k; i<4; i++)
                sum_of_squares += a[i][k]*a[i][k];

            // shift the diagonal element
            sigma = (a[k][k] >= 0) ? sqrt(sum_of_squares) : -sqrt(sum_of_squares);
            a[k][k] += sigma;

            c[k] =  sigma*a[k][k];
            d[k] = -scale*sigma;

            for (j = k+1; j<4; j++)
            {
                // store the scalar product of a_[k] and a_[j]
                sum_of_squares = (T)0;
                for (i = k; i<4; i++)
                    sum_of_squares += a[i][k] * a[i][j];

                tau = sum_of_squares / c[k];

                // prepare the matrix
                for (i=k; i<4; i++)
                    a[i][j] -= tau*a[i][k];
            }
        }
        d[3] = a[3][3];

        // is the matrix singular?
        if (d[3] == (T)0)
            return 1;

        // now we have the QR decomposition. The upper triangle of A contains
        // R, except for the diagonal elements, which are stored in d. c contains
        // the values needed to compute the Householder matrices Q, and the vectors
        // u needed for the determination of the Qs are stored in the lower triangle
        // of A
        //
        // now we need to solve four linear systems of equations, one for each column
        // of the resulting matrix
        T result[4][4];
        result[0][0] = 1; result[0][1] = 0; result[0][2] = 0; result[0][3] = 0;
        result[1][0] = 0; result[1][1] = 1; result[1][2] = 0; result[1][3] = 0;
        result[2][0] = 0; result[2][1] = 0; result[2][2] = 1; result[2][3] = 0;
        result[3][0] = 0; result[3][1] = 0; result[3][2] = 0; result[3][3] = 1;

        for (k=0; k<4; k++) // k generates the k-th column of the inverse
        {
            // form the vector Q^t * b, which is simple, since b = e_k
            for (j=0; j<3; j++)
            {
                sum_of_squares = (T)0;
                for (i=j; i<4; i++)
                    sum_of_squares += a[i][j]*result[i][k];

                tau = sum_of_squares / c[j];

                for (i=j; i<4; i++)
                    result[i][k] -= tau*a[i][j];
            }

            // and solve the resulting system
            result[3][k] /= d[3];
            for (i=2; i>=0; i--)
            {
                sum_of_squares = (T)0;
                for (j=i+1; j<4; j++)
                    sum_of_squares += a[i][j] * result[j][k];

                result[i][k] = (result[i][k] - sum_of_squares) / d[i];
            }
        }

        T* k_ptr = *result;
        inverse.m11 = *k_ptr++;
        inverse.m12 = *k_ptr++;
        inverse.m13 = *k_ptr++;
        inverse.m14 = *k_ptr++;
        inverse.m21 = *k_ptr++;
        inverse.m22 = *k_ptr++;
        inverse.m23 = *k_ptr++;
        inverse.m24 = *k_ptr++;
        inverse.m31 = *k_ptr++;
        inverse.m32 = *k_ptr++;
        inverse.m33 = *k_ptr++;
        inverse.m34 = *k_ptr++;
        inverse.m41 = *k_ptr++;
        inverse.m42 = *k_ptr++;
        inverse.m43 = *k_ptr++;
        inverse.m44 = *k_ptr;

        return true;
    }

    template <typename T>
    BALL_INLINE bool TMatrix4x4<T>::invert()
    {
        return invert(*this);
    }

    template <typename T>
    T TMatrix4x4<T>::getDeterminant() const
    {
        Position i;
        Position j;
        Position k;
        T submatrix[3][3];
        T matrix[4][4] =
        {
            { m11, m12, m13, m14 },
            { m21, m22, m23, m24 },
            { m31, m32, m33, m34 },
            { m41, m42, m43, m44 }
        };
        T determinant = 0;

        for (i = 0; i < 4; i++)
        {
            for (j = 0; j < 3; j++)
            {
                for (k = 0; k < 3; k++)
                {
                    submatrix[j][k] =
                    matrix[j + 1][(k < i) ? k : k + 1];
                }
            }

            determinant += matrix[0][i] * (T)(i / 2.0 == (i >> 1) ? 1 : -1)
                                    * (submatrix[0][0] * submatrix[1][1] * submatrix[2][2]
                                         + submatrix[0][1] * submatrix[1][2] * submatrix[2][0]
                                         + submatrix[0][2] * submatrix[1][0] * submatrix[2][1]
                                         - submatrix[0][2] * submatrix[1][1] * submatrix[2][0]
                                         - submatrix[0][0] * submatrix[1][2] * submatrix[2][1]
                                         - submatrix[0][1] * submatrix[1][0] * submatrix[2][2]);
        }

        return determinant;
    }

    template <typename T>
    void TMatrix4x4<T>::translate(const T& x, const T& y, const T& z)
    {
        m14 += m11 * x + m12 * y + m13 * z;
        m24 += m21 * x + m22 * y + m23 * z;
        m34 += m31 * x + m32 * y + m33 * z;
        m44 += m41 * x + m42 * y + m43 * z;
    }

    template <typename T>
    void TMatrix4x4<T>::translate(const TVector3<T>& v)
    {
        m14 += m11 * v.x + m12 * v.y + m13 * v.z;
        m24 += m21 * v.x + m22 * v.y + m23 * v.z;
        m34 += m31 * v.x + m32 * v.y + m33 * v.z;
        m44 += m41 * v.x + m42 * v.y + m43 * v.z;
    }

    template <typename T>
    void TMatrix4x4<T>::setTranslation(const T& x, const T& y, const T& z)
    {
        m11 = m22 = m33 = m44 = 1;

        m12 = m13 =
        m21 = m23 =
        m31 = m32 =
        m41 = m42 = m43 = 0;

        m14 = x;
        m24 = y;
        m34 = z;
    }

    template <typename T>
    void TMatrix4x4<T>::setTranslation(const TVector3<T>& v)
    {
        m11 = m22 = m33 = m44 = 1;

        m12 = m13 =
        m21 = m23 =
        m31 = m32 =
        m41 = m42 = m43 = 0;

        m14 = v.x;
        m24 = v.y;
        m34 = v.z;
    }

    template <typename T>
    void TMatrix4x4<T>::scale(const T& x_scale, const T& y_scale, const T& z_scale)
    {
        m11 *= x_scale;
        m21 *= x_scale;
        m31 *= x_scale;
        m41 *= x_scale;

        m12 *= y_scale;
        m22 *= y_scale;
        m32 *= y_scale;
        m42 *= y_scale;

        m13 *= z_scale;
        m23 *= z_scale;
        m33 *= z_scale;
        m43 *= z_scale;
    }

    template <typename T>
    void TMatrix4x4<T>::scale(const T& scale)
    {
        m11 *= scale;
        m21 *= scale;
        m31 *= scale;
        m41 *= scale;

        m12 *= scale;
        m22 *= scale;
        m32 *= scale;
        m42 *= scale;

        m13 *= scale;
        m23 *= scale;
        m33 *= scale;
        m43 *= scale;
    }

    template <typename T>
    void TMatrix4x4<T>::scale(const TVector3<T>& v)
    {
        m11 *= v.x;
        m21 *= v.x;
        m31 *= v.x;
        m41 *= v.x;

        m12 *= v.y;
        m22 *= v.y;
        m32 *= v.y;
        m42 *= v.y;

        m13 *= v.z;
        m23 *= v.z;
        m33 *= v.z;
        m43 *= v.z;
    }

    template <typename T>
    void TMatrix4x4<T>::setScale(const T& x_scale, const T& y_scale, const T& z_scale)
    {
        m11 = x_scale;
        m22 = y_scale;
        m33 = z_scale;
        m44 = 1;

        m12 = m13 = m14 =
        m21 = m23 = m24 =
        m31 = m32 = m34 =
        m41 = m42 = m43 = 0;
    }

    template <typename T>
    void TMatrix4x4<T>::setScale(const T& scale)
    {
        m11 = scale;
        m22 = scale;
        m33 = scale;
        m44 = 1;

        m12 = m13 = m14 =
        m21 = m23 = m24 =
        m31 = m32 = m34 =
        m41 = m42 = m43 = 0;
    }

    template <typename T>
    void TMatrix4x4<T>::setScale(const TVector3<T>& v)
    {
        m11 = v.x;
        m22 = v.y;
        m33 = v.z;
        m44 = 1;

        m12 = m13 = m14 =
        m21 = m23 = m24 =
        m31 = m32 = m34 =
        m41 = m42 = m43 = 0;
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::rotateX(const TAngle<T>& phi)
    {
        TMatrix4x4<T> rotation;

        rotation.setRotationX(phi);
        *this *= rotation;
    }

    template <typename T>
    void TMatrix4x4<T>::setRotationX(const TAngle<T>& phi)
    {
        m11 = m44 = 1;

            m12 = m13 = m14
        = m21 = m24
        = m31 = m34
        = m41 = m42 = m43
        = 0;

        m22 = m33 = cos(phi);
        m23 = -(m32 = sin(phi));
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::rotateY(const TAngle<T>& phi)
    {
        TMatrix4x4<T> rotation;

        rotation.setRotationY(phi);
        *this *= rotation;
    }

    template <typename T>
    void TMatrix4x4<T>::setRotationY(const TAngle<T>& phi)
    {
        m22 = m44 = 1;

            m12 = m14
        = m21 = m23 = m24
        = m32 = m34
        = m41 = m42 = m43
        = 0;

        m11 = m33 = cos(phi);
        m31 = -(m13 = sin(phi));
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::rotateZ(const TAngle<T>& phi)
    {
        TMatrix4x4<T> rotation;

        rotation.setRotationZ(phi);
        *this *= rotation;
    }

    template <typename T>
    void TMatrix4x4<T>::setRotationZ(const TAngle<T>& phi)
    {
        m33 = m44 = 1;

        m13 = m14 = m23 = m24 = m31 =
        m32 = m34 = m41 = m42 = m43 = 0;

        m11 =  m22 = cos(phi);
        m12 = -(m21 = sin(phi));
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector3<T>& v)
    {
        rotate(phi, v.x, v.y, v.z);
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const TVector4<T>& v)
    {
        rotate(phi, v.x, v.y, v.z);
    }

    //
    //     Arbitrary axis rotation matrix.
    //
    //  [Taken from the MESA-Library. But modified for additional Speed-Up.]
    //
    //  This function was contributed by Erich Boleyn (erich@uruk.org).
    //
    //  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
    //  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
    //  (which is about the X-axis), and the two composite transforms
    //  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
    //  from the arbitrary axis to the X-axis then back.  They are
    //  all elementary rotations.
    //
    //  Rz' is a rotation about the Z-axis, to bring the axis vector
    //  into the x-z plane.  Then Ry' is applied, rotating about the
    //  Y-axis to bring the axis vector parallel with the X-axis.  The
    //  rotation about the X-axis is then performed.  Ry and Rz are
    //  simply the respective inverse transforms to bring the arbitrary
    //  axis back to it's original orientation.  The first transforms
    //  Rz' and Ry' are considered inverses, since the data from the
    //  arbitrary axis gives you info on how to get to it, not how
    //  to get away from it, and an inverse must be applied.
    //
    //  The basic calculation used is to recognize that the arbitrary
    //  axis vector (x, y, z), since it is of unit length, actually
    //  represents the sines and cosines of the angles to rotate the
    //  X-axis to the same orientation, with theta being the angle about
    //  Z and phi the angle about Y (in the order described above)
    //  as follows:
    //
    //  cos ( theta ) = x / sqrt ( 1 - z^2 )
    //  sin ( theta ) = y / sqrt ( 1 - z^2 )
    //
    //  cos ( phi ) = sqrt ( 1 - z^2 )
    //  sin ( phi ) = z
    //
    //  Note that cos ( phi ) can further be inserted to the above
    //  formulas:
    //
    //  cos ( theta ) = x / cos ( phi )
    //  sin ( theta ) = y / sin ( phi )
    //
    //  ...etc.  Because of those relations and the standard trigonometric
    //  relations, it is pssible to reduce the transforms down to what
    //  is used below.  It may be that any primary axis chosen will give the
    //  same results (modulo a sign convention) using thie method.
    //
    //  Particularly nice is to notice that all divisions that might
    //  have caused trouble when parallel to certain planes or
    //  axis go away with care paid to reducing the expressions.
    //  After checking, it does perform correctly under all cases, since
    //  in all the cases of division where the denominator would have
    //  been zero, the numerator would have been zero as well, giving
    //  the expected result.

    template <typename T>
    void TMatrix4x4<T>::rotate(const TAngle<T>& phi, const T& ax, const T& ay, const T& az)
    {
        T xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
        T x = ax;
        T y = ay;
        T z = az;

        double sin_angle = sin(phi);
        double cos_angle = cos(phi);

        xx = x * x;
        yy = y * y;
        zz = z * z;

        T mag = sqrt(xx + yy + zz);

        if (mag == (T)0)
        {
            m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
            m11 = m22 = m33 = m44 = (T)1;
        }

        x /= mag;
        y /= mag;
        z /= mag;

        // we need to recalculate xx, yy, zz due to the
        // normalization. recalculation is probably faster
        // than normalizing xx, yy, zz
        xx = x*x;
        yy = y*y;
        zz = z*z;

        xy = x * y;
        yz = y * z;
        zx = z * x;
        xs = (T) (x * sin_angle);
        ys = (T) (y * sin_angle);
        zs = (T) (z * sin_angle);
        one_c = (T) (1 - cos_angle);

        m11 = (T)( (one_c * xx) + cos_angle );
        m12 = (one_c * xy) - zs;
        m13 = (one_c * zx) + ys;
        m14 = 0;

        m21 = (one_c * xy) + zs;
        m22 = (T) ((one_c * yy) + cos_angle);
        m23 = (one_c * yz) - xs;
        m24 = 0;

        m31 = (one_c * zx) - ys;
        m32 = (one_c * yz) + xs;
        m33 = (T) ((one_c * zz) + cos_angle);
        m34 = 0;

        m41 = 0;
        m42 = 0;
        m43 = 0;
        m44 = 1;
    }

    template <typename T>
    void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const T& x, const T& y, const T& z)
    {
        m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
        m11 = m22 = m33 = m44 = (T)1;
        rotate(phi, x, y, z);
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector3<T>& v)
    {
        m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
        m11 = m22 = m33 = m44 = (T)1;
        rotate(phi, v.x, v.y, v.z);
    }

    template <typename T>
    BALL_INLINE
    void TMatrix4x4<T>::setRotation(const TAngle<T>& phi, const TVector4<T>& v)
    {
        m12 = m13 = m14 = m21 = m23 = m24 = m31 = m32 = m34 = m41 = m42 = m43 = 0;
        m11 = m22 = m33 = m44 = (T)1;
        rotate(phi, v.x, v.y, v.z);
    }

    template <typename T>
    bool TMatrix4x4<T>::operator == (const TMatrix4x4<T>& m) const
    {
        return
            (   m11 == m.m11
             && m12 == m.m12
             && m13 == m.m13
             && m14 == m.m14
             && m21 == m.m21
             && m22 == m.m22
             && m23 == m.m23
             && m24 == m.m24
             && m31 == m.m31
             && m32 == m.m32
             && m33 == m.m33
             && m34 == m.m34
             && m41 == m.m41
             && m42 == m.m42
             && m43 == m.m43
             && m44 == m.m44);
    }

    template <typename T>
    bool TMatrix4x4<T>::operator != (const TMatrix4x4<T>& m) const
    {
        return
            (   m11 != m.m11
             || m12 != m.m12
             || m13 != m.m13
             || m14 != m.m14
             || m21 != m.m21
             || m22 != m.m22
             || m23 != m.m23
             || m24 != m.m24
             || m31 != m.m31
             || m32 != m.m32
             || m33 != m.m33
             || m34 != m.m34
             || m41 != m.m41
             || m42 != m.m42
             || m43 != m.m43
             || m44 != m.m44);
    }

    template <typename T>
    bool TMatrix4x4<T>::isIdentity() const
    {
        return
            (   m11 == (T)1
             && m12 == (T)0
             && m13 == (T)0
             && m14 == (T)0
             && m21 == (T)0
             && m22 == (T)1
             && m23 == (T)0
             && m24 == (T)0
             && m31 == (T)0
             && m32 == (T)0
             && m33 == (T)1
             && m34 == (T)0
             && m41 == (T)0
             && m42 == (T)0
             && m43 == (T)0
             && m44 == (T)1);
    }

    template <typename T>
    BALL_INLINE
    bool TMatrix4x4<T>::isRegular() const
    {
        return (getDeterminant() != (T)0);
    }

    template <typename T>
    BALL_INLINE
    bool TMatrix4x4<T>::isSingular() const
    {
        return (getDeterminant() == (T)0);
    }

    template <typename T>
    bool TMatrix4x4<T>::isSymmetric() const
    {
        return (   m12 == m21 && m13 == m31
                        && m14 == m41 && m23 == m32
                        && m24 == m42 && m34 == m43);
    }

    template <typename T>
    bool TMatrix4x4<T>::isLowerTriangular() const
    {
        return (   m12 == (T)0
                        && m13 == (T)0
                        && m14 == (T)0
                        && m23 == (T)0
                        && m24 == (T)0
                        && m34 == (T)0);
    }

    template <typename T>
    bool TMatrix4x4<T>::isUpperTriangular() const
    {
        return (   m21 == (T)0
                        && m31 == (T)0
                      && m32 == (T)0
                      && m41 == (T)0
                        && m42 == (T)0
                        && m43 == (T)0);
    }

    template <typename T>
    BALL_INLINE
    bool TMatrix4x4<T>::isDiagonal() const
    {
        return (   m12 == (T)0
                      && m13 == (T)0
                        && m14 == (T)0
                        && m21 == (T)0
                        && m23 == (T)0
                        && m24 == (T)0
                        && m31 == (T)0
                        && m32 == (T)0
                        && m34 == (T)0
                        && m41 == (T)0
                        && m42 == (T)0
                        && m43 == (T)0);
    }

    template <typename T>
    bool TMatrix4x4<T>::isValid() const
    {
        T **ptr = (T **)comp_ptr_;

        return (   *ptr++ == &m11
                        && *ptr++ == &m12
                        && *ptr++ == &m13
                        && *ptr++ == &m14
                        && *ptr++ == &m21
                        && *ptr++ == &m22
                      && *ptr++ == &m23
                        && *ptr++ == &m24
                        && *ptr++ == &m31
                        && *ptr++ == &m32
                        && *ptr++ == &m33
                        && *ptr++ == &m34
                        && *ptr++ == &m41
                        && *ptr++ == &m42
                        && *ptr++ == &m43
                        && *ptr   == &m44);
    }

    template <typename T>
    std::istream& operator >> (std::istream& s, TMatrix4x4<T>& m)
    {
        char c;
        s >> c
          >> m.m11 >> m.m12 >> m.m13 >> m.m14 >> c >> c
          >> m.m21 >> m.m22 >> m.m23 >> m.m24 >> c >> c
          >> m.m31 >> m.m32 >> m.m33 >> m.m34 >> c >> c
            >> m.m41 >> m.m42 >> m.m43 >> m.m44 >> c;

        return s;
    }

    template <typename T>
    std::ostream& operator << (std::ostream& s, const TMatrix4x4<T>& m)
    {
        s << '/'  <<  std::setw(14) << m.m11 << ' ' << std::setw(14) << m.m12 << ' ' << std::setw(14) << m.m13 << ' ' << std::setw(14) << m.m14 << " \\" << std::endl
            << '|'  <<  std::setw(14) << m.m21 << ' ' << std::setw(14) << m.m22 << ' ' << std::setw(14) << m.m23 << ' ' << std::setw(14) << m.m24 << " |"  << std::endl
            << '|'  <<  std::setw(14) << m.m31 << ' ' << std::setw(14) << m.m32 << ' ' << std::setw(14) << m.m33 << ' ' << std::setw(14) << m.m34 << " |"  << std::endl
            << '\\' <<  std::setw(14) << m.m41 << ' ' << std::setw(14) << m.m42 << ' ' << std::setw(14) << m.m43 << ' ' << std::setw(14) << m.m44 << " /" << std::endl;

        return s;
    }

    template <typename T>
    void TMatrix4x4<T>::dump(std::ostream& s, Size depth) const
    {
        BALL_DUMP_STREAM_PREFIX(s);

        BALL_DUMP_HEADER(s, this, this);

        BALL_DUMP_DEPTH(s, depth);
        s << m11 << " " << m12 << " " << m13 << " " << m14 << std::endl;

        BALL_DUMP_DEPTH(s, depth);
        s << m21 << " " << m22 << " " << m23 << " " << m24 << std::endl;

        BALL_DUMP_DEPTH(s, depth);
        s << m31 << " " << m32 << " " << m33 << " " << m34 << std::endl;

        BALL_DUMP_DEPTH(s, depth);
        s << m41 << " " << m42 << " " << m43 << " " << m44 << std::endl;

        BALL_DUMP_STREAM_SUFFIX(s);
    }

    ///
    template <typename T>
    TMatrix4x4<T> operator * (const T& scalar, const TMatrix4x4<T>& m);

    ///
    template <typename T>
    TVector3<T> operator * (const TMatrix4x4<T>& matrix, const TVector3<T>& vector);

    /**	The Default TMatrix4x4 Type.
            This default is predefined for convenience for those cases where single precision is sufficient.
            \ingroup Matrix44
    */
    typedef TMatrix4x4<float> Matrix4x4;

} // namespace BALL

#endif // BALL_MATHS_MATRIX44_H
