Blame c++/perspective/src/matrix.cpp

14f971
#include "matrix.h"
14f971
14f971
14f971
Real
4dc49c
Matrix3::det() const {
4dc49c
    return m00*(m11*m22 - m12*m21)
4dc49c
         + m01*(m12*m20 - m10*m22)
4dc49c
         + m02*(m10*m21 - m11*m20);
4dc49c
}
4dc49c
4dc49c
Matrix3
4dc49c
Matrix3::inverted(bool *success) const {
4dc49c
	const Real rm00 = m11*m22 - m12*m21;
4dc49c
	const Real rm10 = m12*m20 - m10*m22;
4dc49c
	const Real rm20 = m10*m21 - m11*m20;
4dc49c
	const Real d = m00*rm00 + m01*rm10 + m02*rm20;
4dc49c
	
4dc49c
	Real k;
4dc49c
	if (fabs(d) > real_precision) {
4dc49c
		k = 1/d;
4dc49c
		if (success) *success = true;
4dc49c
	} else {
4dc49c
		k = 0;
4dc49c
		if (success) *success = false;
4dc49c
	}
4dc49c
	
4dc49c
	return Matrix3(
4dc49c
		Vector3( m11*m22 - m12*m21,
4dc49c
				 m02*m21 - m01*m22,
4dc49c
				 m01*m12 - m02*m11 )*k,
4dc49c
		Vector3( m12*m20 - m10*m22,
4dc49c
				 m00*m22 - m02*m20,
4dc49c
				 m02*m10 - m00*m12 )*k,
4dc49c
		Vector3( m10*m21 - m11*m20,
4dc49c
				 m01*m20 - m00*m21,
4dc49c
				 m00*m11 - m01*m10 )*k );
4dc49c
}
4dc49c
4dc49c
4dc49c
Real
0662a2
Matrix4::det() const {
0662a2
    return m00 * (m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31))
0662a2
         + m01 * (m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32))
0662a2
         + m02 * (m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31))
0662a2
         + m03 * (m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
0662a2
}
14f971
14f971
Matrix4
a1942e
Matrix4::inverted() const {
0662a2
	Matrix4 r;
0662a2
	r.m00 = m11*(m22*m33 - m23*m32) + m12*(m23*m31 - m21*m33) + m13*(m21*m32 - m22*m31);
0662a2
	r.m10 = m10*(m23*m32 - m22*m33) + m12*(m20*m33 - m23*m30) + m13*(m22*m30 - m20*m32);
0662a2
	r.m20 = m10*(m21*m33 - m23*m31) + m11*(m23*m30 - m20*m33) + m13*(m20*m31 - m21*m30);
0662a2
	r.m30 = m10*(m22*m31 - m21*m32) + m11*(m20*m32 - m22*m30) + m12*(m21*m30 - m20*m31);
0662a2
0662a2
	double det = m00*r.m00 + m01*r.m10 + m02*r.m20 + m03*r.m30;
0662a2
	if (fabs(det) <= real_precision) return zero();
0662a2
	det = 1/det;
0662a2
	r.m00 *= det;
0662a2
	r.m10 *= det;
0662a2
	r.m20 *= det;
0662a2
	r.m30 *= det;
0662a2
0662a2
	r.m01 = det*(m01*(m23*m32 - m22*m33) + m02*(m21*m33 - m23*m31) + m03*(m22*m31 - m21*m32));
0662a2
	r.m11 = det*(m00*(m22*m33 - m23*m32) + m02*(m23*m30 - m20*m33) + m03*(m20*m32 - m22*m30));
0662a2
	r.m21 = det*(m00*(m23*m31 - m21*m33) + m01*(m20*m33 - m23*m30) + m03*(m21*m30 - m20*m31));
0662a2
	r.m31 = det*(m00*(m21*m32 - m22*m31) + m01*(m22*m30 - m20*m32) + m02*(m20*m31 - m21*m30));
0662a2
	r.m02 = det*(m01*(m12*m33 - m13*m32) + m02*(m13*m31 - m11*m33) + m03*(m11*m32 - m12*m31));
0662a2
	r.m12 = det*(m00*(m13*m32 - m12*m33) + m02*(m10*m33 - m13*m30) + m03*(m12*m30 - m10*m32));
0662a2
	r.m22 = det*(m00*(m11*m33 - m13*m31) + m01*(m13*m30 - m10*m33) + m03*(m10*m31 - m11*m30));
0662a2
	r.m32 = det*(m00*(m12*m31 - m11*m32) + m01*(m10*m32 - m12*m30) + m02*(m11*m30 - m10*m31));
0662a2
	r.m03 = det*(m01*(m13*m22 - m12*m23) + m02*(m11*m23 - m13*m21) + m03*(m12*m21 - m11*m22));
0662a2
	r.m13 = det*(m00*(m12*m23 - m13*m22) + m02*(m13*m20 - m10*m23) + m03*(m10*m22 - m12*m20));
0662a2
	r.m23 = det*(m00*(m13*m21 - m11*m23) + m01*(m10*m23 - m13*m20) + m03*(m11*m20 - m10*m21));
0662a2
	r.m33 = det*(m00*(m11*m22 - m12*m21) + m01*(m12*m20 - m10*m22) + m02*(m10*m21 - m11*m20));
0662a2
	return r;
14f971
}