#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;

}