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


#include "ArithSqMat4x4Float.h"
#include "Mat4x4f.h"
#include "Mat4x4d.h"
 
namespace CGLA {
	
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 ;
	}
}
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
	template<class V, class M>
	M invert_affine(const ArithSqMat4x4Float<V,M>& this_mat)
	{
		//   The following com[3]e 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 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 Mat4x4d invert(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
	template Mat4x4d invert_affine(const ArithSqMat4x4Float<Vec4d,Mat4x4d>&);
 
 
}
 

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

Subversion Repositories gelsvn

(root)/trunk/src/CGLA/ArithSqMat4x4Float.cpp – Rev 324 → 327