// -*- 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>

#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
