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