File indexing completed on 2024-04-06 12:04:41
0001 #ifndef DataFormat_Math_invertPosDefMatrix_H
0002 #define DataFormat_Math_invertPosDefMatrix_H
0003
0004 #define SMATRIX_USE_CONSTEXPR
0005 #include "Math/SMatrix.h"
0006 #include "Math/CholeskyDecomp.h"
0007 #include <type_traits>
0008
0009 template <typename T, unsigned int N>
0010 inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> >& m) {
0011 ROOT::Math::CholeskyDecomp<T, N> decomp(m);
0012 if (!decomp) {
0013 return m.Invert();
0014 } else
0015 decomp.Invert(m);
0016 return true;
0017 }
0018
0019 template <typename PDM2>
0020 void fastInvertPDM2(PDM2& mm) {
0021 auto m = mm.Array();
0022
0023 constexpr typename std::remove_reference<decltype(m[0])>::type one = 1.;
0024 auto c0 = one / m[0];
0025 auto c1 = m[1] * m[1] * c0;
0026 auto c2 = one / (m[2] - c1);
0027
0028 auto li21 = c1 * c0 * c2;
0029 m[0] = li21 + c0;
0030 m[1] = -m[1] * c0 * c2;
0031 m[2] = c2;
0032 }
0033
0034 template <>
0035 inline bool invertPosDefMatrix<double, 1>(ROOT::Math::SMatrix<double, 1, 1, ROOT::Math::MatRepSym<double, 1> >& m) {
0036 m(0, 0) = 1. / m(0, 0);
0037 return true;
0038 }
0039 template <>
0040 inline bool invertPosDefMatrix<float, 1>(ROOT::Math::SMatrix<float, 1, 1, ROOT::Math::MatRepSym<float, 1> >& m) {
0041 m(0, 0) = 1.f / m(0, 0);
0042 return true;
0043 }
0044
0045 template <>
0046 inline bool invertPosDefMatrix<double, 2>(ROOT::Math::SMatrix<double, 2, 2, ROOT::Math::MatRepSym<double, 2> >& m) {
0047 fastInvertPDM2(m);
0048 return true;
0049 }
0050 template <>
0051 inline bool invertPosDefMatrix<float, 2>(ROOT::Math::SMatrix<float, 2, 2, ROOT::Math::MatRepSym<float, 2> >& m) {
0052 fastInvertPDM2(m);
0053 return true;
0054 }
0055
0056 template <typename T, unsigned int N>
0057 inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> > const& mIn,
0058 ROOT::Math::SMatrix<T, N, N, ROOT::Math::MatRepSym<T, N> >& mOut) {
0059 ROOT::Math::CholeskyDecomp<T, N> decomp(mIn);
0060 if (!decomp) {
0061 mOut = mIn;
0062 return mOut.Invert();
0063 } else
0064 decomp.Invert(mOut);
0065 return true;
0066 }
0067
0068 #endif
