Subversion Repositories gelsvn

#include "ArithSqMat4x4Float.h"

#include "Mat4x4f.h"

namespace

{

	/* Aux func. computes 3x3 determinants. */

	template<class T>

	inline T d3x3f( T a0, T a1, T a2, 

									T b0, T b1, T b2, 

									T c0, T c1, T c2 )

	{

		return a0*b1*c2 +

			a1*b2*c0 +

			a2*b0*c1 -

			a2*b1*c0 -

			a0*b2*c1 -

			a1*b0*c2 ;

	}

}

namespace CGLA {

	template<class V, class M>

	double 

	determinant(const ArithSqMat4x4Float<V,M>& m)

	{

		typedef typename M::ScalarType ScalarType;

    ScalarType ans;

    ScalarType a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

    /* assign to individual variable names to aid selecting */

		/*  correct elements */

		a1 = m[0][0]; b1 = m[0][1]; 

		c1 = m[0][2]; d1 = m[0][3];

		a2 = m[1][0]; b2 = m[1][1]; 

		c2 = m[1][2]; d2 = m[1][3];

		a3 = m[2][0]; b3 = m[2][1]; 

		c3 = m[2][2]; d3 = m[2][3];

		a4 = m[3][0]; b4 = m[3][1]; 

		c4 = m[3][2]; d4 = m[3][3];

		ans = 

			a1 * d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4)

			- b1 * d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4)

			+ c1 * d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4)

			- d1 * d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);

		return ans;

	}

	template<class V, class M>

	M invert_affine(const ArithSqMat4x4Float<V,M>& this_mat)

	{

		//   The following code has been copied from a gem in Graphics Gems II by

		//   Kevin Wu.                      

		//   The function is very fast, but it can only invert affine matrices. An

		//   exception NotAffine is thrown if the matrix is not affine, and another

		//   exception Singular is thrown if the matrix is singular.

		typedef typename M::ScalarType ScalarType;

		M new_mat;

		ScalarType det_1;

		ScalarType pos, neg, temp;

		if (!(is_tiny(this_mat[3][0]) && 

					is_tiny(this_mat[3][1]) && 

					is_tiny(this_mat[3][2]) && 

					is_tiny(this_mat[3][3]-1.0f)))

			throw(Mat4x4fNotAffine("Can only invert affine matrices"));

#define ACCUMULATE if (temp >= 0.0) pos += temp; else neg += temp

		/*

		 * Calculate the determinant of submatrix A and determine if the

		 * the matrix is singular as limited by the float precision

		 * floating-point this_mat representation.

		 */

		pos = neg = 0.0;

		temp =  this_mat[0][0] * this_mat[1][1] * this_mat[2][2];

		ACCUMULATE;

		temp =  this_mat[1][0] * this_mat[2][1] * this_mat[0][2];

		ACCUMULATE;

		temp =  this_mat[2][0] * this_mat[0][1] * this_mat[1][2];

		ACCUMULATE;

		temp = -this_mat[2][0] * this_mat[1][1] * this_mat[0][2];

		ACCUMULATE;

		temp = -this_mat[1][0] * this_mat[0][1] * this_mat[2][2];

		ACCUMULATE;

		temp = -this_mat[0][0] * this_mat[2][1] * this_mat[1][2];

		ACCUMULATE;

		det_1 = pos + neg;

		/* Is the submatrix A singular? */

		if ((det_1 == 0.0) || (fabs(det_1 / (pos - neg)) < MINUTE)) 

			{

				/* Mat4x4f M has no inverse */

				throw(Mat4x4fSingular("Tried to invert Singular matrix"));

			}

		else {

			/* Calculate inverse(A) = adj(A) / det(A) */

			det_1 = 1.0 / det_1;

			new_mat[0][0] =   ( this_mat[1][1] * this_mat[2][2] -

													this_mat[2][1] * this_mat[1][2] )

				* det_1;

			new_mat[0][1] = - ( this_mat[0][1] * this_mat[2][2] -

													this_mat[2][1] * this_mat[0][2] )

				* det_1;

			new_mat[0][2] =   ( this_mat[0][1] * this_mat[1][2] -

													this_mat[1][1] * this_mat[0][2] )

				* det_1;

			new_mat[1][0] = - ( this_mat[1][0] * this_mat[2][2] -

													this_mat[2][0] * this_mat[1][2] )

				* det_1;

			new_mat[1][1] =   ( this_mat[0][0] * this_mat[2][2] -

													this_mat[2][0] * this_mat[0][2] )

				* det_1;

			new_mat[1][2] = - ( this_mat[0][0] * this_mat[1][2] -

													this_mat[1][0] * this_mat[0][2] )

				* det_1;

			new_mat[2][0] =   ( this_mat[1][0] * this_mat[2][1] -

													this_mat[2][0] * this_mat[1][1] )

				* det_1;

			new_mat[2][1] = - ( this_mat[0][0] * this_mat[2][1] -

													this_mat[2][0] * this_mat[0][1] )

				* det_1;

			new_mat[2][2] =   ( this_mat[0][0] * this_mat[1][1] -

													this_mat[1][0] * this_mat[0][1] )

				* det_1;

			/* Calculate -C * inverse(A) */

			new_mat[0][3] = - ( this_mat[0][3] * new_mat[0][0] +

													this_mat[1][3] * new_mat[0][1] +

													this_mat[2][3] * new_mat[0][2] );

			new_mat[1][3] = - ( this_mat[0][3] * new_mat[1][0] +

													this_mat[1][3] * new_mat[1][1] +

													this_mat[2][3] * new_mat[1][2] );

			new_mat[2][3] = - ( this_mat[0][3] * new_mat[2][0] +

													this_mat[1][3] * new_mat[2][1] +

													this_mat[2][3] * new_mat[2][2] );

			/* Fill in last column */

			new_mat[3][0] = new_mat[3][1] = new_mat[3][2] = 0.0;

			new_mat[3][3] = 1.0;

			return new_mat;

		}

#undef ACCUMULATE

	}

	template<class V, class M>

	M adjoint(const ArithSqMat4x4Float<V,M>& in)

	{

		float a1, a2, a3, a4, b1, b2, b3, b4;

		float c1, c2, c3, c4, d1, d2, d3, d4;

		/* assign to individual variable names to aid  */

		/* selecting correct values  */

		a1 = in[0][0]; b1 = in[0][1]; 

		c1 = in[0][2]; d1 = in[0][3];

		a2 = in[1][0]; b2 = in[1][1]; 

		c2 = in[1][2]; d2 = in[1][3];

		a3 = in[2][0]; b3 = in[2][1];

		c3 = in[2][2]; d3 = in[2][3];

		a4 = in[3][0]; b4 = in[3][1]; 

		c4 = in[3][2]; d4 = in[3][3];

		/* row column labeling reversed since we transpose rows & columns */

		M out;

		out[0][0]  =   d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4);

		out[1][0]  = - d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4);

		out[2][0]  =   d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4);

		out[3][0]  = - d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);

		out[0][1]  = - d3x3f( b1, b3, b4, c1, c3, c4, d1, d3, d4);

		out[1][1]  =   d3x3f( a1, a3, a4, c1, c3, c4, d1, d3, d4);

		out[2][1]  = - d3x3f( a1, a3, a4, b1, b3, b4, d1, d3, d4);

		out[3][1]  =   d3x3f( a1, a3, a4, b1, b3, b4, c1, c3, c4);

		out[0][2]  =   d3x3f( b1, b2, b4, c1, c2, c4, d1, d2, d4);

		out[1][2]  = - d3x3f( a1, a2, a4, c1, c2, c4, d1, d2, d4);

		out[2][2]  =   d3x3f( a1, a2, a4, b1, b2, b4, d1, d2, d4);

		out[3][2]  = - d3x3f( a1, a2, a4, b1, b2, b4, c1, c2, c4);

		out[0][3]  = - d3x3f( b1, b2, b3, c1, c2, c3, d1, d2, d3);

		out[1][3]  =   d3x3f( a1, a2, a3, c1, c2, c3, d1, d2, d3);

		out[2][3]  = - d3x3f( a1, a2, a3, b1, b2, b3, d1, d2, d3);

		out[3][3]  =   d3x3f( a1, a2, a3, b1, b2, b3, c1, c2, c3);

		return out;

	}

	template<class V, class M>

	M invert(const ArithSqMat4x4Float<V,M>& in)

	{

		float det = determinant( in );

		if (is_tiny(det)) 

			throw(Mat4x4fSingular("Tried to invert Singular matrix"));

		M out = adjoint(in);

		out/=det;

		return out;

	}

	template class ArithSqMat4x4Float<Vec4f, Mat4x4f>;

	template Mat4x4f adjoint(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);

	template double determinant(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);

	template Mat4x4f invert(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);

	template Mat4x4f invert_affine(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);

}