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