Blame c++/vector/matrix.cpp
|
|
145120 |
|
|
|
145120 |
#include "matrix.h"
|
|
|
145120 |
|
|
|
145120 |
|
|
|
145120 |
Matrix
|
|
|
145120 |
Matrix::get_inverted() const {
|
|
|
145120 |
Real d = det();
|
|
|
145120 |
if (equal(d, 0)) {
|
|
|
145120 |
// try to make back transform for X-axis
|
|
|
145120 |
if (equal(m02, 0) && equal(m10, 0) && equal(m11, 0) && equal(m12, 0) && equal(m22, 1)) {
|
|
|
145120 |
d = axis_x().lensqr();
|
|
|
145120 |
if (d > precision) {
|
|
|
145120 |
d = 1.0/d;
|
|
|
145120 |
return Matrix(
|
|
|
145120 |
d*m00, 0, 0,
|
|
|
145120 |
d*m01, 0, 0,
|
|
|
145120 |
-d*(m20*m00 + m21*m01), 0, 1 );
|
|
|
145120 |
}
|
|
|
145120 |
}
|
|
|
145120 |
|
|
|
145120 |
// try to make back transform for Y-axis
|
|
|
145120 |
if (equal(m00, 0) && equal(m01, 0) && equal(m02, 0) && equal(m12, 0) && equal(m22, 1)) {
|
|
|
145120 |
d = axis_y().lensqr();
|
|
|
145120 |
if (d > precision) {
|
|
|
145120 |
d = 1/d;
|
|
|
145120 |
return Matrix(
|
|
|
145120 |
0, d*m10, 0,
|
|
|
145120 |
0, d*m11, 0,
|
|
|
145120 |
0, -d*(m20*m10 + m21*m11), 1 );
|
|
|
145120 |
}
|
|
|
145120 |
}
|
|
|
145120 |
|
|
|
145120 |
// give up
|
|
|
145120 |
return Matrix( 0, 0, 0,
|
|
|
145120 |
0, 0, 0,
|
|
|
145120 |
0, 0, 0 );
|
|
|
145120 |
}
|
|
|
145120 |
|
|
|
145120 |
// proper inversion
|
|
|
145120 |
Real p = 1/d, m = -p;
|
|
|
145120 |
return Matrix(
|
|
|
145120 |
p*(m11*m22 - m12*m21), // row0
|
|
|
145120 |
m*(m01*m22 - m02*m21),
|
|
|
145120 |
p*(m01*m12 - m02*m11),
|
|
|
145120 |
m*(m10*m22 - m12*m20), // row1
|
|
|
145120 |
p*(m00*m22 - m02*m20),
|
|
|
145120 |
m*(m00*m12 - m02*m10),
|
|
|
145120 |
p*(m10*m21 - m11*m20), // row2
|
|
|
145120 |
m*(m00*m21 - m01*m20),
|
|
|
145120 |
p*(m00*m11 - m01*m10) );
|
|
|
145120 |
}
|