StRoot: StarRoot/TRSymMatrix.cxx Source File

00001 #include <iomanip>
00002 #include "TMath.h"
00003 #include "TRVector.h"
00004 #include "TRSymMatrix.h"
00005 #include "TRDiagMatrix.h"
00006 #include "TError.h"
00007 ClassImp(TRSymMatrix);
00008 //________________________________________________________________________________
00009 TRSymMatrix::TRSymMatrix(Int_t nrows,Double_t a0, ...) : 
00010   TRArray(nrows*(nrows+1)/2), fNrows(nrows) {
00011   __VA_LIST__(a0);
00012 }
00013 //________________________________________________________________________________
00014 TRSymMatrix::TRSymMatrix(Int_t nrows,const Double_t *Array) : TRArray() {
00015   fNrows = TMath::Abs(nrows);
00016   if (nrows > 0) Set(fNrows*(fNrows+1)/2,Array);
00017   else {// Upper to Lower
00018     Set(fNrows*(fNrows+1)/2);
00019     Int_t ij;
00020     Int_t i = 0, j = 0;
00021     for (Int_t l = 0; l < fN; l++) {
00022       ij = i*(i+1)/2 + j;
00023       fArray[ij] = Array[l];
00024       if (i < fNrows - 1) i++;
00025       else {
00026         j++;
00027         i = j;
00028       }
00029     }
00030   }
00031 }
00032 //________________________________________________________________________________
00033 TRSymMatrix::TRSymMatrix(Int_t nrows,const Float_t *Array) : TRArray() {
00034   fNrows = TMath::Abs(nrows);
00035   if (nrows > 0) {
00036     Set(fNrows*(fNrows+1)/2);
00037     TCL::ucopy(Array,fArray,fNrows*(fNrows+1)/2);
00038   }
00039   else {// Upper to Lower
00040     Set(fNrows*(fNrows+1)/2);
00041     Int_t ij;
00042     Int_t i = 0, j = 0;
00043     for (Int_t l = 0; l < fN; l++) {
00044       ij = i*(i+1)/2 + j;
00045       fArray[ij] = Array[l];
00046       if (i < fNrows - 1) i++;
00047       else {
00048         j++;
00049         i = j;
00050       }
00051     }
00052   }
00053 }
00054 //________________________________________________________________________________
00055 TRSymMatrix::TRSymMatrix(Int_t nrows,const Char_t *s) : TRArray(nrows*(nrows+1)/2,s), fNrows(nrows) {}
00056 //________________________________________________________________________________
00057 TRSymMatrix::TRSymMatrix(ETRMatrixCreatorsOp kop,Int_t nrows) :
00058   TRArray(nrows*(nrows+1)/2), fNrows(nrows){
00059  switch (kop) {
00060   case kZero:
00061     break;
00062   case kUnit:
00063     for (int i=0; i<fNrows; i++) fArray[i*(i+1)/2+i] = 1;
00064     break;
00065   default:
00066     Error("TRSymMatrix(ETRMatrixCreatorsOp)", "operation %d not yet implemented", kop);
00067   }
00068   
00069 }
00070 //________________________________________________________________________________
00071 TRSymMatrix::TRSymMatrix(const TRSymMatrix& S,ETRMatrixCreatorsOp kop) {
00072   switch (kop) {
00073   case kInvertedPosDef:
00074     SpmInv(S);
00075     break;
00076   case kInverted:
00077     fNrows = S.GetNcols();
00078     Set(fNrows*(fNrows+1)/2);
00079     TCL::trsinv(S.GetArray(),fArray, fNrows);
00080     break;
00081   case kInvertedA:
00082     fNrows = S.GetNcols();
00083     Set(fNrows*(fNrows+1)/2);
00084     fValid = ! TrsInv(S.GetArray(),fArray, fNrows);
00085     break;
00086   default:
00087     Error("TRSymMatrix(ETRMatrixCreatorsOp)", "operation %d not yet implemented", kop);
00088   }
00089 }
00090 //________________________________________________________________________________
00091 TRSymMatrix::TRSymMatrix(const TRMatrix& A) {
00092   Int_t NI = A.GetNrows(); fNrows = NI;
00093   Int_t NJ = A.GetNcols();
00094   assert(NI == NJ);
00095   Set(fNrows*(fNrows+1)/2);
00096   TCL::trpck(A.GetArray(),fArray,NI);
00097 }
00098 //________________________________________________________________________________
00099 TRSymMatrix::TRSymMatrix(const TRMatrix& A,ETRMatrixCreatorsOp kop,const TRSymMatrix& S) {
00100   Int_t M, N;
00101   switch (kop) { // 
00102   case kAxSxAT: //A[M,N]*S[N,N]*AT[M,N] => R[M,M]; 
00103     M = A.GetNrows();
00104     N = S.GetNrows();
00105     assert(N == A.GetNcols());
00106     fNrows = M;
00107     Set(fNrows*(fNrows+1)/2);
00108     TCL::trasat(A.GetArray(),S.GetArray(),fArray,M,N);
00109     break;
00110   case kATxSxA: //BT[N,M]*S[N,N]*B[N,M] => R[M,M]; 
00111     M = A.GetNcols();
00112     N = S.GetNrows();
00113     assert(N == A.GetNrows());
00114     fNrows = M;
00115     Set(fNrows*(fNrows+1)/2);
00116     TCL::tratsa(A.GetArray(),S.GetArray(),fArray,M,N);
00117     break;
00118   default:
00119     Error("TRSymMatrix(ETRMatrixCreatorsOp)", "operation %d not yet implemented", kop);
00120   }
00121 }
00122 //________________________________________________________________________________
00123 TRSymMatrix::TRSymMatrix(const TRSymMatrix& Q,ETRMatrixCreatorsOp kop,const TRSymMatrix& T){
00124   assert (kop == kRxSxR);
00125   Int_t M = Q.GetNcols();
00126   assert(M == T.GetNcols());
00127   fNrows = M;
00128   Set(fNrows*(fNrows+1)/2);
00129   TCL::trqsq(Q.GetArray(),T.GetArray(),fArray,M);
00130 }
00131 //________________________________________________________________________________
00132 TRSymMatrix::TRSymMatrix(const TRMatrix& A,ETRMatrixCreatorsOp kop) {
00133   Int_t M, N;
00134   switch (kop) {
00135   case kAxAT: // A[M,N]*AT[M,N] => S[M,M]
00136     M = A.GetNrows();    
00137     N = A.GetNcols();    
00138     fNrows = M;
00139     Set(fNrows*(fNrows+1)/2);
00140     TCL::traat(A.GetArray(),fArray,M,N);
00141     break;
00142   case kATxA: // AT[N,M]*A[N,M] => S[M,M]
00143     N = A.GetNrows();    
00144     M = A.GetNcols();    
00145     fNrows = M;
00146     Set(fNrows*(fNrows+1)/2);
00147     TCL::trata(A.GetArray(),fArray,M,N);
00148     break;
00149   default:
00150     Error("TRSymMatrix(ETRMatrixCreatorsOp)", "operation %d not yet implemented", kop);
00151   }
00152 }
00153 //________________________________________________________________________________
00154 Double_t TRSymMatrix::Product(const TRVector& A,ETRMatrixCreatorsOp /* kop */) {
00155   Int_t M, N; // N == 1
00156   Double_t Value;
00157   //  case kAxSxAT: //A[M,N]*S[N,N]*AT[M,N] => R[M,M]; 
00158   //  case kATxSxA: //BT[N,M]*S[N,N]*B[N,M] => R[M,M]; 
00159   M = A.GetNcols();
00160   N = GetNrows();
00161   assert(N == A.GetNrows());
00162   TCL::tratsa(A.GetArray(),GetArray(),&Value,M,N);
00163   return Value;
00164 }
00165 //________________________________________________________________________________
00166 ostream& operator<<(ostream& s,const TRSymMatrix &target) {
00167   static const int width = 10;
00168   Int_t Nrows = target.GetNrows();
00169   const Double_t *Array = target.GetArray();
00170   s << "Semi Positive DefinedSymMatrix Size \t[" 
00171     << Nrows << "," << Nrows << "]" << endl;
00172   if (Array) {
00173     s.setf(ios::fixed,ios::scientific);
00174     s.setf(ios::showpos);
00175     for (int i = 0; i< Nrows; i++) {
00176       for (int j = 0; j <= i; j++)
00177         s << std::setw(width) << std::setprecision(width-3) << Array[i*(i+1)/2 + j] << ":\t";
00178       s << endl;
00179     }
00180     s.unsetf(ios::showpos);
00181   }
00182   else s << " Empty";   
00183   return s;
00184 }
00185 //________________________________________________________________________________
00186 void TRSymMatrix::Print(Option_t *opt) const {if (opt) {}; cout << *this << endl;}
00187 //________________________________________________________________________________
00188 Int_t TRSymMatrix::SpmInv(const TRSymMatrix &S, TRVector *B) {
00189   if (&S != this) *this = S;
00190   Double_t *diag = new Double_t[fNrows];
00191   Bool_t   *flag = new Bool_t[fNrows];
00192   Double_t *b = 0;
00193   if (B) b = B->GetArray();
00194   Int_t nrank = 0;
00195   spminv(fArray, b, fNrows, nrank, diag, flag);
00196   delete [] diag;
00197   delete [] flag;
00198   return nrank;
00199 }
00200 //________________________________________________________________________________
00201 Int_t TRSymMatrix::spminv(Double_t *v, Double_t *b, Int_t n, 
00202                           Int_t &nrank, Double_t *diag, Bool_t *flag) {
00203   // taken from Millepede (V.Blobel)
00204   /* Local variables */
00205   static Int_t i, j, k, l, jj, jk, kk, jl, lk;
00206   static Double_t vjk, vkk;
00207   
00208   /*     obtain solution of a system of linear equations with symmetric */
00209   /*     matrix and the inverse. */
00210   
00211   /*                    - - - */
00212   /*        CALL SPMINV(V,B,N,NRANK,...,...)      solve  V * X = B */
00213   /*                    - -   ----- */
00214   
00215   /*           V = symmetric N-by-N matrix in symmetric storage mode */
00216   /*               V(1) = V11, V(2) = V12, V(3) = V22, V(4) = V13, . . . */
00217   /*               replaced by inverse matrix */
00218   /*           B = N-vector, replaced by solution vector */
00219   
00220   /*     DIAG(N) =  double precision scratch array */
00221   /*     FLAG(N) =  Bool_t scratch array */
00222   
00223   /*     Method of solution is by elimination selecting the  pivot  on  the */
00224   /*     diagonal each stage. The rank of the matrix is returned in  NRANK. */
00225   /*     For NRANK ne N, all remaining  rows  and  cols  of  the  resulting */
00226   /*     matrix V and the corresponding elements of  B  are  set  to  zero. */
00227   
00228   /*     ... */
00229   /* Parameter adjustments */
00230   --v;
00231   --diag;
00232   if (b) --b;
00233   --flag;
00234   
00235   /* Function Body */
00236   for (i = 1; i <= n; ++i) {
00237     flag[i] = kTRUE; /* reset flags */
00238     diag[i] = TMath::Abs(v[(i * i + i) / 2]); /* save TMath::Abs of diagonal elements */
00239   }
00240   nrank = 0;
00241   for (i = 1; i <= n; ++i) {/* start of loop */
00242     k = jj = kk = 0;
00243     vkk = 0.;
00244     for (j = 1; j <= n; ++j) {/* search for pivot */
00245       jj += j;
00246       if (flag[j]) {/* not used so far  Computing MAX */
00247         if (TMath::Abs(v[jj]) > TMath::Max(TMath::Abs(vkk),diag[j] * 1e-10)) {
00248           vkk = v[jj]; /* pivot (candidate) */
00249           k = j;       /* index of pivot */
00250           kk = jj;     /* index of diagonal element */
00251         }
00252       }
00253     }
00254     if (k != 0) {            /* pivot found */
00255       ++nrank;               /* increase rank and ... */
00256       flag[k] = kFALSE;      /* ... reset flag */
00257       vkk = 1. / vkk;
00258       v[kk] = -vkk;
00259       if (b) b[k] *= vkk;
00260       jk = kk - k;
00261       jl = 0;
00262       for (j = 1; j <= n; ++j) {/* elimination */
00263         if (j == k) {
00264           jk = kk;
00265           jl += j;
00266         } else {
00267           if (j < k) {
00268             ++jk;
00269           } else {
00270             jk = jk + j - 1;
00271           }
00272           vjk = v[jk];
00273           v[jk] = vkk * vjk;
00274           if (b) b[j] -= b[k] * vjk;
00275           lk = kk - k;
00276           for (l = 1; l <= j; ++l) {
00277             ++jl;
00278             if (l == k) {
00279               lk = kk;
00280             } else {
00281               if (l < k) {
00282                 ++lk;
00283               } else {
00284                 lk = lk + l - 1;
00285               }
00286               v[jl] -= v[lk] * vjk;
00287             }
00288           }
00289         }
00290       }
00291     } else {
00292       for (k = 1; k <= n; ++k) {
00293         if (flag[k]) {
00294           if (b) b[k] = 0.; /* clear vector element */
00295           for (j = 1; j <= k; ++j) {
00296             if (flag[j]) {
00297               v[(k * k - k) / 2 + j] = 0.;
00298             } /* clear matrix row/col */
00299           }
00300         }
00301       }
00302       goto L10;
00303     }
00304   } /* end of loop */
00305  L10:  Int_t nn = (n * n + n) / 2;
00306     for (Int_t ij = 1; ij <= nn; ++ij) {
00307     v[ij] = -v[ij]; /* finally reverse sign of all matrix elements */
00308   }
00309   return 0;
00310 } 
00311 //________________________________________________________________________________
00312 Int_t TRSymMatrix::TrsInv(const Double_t *g, Double_t *gi, Int_t n) {
00313   Int_t fail = TrchLU(g, gi, n);
00314   fail += TrInv(gi, gi, n);
00315   TrsmUL(gi, gi,n);
00316   return fail;
00317 }
00318 //________________________________________________________________________________
00319 Int_t TRSymMatrix::TrchLU(const double *a, double *b, int n) {
00320    Int_t fail = 0;
00321    /* Local variables */
00322    int ipiv, kpiv, i__, j;
00323    double r__, dc;
00324    int id, kd;
00325    double sum;
00326 
00327 
00328    /* CERN PROGLIB# F112    TRCHLU          .VERSION KERNFOR  4.16  870601 */
00329    /* ORIG. 18/12/74 W.HART */
00330 
00331 
00332    /* Parameter adjuTments */
00333    --b;    --a;
00334 
00335    /* Function Body */
00336    ipiv = 0;
00337 
00338    i__ = 0;
00339 
00340    do {
00341       ++i__;
00342       ipiv += i__;
00343       kpiv = ipiv;
00344       r__ = a[ipiv];
00345 
00346       for (j = i__; j <= n; ++j) {
00347          sum = 0.;
00348          if (i__ == 1)       goto L40;
00349          if (r__ == 0.)      goto L42;
00350          id = ipiv - i__ + 1;
00351          kd = kpiv - i__ + 1;
00352 
00353          do {
00354             sum += b[kd] * b[id];
00355             ++kd;   ++id;
00356          } while (id < ipiv);
00357 
00358 L40:
00359          sum = a[kpiv] - sum;
00360 L42:
00361          if (j != i__) b[kpiv] = sum * r__;
00362          else {
00363            if (sum > 0) dc = TMath::Sqrt(sum);
00364            else         dc = 0;
00365            b[kpiv] = dc;
00366            if (r__ > 0. && dc > 0)  r__ = (double)1. / dc;
00367            else                    {r__ = 0; fail++;}
00368          }
00369          kpiv += j;
00370       }
00371 
00372    } while  (i__ < n);
00373    return fail;
00374 }
00375 //________________________________________________________________________________
00376 Int_t TRSymMatrix::TrInv(const Double_t *g, Double_t *gi, Int_t n)  {TCL::trinv(g, gi, n); return 0;}
00377 //________________________________________________________________________________
00378 Int_t TRSymMatrix::TrsmUL(const Double_t *g, Double_t *gi, Int_t n) {TCL::trsmul(g, gi, n); return 0;}
00379 #if 0
00380 //________________________________________________________________________________
00381 Double_t &TRSymMatrix::operator()(Int_t i,Int_t j){
00382   //  assert(! (j < 0 || j >= fNrows));
00383   if (j < 0 || j >= fNrows) {
00384     ::Error("TRSymMatrix::operator()", "index j %d out of bounds (size: %d, this: %p)", 
00385             j, fNrows, this); 
00386     j = 0;
00387   }
00388   //  assert(! (i < 0 || i >= fNrows));
00389   if (i < 0 || i >= fNrows) {
00390     ::Error("TRSymMatrix::operator()", "index i %d out of bounds (size: %d, this: %p)", 
00391             i, fNrows, this); 
00392     i = 0;
00393   }
00394   Int_t m = i;
00395   Int_t l = j;
00396   if (i > j) {m = j; l = i;}
00397   return TArrayD::operator[](m + (l+1)*l/2);
00398 }
00399 #endif