Subversion Repositories gelsvn

Rev

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

Rev 113 Rev 595
- 
 
 1 
/* ----------------------------------------------------------------------- *
- 
 
 2 
 * This file is part of GEL, http://www.imm.dtu.dk/GEL
- 
 
 3 
 * Copyright (C) the authors and DTU Informatics
- 
 
 4 
 * For license and list of authors, see ../../doc/intro.pdf
- 
 
 5 
 * ----------------------------------------------------------------------- */
- 
 
 6 
 
1 
#include "ArithSqMat3x3Float.h"
 7 
#include "ArithSqMat3x3Float.h"
2 
#include "Mat3x3f.h"
 8 
#include "Mat3x3f.h"
3 
#include "Mat3x3d.h"
 9 
#include "Mat3x3d.h"
4 
 
 10 
 
5 
namespace CGLA {
 11 
namespace CGLA {
6 
 
 12 
 
7 
	template<class V, class M>
 13 
	template<class V, class M>
8 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
 14 
	M invert(const ArithSqMat3x3Float<V,M>& _a)
9 
	{
 15 
	{
- 
 
 16 
        // From Graphcis Gems IV, Jean-Francois Doue, C++ Vector and Matrix Algebra Routines.
- 
 
 17 
        // From the EULA "Using the code is permitted in any program, product, or library, non-commercial
- 
 
 18 
        // or commercial. Giving credit is not required, though is a nice gesture"
10 
		M a(_a[0], _a[1], _a[2]);
 19 
		M a(_a[0], _a[1], _a[2]);
11 
		M	b;
 20 
		M	b;
12 
		b.identity();
 21 
		b.identity();
13 
 
 22 
 
14 
		unsigned int  i, j, i1;
 23 
		unsigned int  i, j, i1;
15 
 
 24 
 
16 
		for (j=0; j<3; j++)
25 
		for (j=0; j<3; j++)
17 
			{
26 
			{
18 
				i1 = j;                 // Row with largest pivot candidate
 27 
				i1 = j;                 // Row with largest pivot candidate
19 
				for (i=j+1; i<3; i++)
 28 
				for (i=j+1; i<3; i++)
20 
					if (fabs(a[i][j]) > fabs(a[i1][j]))
 29 
					if (fabs(a[i][j]) > fabs(a[i1][j]))
21 
						i1 = i;
 30 
						i1 = i;
22
31
23 
				// Swap rows i1 and j in a and b to put pivot on diagonal
 32 
				// Swap rows i1 and j in a and b to put pivot on diagonal
24 
				V a_tmp = a[i1];
 33 
				V a_tmp = a[i1];
25 
				a[i1] = a[j];
 34 
				a[i1] = a[j];
26 
				a[j]  = a_tmp;
 35 
				a[j]  = a_tmp;
27
36
28 
				V b_tmp = b[i1];
 37 
				V b_tmp = b[i1];
29 
				b[i1] = b[j];
 38 
				b[i1] = b[j];
30 
				b[j]  = b_tmp;
 39 
				b[j]  = b_tmp;
31
40
32 
				// Scale row j to have a unit diagonal
 41 
				// Scale row j to have a unit diagonal
33 
				if (a[j][j] == 0.0)
 42 
				if (a[j][j] == 0.0)
34 
					throw(Mat3x3fSingular("Tried to invert Singular matrix"));
 43 
					throw(Mat3x3fSingular("Tried to invert Singular matrix"));
35
44
36 
				b[j] /= a[j][j];
 45 
				b[j] /= a[j][j];
37 
				a[j] /= a[j][j];
 46 
				a[j] /= a[j][j];
38
47
39 
				// Eliminate off-diagonal elems in col j of a, doing identical ops to b
 48 
				// Eliminate off-diagonal elems in col j of a, doing identical ops to b
40 
				for (i=0; i<3; i++)
 49 
				for (i=0; i<3; i++)
41 
					if (i!=j)
50 
					if (i!=j)
42 
						{
 51 
						{
43 
							b[i] -= a[i][j] * b[j];
 52 
							b[i] -= a[i][j] * b[j];
44 
							a[i] -= a[i][j] * a[j];
 53 
							a[i] -= a[i][j] * a[j];
45 
						}
 54 
						}
46 
			}
 55 
			}
47 
		return b;
 56 
		return b;
48 
	}
57 
	}
49 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
 58 
	template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
50 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
 59 
	template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
51 
 
 60 
 
52 
	template class ArithSqMat3x3Float<Vec3d,Mat3x3d>;
 61 
	template class ArithSqMat3x3Float<Vec3d,Mat3x3d>;
53 
	template Mat3x3d invert(const ArithSqMat3x3Float<Vec3d,Mat3x3d>&);
 62 
	template Mat3x3d invert(const ArithSqMat3x3Float<Vec3d,Mat3x3d>&);
54 
}
 63 
}
55 
 
 64