#include "matrix.h"

// remove from sequence 0, 1, 2, 3, two numbers, see following

// example:

//   r0 = 1, r1 = 2

// initial sequence:

//   0, 1, 2, 3

// remove number at place r0 (1 for ex):

//   0, 2, 3

// remove number at place r1 (2 for ex):

//   0, 2

// so result for index0(1, 2) is 0

//       and for index1(1, 2) is 2

//

// r1\r0 :  0  1  2  3

// -------------------

//     0 : 23 23 13 12 

//     1 : 13 03 03 02 

//     2 : 12 02 01 01

static inline int index0(int r0, int r1)

	{ return r1 ? (r0 ? 1 : 0) : (r0 > 1 ? 1 : 2); }

static inline int index1(int r0, int r1)

	{ return r1 < 2 ? (r0 < 3 ? 3 : 2) : (r0 > 1 ? 1 : 2); }

static inline Real det2(const Matrix4 &m, int r, int c, int rr, int cc) {

	const int r0 = index0(r, rr);

	const int r1 = index1(r, rr);

	const int c0 = index0(c, cc);

	const int c1 = index1(c, cc);

	return m[r0][c0]*m[r1][c1] - m[r0][c1]*m[r1][c0];

}

static inline Real det3(const Matrix4 &m, int r, int c)

	{ return det2(m, r, c, 0, 0) - det2(m, r, c, 0, 1) + det2(m, r, c, 0, 2); }

Real

Matrix4::det() const

	{ return det3(*this, 0, 0) - det3(*this, 0, 1) + det3(*this, 0, 2) - det3(*this, 0, 3); }

Matrix4

Matrix4::invert() const {

	Vector4 dv(

		 det3(*this, 0, 0),

		-det3(*this, 0, 1),

		 det3(*this, 0, 2),

		-det3(*this, 0, 3) );

	Real d = dv.x + dv.y + dv.z + dv.w;

	if (fabs(d) <= realRrecision)

		return zero();

	d = 1.0/d;

	return Matrix4(

		Vector4(dv.x, -det3(*this, 1, 0), det3(*this, 2, 0), -det3(*this, 3, 0))*d,

		Vector4(dv.y,  det3(*this, 1, 1),-det3(*this, 2, 1),  det3(*this, 3, 1))*d,

		Vector4(dv.z, -det3(*this, 1, 2), det3(*this, 2, 2), -det3(*this, 3, 2))*d,

		Vector4(dv.w,  det3(*this, 1, 3),-det3(*this, 2, 3),  det3(*this, 3, 3))*d );

}