source: trunk/source/error_propagation/src/G4ErrorSymMatrix.cc @ 1272

Last change on this file since 1272 was 1228, checked in by garnier, 14 years ago

update geant4.9.3 tag
File size: 80.6 KB
Line 
1//
2// ********************************************************************
3// * License and Disclaimer                                           *
4// *                                                                  *
5// * The  Geant4 software  is  copyright of the Copyright Holders  of *
6// * the Geant4 Collaboration.  It is provided  under  the terms  and *
7// * conditions of the Geant4 Software License,  included in the file *
8// * LICENSE and available at  http://cern.ch/geant4/license .  These *
9// * include a list of copyright holders.                             *
10// *                                                                  *
11// * Neither the authors of this software system, nor their employing *
12// * institutes,nor the agencies providing financial support for this *
13// * work  make  any representation or  warranty, express or implied, *
14// * regarding  this  software system or assume any liability for its *
15// * use.  Please see the license in the file  LICENSE  and URL above *
16// * for the full disclaimer and the limitation of liability.         *
17// *                                                                  *
18// * This  code  implementation is the result of  the  scientific and *
19// * technical work of the GEANT4 collaboration.                      *
20// * By using,  copying,  modifying or  distributing the software (or *
21// * any work based  on the software)  you  agree  to acknowledge its *
22// * use  in  resulting  scientific  publications,  and indicate your *
23// * acceptance of all terms of the Geant4 Software license.          *
24// ********************************************************************
25//
26// $Id: G4ErrorSymMatrix.cc,v 1.3 2007/06/21 15:04:10 gunter Exp $
27// GEANT4 tag $Name: geant4-09-03 $
28//
29// ------------------------------------------------------------
30//      GEANT 4 class implementation file
31// ------------------------------------------------------------
32
33#include "globals.hh"
34#include <iostream>
35#include <cmath>
36
37#include "G4ErrorSymMatrix.hh"
38#include "G4ErrorMatrix.hh"
39
40// Simple operation for all elements
41
42#define SIMPLE_UOP(OPER)                      \
43    G4ErrorMatrixIter a=m.begin();            \
44    G4ErrorMatrixIter e=m.begin()+num_size(); \
45   for(;a<e; a++) (*a) OPER t;
46
47#define SIMPLE_BOP(OPER)                           \
48    G4ErrorMatrixIter a=m.begin();                 \
49    G4ErrorMatrixConstIter b=m2.m.begin();         \
50    G4ErrorMatrixConstIter e=m.begin()+num_size(); \
51   for(;a<e; a++, b++) (*a) OPER (*b);
52
53#define SIMPLE_TOP(OPER)                                 \
54    G4ErrorMatrixConstIter a=m1.m.begin();               \
55    G4ErrorMatrixConstIter b=m2.m.begin();               \
56    G4ErrorMatrixIter t=mret.m.begin();                  \
57    G4ErrorMatrixConstIter e=m1.m.begin()+m1.num_size(); \
58   for( ;a<e; a++, b++, t++) (*t) = (*a) OPER (*b);
59
60#define CHK_DIM_2(r1,r2,c1,c2,fun) \
61   if (r1!=r2 || c1!=c2)  { \
62    G4ErrorMatrix::error("Range error in Matrix function " #fun "(1)."); \
63   }
64
65#define CHK_DIM_1(c1,r2,fun) \
66   if (c1!=r2) { \
67     G4ErrorMatrix::error("Range error in Matrix function " #fun "(2)."); \
68   }
69
70// Constructors. (Default constructors are inlined and in .icc file)
71
72G4ErrorSymMatrix::G4ErrorSymMatrix(G4int p)
73   : m(p*(p+1)/2), nrow(p)
74{
75   size = nrow * (nrow+1) / 2;
76   m.assign(size,0);
77}
78
79G4ErrorSymMatrix::G4ErrorSymMatrix(G4int p, G4int init)
80   : m(p*(p+1)/2), nrow(p)
81{
82   size = nrow * (nrow+1) / 2;
83
84   m.assign(size,0);
85   switch(init)
86   {
87   case 0:
88      break;
89     
90   case 1:
91      {
92         G4ErrorMatrixIter a = m.begin();
93         for(G4int i=1;i<=nrow;i++)
94         {
95            *a = 1.0;
96            a += (i+1);
97         }
98         break;
99      }
100   default:
101      G4ErrorMatrix::error("G4ErrorSymMatrix: initialization must be 0 or 1.");
102   }
103}
104
105//
106// Destructor
107//
108
109G4ErrorSymMatrix::~G4ErrorSymMatrix()
110{
111}
112
113G4ErrorSymMatrix::G4ErrorSymMatrix(const G4ErrorSymMatrix &m1)
114   : m(m1.size), nrow(m1.nrow), size(m1.size)
115{
116   m = m1.m;
117}
118
119//
120//
121// Sub matrix
122//
123//
124
125G4ErrorSymMatrix G4ErrorSymMatrix::sub(G4int min_row, G4int max_row) const
126{
127  G4ErrorSymMatrix mret(max_row-min_row+1);
128  if(max_row > num_row())
129    { G4ErrorMatrix::error("G4ErrorSymMatrix::sub: Index out of range"); }
130  G4ErrorMatrixIter a = mret.m.begin();
131  G4ErrorMatrixConstIter b1 = m.begin() + (min_row+2)*(min_row-1)/2;
132  for(G4int irow=1; irow<=mret.num_row(); irow++)
133  {
134    G4ErrorMatrixConstIter b = b1;
135    for(G4int icol=1; icol<=irow; icol++)
136    {
137      *(a++) = *(b++);
138    }
139    b1 += irow+min_row-1;
140  }
141  return mret;
142}
143
144G4ErrorSymMatrix G4ErrorSymMatrix::sub(G4int min_row, G4int max_row) 
145{
146  G4ErrorSymMatrix mret(max_row-min_row+1);
147  if(max_row > num_row())
148    { G4ErrorMatrix::error("G4ErrorSymMatrix::sub: Index out of range"); }
149  G4ErrorMatrixIter a = mret.m.begin();
150  G4ErrorMatrixIter b1 = m.begin() + (min_row+2)*(min_row-1)/2;
151  for(G4int irow=1; irow<=mret.num_row(); irow++)
152  {
153    G4ErrorMatrixIter b = b1;
154    for(G4int icol=1; icol<=irow; icol++)
155    {
156      *(a++) = *(b++);
157    }
158    b1 += irow+min_row-1;
159  }
160  return mret;
161}
162
163void G4ErrorSymMatrix::sub(G4int row,const G4ErrorSymMatrix &m1)
164{
165  if(row <1 || row+m1.num_row()-1 > num_row() )
166    { G4ErrorMatrix::error("G4ErrorSymMatrix::sub: Index out of range"); }
167  G4ErrorMatrixConstIter a = m1.m.begin();
168  G4ErrorMatrixIter b1 = m.begin() + (row+2)*(row-1)/2;
169  for(G4int irow=1; irow<=m1.num_row(); irow++)
170  {
171    G4ErrorMatrixIter b = b1;
172    for(G4int icol=1; icol<=irow; icol++)
173    {
174      *(b++) = *(a++);
175    }
176    b1 += irow+row-1;
177  }
178}
179
180//
181// Direct sum of two matricies
182//
183
184G4ErrorSymMatrix dsum(const G4ErrorSymMatrix &m1,
185                      const G4ErrorSymMatrix &m2)
186{
187  G4ErrorSymMatrix mret(m1.num_row() + m2.num_row(), 0);
188  mret.sub(1,m1);
189  mret.sub(m1.num_row()+1,m2);
190  return mret;
191}
192
193/* -----------------------------------------------------------------------
194   This section contains support routines for matrix.h. This section contains
195   The two argument functions +,-. They call the copy constructor and +=,-=.
196   ----------------------------------------------------------------------- */
197
198G4ErrorSymMatrix G4ErrorSymMatrix::operator- () const 
199{
200  G4ErrorSymMatrix m2(nrow);
201  G4ErrorMatrixConstIter a=m.begin();
202  G4ErrorMatrixIter b=m2.m.begin();
203  G4ErrorMatrixConstIter e=m.begin()+num_size();
204  for(;a<e; a++, b++) { (*b) = -(*a); }
205  return m2;
206}
207
208G4ErrorMatrix operator+(const G4ErrorMatrix &m1, const G4ErrorSymMatrix &m2)
209{
210  G4ErrorMatrix mret(m1);
211  CHK_DIM_2(m1.num_row(),m2.num_row(), m1.num_col(),m2.num_col(),+);
212  mret += m2;
213  return mret;
214}
215
216G4ErrorMatrix operator+(const G4ErrorSymMatrix &m1, const G4ErrorMatrix &m2)
217{
218  G4ErrorMatrix mret(m2);
219  CHK_DIM_2(m1.num_row(),m2.num_row(),m1.num_col(),m2.num_col(),+);
220  mret += m1;
221  return mret;
222}
223
224G4ErrorSymMatrix operator+(const G4ErrorSymMatrix &m1,
225                           const G4ErrorSymMatrix &m2)
226{
227  G4ErrorSymMatrix mret(m1.nrow);
228  CHK_DIM_1(m1.nrow, m2.nrow,+);
229  SIMPLE_TOP(+)
230  return mret;
231}
232
233//
234// operator -
235//
236
237G4ErrorMatrix operator-(const G4ErrorMatrix &m1, const G4ErrorSymMatrix &m2)
238{
239  G4ErrorMatrix mret(m1);
240  CHK_DIM_2(m1.num_row(),m2.num_row(),m1.num_col(),m2.num_col(),-);
241  mret -= m2;
242  return mret;
243}
244
245G4ErrorMatrix operator-(const G4ErrorSymMatrix &m1, const G4ErrorMatrix &m2)
246{
247  G4ErrorMatrix mret(m1);
248  CHK_DIM_2(m1.num_row(),m2.num_row(),m1.num_col(),m2.num_col(),-);
249  mret -= m2;
250  return mret;
251}
252
253G4ErrorSymMatrix operator-(const G4ErrorSymMatrix &m1,
254                           const G4ErrorSymMatrix &m2)
255{
256  G4ErrorSymMatrix mret(m1.num_row());
257  CHK_DIM_1(m1.num_row(),m2.num_row(),-);
258  SIMPLE_TOP(-)
259  return mret;
260}
261
262/* -----------------------------------------------------------------------
263   This section contains support routines for matrix.h. This file contains
264   The two argument functions *,/. They call copy constructor and then /=,*=.
265   ----------------------------------------------------------------------- */
266
267G4ErrorSymMatrix operator/(const G4ErrorSymMatrix &m1,G4double t)
268{
269  G4ErrorSymMatrix mret(m1);
270  mret /= t;
271  return mret;
272}
273
274G4ErrorSymMatrix operator*(const G4ErrorSymMatrix &m1,G4double t)
275{
276  G4ErrorSymMatrix mret(m1);
277  mret *= t;
278  return mret;
279}
280
281G4ErrorSymMatrix operator*(G4double t,const G4ErrorSymMatrix &m1)
282{
283  G4ErrorSymMatrix mret(m1);
284  mret *= t;
285  return mret;
286}
287
288G4ErrorMatrix operator*(const G4ErrorMatrix &m1, const G4ErrorSymMatrix &m2)
289{
290  G4ErrorMatrix mret(m1.num_row(),m2.num_col());
291  CHK_DIM_1(m1.num_col(),m2.num_row(),*);
292  G4ErrorMatrixConstIter mit1, mit2, sp,snp; //mit2=0
293  G4double temp;
294  G4ErrorMatrixIter mir=mret.m.begin();
295  G4int step,stept;
296  for(mit1=m1.m.begin();
297      mit1<m1.m.begin()+m1.num_row()*m1.num_col(); mit1 = mit2)
298  {
299    for(step=1,snp=m2.m.begin();step<=m2.num_row();)
300    {
301      mit2=mit1;
302      sp=snp;
303      snp+=step;
304      temp=0;
305      while(sp<snp)
306      {
307        temp+=*(sp++)*(*(mit2++));
308      }
309      sp+=step-1;
310      for(stept=++step;stept<=m2.num_row();stept++)
311      {
312        temp+=*sp*(*(mit2++));
313        sp+=stept;
314      }
315      *(mir++)=temp;
316    }
317  }
318  return mret;
319}
320
321G4ErrorMatrix operator*(const G4ErrorSymMatrix &m1, const G4ErrorMatrix &m2)
322{
323  G4ErrorMatrix mret(m1.num_row(),m2.num_col());
324  CHK_DIM_1(m1.num_col(),m2.num_row(),*);
325  G4int step,stept;
326  G4ErrorMatrixConstIter mit1,mit2,sp,snp;
327  G4double temp;
328  G4ErrorMatrixIter mir=mret.m.begin();
329  for(step=1,snp=m1.m.begin();step<=m1.num_row();snp+=step++)
330  {
331    for(mit1=m2.m.begin();mit1<m2.m.begin()+m2.num_col();mit1++)
332    {
333      mit2=mit1;
334      sp=snp;
335      temp=0;
336      while(sp<snp+step)
337      {
338        temp+=*mit2*(*(sp++));
339        mit2+=m2.num_col();
340      }
341      sp+=step-1;
342      for(stept=step+1;stept<=m1.num_row();stept++)
343      {
344        temp+=*mit2*(*sp);
345        mit2+=m2.num_col();
346        sp+=stept;
347      }
348      *(mir++)=temp;
349    }
350  }
351  return mret;
352}
353
354G4ErrorMatrix operator*(const G4ErrorSymMatrix &m1, const G4ErrorSymMatrix &m2)
355{
356  G4ErrorMatrix mret(m1.num_row(),m1.num_row());
357  CHK_DIM_1(m1.num_col(),m2.num_row(),*);
358  G4int step1,stept1,step2,stept2;
359  G4ErrorMatrixConstIter snp1,sp1,snp2,sp2;
360  G4double temp;
361  G4ErrorMatrixIter mr = mret.m.begin();
362  for(step1=1,snp1=m1.m.begin();step1<=m1.num_row();snp1+=step1++)
363  {
364    for(step2=1,snp2=m2.m.begin();step2<=m2.num_row();)
365    {
366      sp1=snp1;
367      sp2=snp2;
368      snp2+=step2;
369      temp=0;
370      if(step1<step2)
371      {
372        while(sp1<snp1+step1)
373          { temp+=(*(sp1++))*(*(sp2++)); }
374        sp1+=step1-1;
375        for(stept1=step1+1;stept1!=step2+1;sp1+=stept1++)
376          { temp+=(*sp1)*(*(sp2++)); }
377        sp2+=step2-1;
378        for(stept2=++step2;stept2<=m2.num_row();sp1+=stept1++,sp2+=stept2++)
379          { temp+=(*sp1)*(*sp2); }
380      }
381      else
382      {
383        while(sp2<snp2)
384          { temp+=(*(sp1++))*(*(sp2++)); }
385        sp2+=step2-1;
386        for(stept2=++step2;stept2!=step1+1;sp2+=stept2++)
387          { temp+=(*(sp1++))*(*sp2); }
388        sp1+=step1-1;
389        for(stept1=step1+1;stept1<=m1.num_row();sp1+=stept1++,sp2+=stept2++)
390          { temp+=(*sp1)*(*sp2); }
391      }
392      *(mr++)=temp;
393    }
394  }
395  return mret;
396}
397
398/* -----------------------------------------------------------------------
399   This section contains the assignment and inplace operators =,+=,-=,*=,/=.
400   ----------------------------------------------------------------------- */
401
402G4ErrorMatrix & G4ErrorMatrix::operator+=(const G4ErrorSymMatrix &m2)
403{
404  CHK_DIM_2(num_row(),m2.num_row(),num_col(),m2.num_col(),+=);
405  G4int n = num_col();
406  G4ErrorMatrixConstIter sjk = m2.m.begin();
407  G4ErrorMatrixIter m1j = m.begin();
408  G4ErrorMatrixIter mj = m.begin();
409  // j >= k
410  for(G4int j=1;j<=num_row();j++)
411  {
412    G4ErrorMatrixIter mjk = mj;
413    G4ErrorMatrixIter mkj = m1j;
414    for(G4int k=1;k<=j;k++)
415    {
416      *(mjk++) += *sjk;
417      if(j!=k) *mkj += *sjk;
418      sjk++;
419      mkj += n;
420    }
421    mj += n;
422    m1j++;
423  }
424  return (*this);
425}
426
427G4ErrorSymMatrix & G4ErrorSymMatrix::operator+=(const G4ErrorSymMatrix &m2)
428{
429  CHK_DIM_2(num_row(),m2.num_row(),num_col(),m2.num_col(),+=);
430  SIMPLE_BOP(+=)
431  return (*this);
432}
433
434G4ErrorMatrix & G4ErrorMatrix::operator-=(const G4ErrorSymMatrix &m2)
435{
436  CHK_DIM_2(num_row(),m2.num_row(),num_col(),m2.num_col(),-=);
437  G4int n = num_col();
438  G4ErrorMatrixConstIter sjk = m2.m.begin();
439  G4ErrorMatrixIter m1j = m.begin();
440  G4ErrorMatrixIter mj = m.begin();
441  // j >= k
442  for(G4int j=1;j<=num_row();j++)
443  {
444    G4ErrorMatrixIter mjk = mj;
445    G4ErrorMatrixIter mkj = m1j;
446    for(G4int k=1;k<=j;k++)
447    {
448      *(mjk++) -= *sjk;
449      if(j!=k) *mkj -= *sjk;
450      sjk++;
451      mkj += n;
452    }
453    mj += n;
454    m1j++;
455  }
456  return (*this);
457}
458
459G4ErrorSymMatrix & G4ErrorSymMatrix::operator-=(const G4ErrorSymMatrix &m2)
460{
461  CHK_DIM_2(num_row(),m2.num_row(),num_col(),m2.num_col(),-=);
462  SIMPLE_BOP(-=)
463  return (*this);
464}
465
466G4ErrorSymMatrix & G4ErrorSymMatrix::operator/=(G4double t)
467{
468  SIMPLE_UOP(/=)
469  return (*this);
470}
471
472G4ErrorSymMatrix & G4ErrorSymMatrix::operator*=(G4double t)
473{
474  SIMPLE_UOP(*=)
475  return (*this);
476}
477
478G4ErrorMatrix & G4ErrorMatrix::operator=(const G4ErrorSymMatrix &m1)
479{
480   if(m1.nrow*m1.nrow != size)
481   {
482      size = m1.nrow * m1.nrow;
483      m.resize(size);
484   }
485   nrow = m1.nrow;
486   ncol = m1.nrow;
487   G4int n = ncol;
488   G4ErrorMatrixConstIter sjk = m1.m.begin();
489   G4ErrorMatrixIter m1j = m.begin();
490   G4ErrorMatrixIter mj = m.begin();
491   // j >= k
492   for(G4int j=1;j<=num_row();j++)
493   {
494      G4ErrorMatrixIter mjk = mj;
495      G4ErrorMatrixIter mkj = m1j;
496      for(G4int k=1;k<=j;k++)
497      {
498         *(mjk++) = *sjk;
499         if(j!=k) *mkj = *sjk;
500         sjk++;
501         mkj += n;
502      }
503      mj += n;
504      m1j++;
505   }
506   return (*this);
507}
508
509G4ErrorSymMatrix & G4ErrorSymMatrix::operator=(const G4ErrorSymMatrix &m1)
510{
511   if(m1.nrow != nrow)
512   {
513      nrow = m1.nrow;
514      size = m1.size;
515      m.resize(size);
516   }
517   m = m1.m;
518   return (*this);
519}
520
521// Print the Matrix.
522
523std::ostream& operator<<(std::ostream &s, const G4ErrorSymMatrix &q)
524{
525  s << G4endl;
526
527  // Fixed format needs 3 extra characters for field,
528  // while scientific needs 7
529
530  G4int width;
531  if(s.flags() & std::ios::fixed)
532  {
533    width = s.precision()+3;
534  }
535  else
536  {
537    width = s.precision()+7;
538  }
539  for(G4int irow = 1; irow<= q.num_row(); irow++)
540  {
541    for(G4int icol = 1; icol <= q.num_col(); icol++)
542    {
543      s.width(width);
544      s << q(irow,icol) << " ";
545    }
546    s << G4endl;
547  }
548  return s;
549}
550
551G4ErrorSymMatrix G4ErrorSymMatrix::
552apply(G4double (*f)(G4double, G4int, G4int)) const
553{
554  G4ErrorSymMatrix mret(num_row());
555  G4ErrorMatrixConstIter a = m.begin();
556  G4ErrorMatrixIter b = mret.m.begin();
557  for(G4int ir=1;ir<=num_row();ir++)
558  {
559    for(G4int ic=1;ic<=ir;ic++)
560    {
561      *(b++) = (*f)(*(a++), ir, ic);
562    }
563  }
564  return mret;
565}
566
567void G4ErrorSymMatrix::assign (const G4ErrorMatrix &m1)
568{
569   if(m1.nrow != nrow)
570   {
571      nrow = m1.nrow;
572      size = nrow * (nrow+1) / 2;
573      m.resize(size);
574   }
575   G4ErrorMatrixConstIter a = m1.m.begin();
576   G4ErrorMatrixIter b = m.begin();
577   for(G4int r=1;r<=nrow;r++)
578   {
579      G4ErrorMatrixConstIter d = a;
580      for(G4int c=1;c<=r;c++)
581      {
582         *(b++) = *(d++);
583      }
584      a += nrow;
585   }
586}
587
588G4ErrorSymMatrix G4ErrorSymMatrix::similarity(const G4ErrorMatrix &m1) const
589{
590  G4ErrorSymMatrix mret(m1.num_row());
591  G4ErrorMatrix temp = m1*(*this);
592
593// If m1*(*this) has correct dimensions, then so will the m1.T multiplication.
594// So there is no need to check dimensions again.
595
596  G4int n = m1.num_col();
597  G4ErrorMatrixIter mr = mret.m.begin();
598  G4ErrorMatrixIter tempr1 = temp.m.begin();
599  for(G4int r=1;r<=mret.num_row();r++)
600  {
601    G4ErrorMatrixConstIter m1c1 = m1.m.begin();
602    for(G4int c=1;c<=r;c++)
603    {
604      G4double tmp = 0.0;
605      G4ErrorMatrixIter tempri = tempr1;
606      G4ErrorMatrixConstIter m1ci = m1c1;
607      for(G4int i=1;i<=m1.num_col();i++)
608      {
609        tmp+=(*(tempri++))*(*(m1ci++));
610      }
611      *(mr++) = tmp;
612      m1c1 += n;
613    }
614    tempr1 += n;
615  }
616  return mret;
617}
618
619G4ErrorSymMatrix G4ErrorSymMatrix::similarity(const G4ErrorSymMatrix &m1) const
620{
621  G4ErrorSymMatrix mret(m1.num_row());
622  G4ErrorMatrix temp = m1*(*this);
623  G4int n = m1.num_col();
624  G4ErrorMatrixIter mr = mret.m.begin();
625  G4ErrorMatrixIter tempr1 = temp.m.begin();
626  for(G4int r=1;r<=mret.num_row();r++)
627  {
628    G4ErrorMatrixConstIter m1c1 = m1.m.begin();
629    G4int c;
630    for(c=1;c<=r;c++)
631    {
632      G4double tmp = 0.0;
633      G4ErrorMatrixIter tempri = tempr1;
634      G4ErrorMatrixConstIter m1ci = m1c1;
635      G4int i;
636      for(i=1;i<c;i++)
637      {
638        tmp+=(*(tempri++))*(*(m1ci++));
639      }
640      for(i=c;i<=m1.num_col();i++)
641      {
642        tmp+=(*(tempri++))*(*(m1ci));
643        m1ci += i;
644      }
645      *(mr++) = tmp;
646      m1c1 += c;
647    }
648    tempr1 += n;
649  }
650  return mret;
651}
652
653G4ErrorSymMatrix G4ErrorSymMatrix::similarityT(const G4ErrorMatrix &m1) const
654{
655  G4ErrorSymMatrix mret(m1.num_col());
656  G4ErrorMatrix temp = (*this)*m1;
657  G4int n = m1.num_col();
658  G4ErrorMatrixIter mrc = mret.m.begin();
659  G4ErrorMatrixIter temp1r = temp.m.begin();
660  for(G4int r=1;r<=mret.num_row();r++)
661  {
662    G4ErrorMatrixConstIter m11c = m1.m.begin();
663    for(G4int c=1;c<=r;c++)
664    {
665      G4double tmp = 0.0;
666      G4ErrorMatrixIter tempir = temp1r;
667      G4ErrorMatrixConstIter m1ic = m11c;
668      for(G4int i=1;i<=m1.num_row();i++)
669      {
670        tmp+=(*(tempir))*(*(m1ic));
671        tempir += n;
672        m1ic += n;
673      }
674      *(mrc++) = tmp;
675      m11c++;
676    }
677    temp1r++;
678  }
679  return mret;
680}
681
682void G4ErrorSymMatrix::invert(G4int &ifail)
683{ 
684  ifail = 0;
685
686  switch(nrow)
687  {
688  case 3:
689    {
690      G4double det, temp;
691      G4double t1, t2, t3;
692      G4double c11,c12,c13,c22,c23,c33;
693      c11 = (*(m.begin()+2)) * (*(m.begin()+5))
694          - (*(m.begin()+4)) * (*(m.begin()+4));
695      c12 = (*(m.begin()+4)) * (*(m.begin()+3))
696          - (*(m.begin()+1)) * (*(m.begin()+5));
697      c13 = (*(m.begin()+1)) * (*(m.begin()+4))
698          - (*(m.begin()+2)) * (*(m.begin()+3));
699      c22 = (*(m.begin()+5)) * (*m.begin())
700          - (*(m.begin()+3)) * (*(m.begin()+3));
701      c23 = (*(m.begin()+3)) * (*(m.begin()+1))
702          - (*(m.begin()+4)) * (*m.begin());
703      c33 = (*m.begin()) * (*(m.begin()+2))
704          - (*(m.begin()+1)) * (*(m.begin()+1));
705      t1 = std::fabs(*m.begin());
706      t2 = std::fabs(*(m.begin()+1));
707      t3 = std::fabs(*(m.begin()+3));
708      if (t1 >= t2)
709      {
710        if (t3 >= t1)
711        {
712          temp = *(m.begin()+3);
713          det = c23*c12-c22*c13;
714        }
715        else
716        {
717          temp = *m.begin();
718          det = c22*c33-c23*c23;
719        }
720      }
721      else if (t3 >= t2)
722      {
723        temp = *(m.begin()+3);
724        det = c23*c12-c22*c13;
725      }
726      else
727      {
728        temp = *(m.begin()+1);
729        det = c13*c23-c12*c33;
730      }
731      if (det==0)
732      {
733        ifail = 1;
734        return;
735      }
736      {
737        G4double s = temp/det;
738        G4ErrorMatrixIter mm = m.begin();
739        *(mm++) = s*c11;
740        *(mm++) = s*c12;
741        *(mm++) = s*c22;
742        *(mm++) = s*c13;
743        *(mm++) = s*c23;
744        *(mm) = s*c33;
745      }
746    }
747    break;
748 case 2:
749    {
750      G4double det, temp, s;
751      det = (*m.begin())*(*(m.begin()+2)) - (*(m.begin()+1))*(*(m.begin()+1));
752      if (det==0)
753      {
754        ifail = 1;
755        return;
756      }
757      s = 1.0/det;
758      *(m.begin()+1) *= -s;
759      temp = s*(*(m.begin()+2));
760      *(m.begin()+2) = s*(*m.begin());
761      *m.begin() = temp;
762      break;
763    }
764 case 1:
765    {
766      if ((*m.begin())==0)
767      {
768        ifail = 1;
769        return;
770      }
771      *m.begin() = 1.0/(*m.begin());
772      break;
773    }
774 case 5:
775    {
776      invert5(ifail);
777      return;
778    }
779 case 6:
780    {
781      invert6(ifail);
782      return;
783    }
784 case 4:
785    {
786      invert4(ifail);
787      return;
788    }
789 default:
790    {
791      invertBunchKaufman(ifail);
792      return;
793    }
794  }
795  return; // inversion successful
796}
797
798G4double G4ErrorSymMatrix::determinant() const
799{
800  static const G4int max_array = 20;
801
802  // ir must point to an array which is ***1 longer than*** nrow
803
804  static std::vector<G4int> ir_vec (max_array+1); 
805  if (ir_vec.size() <= static_cast<unsigned int>(nrow))
806  {
807    ir_vec.resize(nrow+1);
808  }
809  G4int * ir = &ir_vec[0];   
810
811  G4double det;
812  G4ErrorMatrix mt(*this);
813  G4int i = mt.dfact_matrix(det, ir);
814  if(i==0) { return det; }
815  return 0.0;
816}
817
818G4double G4ErrorSymMatrix::trace() const
819{
820   G4double t = 0.0;
821   for (G4int i=0; i<nrow; i++) 
822     { t += *(m.begin() + (i+3)*i/2); }
823   return t;
824}
825
826void G4ErrorSymMatrix::invertBunchKaufman(G4int &ifail)
827{
828  // Bunch-Kaufman diagonal pivoting method
829  // It is decribed in J.R. Bunch, L. Kaufman (1977).
830  // "Some Stable Methods for Calculating Inertia and Solving Symmetric
831  // Linear Systems", Math. Comp. 31, p. 162-179. or in Gene H. Golub,
832  // Charles F. van Loan, "Matrix Computations" (the second edition
833  // has a bug.) and implemented in "lapack"
834  // Mario Stanke, 09/97
835
836  G4int i, j, k, s;
837  G4int pivrow;
838
839  // Establish the two working-space arrays needed:  x and piv are
840  // used as pointers to arrays of doubles and ints respectively, each
841  // of length nrow.  We do not want to reallocate each time through
842  // unless the size needs to grow.  We do not want to leak memory, even
843  // by having a new without a delete that is only done once.
844 
845  static const G4int max_array = 25;
846  static std::vector<G4double> xvec (max_array);
847  static std::vector<G4int>    pivv (max_array);
848  typedef std::vector<G4int>::iterator pivIter; 
849  if (xvec.size() < static_cast<unsigned int>(nrow)) xvec.resize(nrow);
850  if (pivv.size() < static_cast<unsigned int>(nrow)) pivv.resize(nrow);
851    // Note - resize should do  nothing if the size is already larger than nrow,
852    //        but on VC++ there are indications that it does so we check.
853    // Note - the data elements in a vector are guaranteed to be contiguous,
854    //        so x[i] and piv[i] are optimally fast.
855  G4ErrorMatrixIter   x   = xvec.begin();
856    // x[i] is used as helper storage, needs to have at least size nrow.
857  pivIter piv = pivv.begin();
858    // piv[i] is used to store details of exchanges
859     
860  G4double temp1, temp2;
861  G4ErrorMatrixIter ip, mjj, iq;
862  G4double lambda, sigma;
863  const G4double alpha = .6404; // = (1+sqrt(17))/8
864  const G4double epsilon = 32*DBL_EPSILON;
865    // whenever a sum of two doubles is below or equal to epsilon
866    // it is set to zero.
867    // this constant could be set to zero but then the algorithm
868    // doesn't neccessarily detect that a matrix is singular
869 
870  for (i = 0; i < nrow; i++)
871  {
872    piv[i] = i+1;
873  }
874     
875  ifail = 0;
876     
877  // compute the factorization P*A*P^T = L * D * L^T
878  // L is unit lower triangular, D is direct sum of 1x1 and 2x2 matrices
879  // L and D^-1 are stored in A = *this, P is stored in piv[]
880       
881  for (j=1; j < nrow; j+=s)  // main loop over columns
882  {
883          mjj = m.begin() + j*(j-1)/2 + j-1;
884          lambda = 0;           // compute lambda = max of A(j+1:n,j)
885          pivrow = j+1;
886          ip = m.begin() + (j+1)*j/2 + j-1;
887          for (i=j+1; i <= nrow ; ip += i++)
888          {
889            if (std::fabs(*ip) > lambda)
890              {
891                lambda = std::fabs(*ip);
892                pivrow = i;
893              }
894          }
895          if (lambda == 0 )
896            {
897              if (*mjj == 0)
898                {
899                  ifail = 1;
900                  return;
901                }
902              s=1;
903              *mjj = 1./ *mjj;
904            }
905          else
906            {
907              if (std::fabs(*mjj) >= lambda*alpha)
908                {
909                  s=1;
910                  pivrow=j;
911                }
912              else
913                {
914                  sigma = 0;  // compute sigma = max A(pivrow, j:pivrow-1)
915                  ip = m.begin() + pivrow*(pivrow-1)/2+j-1;
916                  for (k=j; k < pivrow; k++)
917                    {
918                      if (std::fabs(*ip) > sigma)
919                        sigma = std::fabs(*ip);
920                      ip++;
921                    }
922                  if (sigma * std::fabs(*mjj) >= alpha * lambda * lambda)
923                    {
924                      s=1;
925                      pivrow = j;
926                    }
927                  else if (std::fabs(*(m.begin()+pivrow*(pivrow-1)/2+pivrow-1)) 
928                                >= alpha * sigma)
929                    { s=1; }
930                  else
931                    { s=2; }
932                }
933              if (pivrow == j)  // no permutation neccessary
934                {
935                  piv[j-1] = pivrow;
936                  if (*mjj == 0)
937                    {
938                      ifail=1;
939                      return;
940                    }
941                  temp2 = *mjj = 1./ *mjj; // invert D(j,j)
942                 
943                  // update A(j+1:n, j+1,n)
944                  for (i=j+1; i <= nrow; i++)
945                    {
946                      temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
947                      ip = m.begin()+i*(i-1)/2+j;
948                      for (k=j+1; k<=i; k++)
949                        {
950                          *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
951                          if (std::fabs(*ip) <= epsilon)
952                            { *ip=0; }
953                          ip++;
954                        }
955                    }
956                  // update L
957                  ip = m.begin() + (j+1)*j/2 + j-1; 
958                  for (i=j+1; i <= nrow; ip += i++)
959                  {
960                    *ip *= temp2;
961                  }
962                }
963              else if (s==1) // 1x1 pivot
964                {
965                  piv[j-1] = pivrow;
966                 
967                  // interchange rows and columns j and pivrow in
968                  // submatrix (j:n,j:n)
969                  ip = m.begin() + pivrow*(pivrow-1)/2 + j;
970                  for (i=j+1; i < pivrow; i++, ip++)
971                    {
972                      temp1 = *(m.begin() + i*(i-1)/2 + j-1);
973                      *(m.begin() + i*(i-1)/2 + j-1)= *ip;
974                      *ip = temp1;
975                    }
976                  temp1 = *mjj;
977                  *mjj = *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1);
978                  *(m.begin()+pivrow*(pivrow-1)/2+pivrow-1) = temp1;
979                  ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;
980                  iq = ip + pivrow-j;
981                  for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
982                    {
983                      temp1 = *iq;
984                      *iq = *ip;
985                      *ip = temp1;
986                    }
987                 
988                  if (*mjj == 0)
989                    {
990                      ifail = 1;
991                      return;
992                    }
993                  temp2 = *mjj = 1./ *mjj; // invert D(j,j)
994                 
995                  // update A(j+1:n, j+1:n)
996                  for (i = j+1; i <= nrow; i++)
997                    {
998                      temp1 = *(m.begin() + i*(i-1)/2 + j-1) * temp2;
999                      ip = m.begin()+i*(i-1)/2+j;
1000                      for (k=j+1; k<=i; k++)
1001                        {
1002                          *ip -= temp1 * *(m.begin() + k*(k-1)/2 + j-1);
1003                          if (std::fabs(*ip) <= epsilon)
1004                            { *ip=0; }
1005                          ip++;
1006                        }
1007                    }
1008                  // update L
1009                  ip = m.begin() + (j+1)*j/2 + j-1;
1010                  for (i=j+1; i<=nrow; ip += i++)
1011                  {
1012                    *ip *= temp2;
1013                  }
1014                }
1015              else // s=2, ie use a 2x2 pivot
1016                {
1017                  piv[j-1] = -pivrow;
1018                  piv[j] = 0; // that means this is the second row of a 2x2 pivot
1019                 
1020                  if (j+1 != pivrow) 
1021                    {
1022                      // interchange rows and columns j+1 and pivrow in
1023                      // submatrix (j:n,j:n)
1024                      ip = m.begin() + pivrow*(pivrow-1)/2 + j+1;
1025                      for (i=j+2; i < pivrow; i++, ip++)
1026                        {
1027                          temp1 = *(m.begin() + i*(i-1)/2 + j);
1028                          *(m.begin() + i*(i-1)/2 + j) = *ip;
1029                          *ip = temp1;
1030                        }
1031                      temp1 = *(mjj + j + 1);
1032                      *(mjj + j + 1) = 
1033                        *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1034                      *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1035                      temp1 = *(mjj + j);
1036                      *(mjj + j) = *(m.begin() + pivrow*(pivrow-1)/2 + j-1);
1037                      *(m.begin() + pivrow*(pivrow-1)/2 + j-1) = temp1;
1038                      ip = m.begin() + (pivrow+1)*pivrow/2 + j;
1039                      iq = ip + pivrow-(j+1);
1040                      for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
1041                        {
1042                          temp1 = *iq;
1043                          *iq = *ip;
1044                          *ip = temp1;
1045                        }
1046                    } 
1047                  // invert D(j:j+1,j:j+1)
1048                  temp2 = *mjj * *(mjj + j + 1) - *(mjj + j) * *(mjj + j); 
1049                  if (temp2 == 0)
1050                  {
1051                    G4cerr
1052                      << "G4ErrorSymMatrix::bunch_invert: error in pivot choice"
1053                      << G4endl;
1054                  }
1055                  temp2 = 1. / temp2;
1056
1057                  // this quotient is guaranteed to exist by the choice
1058                  // of the pivot
1059
1060                  temp1 = *mjj;
1061                  *mjj = *(mjj + j + 1) * temp2;
1062                  *(mjj + j + 1) = temp1 * temp2;
1063                  *(mjj + j) = - *(mjj + j) * temp2;
1064                 
1065                  if (j < nrow-1) // otherwise do nothing
1066                    {
1067                      // update A(j+2:n, j+2:n)
1068                      for (i=j+2; i <= nrow ; i++)
1069                        {
1070                          ip = m.begin() + i*(i-1)/2 + j-1;
1071                          temp1 = *ip * *mjj + *(ip + 1) * *(mjj + j);
1072                          if (std::fabs(temp1 ) <= epsilon)
1073                            { temp1 = 0; }
1074                          temp2 = *ip * *(mjj + j) + *(ip + 1) * *(mjj + j + 1);
1075                          if (std::fabs(temp2 ) <= epsilon)
1076                            { temp2 = 0; }
1077                          for (k = j+2; k <= i ; k++)
1078                            {
1079                              ip = m.begin() + i*(i-1)/2 + k-1;
1080                              iq = m.begin() + k*(k-1)/2 + j-1;
1081                              *ip -= temp1 * *iq + temp2 * *(iq+1);
1082                              if (std::fabs(*ip) <= epsilon)
1083                                { *ip = 0; }
1084                            }
1085                        }
1086                      // update L
1087                      for (i=j+2; i <= nrow ; i++)
1088                        {
1089                          ip = m.begin() + i*(i-1)/2 + j-1;
1090                          temp1 = *ip * *mjj + *(ip+1) * *(mjj + j);
1091                          if (std::fabs(temp1) <= epsilon)
1092                            { temp1 = 0; }
1093                          *(ip+1) = *ip * *(mjj + j) + *(ip+1) * *(mjj + j + 1);
1094                          if (std::fabs(*(ip+1)) <= epsilon)
1095                            { *(ip+1) = 0; }
1096                          *ip = temp1;
1097                        }
1098                    }
1099                }
1100            }
1101  } // end of main loop over columns
1102
1103  if (j == nrow) // the the last pivot is 1x1
1104  {
1105    mjj = m.begin() + j*(j-1)/2 + j-1;
1106    if (*mjj == 0)
1107    {
1108      ifail = 1;
1109      return;
1110    }
1111    else
1112    {
1113      *mjj = 1. / *mjj;
1114    }
1115  } // end of last pivot code
1116
1117  // computing the inverse from the factorization
1118         
1119  for (j = nrow ; j >= 1 ; j -= s) // loop over columns
1120  {
1121          mjj = m.begin() + j*(j-1)/2 + j-1;
1122          if (piv[j-1] > 0) // 1x1 pivot, compute column j of inverse
1123            {
1124              s = 1; 
1125              if (j < nrow)
1126                {
1127                  ip = m.begin() + (j+1)*j/2 + j-1;
1128                  for (i=0; i < nrow-j; ip += 1+j+i++)
1129                  {
1130                    x[i] = *ip;
1131                  }
1132                  for (i=j+1; i<=nrow ; i++)
1133                  {
1134                    temp2=0;
1135                    ip = m.begin() + i*(i-1)/2 + j;
1136                    for (k=0; k <= i-j-1; k++)
1137                      { temp2 += *ip++ * x[k]; }
1138                    for (ip += i-1; k < nrow-j; ip += 1+j+k++) 
1139                      { temp2 += *ip * x[k]; }
1140                    *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1141                  }
1142                  temp2 = 0;
1143                  ip = m.begin() + (j+1)*j/2 + j-1;
1144                  for (k=0; k < nrow-j; ip += 1+j+k++)
1145                    { temp2 += x[k] * *ip; }
1146                  *mjj -= temp2;
1147                }
1148            }
1149          else //2x2 pivot, compute columns j and j-1 of the inverse
1150            {
1151              if (piv[j-1] != 0)
1152                { G4cerr << "error in piv" << piv[j-1] << G4endl; }
1153              s=2; 
1154              if (j < nrow)
1155                {
1156                  ip = m.begin() + (j+1)*j/2 + j-1;
1157                  for (i=0; i < nrow-j; ip += 1+j+i++)
1158                  {
1159                    x[i] = *ip;
1160                  }
1161                  for (i=j+1; i<=nrow ; i++)
1162                  {
1163                    temp2 = 0;
1164                    ip = m.begin() + i*(i-1)/2 + j;
1165                    for (k=0; k <= i-j-1; k++)
1166                      { temp2 += *ip++ * x[k]; }
1167                    for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1168                      { temp2 += *ip * x[k]; }
1169                    *(m.begin()+ i*(i-1)/2 + j-1) = -temp2;
1170                  }   
1171                  temp2 = 0;
1172                  ip = m.begin() + (j+1)*j/2 + j-1;
1173                  for (k=0; k < nrow-j; ip += 1+j+k++)
1174                    { temp2 += x[k] * *ip; }
1175                  *mjj -= temp2;
1176                  temp2 = 0;
1177                  ip = m.begin() + (j+1)*j/2 + j-2;
1178                  for (i=j+1; i <= nrow; ip += i++)
1179                    { temp2 += *ip * *(ip+1); }
1180                  *(mjj-1) -= temp2;
1181                  ip = m.begin() + (j+1)*j/2 + j-2;
1182                  for (i=0; i < nrow-j; ip += 1+j+i++)
1183                  {
1184                    x[i] = *ip;
1185                  }
1186                  for (i=j+1; i <= nrow ; i++)
1187                  {
1188                    temp2 = 0;
1189                    ip = m.begin() + i*(i-1)/2 + j;
1190                    for (k=0; k <= i-j-1; k++)
1191                      { temp2 += *ip++ * x[k]; }
1192                    for (ip += i-1; k < nrow-j; ip += 1+j+k++)
1193                      { temp2 += *ip * x[k]; }
1194                    *(m.begin()+ i*(i-1)/2 + j-2)= -temp2;
1195                  }
1196                  temp2 = 0;
1197                  ip = m.begin() + (j+1)*j/2 + j-2;
1198                  for (k=0; k < nrow-j; ip += 1+j+k++)
1199                    { temp2 += x[k] * *ip; }
1200                  *(mjj-j) -= temp2;
1201                }
1202            } 
1203         
1204          // interchange rows and columns j and piv[j-1]
1205          // or rows and columns j and -piv[j-2]
1206         
1207          pivrow = (piv[j-1]==0)? -piv[j-2] : piv[j-1];
1208          ip = m.begin() + pivrow*(pivrow-1)/2 + j;
1209          for (i=j+1;i < pivrow; i++, ip++)
1210            {
1211              temp1 = *(m.begin() + i*(i-1)/2 + j-1);
1212              *(m.begin() + i*(i-1)/2 + j-1) = *ip;
1213              *ip = temp1;
1214            }
1215          temp1 = *mjj;
1216          *mjj = *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1);
1217          *(m.begin() + pivrow*(pivrow-1)/2 + pivrow-1) = temp1;
1218          if (s==2)
1219            {
1220              temp1 = *(mjj-1);
1221              *(mjj-1) = *( m.begin() + pivrow*(pivrow-1)/2 + j-2);
1222              *( m.begin() + pivrow*(pivrow-1)/2 + j-2) = temp1;
1223            }
1224         
1225          ip = m.begin() + (pivrow+1)*pivrow/2 + j-1;  // &A(i,j)
1226          iq = ip + pivrow-j;
1227          for (i = pivrow+1; i <= nrow; ip += i, iq += i++)
1228            {
1229              temp1 = *iq;
1230              *iq = *ip;
1231              *ip = temp1;
1232            } 
1233  } // end of loop over columns (in computing inverse from factorization)
1234
1235  return; // inversion successful
1236}
1237
1238G4double G4ErrorSymMatrix::posDefFraction5x5 = 1.0;
1239G4double G4ErrorSymMatrix::posDefFraction6x6 = 1.0;
1240G4double G4ErrorSymMatrix::adjustment5x5 = 0.0;
1241G4double G4ErrorSymMatrix::adjustment6x6 = 0.0;
1242const G4double G4ErrorSymMatrix::CHOLESKY_THRESHOLD_5x5 = .5;
1243const G4double G4ErrorSymMatrix::CHOLESKY_THRESHOLD_6x6 = .2;
1244const G4double G4ErrorSymMatrix::CHOLESKY_CREEP_5x5 = .005;
1245const G4double G4ErrorSymMatrix::CHOLESKY_CREEP_6x6 = .002;
1246
1247// Aij are indices for a 6x6 symmetric matrix.
1248//     The indices for 5x5 or 4x4 symmetric matrices are the same,
1249//     ignoring all combinations with an index which is inapplicable.
1250
1251#define A00 0
1252#define A01 1
1253#define A02 3
1254#define A03 6
1255#define A04 10
1256#define A05 15
1257
1258#define A10 1
1259#define A11 2
1260#define A12 4
1261#define A13 7
1262#define A14 11
1263#define A15 16
1264
1265#define A20 3
1266#define A21 4
1267#define A22 5
1268#define A23 8
1269#define A24 12
1270#define A25 17
1271
1272#define A30 6
1273#define A31 7
1274#define A32 8
1275#define A33 9
1276#define A34 13
1277#define A35 18
1278
1279#define A40 10
1280#define A41 11
1281#define A42 12
1282#define A43 13
1283#define A44 14
1284#define A45 19
1285
1286#define A50 15
1287#define A51 16
1288#define A52 17
1289#define A53 18
1290#define A54 19
1291#define A55 20
1292
1293void G4ErrorSymMatrix::invert5(G4int & ifail)
1294{ 
1295      if (posDefFraction5x5 >= CHOLESKY_THRESHOLD_5x5)
1296      {
1297        invertCholesky5(ifail);
1298        posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail);
1299        if (ifail!=0)    // Cholesky failed -- invert using Haywood
1300        {
1301          invertHaywood5(ifail);     
1302        }
1303      }
1304      else
1305      {
1306        if (posDefFraction5x5 + adjustment5x5 >= CHOLESKY_THRESHOLD_5x5)
1307        {
1308          invertCholesky5(ifail);
1309          posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail);
1310          if (ifail!=0)    // Cholesky failed -- invert using Haywood
1311          {
1312            invertHaywood5(ifail);     
1313            adjustment5x5 = 0;
1314          }
1315        }
1316        else
1317        {
1318          invertHaywood5(ifail);     
1319          adjustment5x5 += CHOLESKY_CREEP_5x5;
1320        }
1321      }
1322      return;
1323}
1324
1325void G4ErrorSymMatrix::invert6(G4int & ifail)
1326{
1327      if (posDefFraction6x6 >= CHOLESKY_THRESHOLD_6x6)
1328      {
1329        invertCholesky6(ifail);
1330        posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail);
1331        if (ifail!=0)    // Cholesky failed -- invert using Haywood
1332        {
1333          invertHaywood6(ifail);     
1334        }
1335      }
1336      else
1337      {
1338        if (posDefFraction6x6 + adjustment6x6 >= CHOLESKY_THRESHOLD_6x6)
1339        {
1340          invertCholesky6(ifail);
1341          posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail);
1342          if (ifail!=0)    // Cholesky failed -- invert using Haywood
1343          {
1344            invertHaywood6(ifail);     
1345            adjustment6x6 = 0;
1346          }
1347        }
1348        else
1349        {
1350          invertHaywood6(ifail);     
1351          adjustment6x6 += CHOLESKY_CREEP_6x6;
1352        }
1353      }
1354      return;
1355}
1356
1357void G4ErrorSymMatrix::invertHaywood5  (G4int & ifail)
1358{
1359  ifail = 0;
1360
1361  // Find all NECESSARY 2x2 dets:  (25 of them)
1362
1363  G4double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30];
1364  G4double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30];
1365  G4double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30];
1366  G4double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31];
1367  G4double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31];
1368  G4double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32];
1369  G4double Det2_24_01 = m[A20]*m[A41] - m[A21]*m[A40];
1370  G4double Det2_24_02 = m[A20]*m[A42] - m[A22]*m[A40];
1371  G4double Det2_24_03 = m[A20]*m[A43] - m[A23]*m[A40];
1372  G4double Det2_24_04 = m[A20]*m[A44] - m[A24]*m[A40];
1373  G4double Det2_24_12 = m[A21]*m[A42] - m[A22]*m[A41];
1374  G4double Det2_24_13 = m[A21]*m[A43] - m[A23]*m[A41];
1375  G4double Det2_24_14 = m[A21]*m[A44] - m[A24]*m[A41];
1376  G4double Det2_24_23 = m[A22]*m[A43] - m[A23]*m[A42];
1377  G4double Det2_24_24 = m[A22]*m[A44] - m[A24]*m[A42];
1378  G4double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40];
1379  G4double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40];
1380  G4double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40];
1381  G4double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40];
1382  G4double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41];
1383  G4double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41];
1384  G4double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41];
1385  G4double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42];
1386  G4double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42];
1387  G4double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43];
1388
1389  // Find all NECESSARY 3x3 dets:   (30 of them)
1390
1391  G4double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02
1392                                + m[A12]*Det2_23_01;
1393  G4double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03
1394                                + m[A13]*Det2_23_01;
1395  G4double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03
1396                                + m[A13]*Det2_23_02;
1397  G4double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13
1398                                + m[A13]*Det2_23_12;
1399  G4double Det3_124_012 = m[A10]*Det2_24_12 - m[A11]*Det2_24_02
1400                                + m[A12]*Det2_24_01;
1401  G4double Det3_124_013 = m[A10]*Det2_24_13 - m[A11]*Det2_24_03
1402                                + m[A13]*Det2_24_01;
1403  G4double Det3_124_014 = m[A10]*Det2_24_14 - m[A11]*Det2_24_04
1404                                + m[A14]*Det2_24_01;
1405  G4double Det3_124_023 = m[A10]*Det2_24_23 - m[A12]*Det2_24_03
1406                                + m[A13]*Det2_24_02;
1407  G4double Det3_124_024 = m[A10]*Det2_24_24 - m[A12]*Det2_24_04
1408                                + m[A14]*Det2_24_02;
1409  G4double Det3_124_123 = m[A11]*Det2_24_23 - m[A12]*Det2_24_13
1410                                + m[A13]*Det2_24_12;
1411  G4double Det3_124_124 = m[A11]*Det2_24_24 - m[A12]*Det2_24_14
1412                                + m[A14]*Det2_24_12;
1413  G4double Det3_134_012 = m[A10]*Det2_34_12 - m[A11]*Det2_34_02
1414                                + m[A12]*Det2_34_01;
1415  G4double Det3_134_013 = m[A10]*Det2_34_13 - m[A11]*Det2_34_03
1416                                + m[A13]*Det2_34_01;
1417  G4double Det3_134_014 = m[A10]*Det2_34_14 - m[A11]*Det2_34_04
1418                                + m[A14]*Det2_34_01;
1419  G4double Det3_134_023 = m[A10]*Det2_34_23 - m[A12]*Det2_34_03
1420                                + m[A13]*Det2_34_02;
1421  G4double Det3_134_024 = m[A10]*Det2_34_24 - m[A12]*Det2_34_04
1422                                + m[A14]*Det2_34_02;
1423  G4double Det3_134_034 = m[A10]*Det2_34_34 - m[A13]*Det2_34_04
1424                                + m[A14]*Det2_34_03;
1425  G4double Det3_134_123 = m[A11]*Det2_34_23 - m[A12]*Det2_34_13
1426                                + m[A13]*Det2_34_12;
1427  G4double Det3_134_124 = m[A11]*Det2_34_24 - m[A12]*Det2_34_14
1428                                + m[A14]*Det2_34_12;
1429  G4double Det3_134_134 = m[A11]*Det2_34_34 - m[A13]*Det2_34_14
1430                                + m[A14]*Det2_34_13;
1431  G4double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02
1432                                + m[A22]*Det2_34_01;
1433  G4double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03
1434                                + m[A23]*Det2_34_01;
1435  G4double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04
1436                                + m[A24]*Det2_34_01;
1437  G4double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03
1438                                + m[A23]*Det2_34_02;
1439  G4double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04
1440                                + m[A24]*Det2_34_02;
1441  G4double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04
1442                                + m[A24]*Det2_34_03;
1443  G4double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13
1444                                + m[A23]*Det2_34_12;
1445  G4double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14
1446                                + m[A24]*Det2_34_12;
1447  G4double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14
1448                                + m[A24]*Det2_34_13;
1449  G4double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24
1450                                + m[A24]*Det2_34_23;
1451
1452  // Find all NECESSARY 4x4 dets:   (15 of them)
1453
1454  G4double Det4_0123_0123 = m[A00]*Det3_123_123 - m[A01]*Det3_123_023
1455                                + m[A02]*Det3_123_013 - m[A03]*Det3_123_012;
1456  G4double Det4_0124_0123 = m[A00]*Det3_124_123 - m[A01]*Det3_124_023
1457                                + m[A02]*Det3_124_013 - m[A03]*Det3_124_012;
1458  G4double Det4_0124_0124 = m[A00]*Det3_124_124 - m[A01]*Det3_124_024
1459                                + m[A02]*Det3_124_014 - m[A04]*Det3_124_012;
1460  G4double Det4_0134_0123 = m[A00]*Det3_134_123 - m[A01]*Det3_134_023
1461                                + m[A02]*Det3_134_013 - m[A03]*Det3_134_012;
1462  G4double Det4_0134_0124 = m[A00]*Det3_134_124 - m[A01]*Det3_134_024
1463                                + m[A02]*Det3_134_014 - m[A04]*Det3_134_012;
1464  G4double Det4_0134_0134 = m[A00]*Det3_134_134 - m[A01]*Det3_134_034
1465                                + m[A03]*Det3_134_014 - m[A04]*Det3_134_013;
1466  G4double Det4_0234_0123 = m[A00]*Det3_234_123 - m[A01]*Det3_234_023
1467                                + m[A02]*Det3_234_013 - m[A03]*Det3_234_012;
1468  G4double Det4_0234_0124 = m[A00]*Det3_234_124 - m[A01]*Det3_234_024
1469                                + m[A02]*Det3_234_014 - m[A04]*Det3_234_012;
1470  G4double Det4_0234_0134 = m[A00]*Det3_234_134 - m[A01]*Det3_234_034
1471                                + m[A03]*Det3_234_014 - m[A04]*Det3_234_013;
1472  G4double Det4_0234_0234 = m[A00]*Det3_234_234 - m[A02]*Det3_234_034
1473                                + m[A03]*Det3_234_024 - m[A04]*Det3_234_023;
1474  G4double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023
1475                                + m[A12]*Det3_234_013 - m[A13]*Det3_234_012;
1476  G4double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024
1477                                + m[A12]*Det3_234_014 - m[A14]*Det3_234_012;
1478  G4double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034
1479                                + m[A13]*Det3_234_014 - m[A14]*Det3_234_013;
1480  G4double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034
1481                                + m[A13]*Det3_234_024 - m[A14]*Det3_234_023;
1482  G4double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134
1483                                + m[A13]*Det3_234_124 - m[A14]*Det3_234_123;
1484
1485  // Find the 5x5 det:
1486
1487  G4double det =  m[A00]*Det4_1234_1234
1488                - m[A01]*Det4_1234_0234
1489                + m[A02]*Det4_1234_0134
1490                - m[A03]*Det4_1234_0124
1491                + m[A04]*Det4_1234_0123;
1492
1493  if ( det == 0 )
1494  { 
1495    ifail = 1;
1496    return;
1497  } 
1498
1499  G4double oneOverDet = 1.0/det;
1500  G4double mn1OverDet = - oneOverDet;
1501
1502  m[A00] =  Det4_1234_1234 * oneOverDet;
1503  m[A01] =  Det4_1234_0234 * mn1OverDet;
1504  m[A02] =  Det4_1234_0134 * oneOverDet;
1505  m[A03] =  Det4_1234_0124 * mn1OverDet;
1506  m[A04] =  Det4_1234_0123 * oneOverDet;
1507
1508  m[A11] =  Det4_0234_0234 * oneOverDet;
1509  m[A12] =  Det4_0234_0134 * mn1OverDet;
1510  m[A13] =  Det4_0234_0124 * oneOverDet;
1511  m[A14] =  Det4_0234_0123 * mn1OverDet;
1512
1513  m[A22] =  Det4_0134_0134 * oneOverDet;
1514  m[A23] =  Det4_0134_0124 * mn1OverDet;
1515  m[A24] =  Det4_0134_0123 * oneOverDet;
1516
1517  m[A33] =  Det4_0124_0124 * oneOverDet;
1518  m[A34] =  Det4_0124_0123 * mn1OverDet;
1519
1520  m[A44] =  Det4_0123_0123 * oneOverDet;
1521
1522  return;
1523}
1524
1525void G4ErrorSymMatrix::invertHaywood6  (G4int & ifail)
1526{
1527  ifail = 0;
1528
1529  // Find all NECESSARY 2x2 dets:  (39 of them)
1530
1531  G4double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40];
1532  G4double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40];
1533  G4double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40];
1534  G4double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40];
1535  G4double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41];
1536  G4double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41];
1537  G4double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41];
1538  G4double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42];
1539  G4double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42];
1540  G4double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43];
1541  G4double Det2_35_01 = m[A30]*m[A51] - m[A31]*m[A50];
1542  G4double Det2_35_02 = m[A30]*m[A52] - m[A32]*m[A50];
1543  G4double Det2_35_03 = m[A30]*m[A53] - m[A33]*m[A50];
1544  G4double Det2_35_04 = m[A30]*m[A54] - m[A34]*m[A50];
1545  G4double Det2_35_05 = m[A30]*m[A55] - m[A35]*m[A50];
1546  G4double Det2_35_12 = m[A31]*m[A52] - m[A32]*m[A51];
1547  G4double Det2_35_13 = m[A31]*m[A53] - m[A33]*m[A51];
1548  G4double Det2_35_14 = m[A31]*m[A54] - m[A34]*m[A51];
1549  G4double Det2_35_15 = m[A31]*m[A55] - m[A35]*m[A51];
1550  G4double Det2_35_23 = m[A32]*m[A53] - m[A33]*m[A52];
1551  G4double Det2_35_24 = m[A32]*m[A54] - m[A34]*m[A52];
1552  G4double Det2_35_25 = m[A32]*m[A55] - m[A35]*m[A52];
1553  G4double Det2_35_34 = m[A33]*m[A54] - m[A34]*m[A53];
1554  G4double Det2_35_35 = m[A33]*m[A55] - m[A35]*m[A53];
1555  G4double Det2_45_01 = m[A40]*m[A51] - m[A41]*m[A50];
1556  G4double Det2_45_02 = m[A40]*m[A52] - m[A42]*m[A50];
1557  G4double Det2_45_03 = m[A40]*m[A53] - m[A43]*m[A50];
1558  G4double Det2_45_04 = m[A40]*m[A54] - m[A44]*m[A50];
1559  G4double Det2_45_05 = m[A40]*m[A55] - m[A45]*m[A50];
1560  G4double Det2_45_12 = m[A41]*m[A52] - m[A42]*m[A51];
1561  G4double Det2_45_13 = m[A41]*m[A53] - m[A43]*m[A51];
1562  G4double Det2_45_14 = m[A41]*m[A54] - m[A44]*m[A51];
1563  G4double Det2_45_15 = m[A41]*m[A55] - m[A45]*m[A51];
1564  G4double Det2_45_23 = m[A42]*m[A53] - m[A43]*m[A52];
1565  G4double Det2_45_24 = m[A42]*m[A54] - m[A44]*m[A52];
1566  G4double Det2_45_25 = m[A42]*m[A55] - m[A45]*m[A52];
1567  G4double Det2_45_34 = m[A43]*m[A54] - m[A44]*m[A53];
1568  G4double Det2_45_35 = m[A43]*m[A55] - m[A45]*m[A53];
1569  G4double Det2_45_45 = m[A44]*m[A55] - m[A45]*m[A54];
1570
1571  // Find all NECESSARY 3x3 dets:  (65 of them)
1572
1573  G4double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02
1574                                                + m[A22]*Det2_34_01;
1575  G4double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03
1576                                                + m[A23]*Det2_34_01;
1577  G4double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04
1578                                                + m[A24]*Det2_34_01;
1579  G4double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03
1580                                                + m[A23]*Det2_34_02;
1581  G4double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04
1582                                                + m[A24]*Det2_34_02;
1583  G4double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04
1584                                                + m[A24]*Det2_34_03;
1585  G4double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13
1586                                                + m[A23]*Det2_34_12;
1587  G4double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14
1588                                                + m[A24]*Det2_34_12;
1589  G4double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14
1590                                                + m[A24]*Det2_34_13;
1591  G4double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24
1592                                                + m[A24]*Det2_34_23;
1593  G4double Det3_235_012 = m[A20]*Det2_35_12 - m[A21]*Det2_35_02
1594                                                + m[A22]*Det2_35_01;
1595  G4double Det3_235_013 = m[A20]*Det2_35_13 - m[A21]*Det2_35_03
1596                                                + m[A23]*Det2_35_01;
1597  G4double Det3_235_014 = m[A20]*Det2_35_14 - m[A21]*Det2_35_04
1598                                                + m[A24]*Det2_35_01;
1599  G4double Det3_235_015 = m[A20]*Det2_35_15 - m[A21]*Det2_35_05
1600                                                + m[A25]*Det2_35_01;
1601  G4double Det3_235_023 = m[A20]*Det2_35_23 - m[A22]*Det2_35_03
1602                                                + m[A23]*Det2_35_02;
1603  G4double Det3_235_024 = m[A20]*Det2_35_24 - m[A22]*Det2_35_04
1604                                                + m[A24]*Det2_35_02;
1605  G4double Det3_235_025 = m[A20]*Det2_35_25 - m[A22]*Det2_35_05
1606                                                + m[A25]*Det2_35_02;
1607  G4double Det3_235_034 = m[A20]*Det2_35_34 - m[A23]*Det2_35_04
1608                                                + m[A24]*Det2_35_03;
1609  G4double Det3_235_035 = m[A20]*Det2_35_35 - m[A23]*Det2_35_05
1610                                                + m[A25]*Det2_35_03;
1611  G4double Det3_235_123 = m[A21]*Det2_35_23 - m[A22]*Det2_35_13
1612                                                + m[A23]*Det2_35_12;
1613  G4double Det3_235_124 = m[A21]*Det2_35_24 - m[A22]*Det2_35_14
1614                                                + m[A24]*Det2_35_12;
1615  G4double Det3_235_125 = m[A21]*Det2_35_25 - m[A22]*Det2_35_15
1616                                                + m[A25]*Det2_35_12;
1617  G4double Det3_235_134 = m[A21]*Det2_35_34 - m[A23]*Det2_35_14
1618                                                + m[A24]*Det2_35_13;
1619  G4double Det3_235_135 = m[A21]*Det2_35_35 - m[A23]*Det2_35_15
1620                                                + m[A25]*Det2_35_13;
1621  G4double Det3_235_234 = m[A22]*Det2_35_34 - m[A23]*Det2_35_24
1622                                                + m[A24]*Det2_35_23;
1623  G4double Det3_235_235 = m[A22]*Det2_35_35 - m[A23]*Det2_35_25
1624                                                + m[A25]*Det2_35_23;
1625  G4double Det3_245_012 = m[A20]*Det2_45_12 - m[A21]*Det2_45_02
1626                                                + m[A22]*Det2_45_01;
1627  G4double Det3_245_013 = m[A20]*Det2_45_13 - m[A21]*Det2_45_03
1628                                                + m[A23]*Det2_45_01;
1629  G4double Det3_245_014 = m[A20]*Det2_45_14 - m[A21]*Det2_45_04
1630                                                + m[A24]*Det2_45_01;
1631  G4double Det3_245_015 = m[A20]*Det2_45_15 - m[A21]*Det2_45_05
1632                                                + m[A25]*Det2_45_01;
1633  G4double Det3_245_023 = m[A20]*Det2_45_23 - m[A22]*Det2_45_03
1634                                                + m[A23]*Det2_45_02;
1635  G4double Det3_245_024 = m[A20]*Det2_45_24 - m[A22]*Det2_45_04
1636                                                + m[A24]*Det2_45_02;
1637  G4double Det3_245_025 = m[A20]*Det2_45_25 - m[A22]*Det2_45_05
1638                                                + m[A25]*Det2_45_02;
1639  G4double Det3_245_034 = m[A20]*Det2_45_34 - m[A23]*Det2_45_04
1640                                                + m[A24]*Det2_45_03;
1641  G4double Det3_245_035 = m[A20]*Det2_45_35 - m[A23]*Det2_45_05
1642                                                + m[A25]*Det2_45_03;
1643  G4double Det3_245_045 = m[A20]*Det2_45_45 - m[A24]*Det2_45_05
1644                                                + m[A25]*Det2_45_04;
1645  G4double Det3_245_123 = m[A21]*Det2_45_23 - m[A22]*Det2_45_13
1646                                                + m[A23]*Det2_45_12;
1647  G4double Det3_245_124 = m[A21]*Det2_45_24 - m[A22]*Det2_45_14
1648                                                + m[A24]*Det2_45_12;
1649  G4double Det3_245_125 = m[A21]*Det2_45_25 - m[A22]*Det2_45_15
1650                                                + m[A25]*Det2_45_12;
1651  G4double Det3_245_134 = m[A21]*Det2_45_34 - m[A23]*Det2_45_14
1652                                                + m[A24]*Det2_45_13;
1653  G4double Det3_245_135 = m[A21]*Det2_45_35 - m[A23]*Det2_45_15
1654                                                + m[A25]*Det2_45_13;
1655  G4double Det3_245_145 = m[A21]*Det2_45_45 - m[A24]*Det2_45_15
1656                                                + m[A25]*Det2_45_14;
1657  G4double Det3_245_234 = m[A22]*Det2_45_34 - m[A23]*Det2_45_24
1658                                                + m[A24]*Det2_45_23;
1659  G4double Det3_245_235 = m[A22]*Det2_45_35 - m[A23]*Det2_45_25
1660                                                + m[A25]*Det2_45_23;
1661  G4double Det3_245_245 = m[A22]*Det2_45_45 - m[A24]*Det2_45_25
1662                                                + m[A25]*Det2_45_24;
1663  G4double Det3_345_012 = m[A30]*Det2_45_12 - m[A31]*Det2_45_02
1664                                                + m[A32]*Det2_45_01;
1665  G4double Det3_345_013 = m[A30]*Det2_45_13 - m[A31]*Det2_45_03
1666                                                + m[A33]*Det2_45_01;
1667  G4double Det3_345_014 = m[A30]*Det2_45_14 - m[A31]*Det2_45_04
1668                                                + m[A34]*Det2_45_01;
1669  G4double Det3_345_015 = m[A30]*Det2_45_15 - m[A31]*Det2_45_05
1670                                                + m[A35]*Det2_45_01;
1671  G4double Det3_345_023 = m[A30]*Det2_45_23 - m[A32]*Det2_45_03
1672                                                + m[A33]*Det2_45_02;
1673  G4double Det3_345_024 = m[A30]*Det2_45_24 - m[A32]*Det2_45_04
1674                                                + m[A34]*Det2_45_02;
1675  G4double Det3_345_025 = m[A30]*Det2_45_25 - m[A32]*Det2_45_05
1676                                                + m[A35]*Det2_45_02;
1677  G4double Det3_345_034 = m[A30]*Det2_45_34 - m[A33]*Det2_45_04
1678                                                + m[A34]*Det2_45_03;
1679  G4double Det3_345_035 = m[A30]*Det2_45_35 - m[A33]*Det2_45_05
1680                                                + m[A35]*Det2_45_03;
1681  G4double Det3_345_045 = m[A30]*Det2_45_45 - m[A34]*Det2_45_05
1682                                                + m[A35]*Det2_45_04;
1683  G4double Det3_345_123 = m[A31]*Det2_45_23 - m[A32]*Det2_45_13
1684                                                + m[A33]*Det2_45_12;
1685  G4double Det3_345_124 = m[A31]*Det2_45_24 - m[A32]*Det2_45_14
1686                                                + m[A34]*Det2_45_12;
1687  G4double Det3_345_125 = m[A31]*Det2_45_25 - m[A32]*Det2_45_15
1688                                                + m[A35]*Det2_45_12;
1689  G4double Det3_345_134 = m[A31]*Det2_45_34 - m[A33]*Det2_45_14
1690                                                + m[A34]*Det2_45_13;
1691  G4double Det3_345_135 = m[A31]*Det2_45_35 - m[A33]*Det2_45_15
1692                                                + m[A35]*Det2_45_13;
1693  G4double Det3_345_145 = m[A31]*Det2_45_45 - m[A34]*Det2_45_15
1694                                                + m[A35]*Det2_45_14;
1695  G4double Det3_345_234 = m[A32]*Det2_45_34 - m[A33]*Det2_45_24
1696                                                + m[A34]*Det2_45_23;
1697  G4double Det3_345_235 = m[A32]*Det2_45_35 - m[A33]*Det2_45_25
1698                                                + m[A35]*Det2_45_23;
1699  G4double Det3_345_245 = m[A32]*Det2_45_45 - m[A34]*Det2_45_25
1700                                                + m[A35]*Det2_45_24;
1701  G4double Det3_345_345 = m[A33]*Det2_45_45 - m[A34]*Det2_45_35
1702                                                + m[A35]*Det2_45_34;
1703
1704  // Find all NECESSARY 4x4 dets:  (55 of them)
1705
1706  G4double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023
1707                        + m[A12]*Det3_234_013 - m[A13]*Det3_234_012;
1708  G4double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024
1709                        + m[A12]*Det3_234_014 - m[A14]*Det3_234_012;
1710  G4double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034
1711                        + m[A13]*Det3_234_014 - m[A14]*Det3_234_013;
1712  G4double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034
1713                        + m[A13]*Det3_234_024 - m[A14]*Det3_234_023;
1714  G4double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134
1715                        + m[A13]*Det3_234_124 - m[A14]*Det3_234_123;
1716  G4double Det4_1235_0123 = m[A10]*Det3_235_123 - m[A11]*Det3_235_023
1717                        + m[A12]*Det3_235_013 - m[A13]*Det3_235_012;
1718  G4double Det4_1235_0124 = m[A10]*Det3_235_124 - m[A11]*Det3_235_024
1719                        + m[A12]*Det3_235_014 - m[A14]*Det3_235_012;
1720  G4double Det4_1235_0125 = m[A10]*Det3_235_125 - m[A11]*Det3_235_025
1721                        + m[A12]*Det3_235_015 - m[A15]*Det3_235_012;
1722  G4double Det4_1235_0134 = m[A10]*Det3_235_134 - m[A11]*Det3_235_034
1723                        + m[A13]*Det3_235_014 - m[A14]*Det3_235_013;
1724  G4double Det4_1235_0135 = m[A10]*Det3_235_135 - m[A11]*Det3_235_035
1725                        + m[A13]*Det3_235_015 - m[A15]*Det3_235_013;
1726  G4double Det4_1235_0234 = m[A10]*Det3_235_234 - m[A12]*Det3_235_034
1727                        + m[A13]*Det3_235_024 - m[A14]*Det3_235_023;
1728  G4double Det4_1235_0235 = m[A10]*Det3_235_235 - m[A12]*Det3_235_035
1729                        + m[A13]*Det3_235_025 - m[A15]*Det3_235_023;
1730  G4double Det4_1235_1234 = m[A11]*Det3_235_234 - m[A12]*Det3_235_134
1731                        + m[A13]*Det3_235_124 - m[A14]*Det3_235_123;
1732  G4double Det4_1235_1235 = m[A11]*Det3_235_235 - m[A12]*Det3_235_135
1733                        + m[A13]*Det3_235_125 - m[A15]*Det3_235_123;
1734  G4double Det4_1245_0123 = m[A10]*Det3_245_123 - m[A11]*Det3_245_023
1735                        + m[A12]*Det3_245_013 - m[A13]*Det3_245_012;
1736  G4double Det4_1245_0124 = m[A10]*Det3_245_124 - m[A11]*Det3_245_024
1737                        + m[A12]*Det3_245_014 - m[A14]*Det3_245_012;
1738  G4double Det4_1245_0125 = m[A10]*Det3_245_125 - m[A11]*Det3_245_025
1739                        + m[A12]*Det3_245_015 - m[A15]*Det3_245_012;
1740  G4double Det4_1245_0134 = m[A10]*Det3_245_134 - m[A11]*Det3_245_034
1741                        + m[A13]*Det3_245_014 - m[A14]*Det3_245_013;
1742  G4double Det4_1245_0135 = m[A10]*Det3_245_135 - m[A11]*Det3_245_035
1743                        + m[A13]*Det3_245_015 - m[A15]*Det3_245_013;
1744  G4double Det4_1245_0145 = m[A10]*Det3_245_145 - m[A11]*Det3_245_045
1745                        + m[A14]*Det3_245_015 - m[A15]*Det3_245_014;
1746  G4double Det4_1245_0234 = m[A10]*Det3_245_234 - m[A12]*Det3_245_034
1747                        + m[A13]*Det3_245_024 - m[A14]*Det3_245_023;
1748  G4double Det4_1245_0235 = m[A10]*Det3_245_235 - m[A12]*Det3_245_035
1749                        + m[A13]*Det3_245_025 - m[A15]*Det3_245_023;
1750  G4double Det4_1245_0245 = m[A10]*Det3_245_245 - m[A12]*Det3_245_045
1751                        + m[A14]*Det3_245_025 - m[A15]*Det3_245_024;
1752  G4double Det4_1245_1234 = m[A11]*Det3_245_234 - m[A12]*Det3_245_134
1753                        + m[A13]*Det3_245_124 - m[A14]*Det3_245_123;
1754  G4double Det4_1245_1235 = m[A11]*Det3_245_235 - m[A12]*Det3_245_135
1755                        + m[A13]*Det3_245_125 - m[A15]*Det3_245_123;
1756  G4double Det4_1245_1245 = m[A11]*Det3_245_245 - m[A12]*Det3_245_145
1757                        + m[A14]*Det3_245_125 - m[A15]*Det3_245_124;
1758  G4double Det4_1345_0123 = m[A10]*Det3_345_123 - m[A11]*Det3_345_023
1759                        + m[A12]*Det3_345_013 - m[A13]*Det3_345_012;
1760  G4double Det4_1345_0124 = m[A10]*Det3_345_124 - m[A11]*Det3_345_024
1761                        + m[A12]*Det3_345_014 - m[A14]*Det3_345_012;
1762  G4double Det4_1345_0125 = m[A10]*Det3_345_125 - m[A11]*Det3_345_025
1763                        + m[A12]*Det3_345_015 - m[A15]*Det3_345_012;
1764  G4double Det4_1345_0134 = m[A10]*Det3_345_134 - m[A11]*Det3_345_034
1765                        + m[A13]*Det3_345_014 - m[A14]*Det3_345_013;
1766  G4double Det4_1345_0135 = m[A10]*Det3_345_135 - m[A11]*Det3_345_035
1767                        + m[A13]*Det3_345_015 - m[A15]*Det3_345_013;
1768  G4double Det4_1345_0145 = m[A10]*Det3_345_145 - m[A11]*Det3_345_045
1769                        + m[A14]*Det3_345_015 - m[A15]*Det3_345_014;
1770  G4double Det4_1345_0234 = m[A10]*Det3_345_234 - m[A12]*Det3_345_034
1771                        + m[A13]*Det3_345_024 - m[A14]*Det3_345_023;
1772  G4double Det4_1345_0235 = m[A10]*Det3_345_235 - m[A12]*Det3_345_035
1773                        + m[A13]*Det3_345_025 - m[A15]*Det3_345_023;
1774  G4double Det4_1345_0245 = m[A10]*Det3_345_245 - m[A12]*Det3_345_045
1775                        + m[A14]*Det3_345_025 - m[A15]*Det3_345_024;
1776  G4double Det4_1345_0345 = m[A10]*Det3_345_345 - m[A13]*Det3_345_045
1777                        + m[A14]*Det3_345_035 - m[A15]*Det3_345_034;
1778  G4double Det4_1345_1234 = m[A11]*Det3_345_234 - m[A12]*Det3_345_134
1779                        + m[A13]*Det3_345_124 - m[A14]*Det3_345_123;
1780  G4double Det4_1345_1235 = m[A11]*Det3_345_235 - m[A12]*Det3_345_135
1781                        + m[A13]*Det3_345_125 - m[A15]*Det3_345_123;
1782  G4double Det4_1345_1245 = m[A11]*Det3_345_245 - m[A12]*Det3_345_145
1783                        + m[A14]*Det3_345_125 - m[A15]*Det3_345_124;
1784  G4double Det4_1345_1345 = m[A11]*Det3_345_345 - m[A13]*Det3_345_145
1785                        + m[A14]*Det3_345_135 - m[A15]*Det3_345_134;
1786  G4double Det4_2345_0123 = m[A20]*Det3_345_123 - m[A21]*Det3_345_023
1787                        + m[A22]*Det3_345_013 - m[A23]*Det3_345_012;
1788  G4double Det4_2345_0124 = m[A20]*Det3_345_124 - m[A21]*Det3_345_024
1789                        + m[A22]*Det3_345_014 - m[A24]*Det3_345_012;
1790  G4double Det4_2345_0125 = m[A20]*Det3_345_125 - m[A21]*Det3_345_025
1791                        + m[A22]*Det3_345_015 - m[A25]*Det3_345_012;
1792  G4double Det4_2345_0134 = m[A20]*Det3_345_134 - m[A21]*Det3_345_034
1793                        + m[A23]*Det3_345_014 - m[A24]*Det3_345_013;
1794  G4double Det4_2345_0135 = m[A20]*Det3_345_135 - m[A21]*Det3_345_035
1795                        + m[A23]*Det3_345_015 - m[A25]*Det3_345_013;
1796  G4double Det4_2345_0145 = m[A20]*Det3_345_145 - m[A21]*Det3_345_045
1797                        + m[A24]*Det3_345_015 - m[A25]*Det3_345_014;
1798  G4double Det4_2345_0234 = m[A20]*Det3_345_234 - m[A22]*Det3_345_034
1799                        + m[A23]*Det3_345_024 - m[A24]*Det3_345_023;
1800  G4double Det4_2345_0235 = m[A20]*Det3_345_235 - m[A22]*Det3_345_035
1801                        + m[A23]*Det3_345_025 - m[A25]*Det3_345_023;
1802  G4double Det4_2345_0245 = m[A20]*Det3_345_245 - m[A22]*Det3_345_045
1803                        + m[A24]*Det3_345_025 - m[A25]*Det3_345_024;
1804  G4double Det4_2345_0345 = m[A20]*Det3_345_345 - m[A23]*Det3_345_045
1805                        + m[A24]*Det3_345_035 - m[A25]*Det3_345_034;
1806  G4double Det4_2345_1234 = m[A21]*Det3_345_234 - m[A22]*Det3_345_134
1807                        + m[A23]*Det3_345_124 - m[A24]*Det3_345_123;
1808  G4double Det4_2345_1235 = m[A21]*Det3_345_235 - m[A22]*Det3_345_135
1809                        + m[A23]*Det3_345_125 - m[A25]*Det3_345_123;
1810  G4double Det4_2345_1245 = m[A21]*Det3_345_245 - m[A22]*Det3_345_145
1811                        + m[A24]*Det3_345_125 - m[A25]*Det3_345_124;
1812  G4double Det4_2345_1345 = m[A21]*Det3_345_345 - m[A23]*Det3_345_145
1813                        + m[A24]*Det3_345_135 - m[A25]*Det3_345_134;
1814  G4double Det4_2345_2345 = m[A22]*Det3_345_345 - m[A23]*Det3_345_245
1815                        + m[A24]*Det3_345_235 - m[A25]*Det3_345_234;
1816
1817  // Find all NECESSARY 5x5 dets:  (19 of them)
1818
1819  G4double Det5_01234_01234 = m[A00]*Det4_1234_1234 - m[A01]*Det4_1234_0234
1820    + m[A02]*Det4_1234_0134 - m[A03]*Det4_1234_0124 + m[A04]*Det4_1234_0123;
1821  G4double Det5_01235_01234 = m[A00]*Det4_1235_1234 - m[A01]*Det4_1235_0234
1822    + m[A02]*Det4_1235_0134 - m[A03]*Det4_1235_0124 + m[A04]*Det4_1235_0123;
1823  G4double Det5_01235_01235 = m[A00]*Det4_1235_1235 - m[A01]*Det4_1235_0235
1824    + m[A02]*Det4_1235_0135 - m[A03]*Det4_1235_0125 + m[A05]*Det4_1235_0123;
1825  G4double Det5_01245_01234 = m[A00]*Det4_1245_1234 - m[A01]*Det4_1245_0234
1826    + m[A02]*Det4_1245_0134 - m[A03]*Det4_1245_0124 + m[A04]*Det4_1245_0123;
1827  G4double Det5_01245_01235 = m[A00]*Det4_1245_1235 - m[A01]*Det4_1245_0235
1828    + m[A02]*Det4_1245_0135 - m[A03]*Det4_1245_0125 + m[A05]*Det4_1245_0123;
1829  G4double Det5_01245_01245 = m[A00]*Det4_1245_1245 - m[A01]*Det4_1245_0245
1830    + m[A02]*Det4_1245_0145 - m[A04]*Det4_1245_0125 + m[A05]*Det4_1245_0124;
1831  G4double Det5_01345_01234 = m[A00]*Det4_1345_1234 - m[A01]*Det4_1345_0234
1832    + m[A02]*Det4_1345_0134 - m[A03]*Det4_1345_0124 + m[A04]*Det4_1345_0123;
1833  G4double Det5_01345_01235 = m[A00]*Det4_1345_1235 - m[A01]*Det4_1345_0235
1834    + m[A02]*Det4_1345_0135 - m[A03]*Det4_1345_0125 + m[A05]*Det4_1345_0123;
1835  G4double Det5_01345_01245 = m[A00]*Det4_1345_1245 - m[A01]*Det4_1345_0245
1836    + m[A02]*Det4_1345_0145 - m[A04]*Det4_1345_0125 + m[A05]*Det4_1345_0124;
1837  G4double Det5_01345_01345 = m[A00]*Det4_1345_1345 - m[A01]*Det4_1345_0345
1838    + m[A03]*Det4_1345_0145 - m[A04]*Det4_1345_0135 + m[A05]*Det4_1345_0134;
1839  G4double Det5_02345_01234 = m[A00]*Det4_2345_1234 - m[A01]*Det4_2345_0234
1840    + m[A02]*Det4_2345_0134 - m[A03]*Det4_2345_0124 + m[A04]*Det4_2345_0123;
1841  G4double Det5_02345_01235 = m[A00]*Det4_2345_1235 - m[A01]*Det4_2345_0235
1842    + m[A02]*Det4_2345_0135 - m[A03]*Det4_2345_0125 + m[A05]*Det4_2345_0123;
1843  G4double Det5_02345_01245 = m[A00]*Det4_2345_1245 - m[A01]*Det4_2345_0245
1844    + m[A02]*Det4_2345_0145 - m[A04]*Det4_2345_0125 + m[A05]*Det4_2345_0124;
1845  G4double Det5_02345_01345 = m[A00]*Det4_2345_1345 - m[A01]*Det4_2345_0345
1846    + m[A03]*Det4_2345_0145 - m[A04]*Det4_2345_0135 + m[A05]*Det4_2345_0134;
1847  G4double Det5_02345_02345 = m[A00]*Det4_2345_2345 - m[A02]*Det4_2345_0345
1848    + m[A03]*Det4_2345_0245 - m[A04]*Det4_2345_0235 + m[A05]*Det4_2345_0234;
1849  G4double Det5_12345_01234 = m[A10]*Det4_2345_1234 - m[A11]*Det4_2345_0234
1850    + m[A12]*Det4_2345_0134 - m[A13]*Det4_2345_0124 + m[A14]*Det4_2345_0123;
1851  G4double Det5_12345_01235 = m[A10]*Det4_2345_1235 - m[A11]*Det4_2345_0235
1852    + m[A12]*Det4_2345_0135 - m[A13]*Det4_2345_0125 + m[A15]*Det4_2345_0123;
1853  G4double Det5_12345_01245 = m[A10]*Det4_2345_1245 - m[A11]*Det4_2345_0245
1854    + m[A12]*Det4_2345_0145 - m[A14]*Det4_2345_0125 + m[A15]*Det4_2345_0124;
1855  G4double Det5_12345_01345 = m[A10]*Det4_2345_1345 - m[A11]*Det4_2345_0345
1856    + m[A13]*Det4_2345_0145 - m[A14]*Det4_2345_0135 + m[A15]*Det4_2345_0134;
1857  G4double Det5_12345_02345 = m[A10]*Det4_2345_2345 - m[A12]*Det4_2345_0345
1858    + m[A13]*Det4_2345_0245 - m[A14]*Det4_2345_0235 + m[A15]*Det4_2345_0234;
1859  G4double Det5_12345_12345 = m[A11]*Det4_2345_2345 - m[A12]*Det4_2345_1345
1860    + m[A13]*Det4_2345_1245 - m[A14]*Det4_2345_1235 + m[A15]*Det4_2345_1234;
1861
1862  // Find the determinant
1863
1864  G4double det =  m[A00]*Det5_12345_12345
1865                - m[A01]*Det5_12345_02345
1866                + m[A02]*Det5_12345_01345
1867                - m[A03]*Det5_12345_01245
1868                + m[A04]*Det5_12345_01235
1869                - m[A05]*Det5_12345_01234;
1870
1871  if ( det == 0 )
1872  { 
1873    ifail = 1;
1874    return;
1875  } 
1876
1877  G4double oneOverDet = 1.0/det;
1878  G4double mn1OverDet = - oneOverDet;
1879
1880  m[A00] =  Det5_12345_12345*oneOverDet;
1881  m[A01] =  Det5_12345_02345*mn1OverDet;
1882  m[A02] =  Det5_12345_01345*oneOverDet;
1883  m[A03] =  Det5_12345_01245*mn1OverDet;
1884  m[A04] =  Det5_12345_01235*oneOverDet;
1885  m[A05] =  Det5_12345_01234*mn1OverDet;
1886
1887  m[A11] =  Det5_02345_02345*oneOverDet;
1888  m[A12] =  Det5_02345_01345*mn1OverDet;
1889  m[A13] =  Det5_02345_01245*oneOverDet;
1890  m[A14] =  Det5_02345_01235*mn1OverDet;
1891  m[A15] =  Det5_02345_01234*oneOverDet;
1892
1893  m[A22] =  Det5_01345_01345*oneOverDet;
1894  m[A23] =  Det5_01345_01245*mn1OverDet;
1895  m[A24] =  Det5_01345_01235*oneOverDet;
1896  m[A25] =  Det5_01345_01234*mn1OverDet;
1897
1898  m[A33] =  Det5_01245_01245*oneOverDet;
1899  m[A34] =  Det5_01245_01235*mn1OverDet;
1900  m[A35] =  Det5_01245_01234*oneOverDet;
1901
1902  m[A44] =  Det5_01235_01235*oneOverDet;
1903  m[A45] =  Det5_01235_01234*mn1OverDet;
1904
1905  m[A55] =  Det5_01234_01234*oneOverDet;
1906
1907  return;
1908}
1909
1910void G4ErrorSymMatrix::invertCholesky5 (G4int & ifail)
1911{
1912
1913  // Invert by
1914  //
1915  // a) decomposing M = G*G^T with G lower triangular
1916  //    (if M is not positive definite this will fail, leaving this unchanged)
1917  // b) inverting G to form H
1918  // c) multiplying H^T * H to get M^-1.
1919  //
1920  // If the matrix is pos. def. it is inverted and 1 is returned.
1921  // If the matrix is not pos. def. it remains unaltered and 0 is returned.
1922
1923  G4double h10;                           // below-diagonal elements of H
1924  G4double h20, h21;
1925  G4double h30, h31, h32;
1926  G4double h40, h41, h42, h43;
1927
1928  G4double h00, h11, h22, h33, h44;       // 1/diagonal elements of G =
1929                                          // diagonal elements of H
1930
1931  G4double g10;                           // below-diagonal elements of G
1932  G4double g20, g21;
1933  G4double g30, g31, g32;
1934  G4double g40, g41, g42, g43;
1935
1936  ifail = 1;  // We start by assuing we won't succeed...
1937
1938  // Form G -- compute diagonal members of H directly rather than of G
1939  //-------
1940
1941  // Scale first column by 1/sqrt(A00)
1942
1943  h00 = m[A00]; 
1944  if (h00 <= 0) { return; }
1945  h00 = 1.0 / std::sqrt(h00);
1946
1947  g10 = m[A10] * h00;
1948  g20 = m[A20] * h00;
1949  g30 = m[A30] * h00;
1950  g40 = m[A40] * h00;
1951
1952  // Form G11 (actually, just h11)
1953
1954  h11 = m[A11] - (g10 * g10);
1955  if (h11 <= 0) { return; }
1956  h11 = 1.0 / std::sqrt(h11);
1957
1958  // Subtract inter-column column dot products from rest of column 1 and
1959  // scale to get column 1 of G
1960
1961  g21 = (m[A21] - (g10 * g20)) * h11;
1962  g31 = (m[A31] - (g10 * g30)) * h11;
1963  g41 = (m[A41] - (g10 * g40)) * h11;
1964
1965  // Form G22 (actually, just h22)
1966
1967  h22 = m[A22] - (g20 * g20) - (g21 * g21);
1968  if (h22 <= 0) { return; }
1969  h22 = 1.0 / std::sqrt(h22);
1970
1971  // Subtract inter-column column dot products from rest of column 2 and
1972  // scale to get column 2 of G
1973
1974  g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22;
1975  g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22;
1976
1977  // Form G33 (actually, just h33)
1978
1979  h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32);
1980  if (h33 <= 0) { return; }
1981  h33 = 1.0 / std::sqrt(h33);
1982
1983  // Subtract inter-column column dot product from A43 and scale to get G43
1984
1985  g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33;
1986
1987  // Finally form h44 - if this is possible inversion succeeds
1988
1989  h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43);
1990  if (h44 <= 0) { return; }
1991  h44 = 1.0 / std::sqrt(h44);
1992
1993  // Form H = 1/G -- diagonal members of H are already correct
1994  //-------------
1995
1996  // The order here is dictated by speed considerations
1997
1998  h43 = -h33 *  g43 * h44;
1999  h32 = -h22 *  g32 * h33;
2000  h42 = -h22 * (g32 * h43 + g42 * h44);
2001  h21 = -h11 *  g21 * h22;
2002  h31 = -h11 * (g21 * h32 + g31 * h33);
2003  h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44);
2004  h10 = -h00 *  g10 * h11;
2005  h20 = -h00 * (g10 * h21 + g20 * h22);
2006  h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33);
2007  h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44);
2008
2009  // Change this to its inverse = H^T*H
2010  //------------------------------------
2011
2012  m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40;
2013  m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41;
2014  m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41;
2015  m[A02] = h20 * h22 + h30 * h32 + h40 * h42;
2016  m[A12] = h21 * h22 + h31 * h32 + h41 * h42;
2017  m[A22] = h22 * h22 + h32 * h32 + h42 * h42;
2018  m[A03] = h30 * h33 + h40 * h43;
2019  m[A13] = h31 * h33 + h41 * h43;
2020  m[A23] = h32 * h33 + h42 * h43;
2021  m[A33] = h33 * h33 + h43 * h43;
2022  m[A04] = h40 * h44;
2023  m[A14] = h41 * h44;
2024  m[A24] = h42 * h44;
2025  m[A34] = h43 * h44;
2026  m[A44] = h44 * h44;
2027
2028  ifail = 0;
2029  return;
2030}
2031
2032void G4ErrorSymMatrix::invertCholesky6 (G4int & ifail)
2033{
2034  // Invert by
2035  //
2036  // a) decomposing M = G*G^T with G lower triangular
2037  //    (if M is not positive definite this will fail, leaving this unchanged)
2038  // b) inverting G to form H
2039  // c) multiplying H^T * H to get M^-1.
2040  //
2041  // If the matrix is pos. def. it is inverted and 1 is returned.
2042  // If the matrix is not pos. def. it remains unaltered and 0 is returned.
2043
2044  G4double h10;                           // below-diagonal elements of H
2045  G4double h20, h21;
2046  G4double h30, h31, h32;
2047  G4double h40, h41, h42, h43;
2048  G4double h50, h51, h52, h53, h54;
2049
2050  G4double h00, h11, h22, h33, h44, h55;  // 1/diagonal elements of G =
2051                                          // diagonal elements of H
2052
2053  G4double g10;                           // below-diagonal elements of G
2054  G4double g20, g21;
2055  G4double g30, g31, g32;
2056  G4double g40, g41, g42, g43;
2057  G4double g50, g51, g52, g53, g54;
2058
2059  ifail = 1;  // We start by assuing we won't succeed...
2060
2061  // Form G -- compute diagonal members of H directly rather than of G
2062  //-------
2063
2064  // Scale first column by 1/sqrt(A00)
2065
2066  h00 = m[A00]; 
2067  if (h00 <= 0) { return; }
2068  h00 = 1.0 / std::sqrt(h00);
2069
2070  g10 = m[A10] * h00;
2071  g20 = m[A20] * h00;
2072  g30 = m[A30] * h00;
2073  g40 = m[A40] * h00;
2074  g50 = m[A50] * h00;
2075
2076  // Form G11 (actually, just h11)
2077
2078  h11 = m[A11] - (g10 * g10);
2079  if (h11 <= 0) { return; }
2080  h11 = 1.0 / std::sqrt(h11);
2081
2082  // Subtract inter-column column dot products from rest of column 1 and
2083  // scale to get column 1 of G
2084
2085  g21 = (m[A21] - (g10 * g20)) * h11;
2086  g31 = (m[A31] - (g10 * g30)) * h11;
2087  g41 = (m[A41] - (g10 * g40)) * h11;
2088  g51 = (m[A51] - (g10 * g50)) * h11;
2089
2090  // Form G22 (actually, just h22)
2091
2092  h22 = m[A22] - (g20 * g20) - (g21 * g21);
2093  if (h22 <= 0) { return; }
2094  h22 = 1.0 / std::sqrt(h22);
2095
2096  // Subtract inter-column column dot products from rest of column 2 and
2097  // scale to get column 2 of G
2098
2099  g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22;
2100  g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22;
2101  g52 = (m[A52] - (g20 * g50) - (g21 * g51)) * h22;
2102
2103  // Form G33 (actually, just h33)
2104
2105  h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32);
2106  if (h33 <= 0) { return; }
2107  h33 = 1.0 / std::sqrt(h33);
2108
2109  // Subtract inter-column column dot products from rest of column 3 and
2110  // scale to get column 3 of G
2111
2112  g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33;
2113  g53 = (m[A53] - (g30 * g50) - (g31 * g51) - (g32 * g52)) * h33;
2114
2115  // Form G44 (actually, just h44)
2116
2117  h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43);
2118  if (h44 <= 0) { return; }
2119  h44 = 1.0 / std::sqrt(h44);
2120
2121  // Subtract inter-column column dot product from M54 and scale to get G54
2122
2123  g54 = (m[A54] - (g40 * g50) - (g41 * g51) - (g42 * g52) - (g43 * g53)) * h44;
2124
2125  // Finally form h55 - if this is possible inversion succeeds
2126
2127  h55 = m[A55] - (g50*g50) - (g51*g51) - (g52*g52) - (g53*g53) - (g54*g54);
2128  if (h55 <= 0) { return; }
2129  h55 = 1.0 / std::sqrt(h55);
2130
2131  // Form H = 1/G -- diagonal members of H are already correct
2132  //-------------
2133
2134  // The order here is dictated by speed considerations
2135
2136  h54 = -h44 *  g54 * h55;
2137  h43 = -h33 *  g43 * h44;
2138  h53 = -h33 * (g43 * h54 + g53 * h55);
2139  h32 = -h22 *  g32 * h33;
2140  h42 = -h22 * (g32 * h43 + g42 * h44);
2141  h52 = -h22 * (g32 * h53 + g42 * h54 + g52 * h55);
2142  h21 = -h11 *  g21 * h22;
2143  h31 = -h11 * (g21 * h32 + g31 * h33);
2144  h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44);
2145  h51 = -h11 * (g21 * h52 + g31 * h53 + g41 * h54 + g51 * h55);
2146  h10 = -h00 *  g10 * h11;
2147  h20 = -h00 * (g10 * h21 + g20 * h22);
2148  h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33);
2149  h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44);
2150  h50 = -h00 * (g10 * h51 + g20 * h52 + g30 * h53 + g40 * h54 + g50 * h55);
2151
2152  // Change this to its inverse = H^T*H
2153  //------------------------------------
2154
2155  m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40 + h50*h50;
2156  m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41 + h50 * h51;
2157  m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41 + h51 * h51;
2158  m[A02] = h20 * h22 + h30 * h32 + h40 * h42 + h50 * h52;
2159  m[A12] = h21 * h22 + h31 * h32 + h41 * h42 + h51 * h52;
2160  m[A22] = h22 * h22 + h32 * h32 + h42 * h42 + h52 * h52;
2161  m[A03] = h30 * h33 + h40 * h43 + h50 * h53;
2162  m[A13] = h31 * h33 + h41 * h43 + h51 * h53;
2163  m[A23] = h32 * h33 + h42 * h43 + h52 * h53;
2164  m[A33] = h33 * h33 + h43 * h43 + h53 * h53;
2165  m[A04] = h40 * h44 + h50 * h54;
2166  m[A14] = h41 * h44 + h51 * h54;
2167  m[A24] = h42 * h44 + h52 * h54;
2168  m[A34] = h43 * h44 + h53 * h54;
2169  m[A44] = h44 * h44 + h54 * h54;
2170  m[A05] = h50 * h55;
2171  m[A15] = h51 * h55;
2172  m[A25] = h52 * h55;
2173  m[A35] = h53 * h55;
2174  m[A45] = h54 * h55;
2175  m[A55] = h55 * h55;
2176
2177  ifail = 0;
2178  return;
2179}
2180
2181void G4ErrorSymMatrix::invert4  (G4int & ifail)
2182{
2183  ifail = 0;
2184
2185  // Find all NECESSARY 2x2 dets:  (14 of them)
2186
2187  G4double Det2_12_01 = m[A10]*m[A21] - m[A11]*m[A20];
2188  G4double Det2_12_02 = m[A10]*m[A22] - m[A12]*m[A20];
2189  G4double Det2_12_12 = m[A11]*m[A22] - m[A12]*m[A21];
2190  G4double Det2_13_01 = m[A10]*m[A31] - m[A11]*m[A30];
2191  G4double Det2_13_02 = m[A10]*m[A32] - m[A12]*m[A30];
2192  G4double Det2_13_03 = m[A10]*m[A33] - m[A13]*m[A30];
2193  G4double Det2_13_12 = m[A11]*m[A32] - m[A12]*m[A31];
2194  G4double Det2_13_13 = m[A11]*m[A33] - m[A13]*m[A31];
2195  G4double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30];
2196  G4double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30];
2197  G4double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30];
2198  G4double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31];
2199  G4double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31];
2200  G4double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32];
2201
2202  // Find all NECESSARY 3x3 dets:   (10 of them)
2203
2204  G4double Det3_012_012 = m[A00]*Det2_12_12 - m[A01]*Det2_12_02
2205                                + m[A02]*Det2_12_01;
2206  G4double Det3_013_012 = m[A00]*Det2_13_12 - m[A01]*Det2_13_02
2207                                + m[A02]*Det2_13_01;
2208  G4double Det3_013_013 = m[A00]*Det2_13_13 - m[A01]*Det2_13_03
2209                                + m[A03]*Det2_13_01;
2210  G4double Det3_023_012 = m[A00]*Det2_23_12 - m[A01]*Det2_23_02
2211                                + m[A02]*Det2_23_01;
2212  G4double Det3_023_013 = m[A00]*Det2_23_13 - m[A01]*Det2_23_03
2213                                + m[A03]*Det2_23_01;
2214  G4double Det3_023_023 = m[A00]*Det2_23_23 - m[A02]*Det2_23_03
2215                                + m[A03]*Det2_23_02;
2216  G4double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02
2217                                + m[A12]*Det2_23_01;
2218  G4double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03
2219                                + m[A13]*Det2_23_01;
2220  G4double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03
2221                                + m[A13]*Det2_23_02;
2222  G4double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13
2223                                + m[A13]*Det2_23_12;
2224
2225  // Find the 4x4 det:
2226
2227  G4double det =  m[A00]*Det3_123_123
2228                - m[A01]*Det3_123_023
2229                + m[A02]*Det3_123_013
2230                - m[A03]*Det3_123_012;
2231
2232  if ( det == 0 )
2233  { 
2234    ifail = 1;
2235    return;
2236  } 
2237
2238  G4double oneOverDet = 1.0/det;
2239  G4double mn1OverDet = - oneOverDet;
2240
2241  m[A00] =  Det3_123_123 * oneOverDet;
2242  m[A01] =  Det3_123_023 * mn1OverDet;
2243  m[A02] =  Det3_123_013 * oneOverDet;
2244  m[A03] =  Det3_123_012 * mn1OverDet;
2245
2246
2247  m[A11] =  Det3_023_023 * oneOverDet;
2248  m[A12] =  Det3_023_013 * mn1OverDet;
2249  m[A13] =  Det3_023_012 * oneOverDet;
2250
2251  m[A22] =  Det3_013_013 * oneOverDet;
2252  m[A23] =  Det3_013_012 * mn1OverDet;
2253
2254  m[A33] =  Det3_012_012 * oneOverDet;
2255
2256  return;
2257}
2258
2259void G4ErrorSymMatrix::invertHaywood4  (G4int & ifail)
2260{
2261  invert4(ifail); // For the 4x4 case, the method we use for invert is already
2262                  // the Haywood method.
2263}
2264
Note: See TracBrowser for help on using the repository browser.