libroot-math-smatrix-dev 5.34.14-1build1 / usr / include / root / Math / MatrixInversion.icc

// @(#)root/smatrix:$Id$
// Authors: CLHEP authors, L. Moneta    2006  

#ifndef ROOT_Math_MatrixInversion_icc
#define ROOT_Math_MatrixInversion_icc


#include "Math/SVector.h"
#include <limits>


// inversion algorithms for matrices
// taken  from CLHEP (L. Moneta May 2006)

namespace ROOT { 

  namespace Math { 


  /** General Inversion for a symmetric matrix 
      Bunch-Kaufman diagonal pivoting method
      It is decribed in J.R. Bunch, L. Kaufman (1977). 
      "Some Stable Methods for Calculating Inertia and Solving Symmetric 
      Linear Systems", Math. Comp. 31, p. 162-179. or in Gene H. Golub, 
      /Charles F. van Loan, "Matrix Computations" (the second edition 
      has a bug.) and implemented in "lapack"
      Mario Stanke, 09/97
      
  */

template <unsigned int idim, unsigned int N>
template<class T> 
void Inverter<idim,N>::InvertBunchKaufman(MatRepSym<T,idim> & rhs, int &ifail) {




   int i, j, k, s;
   int pivrow;

   const int nrow = MatRepSym<T,idim>::kRows;

   // Establish the two working-space arrays needed:  x and piv are
   // used as pointers to arrays of doubles and ints respectively, each
   // of length nrow.  We do not want to reallocate each time through
   // unless the size needs to grow.  We do not want to leak memory, even
   // by having a new without a delete that is only done once.
  

   SVector<T, MatRepSym<T,idim>::kRows> xvec; 
   SVector<int, MatRepSym<T,idim>::kRows> pivv; 
  
   typedef int* pivIter; 
   typedef T* mIter; 


   // Note - resize shuld do  nothing if the size is already larger than nrow,
   //        but on VC++ there are indications that it does so we check.
   // Note - the data elements in a vector are guaranteed to be contiguous,
   //        so x[i] and piv[i] are optimally fast.
   mIter   x   = xvec.begin();
   // x[i] is used as helper storage, needs to have at least size nrow.
   pivIter piv = pivv.begin();
   // piv[i] is used to store details of exchanges
      
   double temp1, temp2;
   mIter ip, mjj, iq;
   double lambda, sigma;
   const double alpha = .6404; // = (1+sqrt(17))/8
   // LM (04/2009) remove this useless check (it is not in LAPACK) which fails inversion of 
   // a matrix with  values < epsilon in the diagonal
   //
   //const double epsilon = 32*std::numeric_limits<T>::epsilon();
   // whenever a sum of two doubles is below or equal to epsilon
   // it is set to zero.
   // this constant could be set to zero but then the algorithm
   // doesn't neccessarily detect that a matrix is singular
  
   for (i = 0; i < nrow; i++)
      piv[i] = i+1;
      
   ifail = 0;
      
   // compute the factorization P*A*P^T = L * D * L^T 
   // L is unit lower triangular, D is direct sum of 1x1 and 2x2 matrices
   // L and D^-1 are stored in A = *this, P is stored in piv[]
	
   for (j=1; j < nrow; j+=s)  // main loop over columns
   {
      mjj = rhs.Array() + j*(j-1)/2 + j-1;
      lambda = 0;           // compute lambda = max of A(j+1:n,j)
      pivrow = j+1;
      ip = rhs.Array() + (j+1)*j/2 + j-1;
      for (i=j+1; i <= nrow ; ip += i++)
         if (std::abs(*ip) > lambda)
         {
            lambda = std::abs(*ip);
            pivrow = i;
         }
	  
      if (lambda == 0 )
      {
         if (*mjj == 0)
         {
            ifail = 1;
            return;
         }
         s=1;
         *mjj = 1.0f / *mjj;
      }
      else
      {
         if (std::abs(*mjj) >= lambda*alpha)
         {
            s=1;
            pivrow=j;
         }
         else
         {
            sigma = 0;  // compute sigma = max A(pivrow, j:pivrow-1)
            ip = rhs.Array() + pivrow*(pivrow-1)/2+j-1;
            for (k=j; k < pivrow; k++)
            {
               if (std::abs(*ip) > sigma)
                  sigma = std::abs(*ip);
               ip++;
            }
            // sigma cannot be zero because it is at least lambda which is not zero
            if ( std::abs(*mjj) >= alpha * lambda * (lambda/ sigma) )
            {
               s=1;
               pivrow = j;
            }
            else if (std::abs(*(rhs.Array()+pivrow*(pivrow-1)/2+pivrow-1)) 
                     >= alpha * sigma)
               s=1;
            else
               s=2;
         }
         if (pivrow == j)  // no permutation neccessary
         {
            piv[j-1] = pivrow;
            if (*mjj == 0)
            {
               ifail=1;
               return;
            }
            temp2 = *mjj = 1.0f/ *mjj; // invert D(j,j)
		  
            // update A(j+1:n, j+1,n)
            for (i=j+1; i <= nrow; i++)
            {
               temp1 = *(rhs.Array() + i*(i-1)/2 + j-1) * temp2;
               ip = rhs.Array()+i*(i-1)/2+j;
               for (k=j+1; k<=i; k++)
               {
                  *ip -= static_cast<T> ( temp1 * *(rhs.Array() + k*(k-1)/2 + j-1) );
//                   if (std::abs(*ip) <= epsilon)
//                      *ip=0;
                  ip++;
               }
            }
            // update L 
            ip = rhs.Array() + (j+1)*j/2 + j-1; 
            for (i=j+1; i <= nrow; ip += i++)
               *ip *= static_cast<T> ( temp2 );
         }
         else if (s==1) // 1x1 pivot 
         {
            piv[j-1] = pivrow;
		  
            // interchange rows and columns j and pivrow in
            // submatrix (j:n,j:n)
            ip = rhs.Array() + pivrow*(pivrow-1)/2 + j;
            for (i=j+1; i < pivrow; i++, ip++)
            {
               temp1 = *(rhs.Array() + i*(i-1)/2 + j-1);
               *(rhs.Array() + i*(i-1)/2 + j-1)= *ip;
               *ip = static_cast<T> ( temp1 );
            }
            temp1 = *mjj;
            *mjj = *(rhs.Array()+pivrow*(pivrow-1)/2+pivrow-1);
            *(rhs.Array()+pivrow*(pivrow-1)/2+pivrow-1) = static_cast<T> (temp1 );
            ip = rhs.Array() + (pivrow+1)*pivrow/2 + j-1;
            iq = ip + pivrow-j;
            for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
            {
               temp1 = *iq;
               *iq = *ip;
               *ip = static_cast<T>( temp1 );
            }
		  
            if (*mjj == 0)
            {
               ifail = 1;
               return;
            }
            temp2 = *mjj = 1.0f / *mjj; // invert D(j,j)
		  
            // update A(j+1:n, j+1:n)
            for (i = j+1; i <= nrow; i++)
            {
               temp1 = *(rhs.Array() + i*(i-1)/2 + j-1) * temp2;
               ip = rhs.Array()+i*(i-1)/2+j;
               for (k=j+1; k<=i; k++)
               {
                  *ip -= static_cast<T> (temp1 * *(rhs.Array() + k*(k-1)/2 + j-1) );
//                   if (std::abs(*ip) <= epsilon)
//                      *ip=0;
                  ip++;
               }
            }
            // update L
            ip = rhs.Array() + (j+1)*j/2 + j-1;
            for (i=j+1; i<=nrow; ip += i++)
               *ip *= static_cast<T>( temp2 );
         }
         else // s=2, ie use a 2x2 pivot
         {
            piv[j-1] = -pivrow;
            piv[j] = 0; // that means this is the second row of a 2x2 pivot
		  
            if (j+1 != pivrow) 
            {
               // interchange rows and columns j+1 and pivrow in
               // submatrix (j:n,j:n) 
               ip = rhs.Array() + pivrow*(pivrow-1)/2 + j+1;
               for (i=j+2; i < pivrow; i++, ip++)
               {
                  temp1 = *(rhs.Array() + i*(i-1)/2 + j);
                  *(rhs.Array() + i*(i-1)/2 + j) = *ip;
                  *ip = static_cast<T>( temp1 );
               }
               temp1 = *(mjj + j + 1);
               *(mjj + j + 1) = 
                  *(rhs.Array() + pivrow*(pivrow-1)/2 + pivrow-1);
               *(rhs.Array() + pivrow*(pivrow-1)/2 + pivrow-1) = static_cast<T>( temp1 );
               temp1 = *(mjj + j);
               *(mjj + j) = *(rhs.Array() + pivrow*(pivrow-1)/2 + j-1);
               *(rhs.Array() + pivrow*(pivrow-1)/2 + j-1) = static_cast<T>( temp1 );
               ip = rhs.Array() + (pivrow+1)*pivrow/2 + j;
               iq = ip + pivrow-(j+1);
               for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
               {
                  temp1 = *iq;
                  *iq = *ip;
                  *ip = static_cast<T>( temp1 );
               }
            } 
            // invert D(j:j+1,j:j+1)
            temp2 = *mjj * *(mjj + j + 1) - *(mjj + j) * *(mjj + j); 
            if (temp2 == 0)
               std::cerr
                  << "SymMatrix::bunch_invert: error in pivot choice" 
                  << std::endl;
            temp2 = 1. / temp2;
            // this quotient is guaranteed to exist by the choice 
            // of the pivot
            temp1 = *mjj;
            *mjj = static_cast<T>( *(mjj + j + 1) * temp2 );
            *(mjj + j + 1) = static_cast<T>( temp1 * temp2 );
            *(mjj + j) = static_cast<T>( - *(mjj + j) * temp2 );
		  
            if (j < nrow-1) // otherwise do nothing
            {
               // update A(j+2:n, j+2:n)
               for (i=j+2; i <= nrow ; i++)
               {
                  ip = rhs.Array() + i*(i-1)/2 + j-1;
                  temp1 = *ip * *mjj + *(ip + 1) * *(mjj + j);
//                   if (std::abs(temp1 ) <= epsilon)
//                      temp1 = 0;
                  temp2 = *ip * *(mjj + j) + *(ip + 1) * *(mjj + j + 1);
//                   if (std::abs(temp2 ) <= epsilon)
//                      temp2 = 0;
                  for (k = j+2; k <= i ; k++)
                  {
                     ip = rhs.Array() + i*(i-1)/2 + k-1;
                     iq = rhs.Array() + k*(k-1)/2 + j-1;
                     *ip -= static_cast<T>( temp1 * *iq + temp2 * *(iq+1) );
//                      if (std::abs(*ip) <= epsilon)
//                         *ip = 0;
                  }
               }
               // update L
               for (i=j+2; i <= nrow ; i++)
               {
                  ip = rhs.Array() + i*(i-1)/2 + j-1;
                  temp1 = *ip * *mjj + *(ip+1) * *(mjj + j);
//                   if (std::abs(temp1) <= epsilon)
//                      temp1 = 0;
                  *(ip+1) = *ip * *(mjj + j) 
                     + *(ip+1) * *(mjj + j + 1);
//                   if (std::abs(*(ip+1)) <= epsilon)
//                      *(ip+1) = 0;
                  *ip = static_cast<T>( temp1 );
               }
            }
         }
      }
   } // end of main loop over columns

   if (j == nrow) // the the last pivot is 1x1
   {
      mjj = rhs.Array() + j*(j-1)/2 + j-1;
      if (*mjj == 0)
      {
         ifail = 1;
         return;
      }
      else
         *mjj = 1.0f  / *mjj;
   } // end of last pivot code

   // computing the inverse from the factorization
	 
   for (j = nrow ; j >= 1 ; j -= s) // loop over columns
   {
      mjj = rhs.Array() + j*(j-1)/2 + j-1;
      if (piv[j-1] > 0) // 1x1 pivot, compute column j of inverse
      {
         s = 1; 
         if (j < nrow)
         {
            ip = rhs.Array() + (j+1)*j/2 + j-1;
            for (i=0; i < nrow-j; ip += 1+j+i++)
               x[i] = *ip;
            for (i=j+1; i<=nrow ; i++)
            {
               temp2=0;
               ip = rhs.Array() + i*(i-1)/2 + j;
               for (k=0; k <= i-j-1; k++)
                  temp2 += *ip++ * x[k];
               for (ip += i-1; k < nrow-j; ip += 1+j+k++) 
                  temp2 += *ip * x[k];
               *(rhs.Array()+ i*(i-1)/2 + j-1) = static_cast<T>( -temp2 );
            }
            temp2 = 0;
            ip = rhs.Array() + (j+1)*j/2 + j-1;
            for (k=0; k < nrow-j; ip += 1+j+k++)
               temp2 += x[k] * *ip;
            *mjj -= static_cast<T>( temp2 );
         }
      }
      else //2x2 pivot, compute columns j and j-1 of the inverse
      {
         if (piv[j-1] != 0)
            std::cerr << "error in piv" << piv[j-1] << std::endl;
         s=2; 
         if (j < nrow)
         {
            ip = rhs.Array() + (j+1)*j/2 + j-1;
            for (i=0; i < nrow-j; ip += 1+j+i++)
               x[i] = *ip;
            for (i=j+1; i<=nrow ; i++)
            {
               temp2 = 0;
               ip = rhs.Array() + i*(i-1)/2 + j;
               for (k=0; k <= i-j-1; k++)
                  temp2 += *ip++ * x[k];
               for (ip += i-1; k < nrow-j; ip += 1+j+k++)
                  temp2 += *ip * x[k];
               *(rhs.Array()+ i*(i-1)/2 + j-1) = static_cast<T>( -temp2 );
            }    
            temp2 = 0;
            ip = rhs.Array() + (j+1)*j/2 + j-1;
            for (k=0; k < nrow-j; ip += 1+j+k++)
               temp2 += x[k] * *ip;
            *mjj -= static_cast<T>( temp2 );
            temp2 = 0;
            ip = rhs.Array() + (j+1)*j/2 + j-2;
            for (i=j+1; i <= nrow; ip += i++)
               temp2 += *ip * *(ip+1);
            *(mjj-1) -= static_cast<T>( temp2 );
            ip = rhs.Array() + (j+1)*j/2 + j-2;
            for (i=0; i < nrow-j; ip += 1+j+i++)
               x[i] = *ip;
            for (i=j+1; i <= nrow ; i++)
            {
               temp2 = 0;
               ip = rhs.Array() + i*(i-1)/2 + j;
               for (k=0; k <= i-j-1; k++)
                  temp2 += *ip++ * x[k];
               for (ip += i-1; k < nrow-j; ip += 1+j+k++)
                  temp2 += *ip * x[k];
               *(rhs.Array()+ i*(i-1)/2 + j-2)= static_cast<T>( -temp2 );
            }
            temp2 = 0;
            ip = rhs.Array() + (j+1)*j/2 + j-2;
            for (k=0; k < nrow-j; ip += 1+j+k++)
               temp2 += x[k] * *ip;
            *(mjj-j) -= static_cast<T>( temp2 );
         }
      }  
	  
      // interchange rows and columns j and piv[j-1] 
      // or rows and columns j and -piv[j-2]
	  
      pivrow = (piv[j-1]==0)? -piv[j-2] : piv[j-1];
      ip = rhs.Array() + pivrow*(pivrow-1)/2 + j;
      for (i=j+1;i < pivrow; i++, ip++)
      {
         temp1 = *(rhs.Array() + i*(i-1)/2 + j-1);
         *(rhs.Array() + i*(i-1)/2 + j-1) = *ip;
         *ip = static_cast<T>( temp1 );
      }
      temp1 = *mjj;
      *mjj = *(rhs.Array() + pivrow*(pivrow-1)/2 + pivrow-1);
      *(rhs.Array() + pivrow*(pivrow-1)/2 + pivrow-1) = static_cast<T>( temp1 );
      if (s==2)
      {
         temp1 = *(mjj-1);
         *(mjj-1) = *( rhs.Array() + pivrow*(pivrow-1)/2 + j-2);
         *( rhs.Array() + pivrow*(pivrow-1)/2 + j-2) = static_cast<T>( temp1 );
      }
	  
      ip = rhs.Array() + (pivrow+1)*pivrow/2 + j-1;  // &A(i,j)
      iq = ip + pivrow-j;
      for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
      {
         temp1 = *iq;
         *iq = *ip;
         *ip = static_cast<T>(temp1);
      } 
   } // end of loop over columns (in computing inverse from factorization)

   return; // inversion successful

}



/**
   LU factorization : code originally from CERNLIB dfact routine and ported in C++ for CLHEP
*/

template <unsigned int idim, unsigned int n>
template<class T> 
int Inverter<idim,n>::DfactMatrix(MatRepStd<T,idim,n> & rhs, T &det, unsigned int *ir) {
  
   if (idim != n) return   -1; 

   int ifail, jfail;

   typedef T* mIter; 


   double tf;
   double g1 = 1.0e-19, g2 = 1.0e19;

   double p, q, t;
   double s11, s12;

   // LM (04.09) : remove useless check on epsilon and set it to zero
   const double epsilon = 0.0;
   //double epsilon = 8*std::numeric_limits<T>::epsilon();
   // could be set to zero (like it was before)
   // but then the algorithm often doesn't detect
   // that a matrix is singular

   int normal = 0, imposs = -1;
   int jrange = 0, jover = 1, junder = -1;
   ifail = normal;
   jfail = jrange;
   int nxch = 0;
   det = 1.0;
   mIter mj = rhs.Array();
   mIter mjj = mj;
   for (unsigned int j=1;j<=n;j++) {
      unsigned int k = j;
      p = (std::abs(*mjj));
      if (j!=n) {
         mIter mij = mj + n + j - 1; 
         for (unsigned int i=j+1;i<=n;i++) {
            q = (std::abs(*(mij)));
            if (q > p) {
               k = i;
               p = q;
            }
            mij += n;
         }
         if (k==j) {
            if (p <= epsilon) {
               det = 0;
               ifail = imposs;
               jfail = jrange;
               return ifail;
            }
            det = -det; // in this case the sign of the determinant
            // must not change. So I change it twice. 
         }
         mIter mjl = mj;
         mIter mkl = rhs.Array() + (k-1)*n;
         for (unsigned int l=1;l<=n;l++) {
            tf = *mjl;
            *(mjl++) = *mkl;
            *(mkl++) = static_cast<T>(tf);
         }
         nxch = nxch + 1;  // this makes the determinant change its sign
         ir[nxch] = (((j)<<12)+(k));
      } else {
         if (p <= epsilon) {
            det = 0.0;
            ifail = imposs;
            jfail = jrange;
            return ifail;
         }
      }
      det *= *mjj;
      *mjj = 1.0f / *mjj;
      t = (std::abs(det));
      if (t < g1) {
         det = 0.0;
         if (jfail == jrange) jfail = junder;
      } else if (t > g2) {
         det = 1.0;
         if (jfail==jrange) jfail = jover;
      }
      if (j!=n) {
         mIter mk = mj + n;
         mIter mkjp = mk + j;
         mIter mjk = mj + j;
         for (k=j+1;k<=n;k++) {
            s11 = - (*mjk);
            s12 = - (*mkjp);
            if (j!=1) {
               mIter mik = rhs.Array() + k - 1;
               mIter mijp = rhs.Array() + j;
               mIter mki = mk;
               mIter mji = mj;
               for (unsigned int i=1;i<j;i++) {
                  s11 += (*mik) * (*(mji++));
                  s12 += (*mijp) * (*(mki++));
                  mik += n;
                  mijp += n;
               }
            }
            // cast to avoid warnings from double to float conversions
            *(mjk++) = static_cast<T>( - s11 * (*mjj) );
            *(mkjp) = static_cast<T> ( -(((*(mjj+1)))*((*(mkjp-1)))+(s12)) );
            mk += n;
            mkjp += n;
         }
      }
      mj += n;
      mjj += (n+1);
   }
   if (nxch%2==1) det = -det;
   if (jfail !=jrange) det = 0.0;
   ir[n] = nxch;
   return 0;
}



    /** 
	Inversion for General square matrices. 
	Code from  dfinv routine from CERNLIB
	Assumed first the LU decomposition via DfactMatrix function
	
	taken from CLHEP : L. Moneta May 2006
    */

template <unsigned int idim, unsigned int n>
template<class T> 
int Inverter<idim,n>::DfinvMatrix(MatRepStd<T,idim,n> & rhs,unsigned int * ir) {


   typedef T* mIter; 

   if (idim != n) return   -1; 


   double s31, s32;
   register double s33, s34;

   mIter m11 = rhs.Array();
   mIter m12 = m11 + 1;
   mIter m21 = m11 + n;
   mIter m22 = m12 + n;
   *m21 = -(*m22) * (*m11) * (*m21);
   *m12 = -(*m12);
   if (n>2) {
      mIter mi = rhs.Array() + 2 * n;
      mIter mii= rhs.Array() + 2 * n + 2;
      mIter mimim = rhs.Array() + n + 1;
      for (unsigned int i=3;i<=n;i++) {
         unsigned int im2 = i - 2;
         mIter mj = rhs.Array();
         mIter mji = mj + i - 1;
         mIter mij = mi;
         for (unsigned int j=1;j<=im2;j++) { 
            s31 = 0.0;
            s32 = *mji;
            mIter mkj = mj + j - 1;
            mIter mik = mi + j - 1;
            mIter mjkp = mj + j;
            mIter mkpi = mj + n + i - 1;
            for (unsigned int k=j;k<=im2;k++) {
               s31 += (*mkj) * (*(mik++));
               s32 += (*(mjkp++)) * (*mkpi);
               mkj += n;
               mkpi += n;
            }
            *mij = static_cast<T>( -(*mii) * (((*(mij-n)))*( (*(mii-1)))+(s31)) );
            *mji = static_cast<T> ( -s32 );
            mj += n;
            mji += n;
            mij++;
         }
         *(mii-1) = -(*mii) * (*mimim) * (*(mii-1));
         *(mimim+1) = -(*(mimim+1));
         mi += n;
         mimim += (n+1);
         mii += (n+1);
      }
   }
   mIter mi = rhs.Array();
   mIter mii = rhs.Array();
   for (unsigned  int i=1;i<n;i++) {
      unsigned int ni = n - i;
      mIter mij = mi;
      //int j;
      for (unsigned j=1; j<=i;j++) {
         s33 = *mij;
         register mIter mikj = mi + n + j - 1;
         register mIter miik = mii + 1;
         mIter min_end = mi + n;
         for (;miik<min_end;) {
            s33 += (*mikj) * (*(miik++));
            mikj += n;
         }
         *(mij++) = static_cast<T> ( s33 );
      }
      for (unsigned j=1;j<=ni;j++) {
         s34 = 0.0;
         mIter miik = mii + j;
         mIter mikij = mii + j * n + j;
         for (unsigned int k=j;k<=ni;k++) {
            s34 += *mikij * (*(miik++));
            mikij += n;
         }
         *(mii+j) = s34;
      }
      mi += n;
      mii += (n+1);
   }
   unsigned int nxch = ir[n];
   if (nxch==0) return 0;
   for (unsigned int mm=1;mm<=nxch;mm++) {
      unsigned int k = nxch - mm + 1;
      int ij = ir[k];
      int i = ij >> 12;
      int j = ij%4096;
      mIter mki = rhs.Array() + i - 1;
      mIter mkj = rhs.Array() + j - 1;
      for (k=1; k<=n;k++) {
         // 2/24/05 David Sachs fix of improper swap bug that was present
         // for many years:
         T ti = *mki; // 2/24/05
         *mki = *mkj;
         *mkj = ti;	// 2/24/05
         mki += n;
         mkj += n;
      }
   }
   return 0;
}


  }  // end namespace Math
}    // end namespace ROOT

#endif