#include "matrix.h"


Real
Matrix3::det() const {
    return m00*(m11*m22 - m12*m21)
         + m01*(m12*m20 - m10*m22)
         + m02*(m10*m21 - m11*m20);
}

Matrix3
Matrix3::inverted(bool *success) const {
	const Real rm00 = m11*m22 - m12*m21;
	const Real rm10 = m12*m20 - m10*m22;
	const Real rm20 = m10*m21 - m11*m20;
	const Real d = m00*rm00 + m01*rm10 + m02*rm20;
	
	Real k;
	if (fabs(d) > real_precision) {
		k = 1/d;
		if (success) *success = true;
	} else {
		k = 0;
		if (success) *success = false;
	}
	
	return Matrix3(
		Vector3( m11*m22 - m12*m21,
				 m02*m21 - m01*m22,
				 m01*m12 - m02*m11 )*k,
		Vector3( m12*m20 - m10*m22,
				 m00*m22 - m02*m20,
				 m02*m10 - m00*m12 )*k,
		Vector3( m10*m21 - m11*m20,
				 m01*m20 - m00*m21,
				 m00*m11 - m01*m10 )*k );
}


Real
Matrix4::det() const {
    return m00 * (m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31))
         + m01 * (m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32))
         + m02 * (m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31))
         + m03 * (m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
}

Matrix4
Matrix4::inverted() const {
	Matrix4 r;
	r.m00 = m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31);
	r.m10 = m10*(m23*m32 - m22*m33) + m12*(m20*m33 - m23*m30) + m13*(m22*m30 - m20*m32);
	r.m20 = m10*(m21*m33 - m23*m31) + m11*(m23*m30 - m20*m33) + m13*(m20*m31 - m21*m30);
	r.m30 = m10*(m22*m31 - m21*m32) + m11*(m20*m32 - m22*m30) + m12*(m21*m30 - m20*m31);

	double det = m00*r.m00 + m01*r.m10 + m02*r.m20 + m03*r.m30;
	if (fabs(det) <= real_precision) return zero();
	det = 1/det;
	r.m00 *= det;
	r.m10 *= det;
	r.m20 *= det;
	r.m30 *= det;

	r.m01 = det*(m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32));
	r.m11 = det*(m00*(m22*m33 - m23*m32) + m02*(m23*m30 - m20*m33) + m03*(m20*m32 - m22*m30));
	r.m21 = det*(m00*(m23*m31 - m21*m33) + m01*(m20*m33 - m23*m30) + m03*(m21*m30 - m20*m31));
	r.m31 = det*(m00*(m21*m32 - m22*m31) + m01*(m22*m30 - m20*m32) + m02*(m20*m31 - m21*m30));
	r.m02 = det*(m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31));
	r.m12 = det*(m00*(m13*m32 - m12*m33) + m02*(m10*m33 - m13*m30) + m03*(m12*m30 - m10*m32));
	r.m22 = det*(m00*(m11*m33 - m13*m31) + m01*(m13*m30 - m10*m33) + m03*(m10*m31 - m11*m30));
	r.m32 = det*(m00*(m12*m31 - m11*m32) + m01*(m10*m32 - m12*m30) + m02*(m11*m30 - m10*m31));
	r.m03 = det*(m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
	r.m13 = det*(m00*(m12*m23 - m13*m22) + m02*(m13*m20 - m10*m23) + m03*(m10*m22 - m12*m20));
	r.m23 = det*(m00*(m13*m21 - m11*m23) + m01*(m10*m23 - m13*m20) + m03*(m11*m20 - m10*m21));
	r.m33 = det*(m00*(m11*m22 - m12*m21) + m01*(m12*m20 - m10*m22) + m02*(m10*m21 - m11*m20));
	return r;
}