Subversion Repositories gelsvn

Rev

Rev 595 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 595 Rev 692
Line 11... Line 11...
11 
namespace CGLA {
 11 
namespace CGLA {
12 
 
 12 
 
13 
	template<class V, class M>
 13 
	template<class V, class M>
14 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
 14 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
15 
	{
 15 
	{
16 
        // From Graphcis Gems IV, Jean-Francois Doue, C++ Vector and Matrix Algebra Routines.
 16 
    // Based on Cramer's rule (Akenine-Möller et al. 2008, Section A.3.1) and
17 
        // From the EULA "Using the code is permitted in any program, product, or library, non-commercial
17 
    // Cedrick Collomb. A tutorial on inverting 3 by 3 matrices with cross products.
18 
        // or commercial. Giving credit is not required, though is a nice gesture"
 18 
    // http://www.emptyloop.com/technotes/
19 
		M a(_a[0], _a[1], _a[2]);
 - 
 
20 
		M	b;
 - 
 
21 
		b.identity();
 - 
 
22 
 
 19 
 
23 
		unsigned int  i, j, i1;
 - 
 
24 
 
 - 
 
25 
		for (j=0; j<3; j++)
20 
    M a = transpose(_a);
26 
			{
- 
 
27 
				i1 = j;                 // Row with largest pivot candidate
 - 
 
28 
				for (i=j+1; i<3; i++)
 - 
 
29 
					if (fabs(a[i][j]) > fabs(a[i1][j]))
 21 
    V a1a2 = cross(a[1], a[2]);
30 
						i1 = i;
 - 
 
31
- 
 
32 
				// Swap rows i1 and j in a and b to put pivot on diagonal
 - 
 
33 
				V a_tmp = a[i1];
 22 
    float det = dot(a[0], a1a2);
34 
				a[i1] = a[j];
 - 
 
35 
				a[j]  = a_tmp;
 - 
 
36
- 
 
37 
				V b_tmp = b[i1];
 - 
 
38 
				b[i1] = b[j];
 - 
 
39 
				b[j]  = b_tmp;
 - 
 
40
- 
 
41 
				// Scale row j to have a unit diagonal
 - 
 
42 
				if (a[j][j] == 0.0)
 23 
    if(fabs(det) < TINYf)
43 
					throw(Mat3x3fSingular("Tried to invert Singular matrix"));
 24 
      throw(Mat3x3fSingular("Tried to invert singular matrix"));
44
- 
 
45 
				b[j] /= a[j][j];
 - 
 
46 
				a[j] /= a[j][j];
 - 
 
47
- 
 
48 
				// Eliminate off-diagonal elems in col j of a, doing identical ops to b
 25 
    return M(a1a2, cross(a[2], a[0]), cross(a[0], a[1]))*(1.0f/det);
49 
				for (i=0; i<3; i++)
 - 
 
50 
					if (i!=j)
- 
 
51 
						{
 - 
 
52 
							b[i] -= a[i][j] * b[j];
 - 
 
53 
							a[i] -= a[i][j] * a[j];
 - 
 
54 
						}
 - 
 
55 
			}
 - 
 
56 
		return b;
 - 
 
57 
	}
26 
	}
58 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
 27 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
59 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
 28 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
60 
 
 29 
 
61 
	template class ArithSqMat3x3Float<Vec3d,Mat3x3d>;
 30 
	template class ArithSqMat3x3Float<Vec3d,Mat3x3d>;