1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
|
template <typename REAL> Matrix<REAL,3> matrixInvert(const Matrix<REAL,3>& p_matrix)
{
REAL det = p_matrix(0,0)*p_matrix(1,1)*p_matrix(2,2) +
p_matrix(1,0)*p_matrix(2,1)*p_matrix(0,2) +
p_matrix(2,0)*p_matrix(0,1)*p_matrix(1,2) -
p_matrix(2,0)*p_matrix(1,1)*p_matrix(0,2) -
p_matrix(2,1)*p_matrix(1,2)*p_matrix(0,0) -
p_matrix(2,2)*p_matrix(1,0)*p_matrix(0,1);
if(det==0) throw std::logic_error("Inverting matrix with null det");
return Matrix3<REAL>(
(p_matrix(1,1)*p_matrix(2,2) - p_matrix(2,1) * p_matrix(1,2)) / det,
(p_matrix(2,0)*p_matrix(1,2) - p_matrix(1,0) * p_matrix(2,2)) / det,
(p_matrix(1,0)*p_matrix(2,1) - p_matrix(2,0) * p_matrix(1,1)) / det,
(p_matrix(2,1)*p_matrix(0,2) - p_matrix(0,1) * p_matrix(2,2)) / det,
(p_matrix(0,0)*p_matrix(2,2) - p_matrix(2,0) * p_matrix(0,2)) / det,
(p_matrix(2,0)*p_matrix(0,1) - p_matrix(0,0) * p_matrix(2,1)) / det,
(p_matrix(0,1)*p_matrix(1,2) - p_matrix(1,1) * p_matrix(0,2)) / det,
(p_matrix(1,0)*p_matrix(0,2) - p_matrix(0,0) * p_matrix(1,2)) / det,
(p_matrix(0,0)*p_matrix(1,1) - p_matrix(1,0) * p_matrix(0,1)) / det);
} |
Partager