Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2021-02-14 12:54:02

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