Mercurial > hg > orthanc-stone
diff Framework/Toolbox/LinearAlgebra.cpp @ 163:8c5b24892ed2 wasm
LinearAlgebra::InvertMatrix
author | Sebastien Jodogne <s.jodogne@gmail.com> |
---|---|
date | Wed, 14 Feb 2018 14:26:26 +0100 |
parents | 197a5ddaf68c |
children | 432b1f812d14 |
line wrap: on
line diff
--- a/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 12:24:56 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 14:26:26 2018 +0100 @@ -449,5 +449,68 @@ throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); } } + + + void InvertMatrix(Matrix& target, + const Matrix& source) + { + if (source.size1() != source.size2()) + { + LOG(ERROR) << "Inverse only exists for square matrices"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + double determinant = ComputeDeterminant(source); + + if (IsCloseToZero(determinant)) + { + LOG(ERROR) << "Cannot invert singular matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + double denominator = 1.0 / determinant; + + target.resize(source.size1(), source.size2()); + + if (source.size1() == 1) + { + target(0, 0) = denominator; + } + else if (source.size1() == 2) + { + // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_2_%C3%97_2_matrices + target(0, 0) = source(1, 1) * denominator; + target(0, 1) = -source(0, 1) * denominator; + target(1, 0) = -source(1, 0) * denominator; + target(1, 1) = source(0, 0) * denominator; + } + else if (source.size1() == 3) + { + // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices + const double a = source(0, 0); + const double b = source(0, 1); + const double c = source(0, 2); + const double d = source(1, 0); + const double e = source(1, 1); + const double f = source(1, 2); + const double g = source(2, 0); + const double h = source(2, 1); + const double i = source(2, 2); + + target(0, 0) = (e * i - f * h) * denominator; + target(0, 1) = -(b * i - c * h) * denominator; + target(0, 2) = (b * f - c * e) * denominator; + target(1, 0) = -(d * i - f * g) * denominator; + target(1, 1) = (a * i - c * g) * denominator; + target(1, 2) = -(a * f - c * d) * denominator; + target(2, 0) = (d * h - e * g) * denominator; + target(2, 1) = -(a * h - b * g) * denominator; + target(2, 2) = (a * e - b * d) * denominator; + } + else + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_NotImplemented); + } + } } }