|
 |
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 |
}
|