invertPosDefMatrix.h

Go to the documentation of this file.

#ifndef DataFormat_Math_invertPosDefMatrix_H
#define DataFormat_Math_invertPosDefMatrix_H

#define SMATRIX_USE_CONSTEXPR
#include "Math/SMatrix.h"
#include "Math/CholeskyDecomp.h"
#include<type_traits>

template<typename T,unsigned int N>
inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T,N,N,ROOT::Math::MatRepSym<T,N> > & m) {
  
  ROOT::Math::CholeskyDecomp<T,N> decomp(m);
  if (!decomp) {
    return m.Invert();
  } else 
    decomp.Invert(m);
  return true;

}

template<typename PDM2>
void fastInvertPDM2(PDM2&mm) {
  auto m = mm.Array();

  constexpr typename std::remove_reference<decltype(m[0])>::type one = 1.;
  auto c0 = one/m[0];
  auto c1 = m[1]*m[1]* c0;
  auto c2 = one/(m[2] - c1);

  auto li21 = c1 * c0 * c2;
  m[0] = li21 + c0;
  m[1] = - m[1]*c0*c2;
  m[2] = c2;
}

template<>
inline bool invertPosDefMatrix<double,1>(ROOT::Math::SMatrix<double,1,1,ROOT::Math::MatRepSym<double,1> > & m) {
  m(0,0) = 1./m(0,0); 
  return true;
}
template<>
inline bool invertPosDefMatrix<float,1>(ROOT::Math::SMatrix<float,1,1,ROOT::Math::MatRepSym<float,1> > & m) {
  m(0,0) = 1.f/m(0,0);
  return true;
}


template<>
inline bool invertPosDefMatrix<double,2>(ROOT::Math::SMatrix<double,2,2,ROOT::Math::MatRepSym<double,2> > & m) {
  fastInvertPDM2(m); 
  return true;
}
template<>
inline bool invertPosDefMatrix<float,2>(ROOT::Math::SMatrix<float,2,2,ROOT::Math::MatRepSym<float,2> > & m) {
  fastInvertPDM2(m); 
  return true;
}



template<typename T,unsigned int N>
inline bool invertPosDefMatrix(ROOT::Math::SMatrix<T,N,N,ROOT::Math::MatRepSym<T,N> > const & mIn,
            ROOT::Math::SMatrix<T,N,N,ROOT::Math::MatRepSym<T,N> > & mOut) {
  
  ROOT::Math::CholeskyDecomp<T,N> decomp(mIn);
  if (!decomp) {
    mOut=mIn;
    return mOut.Invert();
  } else 
    decomp.Invert(mOut);
  return true;

}

#endif