|
|
14f971 |
#include "matrix.h"
|
|
|
14f971 |
|
|
|
14f971 |
|
|
|
14f971 |
// remove from sequence 0, 1, 2, 3, two numbers, see following
|
|
|
14f971 |
// example:
|
|
|
14f971 |
// r0 = 1, r1 = 2
|
|
|
14f971 |
// initial sequence:
|
|
|
14f971 |
// 0, 1, 2, 3
|
|
|
14f971 |
// remove number at place r0 (1 for ex):
|
|
|
14f971 |
// 0, 2, 3
|
|
|
14f971 |
// remove number at place r1 (2 for ex):
|
|
|
14f971 |
// 0, 2
|
|
|
14f971 |
// so result for index0(1, 2) is 0
|
|
|
14f971 |
// and for index1(1, 2) is 2
|
|
|
14f971 |
//
|
|
|
fdabb2 |
// r1\r0 : 0 1 2 3
|
|
|
fdabb2 |
// ---------------------------
|
|
|
fdabb2 |
// -- : _123 0_23 01_3 012_
|
|
|
fdabb2 |
// 0 : _.23 ._23 .1_3 .12_
|
|
|
fdabb2 |
// 1 : _1.3 0_.3 0._3 0.2_
|
|
|
fdabb2 |
// 2 : _12. 0_2. 01_. 01._
|
|
|
fdabb2 |
// ---------------------------
|
|
|
fdabb2 |
// 0 : 23 23 13 12
|
|
|
fdabb2 |
// 1 : 13 03 03 02
|
|
|
fdabb2 |
// 2 : 12 02 01 01
|
|
|
fdabb2 |
|
|
|
14f971 |
static inline int index0(int r0, int r1)
|
|
|
fdabb2 |
{ return r1 ? (r0 ? 0 : 1) : (r0 > 1 ? 1 : 2); }
|
|
|
14f971 |
static inline int index1(int r0, int r1)
|
|
|
14f971 |
{ return r1 < 2 ? (r0 < 3 ? 3 : 2) : (r0 > 1 ? 1 : 2); }
|
|
|
fdabb2 |
static inline int index0(int r)
|
|
|
fdabb2 |
{ return r ? 1 : 0; }
|
|
|
fdabb2 |
static inline int index1(int r)
|
|
|
fdabb2 |
{ return r > 0 ? 1 : 2; }
|
|
|
fdabb2 |
static inline int index2(int r)
|
|
|
fdabb2 |
{ return r > 1 ? 2 : 3; }
|
|
|
14f971 |
|
|
|
14f971 |
static inline Real det2(const Matrix4 &m, int r, int c, int rr, int cc) {
|
|
|
14f971 |
const int r0 = index0(r, rr);
|
|
|
14f971 |
const int r1 = index1(r, rr);
|
|
|
14f971 |
const int c0 = index0(c, cc);
|
|
|
14f971 |
const int c1 = index1(c, cc);
|
|
|
14f971 |
return m[r0][c0]*m[r1][c1] - m[r0][c1]*m[r1][c0];
|
|
|
14f971 |
}
|
|
|
fdabb2 |
|
|
|
fdabb2 |
static inline Real det3(const Matrix4 &m, int r, int c) {
|
|
|
fdabb2 |
const int r0 = index0(r);
|
|
|
fdabb2 |
return m[r0][index0(c)] * det2(m, r, c, 0, 0)
|
|
|
fdabb2 |
- m[r0][index1(c)] * det2(m, r, c, 0, 1)
|
|
|
fdabb2 |
+ m[r0][index2(c)] * det2(m, r, c, 0, 2);
|
|
|
fdabb2 |
}
|
|
|
14f971 |
|
|
|
14f971 |
|
|
|
14f971 |
Real
|
|
|
14f971 |
Matrix4::det() const
|
|
|
fdabb2 |
{ return m00*det3(*this, 0, 0) - m01*det3(*this, 0, 1) + m02*det3(*this, 0, 2) - m03*det3(*this, 0, 3); }
|
|
|
14f971 |
|
|
|
14f971 |
Matrix4
|
|
|
14f971 |
Matrix4::invert() const {
|
|
|
14f971 |
Vector4 dv(
|
|
|
14f971 |
det3(*this, 0, 0),
|
|
|
14f971 |
-det3(*this, 0, 1),
|
|
|
14f971 |
det3(*this, 0, 2),
|
|
|
14f971 |
-det3(*this, 0, 3) );
|
|
|
14f971 |
Real d = dv.x + dv.y + dv.z + dv.w;
|
|
|
fdabb2 |
if (fabs(d) <= real_precision)
|
|
|
14f971 |
return zero();
|
|
|
14f971 |
|
|
|
14f971 |
d = 1.0/d;
|
|
|
14f971 |
return Matrix4(
|
|
|
14f971 |
Vector4(dv.x, -det3(*this, 1, 0), det3(*this, 2, 0), -det3(*this, 3, 0))*d,
|
|
|
14f971 |
Vector4(dv.y, det3(*this, 1, 1),-det3(*this, 2, 1), det3(*this, 3, 1))*d,
|
|
|
14f971 |
Vector4(dv.z, -det3(*this, 1, 2), det3(*this, 2, 2), -det3(*this, 3, 2))*d,
|
|
|
14f971 |
Vector4(dv.w, det3(*this, 1, 3),-det3(*this, 2, 3), det3(*this, 3, 3))*d );
|
|
|
14f971 |
}
|