WebSVN – gelsvn – Diff – /branches/vc2005beta-branch/src/CGLA/ArithSqMat4x4Float.cpp


#include "ArithSqMat4x4Float.h"
#include "Mat4x4f.h"
#include "Mat4x4d.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.0)))
			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)
	{
		double a1, a2, a3, a4, b1, b2, b3, b4;
		double 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)
	{
		double 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>&);
 
	template class ArithSqMat4x4Float<Vec4d, Mat4x4d>;
	template Mat4x4d adjoint(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
	template double determinant(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
	template Mat4x4d invert(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
	template Mat4x4d invert_affine(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
 
 
}
 

Subversion Repositories gelsvn

(root)/branches/vc2005beta-branch/src/CGLA/ArithSqMat4x4Float.cpp – Rev 113 → 119

Rev 113		Rev 119
1	`#include "ArithSqMat4x4Float.h"`	1	`#include "ArithSqMat4x4Float.h"`
2	`#include "Mat4x4f.h"`	2	`#include "Mat4x4f.h"`
3	`#include "Mat4x4d.h"`	3	`#include "Mat4x4d.h"`
4		4
5	`namespace`	5	`namespace`
6	`{`	6	`{`
7		7
8	`/* Aux func. computes 3x3 determinants. */`	8	`/* Aux func. computes 3x3 determinants. */`
9	`template<class T>`	9	`template<class T>`
10	`inline T d3x3f( T a0, T a1, T a2,`	10	`inline T d3x3f( T a0, T a1, T a2,`
11	`T b0, T b1, T b2,`	11	`T b0, T b1, T b2,`
12	`T c0, T c1, T c2 )`	12	`T c0, T c1, T c2 )`
13	`{`	13	`{`
14	`return a0b1c2 +`	14	`return a0b1c2 +`
15	`a1b2c0 +`	15	`a1b2c0 +`
16	`a2b0c1 -`	16	`a2b0c1 -`
17	`a2b1c0 -`	17	`a2b1c0 -`
18	`a0b2c1 -`	18	`a0b2c1 -`
19	`a1b0c2 ;`	19	`a1b0c2 ;`
20	`}`	20	`}`
21	`}`	21	`}`
22		22
23		23
24	`namespace CGLA {`	24	`namespace CGLA {`
25		25
26		26
27		27
28	`template<class V, class M>`	28	`template<class V, class M>`
29	`double`	29	`double`
30	`determinant(const ArithSqMat4x4Float<V,M>& m)`	30	`determinant(const ArithSqMat4x4Float<V,M>& m)`
31	`{`	31	`{`
32	`typedef typename M::ScalarType ScalarType;`	32	`typedef typename M::ScalarType ScalarType;`
33	`ScalarType ans;`	33	`ScalarType ans;`
34	`ScalarType a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;`	34	`ScalarType a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;`
35		35
36	`/* assign to individual variable names to aid selecting */`	36	`/* assign to individual variable names to aid selecting */`
37	`/* correct elements */`	37	`/* correct elements */`
38		38
39	`a1 = m[0][0]; b1 = m[0][1];`	39	`a1 = m[0][0]; b1 = m[0][1];`
40	`c1 = m[0][2]; d1 = m[0][3];`	40	`c1 = m[0][2]; d1 = m[0][3];`
41		41
42	`a2 = m[1][0]; b2 = m[1][1];`	42	`a2 = m[1][0]; b2 = m[1][1];`
43	`c2 = m[1][2]; d2 = m[1][3];`	43	`c2 = m[1][2]; d2 = m[1][3];`
44		44
45	`a3 = m[2][0]; b3 = m[2][1];`	45	`a3 = m[2][0]; b3 = m[2][1];`
46	`c3 = m[2][2]; d3 = m[2][3];`	46	`c3 = m[2][2]; d3 = m[2][3];`
47		47
48	`a4 = m[3][0]; b4 = m[3][1];`	48	`a4 = m[3][0]; b4 = m[3][1];`
49	`c4 = m[3][2]; d4 = m[3][3];`	49	`c4 = m[3][2]; d4 = m[3][3];`
50		50
51	`ans =`	51	`ans =`
52	`a1 * d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4)`	52	`a1 * d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4)`
53	`- b1 * d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4)`	53	`- b1 * d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4)`
54	`+ c1 * d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4)`	54	`+ c1 * d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4)`
55	`- d1 * d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);`	55	`- d1 * d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);`
56	`return ans;`	56	`return ans;`
57	`}`	57	`}`
58		58
59		59
60	`template<class V, class M>`	60	`template<class V, class M>`
61	`M invert_affine(const ArithSqMat4x4Float<V,M>& this_mat)`	61	`M invert_affine(const ArithSqMat4x4Float<V,M>& this_mat)`
62	`{`	62	`{`
63	`// The following code has been copied from a gem in Graphics Gems II by`	63	`// The following code has been copied from a gem in Graphics Gems II by`
64	`// Kevin Wu.`	64	`// Kevin Wu.`
65	`// The function is very fast, but it can only invert affine matrices. An`	65	`// The function is very fast, but it can only invert affine matrices. An`
66	`// exception NotAffine is thrown if the matrix is not affine, and another`	66	`// exception NotAffine is thrown if the matrix is not affine, and another`
67	`// exception Singular is thrown if the matrix is singular.`	67	`// exception Singular is thrown if the matrix is singular.`
68		68
69	`typedef typename M::ScalarType ScalarType;`	69	`typedef typename M::ScalarType ScalarType;`
70		70
71	`M new_mat;`	71	`M new_mat;`
72	`ScalarType det_1;`	72	`ScalarType det_1;`
73	`ScalarType pos, neg, temp;`	73	`ScalarType pos, neg, temp;`
74		74
75		75
76	`if (!(is_tiny(this_mat[3][0]) &&`	76	`if (!(is_tiny(this_mat[3][0]) &&`
77	`is_tiny(this_mat[3][1]) &&`	77	`is_tiny(this_mat[3][1]) &&`
78	`is_tiny(this_mat[3][2]) &&`	78	`is_tiny(this_mat[3][2]) &&`
79	`is_tiny(this_mat[3][3]-1.0)))`	79	`is_tiny(this_mat[3][3]-1.0)))`
80	`throw(Mat4x4fNotAffine("Can only invert affine matrices"));`	80	`throw(Mat4x4fNotAffine("Can only invert affine matrices"));`
81		81
82	`#define ACCUMULATE if (temp >= 0.0) pos += temp; else neg += temp`	82	`#define ACCUMULATE if (temp >= 0.0) pos += temp; else neg += temp`
83		83
84	`/*`	84	`/*`
85	`* Calculate the determinant of submatrix A and determine if the`	85	`* Calculate the determinant of submatrix A and determine if the`
86	`* the matrix is singular as limited by the float precision`	86	`* the matrix is singular as limited by the float precision`
87	`* floating-point this_mat representation.`	87	`* floating-point this_mat representation.`
88	`*/`	88	`*/`
89		89
90	`pos = neg = 0.0;`	90	`pos = neg = 0.0;`
91	`temp = this_mat[0][0] * this_mat[1][1] * this_mat[2][2];`	91	`temp = this_mat[0][0] * this_mat[1][1] * this_mat[2][2];`
92	`ACCUMULATE;`	92	`ACCUMULATE;`
93	`temp = this_mat[1][0] * this_mat[2][1] * this_mat[0][2];`	93	`temp = this_mat[1][0] * this_mat[2][1] * this_mat[0][2];`
94	`ACCUMULATE;`	94	`ACCUMULATE;`
95	`temp = this_mat[2][0] * this_mat[0][1] * this_mat[1][2];`	95	`temp = this_mat[2][0] * this_mat[0][1] * this_mat[1][2];`
96	`ACCUMULATE;`	96	`ACCUMULATE;`
97	`temp = -this_mat[2][0] * this_mat[1][1] * this_mat[0][2];`	97	`temp = -this_mat[2][0] * this_mat[1][1] * this_mat[0][2];`
98	`ACCUMULATE;`	98	`ACCUMULATE;`
99	`temp = -this_mat[1][0] * this_mat[0][1] * this_mat[2][2];`	99	`temp = -this_mat[1][0] * this_mat[0][1] * this_mat[2][2];`
100	`ACCUMULATE;`	100	`ACCUMULATE;`
101	`temp = -this_mat[0][0] * this_mat[2][1] * this_mat[1][2];`	101	`temp = -this_mat[0][0] * this_mat[2][1] * this_mat[1][2];`
102	`ACCUMULATE;`	102	`ACCUMULATE;`
103	`det_1 = pos + neg;`	103	`det_1 = pos + neg;`
104		104
105	`/* Is the submatrix A singular? */`	105	`/* Is the submatrix A singular? */`
106	`if ((det_1 == 0.0) \|\| (fabs(det_1 / (pos - neg)) < MINUTE))`	106	`if ((det_1 == 0.0) \|\| (fabs(det_1 / (pos - neg)) < MINUTE))`
107	`{`	107	`{`
108	`/* Mat4x4f M has no inverse */`	108	`/* Mat4x4f M has no inverse */`
109	`throw(Mat4x4fSingular("Tried to invert Singular matrix"));`	109	`throw(Mat4x4fSingular("Tried to invert Singular matrix"));`
110	`}`	110	`}`
111		111
112	`else {`	112	`else {`
113		113
114	`/* Calculate inverse(A) = adj(A) / det(A) */`	114	`/* Calculate inverse(A) = adj(A) / det(A) */`
115	`det_1 = 1.0 / det_1;`	115	`det_1 = 1.0 / det_1;`
116	`new_mat[0][0] = ( this_mat[1][1] * this_mat[2][2] -`	116	`new_mat[0][0] = ( this_mat[1][1] * this_mat[2][2] -`
117	`this_mat[2][1] * this_mat[1][2] )`	117	`this_mat[2][1] * this_mat[1][2] )`
118	`* det_1;`	118	`* det_1;`
119	`new_mat[0][1] = - ( this_mat[0][1] * this_mat[2][2] -`	119	`new_mat[0][1] = - ( this_mat[0][1] * this_mat[2][2] -`
120	`this_mat[2][1] * this_mat[0][2] )`	120	`this_mat[2][1] * this_mat[0][2] )`
121	`* det_1;`	121	`* det_1;`
122	`new_mat[0][2] = ( this_mat[0][1] * this_mat[1][2] -`	122	`new_mat[0][2] = ( this_mat[0][1] * this_mat[1][2] -`
123	`this_mat[1][1] * this_mat[0][2] )`	123	`this_mat[1][1] * this_mat[0][2] )`
124	`* det_1;`	124	`* det_1;`
125	`new_mat[1][0] = - ( this_mat[1][0] * this_mat[2][2] -`	125	`new_mat[1][0] = - ( this_mat[1][0] * this_mat[2][2] -`
126	`this_mat[2][0] * this_mat[1][2] )`	126	`this_mat[2][0] * this_mat[1][2] )`
127	`* det_1;`	127	`* det_1;`
128	`new_mat[1][1] = ( this_mat[0][0] * this_mat[2][2] -`	128	`new_mat[1][1] = ( this_mat[0][0] * this_mat[2][2] -`
129	`this_mat[2][0] * this_mat[0][2] )`	129	`this_mat[2][0] * this_mat[0][2] )`
130	`* det_1;`	130	`* det_1;`
131	`new_mat[1][2] = - ( this_mat[0][0] * this_mat[1][2] -`	131	`new_mat[1][2] = - ( this_mat[0][0] * this_mat[1][2] -`
132	`this_mat[1][0] * this_mat[0][2] )`	132	`this_mat[1][0] * this_mat[0][2] )`
133	`* det_1;`	133	`* det_1;`
134	`new_mat[2][0] = ( this_mat[1][0] * this_mat[2][1] -`	134	`new_mat[2][0] = ( this_mat[1][0] * this_mat[2][1] -`
135	`this_mat[2][0] * this_mat[1][1] )`	135	`this_mat[2][0] * this_mat[1][1] )`
136	`* det_1;`	136	`* det_1;`
137	`new_mat[2][1] = - ( this_mat[0][0] * this_mat[2][1] -`	137	`new_mat[2][1] = - ( this_mat[0][0] * this_mat[2][1] -`
138	`this_mat[2][0] * this_mat[0][1] )`	138	`this_mat[2][0] * this_mat[0][1] )`
139	`* det_1;`	139	`* det_1;`
140	`new_mat[2][2] = ( this_mat[0][0] * this_mat[1][1] -`	140	`new_mat[2][2] = ( this_mat[0][0] * this_mat[1][1] -`
141	`this_mat[1][0] * this_mat[0][1] )`	141	`this_mat[1][0] * this_mat[0][1] )`
142	`* det_1;`	142	`* det_1;`
143		143
144	`/* Calculate -C * inverse(A) */`	144	`/* Calculate -C * inverse(A) */`
145	`new_mat[0][3] = - ( this_mat[0][3] * new_mat[0][0] +`	145	`new_mat[0][3] = - ( this_mat[0][3] * new_mat[0][0] +`
146	`this_mat[1][3] * new_mat[0][1] +`	146	`this_mat[1][3] * new_mat[0][1] +`
147	`this_mat[2][3] * new_mat[0][2] );`	147	`this_mat[2][3] * new_mat[0][2] );`
148	`new_mat[1][3] = - ( this_mat[0][3] * new_mat[1][0] +`	148	`new_mat[1][3] = - ( this_mat[0][3] * new_mat[1][0] +`
149	`this_mat[1][3] * new_mat[1][1] +`	149	`this_mat[1][3] * new_mat[1][1] +`
150	`this_mat[2][3] * new_mat[1][2] );`	150	`this_mat[2][3] * new_mat[1][2] );`
151	`new_mat[2][3] = - ( this_mat[0][3] * new_mat[2][0] +`	151	`new_mat[2][3] = - ( this_mat[0][3] * new_mat[2][0] +`
152	`this_mat[1][3] * new_mat[2][1] +`	152	`this_mat[1][3] * new_mat[2][1] +`
153	`this_mat[2][3] * new_mat[2][2] );`	153	`this_mat[2][3] * new_mat[2][2] );`
154		154
155	`/* Fill in last column */`	155	`/* Fill in last column */`
156	`new_mat[3][0] = new_mat[3][1] = new_mat[3][2] = 0.0;`	156	`new_mat[3][0] = new_mat[3][1] = new_mat[3][2] = 0.0;`
157	`new_mat[3][3] = 1.0;`	157	`new_mat[3][3] = 1.0;`
158		158
159	`return new_mat;`	159	`return new_mat;`
160		160
161	`}`	161	`}`
162		162
163	`#undef ACCUMULATE`	163	`#undef ACCUMULATE`
164	`}`	164	`}`
165		165
166	`template<class V, class M>`	166	`template<class V, class M>`
167	`M adjoint(const ArithSqMat4x4Float<V,M>& in)`	167	`M adjoint(const ArithSqMat4x4Float<V,M>& in)`
168	`{`	168	`{`
169	`double a1, a2, a3, a4, b1, b2, b3, b4;`	169	`double a1, a2, a3, a4, b1, b2, b3, b4;`
170	`double c1, c2, c3, c4, d1, d2, d3, d4;`	170	`double c1, c2, c3, c4, d1, d2, d3, d4;`
171		171
172	`/* assign to individual variable names to aid */`	172	`/* assign to individual variable names to aid */`
173	`/* selecting correct values */`	173	`/* selecting correct values */`
174		174
175	`a1 = in[0][0]; b1 = in[0][1];`	175	`a1 = in[0][0]; b1 = in[0][1];`
176	`c1 = in[0][2]; d1 = in[0][3];`	176	`c1 = in[0][2]; d1 = in[0][3];`
177		177
178	`a2 = in[1][0]; b2 = in[1][1];`	178	`a2 = in[1][0]; b2 = in[1][1];`
179	`c2 = in[1][2]; d2 = in[1][3];`	179	`c2 = in[1][2]; d2 = in[1][3];`
180		180
181	`a3 = in[2][0]; b3 = in[2][1];`	181	`a3 = in[2][0]; b3 = in[2][1];`
182	`c3 = in[2][2]; d3 = in[2][3];`	182	`c3 = in[2][2]; d3 = in[2][3];`
183		183
184	`a4 = in[3][0]; b4 = in[3][1];`	184	`a4 = in[3][0]; b4 = in[3][1];`
185	`c4 = in[3][2]; d4 = in[3][3];`	185	`c4 = in[3][2]; d4 = in[3][3];`
186		186
187		187
188	`/* row column labeling reversed since we transpose rows & columns */`	188	`/* row column labeling reversed since we transpose rows & columns */`
189		189
190	`M out;`	190	`M out;`
191	`out[0][0] = d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4);`	191	`out[0][0] = d3x3f( b2, b3, b4, c2, c3, c4, d2, d3, d4);`
192	`out[1][0] = - d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4);`	192	`out[1][0] = - d3x3f( a2, a3, a4, c2, c3, c4, d2, d3, d4);`
193	`out[2][0] = d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4);`	193	`out[2][0] = d3x3f( a2, a3, a4, b2, b3, b4, d2, d3, d4);`
194	`out[3][0] = - d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);`	194	`out[3][0] = - d3x3f( a2, a3, a4, b2, b3, b4, c2, c3, c4);`
195		195
196	`out[0][1] = - d3x3f( b1, b3, b4, c1, c3, c4, d1, d3, d4);`	196	`out[0][1] = - d3x3f( b1, b3, b4, c1, c3, c4, d1, d3, d4);`
197	`out[1][1] = d3x3f( a1, a3, a4, c1, c3, c4, d1, d3, d4);`	197	`out[1][1] = d3x3f( a1, a3, a4, c1, c3, c4, d1, d3, d4);`
198	`out[2][1] = - d3x3f( a1, a3, a4, b1, b3, b4, d1, d3, d4);`	198	`out[2][1] = - d3x3f( a1, a3, a4, b1, b3, b4, d1, d3, d4);`
199	`out[3][1] = d3x3f( a1, a3, a4, b1, b3, b4, c1, c3, c4);`	199	`out[3][1] = d3x3f( a1, a3, a4, b1, b3, b4, c1, c3, c4);`
200		200
201	`out[0][2] = d3x3f( b1, b2, b4, c1, c2, c4, d1, d2, d4);`	201	`out[0][2] = d3x3f( b1, b2, b4, c1, c2, c4, d1, d2, d4);`
202	`out[1][2] = - d3x3f( a1, a2, a4, c1, c2, c4, d1, d2, d4);`	202	`out[1][2] = - d3x3f( a1, a2, a4, c1, c2, c4, d1, d2, d4);`
203	`out[2][2] = d3x3f( a1, a2, a4, b1, b2, b4, d1, d2, d4);`	203	`out[2][2] = d3x3f( a1, a2, a4, b1, b2, b4, d1, d2, d4);`
204	`out[3][2] = - d3x3f( a1, a2, a4, b1, b2, b4, c1, c2, c4);`	204	`out[3][2] = - d3x3f( a1, a2, a4, b1, b2, b4, c1, c2, c4);`
205		205
206	`out[0][3] = - d3x3f( b1, b2, b3, c1, c2, c3, d1, d2, d3);`	206	`out[0][3] = - d3x3f( b1, b2, b3, c1, c2, c3, d1, d2, d3);`
207	`out[1][3] = d3x3f( a1, a2, a3, c1, c2, c3, d1, d2, d3);`	207	`out[1][3] = d3x3f( a1, a2, a3, c1, c2, c3, d1, d2, d3);`
208	`out[2][3] = - d3x3f( a1, a2, a3, b1, b2, b3, d1, d2, d3);`	208	`out[2][3] = - d3x3f( a1, a2, a3, b1, b2, b3, d1, d2, d3);`
209	`out[3][3] = d3x3f( a1, a2, a3, b1, b2, b3, c1, c2, c3);`	209	`out[3][3] = d3x3f( a1, a2, a3, b1, b2, b3, c1, c2, c3);`
210		210
211	`return out;`	211	`return out;`
212	`}`	212	`}`
213		213
214		214
215	`template<class V, class M>`	215	`template<class V, class M>`
216	`M invert(const ArithSqMat4x4Float<V,M>& in)`	216	`M invert(const ArithSqMat4x4Float<V,M>& in)`
217	`{`	217	`{`
218	`double det = determinant( in );`	218	`double det = determinant( in );`
219	`if (is_tiny(det))`	219	`if (is_tiny(det))`
220	`throw(Mat4x4fSingular("Tried to invert Singular matrix"));`	220	`throw(Mat4x4fSingular("Tried to invert Singular matrix"));`
221	`M out = adjoint(in);`	221	`M out = adjoint(in);`
222	`out/=det;`	222	`out/=det;`
223	`return out;`	223	`return out;`
224	`}`	224	`}`
225		225
226		226
227	`template class ArithSqMat4x4Float<Vec4f, Mat4x4f>;`	227	`template class ArithSqMat4x4Float<Vec4f, Mat4x4f>;`
228	`template Mat4x4f adjoint(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`	228	`template Mat4x4f adjoint(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`
229	`template double determinant(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`	229	`template double determinant(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`
230	`template Mat4x4f invert(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`	230	`template Mat4x4f invert(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`
231	`template Mat4x4f invert_affine(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`	231	`template Mat4x4f invert_affine(const ArithSqMat4x4Float<Vec4f,Mat4x4f>&);`
232		232
233	`template class ArithSqMat4x4Float<Vec4d, Mat4x4d>;`	233	`template class ArithSqMat4x4Float<Vec4d, Mat4x4d>;`
234	`template Mat4x4d adjoint(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`	234	`template Mat4x4d adjoint(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`
235	`template double determinant(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`	235	`template double determinant(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`
236	`template Mat4x4d invert(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`	236	`template Mat4x4d invert(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`
237	`template Mat4x4d invert_affine(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`	237	`template Mat4x4d invert_affine(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);`
238		238
239		239
240	`}`	240	`}`
241		241