WebSVN – gelsvn – Diff – /trunk/src/CGLA/ArithSqMat4x4Float.cpp


#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.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>&);
 
 
}
 

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

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/ArithSqMat4x4Float.cpp – Rev 2 → 5