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 ;
	}
}
 
 
		/** Compute the determinant of a 4x4 matrix. The code below is what
				I found to be most robust. The original implementation used direct
				computation of the 3x3 sub-determinants and I also tried a direct
				computation based on enumerating all permutations. The code below
				is better. */
 
 
 
	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[1][0]; c1 = m[2][0]; d1 = m[3][0];
 
 
		a2 = m[0][1]; b2 = m[1][1]; c2 = m[2][1]; d2 = m[3][1];
 
 
		a3 = m[0][2]; b3 = m[1][2]; c3 = m[2][2]; d3 = m[3][2];
 
 
 
		a4 = m[0][3]; b4 = m[1][3]; c4 = m[2][3]; d4 = m[3][3];
 
		ans = 
				a1 * (b2*(c3*d4-d3*c4)-c2*(b3*d4-d3*b4)+d2*(b3*c4-c3*b4))
				- b1 * (a2*(c3*d4-d3*c4)-c2*(a3*d4-d3*a4)+d2*(a3*c4-c3*a4))
				+ c1 * (a2*(b3*d4-d3*b4)-b2*(a3*d4-d3*a4)+d2*(a3*b4-b3*a4))
				- d1 * (a2*(b3*c4-c3*b4)-b2*(a3*c4-c3*a4)+c2*(a3*b4-b3*a4));
		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>&);
 
 
}
 

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

Subversion Repositories gelsvn

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