Subversion Repositories gelsvn

Rev

Rev 2 | Rev 45 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2 Rev 5
1 
#include "ArithSqMat3x3Float.h"
 1 
#include "ArithSqMat3x3Float.h"
2 
#include "Mat3x3f.h"
 2 
#include "Mat3x3f.h"
3 
 
 3 
 
4 
namespace CGLA {
 4 
namespace CGLA {
5 
 
 5 
 
6 
	template<class V, class M>
 6 
	template<class V, class M>
7 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
 7 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
8 
	{
 8 
	{
9 
		M a(_a[0], _a[1], _a[2]);
 9 
		M a(_a[0], _a[1], _a[2]);
10 
		M	b;
 10 
		M	b;
11 
		b.identity();
 11 
		b.identity();
12 
 
 12 
 
13 
		int  i, j, i1;
 13 
		int  i, j, i1;
14 
 
 14 
 
15 
		for (j=0; j<3; j++)
15 
		for (j=0; j<3; j++)
16 
			{
16 
			{
17 
				i1 = j;                 // Row with largest pivot candidate
 17 
				i1 = j;                 // Row with largest pivot candidate
18 
				for (i=j+1; i<3; i++)
 18 
				for (i=j+1; i<3; i++)
19 
					if (fabs(a[i][j]) > fabs(a[i1][j]))
 19 
					if (fabs(a[i][j]) > fabs(a[i1][j]))
20 
						i1 = i;
 20 
						i1 = i;
21
21
22 
				// Swap rows i1 and j in a and b to put pivot on diagonal
 22 
				// Swap rows i1 and j in a and b to put pivot on diagonal
23 
				V a_tmp = a[i1];
 23 
				V a_tmp = a[i1];
24 
				a[i1] = a[j];
 24 
				a[i1] = a[j];
25 
				a[j]  = a_tmp;
 25 
				a[j]  = a_tmp;
26
26
27 
				V b_tmp = b[i1];
 27 
				V b_tmp = b[i1];
28 
				b[i1] = b[j];
 28 
				b[i1] = b[j];
29 
				b[j]  = b_tmp;
 29 
				b[j]  = b_tmp;
30
30
31 
				// Scale row j to have a unit diagonal
 31 
				// Scale row j to have a unit diagonal
32 
				if (a[j][j] == 0.0f)
 32 
				if (a[j][j] == 0.0)
33 
					throw(Mat3x3fSingular("Tried to invert Singular matrix"));
 33 
					throw(Mat3x3fSingular("Tried to invert Singular matrix"));
34
34
35 
				b[j] /= a[j][j];
 35 
				b[j] /= a[j][j];
36 
				a[j] /= a[j][j];
 36 
				a[j] /= a[j][j];
37
37
38 
				// Eliminate off-diagonal elems in col j of a, doing identical ops to b
 38 
				// Eliminate off-diagonal elems in col j of a, doing identical ops to b
39 
				for (i=0; i<3; i++)
 39 
				for (i=0; i<3; i++)
40 
					if (i!=j)
40 
					if (i!=j)
41 
						{
 41 
						{
42 
							b[i] -= a[i][j] * b[j];
 42 
							b[i] -= a[i][j] * b[j];
43 
							a[i] -= a[i][j] * a[j];
 43 
							a[i] -= a[i][j] * a[j];
44 
						}
 44 
						}
45 
			}
 45 
			}
46 
		return b;
 46 
		return b;
47 
	}
47 
	}
48 
 
 48 
 
49 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
 49 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
50 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
 50 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
51 
}
 51 
}
52 
 
 52