| 667 |
khor |
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 |
|
|
|
7 |
#include "ArithSqMat3x3Float.h"
|
|
|
8 |
#include "Mat3x3f.h"
|
|
|
9 |
#include "Mat3x3d.h"
|
|
|
10 |
|
|
|
11 |
namespace CGLA {
|
|
|
12 |
|
|
|
13 |
template<class V, class M>
|
|
|
14 |
M invert(const ArithSqMat3x3Float<V,M>& _a)
|
|
|
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"
|
|
|
19 |
M a(_a[0], _a[1], _a[2]);
|
|
|
20 |
M b;
|
|
|
21 |
b.identity();
|
|
|
22 |
|
|
|
23 |
unsigned int i, j, i1;
|
|
|
24 |
|
|
|
25 |
for (j=0; j<3; j++)
|
|
|
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]))
|
|
|
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];
|
|
|
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)
|
|
|
43 |
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
|
|
|
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 |
}
|
|
|
58 |
template class ArithSqMat3x3Float<Vec3f,Mat3x3f>;
|
|
|
59 |
template Mat3x3f invert(const ArithSqMat3x3Float<Vec3f,Mat3x3f>&);
|
|
|
60 |
|
|
|
61 |
template class ArithSqMat3x3Float<Vec3d,Mat3x3d>;
|
|
|
62 |
template Mat3x3d invert(const ArithSqMat3x3Float<Vec3d,Mat3x3d>&);
|
|
|
63 |
}
|