CLHEP VERSION Reference Documentation
   
CLHEP Home Page     CLHEP Documentation     CLHEP Bug Reports

SymMatrixInvert.cc
Go to the documentation of this file.
1 // -*- C++ -*-
2 //
3 // This file is a part of the CLHEP - a Class Library for High Energy Physics.
4 //
5 // This software written by Mark Fischler and Steven Haywood
6 //
7 
8 #ifdef GNUPRAGMA
9 #pragma implementation
10 #endif
11 
12 #include <string.h>
13 #include <float.h> // for DBL_EPSILON
14 #include <cmath>
15 
16 #include "CLHEP/Matrix/defs.h"
17 #include "CLHEP/Matrix/SymMatrix.h"
18 
19 namespace CLHEP {
20 
21 double HepSymMatrix::posDefFraction5x5 = 1.0;
22 double HepSymMatrix::posDefFraction6x6 = 1.0;
23 double HepSymMatrix::adjustment5x5 = 0.0;
24 double HepSymMatrix::adjustment6x6 = 0.0;
25 const double HepSymMatrix::CHOLESKY_THRESHOLD_5x5 = .5;
26 const double HepSymMatrix::CHOLESKY_THRESHOLD_6x6 = .2;
27 const double HepSymMatrix::CHOLESKY_CREEP_5x5 = .005;
28 const double HepSymMatrix::CHOLESKY_CREEP_6x6 = .002;
29 
30 // Aij are indices for a 6x6 symmetric matrix.
31 // The indices for 5x5 or 4x4 symmetric matrices are the same,
32 // ignoring all combinations with an index which is inapplicable.
33 
34 #define A00 0
35 #define A01 1
36 #define A02 3
37 #define A03 6
38 #define A04 10
39 #define A05 15
40 
41 #define A10 1
42 #define A11 2
43 #define A12 4
44 #define A13 7
45 #define A14 11
46 #define A15 16
47 
48 #define A20 3
49 #define A21 4
50 #define A22 5
51 #define A23 8
52 #define A24 12
53 #define A25 17
54 
55 #define A30 6
56 #define A31 7
57 #define A32 8
58 #define A33 9
59 #define A34 13
60 #define A35 18
61 
62 #define A40 10
63 #define A41 11
64 #define A42 12
65 #define A43 13
66 #define A44 14
67 #define A45 19
68 
69 #define A50 15
70 #define A51 16
71 #define A52 17
72 #define A53 18
73 #define A54 19
74 #define A55 20
75 
76 
77 void HepSymMatrix::invert5(int & ifail) {
78  if (posDefFraction5x5 >= CHOLESKY_THRESHOLD_5x5) {
79  invertCholesky5(ifail);
80  posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail);
81  if (ifail!=0) { // Cholesky failed -- invert using Haywood
82  invertHaywood5(ifail);
83  }
84  } else {
85  if (posDefFraction5x5 + adjustment5x5 >= CHOLESKY_THRESHOLD_5x5) {
86  invertCholesky5(ifail);
87  posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail);
88  if (ifail!=0) { // Cholesky failed -- invert using Haywood
89  invertHaywood5(ifail);
90  adjustment5x5 = 0;
91  }
92  } else {
93  invertHaywood5(ifail);
94  adjustment5x5 += CHOLESKY_CREEP_5x5;
95  }
96  }
97  return;
98 }
99 
100 void HepSymMatrix::invert6(int & ifail) {
101  if (posDefFraction6x6 >= CHOLESKY_THRESHOLD_6x6) {
102  invertCholesky6(ifail);
103  posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail);
104  if (ifail!=0) { // Cholesky failed -- invert using Haywood
105  invertHaywood6(ifail);
106  }
107  } else {
108  if (posDefFraction6x6 + adjustment6x6 >= CHOLESKY_THRESHOLD_6x6) {
109  invertCholesky6(ifail);
110  posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail);
111  if (ifail!=0) { // Cholesky failed -- invert using Haywood
112  invertHaywood6(ifail);
113  adjustment6x6 = 0;
114  }
115  } else {
116  invertHaywood6(ifail);
117  adjustment6x6 += CHOLESKY_CREEP_6x6;
118  }
119  }
120  return;
121 }
122 
123 
124 void HepSymMatrix::invertHaywood5 (int & ifail) {
125 
126  ifail = 0;
127 
128  // Find all NECESSARY 2x2 dets: (25 of them)
129 
130  double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30];
131  double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30];
132  double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30];
133  double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31];
134  double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31];
135  double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32];
136  double Det2_24_01 = m[A20]*m[A41] - m[A21]*m[A40];
137  double Det2_24_02 = m[A20]*m[A42] - m[A22]*m[A40];
138  double Det2_24_03 = m[A20]*m[A43] - m[A23]*m[A40];
139  double Det2_24_04 = m[A20]*m[A44] - m[A24]*m[A40];
140  double Det2_24_12 = m[A21]*m[A42] - m[A22]*m[A41];
141  double Det2_24_13 = m[A21]*m[A43] - m[A23]*m[A41];
142  double Det2_24_14 = m[A21]*m[A44] - m[A24]*m[A41];
143  double Det2_24_23 = m[A22]*m[A43] - m[A23]*m[A42];
144  double Det2_24_24 = m[A22]*m[A44] - m[A24]*m[A42];
145  double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40];
146  double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40];
147  double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40];
148  double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40];
149  double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41];
150  double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41];
151  double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41];
152  double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42];
153  double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42];
154  double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43];
155 
156  // Find all NECESSARY 3x3 dets: (30 of them)
157 
158  double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02
159  + m[A12]*Det2_23_01;
160  double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03
161  + m[A13]*Det2_23_01;
162  double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03
163  + m[A13]*Det2_23_02;
164  double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13
165  + m[A13]*Det2_23_12;
166  double Det3_124_012 = m[A10]*Det2_24_12 - m[A11]*Det2_24_02
167  + m[A12]*Det2_24_01;
168  double Det3_124_013 = m[A10]*Det2_24_13 - m[A11]*Det2_24_03
169  + m[A13]*Det2_24_01;
170  double Det3_124_014 = m[A10]*Det2_24_14 - m[A11]*Det2_24_04
171  + m[A14]*Det2_24_01;
172  double Det3_124_023 = m[A10]*Det2_24_23 - m[A12]*Det2_24_03
173  + m[A13]*Det2_24_02;
174  double Det3_124_024 = m[A10]*Det2_24_24 - m[A12]*Det2_24_04
175  + m[A14]*Det2_24_02;
176  double Det3_124_123 = m[A11]*Det2_24_23 - m[A12]*Det2_24_13
177  + m[A13]*Det2_24_12;
178  double Det3_124_124 = m[A11]*Det2_24_24 - m[A12]*Det2_24_14
179  + m[A14]*Det2_24_12;
180  double Det3_134_012 = m[A10]*Det2_34_12 - m[A11]*Det2_34_02
181  + m[A12]*Det2_34_01;
182  double Det3_134_013 = m[A10]*Det2_34_13 - m[A11]*Det2_34_03
183  + m[A13]*Det2_34_01;
184  double Det3_134_014 = m[A10]*Det2_34_14 - m[A11]*Det2_34_04
185  + m[A14]*Det2_34_01;
186  double Det3_134_023 = m[A10]*Det2_34_23 - m[A12]*Det2_34_03
187  + m[A13]*Det2_34_02;
188  double Det3_134_024 = m[A10]*Det2_34_24 - m[A12]*Det2_34_04
189  + m[A14]*Det2_34_02;
190  double Det3_134_034 = m[A10]*Det2_34_34 - m[A13]*Det2_34_04
191  + m[A14]*Det2_34_03;
192  double Det3_134_123 = m[A11]*Det2_34_23 - m[A12]*Det2_34_13
193  + m[A13]*Det2_34_12;
194  double Det3_134_124 = m[A11]*Det2_34_24 - m[A12]*Det2_34_14
195  + m[A14]*Det2_34_12;
196  double Det3_134_134 = m[A11]*Det2_34_34 - m[A13]*Det2_34_14
197  + m[A14]*Det2_34_13;
198  double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02
199  + m[A22]*Det2_34_01;
200  double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03
201  + m[A23]*Det2_34_01;
202  double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04
203  + m[A24]*Det2_34_01;
204  double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03
205  + m[A23]*Det2_34_02;
206  double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04
207  + m[A24]*Det2_34_02;
208  double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04
209  + m[A24]*Det2_34_03;
210  double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13
211  + m[A23]*Det2_34_12;
212  double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14
213  + m[A24]*Det2_34_12;
214  double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14
215  + m[A24]*Det2_34_13;
216  double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24
217  + m[A24]*Det2_34_23;
218 
219  // Find all NECESSARY 4x4 dets: (15 of them)
220 
221  double Det4_0123_0123 = m[A00]*Det3_123_123 - m[A01]*Det3_123_023
222  + m[A02]*Det3_123_013 - m[A03]*Det3_123_012;
223  double Det4_0124_0123 = m[A00]*Det3_124_123 - m[A01]*Det3_124_023
224  + m[A02]*Det3_124_013 - m[A03]*Det3_124_012;
225  double Det4_0124_0124 = m[A00]*Det3_124_124 - m[A01]*Det3_124_024
226  + m[A02]*Det3_124_014 - m[A04]*Det3_124_012;
227  double Det4_0134_0123 = m[A00]*Det3_134_123 - m[A01]*Det3_134_023
228  + m[A02]*Det3_134_013 - m[A03]*Det3_134_012;
229  double Det4_0134_0124 = m[A00]*Det3_134_124 - m[A01]*Det3_134_024
230  + m[A02]*Det3_134_014 - m[A04]*Det3_134_012;
231  double Det4_0134_0134 = m[A00]*Det3_134_134 - m[A01]*Det3_134_034
232  + m[A03]*Det3_134_014 - m[A04]*Det3_134_013;
233  double Det4_0234_0123 = m[A00]*Det3_234_123 - m[A01]*Det3_234_023
234  + m[A02]*Det3_234_013 - m[A03]*Det3_234_012;
235  double Det4_0234_0124 = m[A00]*Det3_234_124 - m[A01]*Det3_234_024
236  + m[A02]*Det3_234_014 - m[A04]*Det3_234_012;
237  double Det4_0234_0134 = m[A00]*Det3_234_134 - m[A01]*Det3_234_034
238  + m[A03]*Det3_234_014 - m[A04]*Det3_234_013;
239  double Det4_0234_0234 = m[A00]*Det3_234_234 - m[A02]*Det3_234_034
240  + m[A03]*Det3_234_024 - m[A04]*Det3_234_023;
241  double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023
242  + m[A12]*Det3_234_013 - m[A13]*Det3_234_012;
243  double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024
244  + m[A12]*Det3_234_014 - m[A14]*Det3_234_012;
245  double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034
246  + m[A13]*Det3_234_014 - m[A14]*Det3_234_013;
247  double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034
248  + m[A13]*Det3_234_024 - m[A14]*Det3_234_023;
249  double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134
250  + m[A13]*Det3_234_124 - m[A14]*Det3_234_123;
251 
252  // Find the 5x5 det:
253 
254  double det = m[A00]*Det4_1234_1234
255  - m[A01]*Det4_1234_0234
256  + m[A02]*Det4_1234_0134
257  - m[A03]*Det4_1234_0124
258  + m[A04]*Det4_1234_0123;
259 
260  if ( det == 0 ) {
261 #ifdef SINGULAR_DIAGNOSTICS
262  std::cerr << "Kramer's rule inversion of a singular 5x5 matrix: "
263  << *this << "\n";
264 #endif
265  ifail = 1;
266  return;
267  }
268 
269  double oneOverDet = 1.0/det;
270  double mn1OverDet = - oneOverDet;
271 
272  m[A00] = Det4_1234_1234 * oneOverDet;
273  m[A01] = Det4_1234_0234 * mn1OverDet;
274  m[A02] = Det4_1234_0134 * oneOverDet;
275  m[A03] = Det4_1234_0124 * mn1OverDet;
276  m[A04] = Det4_1234_0123 * oneOverDet;
277 
278  m[A11] = Det4_0234_0234 * oneOverDet;
279  m[A12] = Det4_0234_0134 * mn1OverDet;
280  m[A13] = Det4_0234_0124 * oneOverDet;
281  m[A14] = Det4_0234_0123 * mn1OverDet;
282 
283  m[A22] = Det4_0134_0134 * oneOverDet;
284  m[A23] = Det4_0134_0124 * mn1OverDet;
285  m[A24] = Det4_0134_0123 * oneOverDet;
286 
287  m[A33] = Det4_0124_0124 * oneOverDet;
288  m[A34] = Det4_0124_0123 * mn1OverDet;
289 
290  m[A44] = Det4_0123_0123 * oneOverDet;
291 
292  return;
293 }
294 
295 void HepSymMatrix::invertHaywood6 (int & ifail) {
296 
297  ifail = 0;
298 
299  // Find all NECESSARY 2x2 dets: (39 of them)
300 
301  double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40];
302  double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40];
303  double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40];
304  double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40];
305  double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41];
306  double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41];
307  double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41];
308  double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42];
309  double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42];
310  double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43];
311  double Det2_35_01 = m[A30]*m[A51] - m[A31]*m[A50];
312  double Det2_35_02 = m[A30]*m[A52] - m[A32]*m[A50];
313  double Det2_35_03 = m[A30]*m[A53] - m[A33]*m[A50];
314  double Det2_35_04 = m[A30]*m[A54] - m[A34]*m[A50];
315  double Det2_35_05 = m[A30]*m[A55] - m[A35]*m[A50];
316  double Det2_35_12 = m[A31]*m[A52] - m[A32]*m[A51];
317  double Det2_35_13 = m[A31]*m[A53] - m[A33]*m[A51];
318  double Det2_35_14 = m[A31]*m[A54] - m[A34]*m[A51];
319  double Det2_35_15 = m[A31]*m[A55] - m[A35]*m[A51];
320  double Det2_35_23 = m[A32]*m[A53] - m[A33]*m[A52];
321  double Det2_35_24 = m[A32]*m[A54] - m[A34]*m[A52];
322  double Det2_35_25 = m[A32]*m[A55] - m[A35]*m[A52];
323  double Det2_35_34 = m[A33]*m[A54] - m[A34]*m[A53];
324  double Det2_35_35 = m[A33]*m[A55] - m[A35]*m[A53];
325  double Det2_45_01 = m[A40]*m[A51] - m[A41]*m[A50];
326  double Det2_45_02 = m[A40]*m[A52] - m[A42]*m[A50];
327  double Det2_45_03 = m[A40]*m[A53] - m[A43]*m[A50];
328  double Det2_45_04 = m[A40]*m[A54] - m[A44]*m[A50];
329  double Det2_45_05 = m[A40]*m[A55] - m[A45]*m[A50];
330  double Det2_45_12 = m[A41]*m[A52] - m[A42]*m[A51];
331  double Det2_45_13 = m[A41]*m[A53] - m[A43]*m[A51];
332  double Det2_45_14 = m[A41]*m[A54] - m[A44]*m[A51];
333  double Det2_45_15 = m[A41]*m[A55] - m[A45]*m[A51];
334  double Det2_45_23 = m[A42]*m[A53] - m[A43]*m[A52];
335  double Det2_45_24 = m[A42]*m[A54] - m[A44]*m[A52];
336  double Det2_45_25 = m[A42]*m[A55] - m[A45]*m[A52];
337  double Det2_45_34 = m[A43]*m[A54] - m[A44]*m[A53];
338  double Det2_45_35 = m[A43]*m[A55] - m[A45]*m[A53];
339  double Det2_45_45 = m[A44]*m[A55] - m[A45]*m[A54];
340 
341  // Find all NECESSARY 3x3 dets: (65 of them)
342 
343  double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02
344  + m[A22]*Det2_34_01;
345  double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03
346  + m[A23]*Det2_34_01;
347  double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04
348  + m[A24]*Det2_34_01;
349  double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03
350  + m[A23]*Det2_34_02;
351  double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04
352  + m[A24]*Det2_34_02;
353  double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04
354  + m[A24]*Det2_34_03;
355  double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13
356  + m[A23]*Det2_34_12;
357  double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14
358  + m[A24]*Det2_34_12;
359  double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14
360  + m[A24]*Det2_34_13;
361  double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24
362  + m[A24]*Det2_34_23;
363  double Det3_235_012 = m[A20]*Det2_35_12 - m[A21]*Det2_35_02
364  + m[A22]*Det2_35_01;
365  double Det3_235_013 = m[A20]*Det2_35_13 - m[A21]*Det2_35_03
366  + m[A23]*Det2_35_01;
367  double Det3_235_014 = m[A20]*Det2_35_14 - m[A21]*Det2_35_04
368  + m[A24]*Det2_35_01;
369  double Det3_235_015 = m[A20]*Det2_35_15 - m[A21]*Det2_35_05
370  + m[A25]*Det2_35_01;
371  double Det3_235_023 = m[A20]*Det2_35_23 - m[A22]*Det2_35_03
372  + m[A23]*Det2_35_02;
373  double Det3_235_024 = m[A20]*Det2_35_24 - m[A22]*Det2_35_04
374  + m[A24]*Det2_35_02;
375  double Det3_235_025 = m[A20]*Det2_35_25 - m[A22]*Det2_35_05
376  + m[A25]*Det2_35_02;
377  double Det3_235_034 = m[A20]*Det2_35_34 - m[A23]*Det2_35_04
378  + m[A24]*Det2_35_03;
379  double Det3_235_035 = m[A20]*Det2_35_35 - m[A23]*Det2_35_05
380  + m[A25]*Det2_35_03;
381  double Det3_235_123 = m[A21]*Det2_35_23 - m[A22]*Det2_35_13
382  + m[A23]*Det2_35_12;
383  double Det3_235_124 = m[A21]*Det2_35_24 - m[A22]*Det2_35_14
384  + m[A24]*Det2_35_12;
385  double Det3_235_125 = m[A21]*Det2_35_25 - m[A22]*Det2_35_15
386  + m[A25]*Det2_35_12;
387  double Det3_235_134 = m[A21]*Det2_35_34 - m[A23]*Det2_35_14
388  + m[A24]*Det2_35_13;
389  double Det3_235_135 = m[A21]*Det2_35_35 - m[A23]*Det2_35_15
390  + m[A25]*Det2_35_13;
391  double Det3_235_234 = m[A22]*Det2_35_34 - m[A23]*Det2_35_24
392  + m[A24]*Det2_35_23;
393  double Det3_235_235 = m[A22]*Det2_35_35 - m[A23]*Det2_35_25
394  + m[A25]*Det2_35_23;
395  double Det3_245_012 = m[A20]*Det2_45_12 - m[A21]*Det2_45_02
396  + m[A22]*Det2_45_01;
397  double Det3_245_013 = m[A20]*Det2_45_13 - m[A21]*Det2_45_03
398  + m[A23]*Det2_45_01;
399  double Det3_245_014 = m[A20]*Det2_45_14 - m[A21]*Det2_45_04
400  + m[A24]*Det2_45_01;
401  double Det3_245_015 = m[A20]*Det2_45_15 - m[A21]*Det2_45_05
402  + m[A25]*Det2_45_01;
403  double Det3_245_023 = m[A20]*Det2_45_23 - m[A22]*Det2_45_03
404  + m[A23]*Det2_45_02;
405  double Det3_245_024 = m[A20]*Det2_45_24 - m[A22]*Det2_45_04
406  + m[A24]*Det2_45_02;
407  double Det3_245_025 = m[A20]*Det2_45_25 - m[A22]*Det2_45_05
408  + m[A25]*Det2_45_02;
409  double Det3_245_034 = m[A20]*Det2_45_34 - m[A23]*Det2_45_04
410  + m[A24]*Det2_45_03;
411  double Det3_245_035 = m[A20]*Det2_45_35 - m[A23]*Det2_45_05
412  + m[A25]*Det2_45_03;
413  double Det3_245_045 = m[A20]*Det2_45_45 - m[A24]*Det2_45_05
414  + m[A25]*Det2_45_04;
415  double Det3_245_123 = m[A21]*Det2_45_23 - m[A22]*Det2_45_13
416  + m[A23]*Det2_45_12;
417  double Det3_245_124 = m[A21]*Det2_45_24 - m[A22]*Det2_45_14
418  + m[A24]*Det2_45_12;
419  double Det3_245_125 = m[A21]*Det2_45_25 - m[A22]*Det2_45_15
420  + m[A25]*Det2_45_12;
421  double Det3_245_134 = m[A21]*Det2_45_34 - m[A23]*Det2_45_14
422  + m[A24]*Det2_45_13;
423  double Det3_245_135 = m[A21]*Det2_45_35 - m[A23]*Det2_45_15
424  + m[A25]*Det2_45_13;
425  double Det3_245_145 = m[A21]*Det2_45_45 - m[A24]*Det2_45_15
426  + m[A25]*Det2_45_14;
427  double Det3_245_234 = m[A22]*Det2_45_34 - m[A23]*Det2_45_24
428  + m[A24]*Det2_45_23;
429  double Det3_245_235 = m[A22]*Det2_45_35 - m[A23]*Det2_45_25
430  + m[A25]*Det2_45_23;
431  double Det3_245_245 = m[A22]*Det2_45_45 - m[A24]*Det2_45_25
432  + m[A25]*Det2_45_24;
433  double Det3_345_012 = m[A30]*Det2_45_12 - m[A31]*Det2_45_02
434  + m[A32]*Det2_45_01;
435  double Det3_345_013 = m[A30]*Det2_45_13 - m[A31]*Det2_45_03
436  + m[A33]*Det2_45_01;
437  double Det3_345_014 = m[A30]*Det2_45_14 - m[A31]*Det2_45_04
438  + m[A34]*Det2_45_01;
439  double Det3_345_015 = m[A30]*Det2_45_15 - m[A31]*Det2_45_05
440  + m[A35]*Det2_45_01;
441  double Det3_345_023 = m[A30]*Det2_45_23 - m[A32]*Det2_45_03
442  + m[A33]*Det2_45_02;
443  double Det3_345_024 = m[A30]*Det2_45_24 - m[A32]*Det2_45_04
444  + m[A34]*Det2_45_02;
445  double Det3_345_025 = m[A30]*Det2_45_25 - m[A32]*Det2_45_05
446  + m[A35]*Det2_45_02;
447  double Det3_345_034 = m[A30]*Det2_45_34 - m[A33]*Det2_45_04
448  + m[A34]*Det2_45_03;
449  double Det3_345_035 = m[A30]*Det2_45_35 - m[A33]*Det2_45_05
450  + m[A35]*Det2_45_03;
451  double Det3_345_045 = m[A30]*Det2_45_45 - m[A34]*Det2_45_05
452  + m[A35]*Det2_45_04;
453  double Det3_345_123 = m[A31]*Det2_45_23 - m[A32]*Det2_45_13
454  + m[A33]*Det2_45_12;
455  double Det3_345_124 = m[A31]*Det2_45_24 - m[A32]*Det2_45_14
456  + m[A34]*Det2_45_12;
457  double Det3_345_125 = m[A31]*Det2_45_25 - m[A32]*Det2_45_15
458  + m[A35]*Det2_45_12;
459  double Det3_345_134 = m[A31]*Det2_45_34 - m[A33]*Det2_45_14
460  + m[A34]*Det2_45_13;
461  double Det3_345_135 = m[A31]*Det2_45_35 - m[A33]*Det2_45_15
462  + m[A35]*Det2_45_13;
463  double Det3_345_145 = m[A31]*Det2_45_45 - m[A34]*Det2_45_15
464  + m[A35]*Det2_45_14;
465  double Det3_345_234 = m[A32]*Det2_45_34 - m[A33]*Det2_45_24
466  + m[A34]*Det2_45_23;
467  double Det3_345_235 = m[A32]*Det2_45_35 - m[A33]*Det2_45_25
468  + m[A35]*Det2_45_23;
469  double Det3_345_245 = m[A32]*Det2_45_45 - m[A34]*Det2_45_25
470  + m[A35]*Det2_45_24;
471  double Det3_345_345 = m[A33]*Det2_45_45 - m[A34]*Det2_45_35
472  + m[A35]*Det2_45_34;
473 
474  // Find all NECESSARY 4x4 dets: (55 of them)
475 
476  double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023
477  + m[A12]*Det3_234_013 - m[A13]*Det3_234_012;
478  double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024
479  + m[A12]*Det3_234_014 - m[A14]*Det3_234_012;
480  double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034
481  + m[A13]*Det3_234_014 - m[A14]*Det3_234_013;
482  double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034
483  + m[A13]*Det3_234_024 - m[A14]*Det3_234_023;
484  double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134
485  + m[A13]*Det3_234_124 - m[A14]*Det3_234_123;
486  double Det4_1235_0123 = m[A10]*Det3_235_123 - m[A11]*Det3_235_023
487  + m[A12]*Det3_235_013 - m[A13]*Det3_235_012;
488  double Det4_1235_0124 = m[A10]*Det3_235_124 - m[A11]*Det3_235_024
489  + m[A12]*Det3_235_014 - m[A14]*Det3_235_012;
490  double Det4_1235_0125 = m[A10]*Det3_235_125 - m[A11]*Det3_235_025
491  + m[A12]*Det3_235_015 - m[A15]*Det3_235_012;
492  double Det4_1235_0134 = m[A10]*Det3_235_134 - m[A11]*Det3_235_034
493  + m[A13]*Det3_235_014 - m[A14]*Det3_235_013;
494  double Det4_1235_0135 = m[A10]*Det3_235_135 - m[A11]*Det3_235_035
495  + m[A13]*Det3_235_015 - m[A15]*Det3_235_013;
496  double Det4_1235_0234 = m[A10]*Det3_235_234 - m[A12]*Det3_235_034
497  + m[A13]*Det3_235_024 - m[A14]*Det3_235_023;
498  double Det4_1235_0235 = m[A10]*Det3_235_235 - m[A12]*Det3_235_035
499  + m[A13]*Det3_235_025 - m[A15]*Det3_235_023;
500  double Det4_1235_1234 = m[A11]*Det3_235_234 - m[A12]*Det3_235_134
501  + m[A13]*Det3_235_124 - m[A14]*Det3_235_123;
502  double Det4_1235_1235 = m[A11]*Det3_235_235 - m[A12]*Det3_235_135
503  + m[A13]*Det3_235_125 - m[A15]*Det3_235_123;
504  double Det4_1245_0123 = m[A10]*Det3_245_123 - m[A11]*Det3_245_023
505  + m[A12]*Det3_245_013 - m[A13]*Det3_245_012;
506  double Det4_1245_0124 = m[A10]*Det3_245_124 - m[A11]*Det3_245_024
507  + m[A12]*Det3_245_014 - m[A14]*Det3_245_012;
508  double Det4_1245_0125 = m[A10]*Det3_245_125 - m[A11]*Det3_245_025
509  + m[A12]*Det3_245_015 - m[A15]*Det3_245_012;
510  double Det4_1245_0134 = m[A10]*Det3_245_134 - m[A11]*Det3_245_034
511  + m[A13]*Det3_245_014 - m[A14]*Det3_245_013;
512  double Det4_1245_0135 = m[A10]*Det3_245_135 - m[A11]*Det3_245_035
513  + m[A13]*Det3_245_015 - m[A15]*Det3_245_013;
514  double Det4_1245_0145 = m[A10]*Det3_245_145 - m[A11]*Det3_245_045
515  + m[A14]*Det3_245_015 - m[A15]*Det3_245_014;
516  double Det4_1245_0234 = m[A10]*Det3_245_234 - m[A12]*Det3_245_034
517  + m[A13]*Det3_245_024 - m[A14]*Det3_245_023;
518  double Det4_1245_0235 = m[A10]*Det3_245_235 - m[A12]*Det3_245_035
519  + m[A13]*Det3_245_025 - m[A15]*Det3_245_023;
520  double Det4_1245_0245 = m[A10]*Det3_245_245 - m[A12]*Det3_245_045
521  + m[A14]*Det3_245_025 - m[A15]*Det3_245_024;
522  double Det4_1245_1234 = m[A11]*Det3_245_234 - m[A12]*Det3_245_134
523  + m[A13]*Det3_245_124 - m[A14]*Det3_245_123;
524  double Det4_1245_1235 = m[A11]*Det3_245_235 - m[A12]*Det3_245_135
525  + m[A13]*Det3_245_125 - m[A15]*Det3_245_123;
526  double Det4_1245_1245 = m[A11]*Det3_245_245 - m[A12]*Det3_245_145
527  + m[A14]*Det3_245_125 - m[A15]*Det3_245_124;
528  double Det4_1345_0123 = m[A10]*Det3_345_123 - m[A11]*Det3_345_023
529  + m[A12]*Det3_345_013 - m[A13]*Det3_345_012;
530  double Det4_1345_0124 = m[A10]*Det3_345_124 - m[A11]*Det3_345_024
531  + m[A12]*Det3_345_014 - m[A14]*Det3_345_012;
532  double Det4_1345_0125 = m[A10]*Det3_345_125 - m[A11]*Det3_345_025
533  + m[A12]*Det3_345_015 - m[A15]*Det3_345_012;
534  double Det4_1345_0134 = m[A10]*Det3_345_134 - m[A11]*Det3_345_034
535  + m[A13]*Det3_345_014 - m[A14]*Det3_345_013;
536  double Det4_1345_0135 = m[A10]*Det3_345_135 - m[A11]*Det3_345_035
537  + m[A13]*Det3_345_015 - m[A15]*Det3_345_013;
538  double Det4_1345_0145 = m[A10]*Det3_345_145 - m[A11]*Det3_345_045
539  + m[A14]*Det3_345_015 - m[A15]*Det3_345_014;
540  double Det4_1345_0234 = m[A10]*Det3_345_234 - m[A12]*Det3_345_034
541  + m[A13]*Det3_345_024 - m[A14]*Det3_345_023;
542  double Det4_1345_0235 = m[A10]*Det3_345_235 - m[A12]*Det3_345_035
543  + m[A13]*Det3_345_025 - m[A15]*Det3_345_023;
544  double Det4_1345_0245 = m[A10]*Det3_345_245 - m[A12]*Det3_345_045
545  + m[A14]*Det3_345_025 - m[A15]*Det3_345_024;
546  double Det4_1345_0345 = m[A10]*Det3_345_345 - m[A13]*Det3_345_045
547  + m[A14]*Det3_345_035 - m[A15]*Det3_345_034;
548  double Det4_1345_1234 = m[A11]*Det3_345_234 - m[A12]*Det3_345_134
549  + m[A13]*Det3_345_124 - m[A14]*Det3_345_123;
550  double Det4_1345_1235 = m[A11]*Det3_345_235 - m[A12]*Det3_345_135
551  + m[A13]*Det3_345_125 - m[A15]*Det3_345_123;
552  double Det4_1345_1245 = m[A11]*Det3_345_245 - m[A12]*Det3_345_145
553  + m[A14]*Det3_345_125 - m[A15]*Det3_345_124;
554  double Det4_1345_1345 = m[A11]*Det3_345_345 - m[A13]*Det3_345_145
555  + m[A14]*Det3_345_135 - m[A15]*Det3_345_134;
556  double Det4_2345_0123 = m[A20]*Det3_345_123 - m[A21]*Det3_345_023
557  + m[A22]*Det3_345_013 - m[A23]*Det3_345_012;
558  double Det4_2345_0124 = m[A20]*Det3_345_124 - m[A21]*Det3_345_024
559  + m[A22]*Det3_345_014 - m[A24]*Det3_345_012;
560  double Det4_2345_0125 = m[A20]*Det3_345_125 - m[A21]*Det3_345_025
561  + m[A22]*Det3_345_015 - m[A25]*Det3_345_012;
562  double Det4_2345_0134 = m[A20]*Det3_345_134 - m[A21]*Det3_345_034
563  + m[A23]*Det3_345_014 - m[A24]*Det3_345_013;
564  double Det4_2345_0135 = m[A20]*Det3_345_135 - m[A21]*Det3_345_035
565  + m[A23]*Det3_345_015 - m[A25]*Det3_345_013;
566  double Det4_2345_0145 = m[A20]*Det3_345_145 - m[A21]*Det3_345_045
567  + m[A24]*Det3_345_015 - m[A25]*Det3_345_014;
568  double Det4_2345_0234 = m[A20]*Det3_345_234 - m[A22]*Det3_345_034
569  + m[A23]*Det3_345_024 - m[A24]*Det3_345_023;
570  double Det4_2345_0235 = m[A20]*Det3_345_235 - m[A22]*Det3_345_035
571  + m[A23]*Det3_345_025 - m[A25]*Det3_345_023;
572  double Det4_2345_0245 = m[A20]*Det3_345_245 - m[A22]*Det3_345_045
573  + m[A24]*Det3_345_025 - m[A25]*Det3_345_024;
574  double Det4_2345_0345 = m[A20]*Det3_345_345 - m[A23]*Det3_345_045
575  + m[A24]*Det3_345_035 - m[A25]*Det3_345_034;
576  double Det4_2345_1234 = m[A21]*Det3_345_234 - m[A22]*Det3_345_134
577  + m[A23]*Det3_345_124 - m[A24]*Det3_345_123;
578  double Det4_2345_1235 = m[A21]*Det3_345_235 - m[A22]*Det3_345_135
579  + m[A23]*Det3_345_125 - m[A25]*Det3_345_123;
580  double Det4_2345_1245 = m[A21]*Det3_345_245 - m[A22]*Det3_345_145
581  + m[A24]*Det3_345_125 - m[A25]*Det3_345_124;
582  double Det4_2345_1345 = m[A21]*Det3_345_345 - m[A23]*Det3_345_145
583  + m[A24]*Det3_345_135 - m[A25]*Det3_345_134;
584  double Det4_2345_2345 = m[A22]*Det3_345_345 - m[A23]*Det3_345_245
585  + m[A24]*Det3_345_235 - m[A25]*Det3_345_234;
586 
587  // Find all NECESSARY 5x5 dets: (19 of them)
588 
589  double Det5_01234_01234 = m[A00]*Det4_1234_1234 - m[A01]*Det4_1234_0234
590  + m[A02]*Det4_1234_0134 - m[A03]*Det4_1234_0124 + m[A04]*Det4_1234_0123;
591  double Det5_01235_01234 = m[A00]*Det4_1235_1234 - m[A01]*Det4_1235_0234
592  + m[A02]*Det4_1235_0134 - m[A03]*Det4_1235_0124 + m[A04]*Det4_1235_0123;
593  double Det5_01235_01235 = m[A00]*Det4_1235_1235 - m[A01]*Det4_1235_0235
594  + m[A02]*Det4_1235_0135 - m[A03]*Det4_1235_0125 + m[A05]*Det4_1235_0123;
595  double Det5_01245_01234 = m[A00]*Det4_1245_1234 - m[A01]*Det4_1245_0234
596  + m[A02]*Det4_1245_0134 - m[A03]*Det4_1245_0124 + m[A04]*Det4_1245_0123;
597  double Det5_01245_01235 = m[A00]*Det4_1245_1235 - m[A01]*Det4_1245_0235
598  + m[A02]*Det4_1245_0135 - m[A03]*Det4_1245_0125 + m[A05]*Det4_1245_0123;
599  double Det5_01245_01245 = m[A00]*Det4_1245_1245 - m[A01]*Det4_1245_0245
600  + m[A02]*Det4_1245_0145 - m[A04]*Det4_1245_0125 + m[A05]*Det4_1245_0124;
601  double Det5_01345_01234 = m[A00]*Det4_1345_1234 - m[A01]*Det4_1345_0234
602  + m[A02]*Det4_1345_0134 - m[A03]*Det4_1345_0124 + m[A04]*Det4_1345_0123;
603  double Det5_01345_01235 = m[A00]*Det4_1345_1235 - m[A01]*Det4_1345_0235
604  + m[A02]*Det4_1345_0135 - m[A03]*Det4_1345_0125 + m[A05]*Det4_1345_0123;
605  double Det5_01345_01245 = m[A00]*Det4_1345_1245 - m[A01]*Det4_1345_0245
606  + m[A02]*Det4_1345_0145 - m[A04]*Det4_1345_0125 + m[A05]*Det4_1345_0124;
607  double Det5_01345_01345 = m[A00]*Det4_1345_1345 - m[A01]*Det4_1345_0345
608  + m[A03]*Det4_1345_0145 - m[A04]*Det4_1345_0135 + m[A05]*Det4_1345_0134;
609  double Det5_02345_01234 = m[A00]*Det4_2345_1234 - m[A01]*Det4_2345_0234
610  + m[A02]*Det4_2345_0134 - m[A03]*Det4_2345_0124 + m[A04]*Det4_2345_0123;
611  double Det5_02345_01235 = m[A00]*Det4_2345_1235 - m[A01]*Det4_2345_0235
612  + m[A02]*Det4_2345_0135 - m[A03]*Det4_2345_0125 + m[A05]*Det4_2345_0123;
613  double Det5_02345_01245 = m[A00]*Det4_2345_1245 - m[A01]*Det4_2345_0245
614  + m[A02]*Det4_2345_0145 - m[A04]*Det4_2345_0125 + m[A05]*Det4_2345_0124;
615  double Det5_02345_01345 = m[A00]*Det4_2345_1345 - m[A01]*Det4_2345_0345
616  + m[A03]*Det4_2345_0145 - m[A04]*Det4_2345_0135 + m[A05]*Det4_2345_0134;
617  double Det5_02345_02345 = m[A00]*Det4_2345_2345 - m[A02]*Det4_2345_0345
618  + m[A03]*Det4_2345_0245 - m[A04]*Det4_2345_0235 + m[A05]*Det4_2345_0234;
619  double Det5_12345_01234 = m[A10]*Det4_2345_1234 - m[A11]*Det4_2345_0234
620  + m[A12]*Det4_2345_0134 - m[A13]*Det4_2345_0124 + m[A14]*Det4_2345_0123;
621  double Det5_12345_01235 = m[A10]*Det4_2345_1235 - m[A11]*Det4_2345_0235
622  + m[A12]*Det4_2345_0135 - m[A13]*Det4_2345_0125 + m[A15]*Det4_2345_0123;
623  double Det5_12345_01245 = m[A10]*Det4_2345_1245 - m[A11]*Det4_2345_0245
624  + m[A12]*Det4_2345_0145 - m[A14]*Det4_2345_0125 + m[A15]*Det4_2345_0124;
625  double Det5_12345_01345 = m[A10]*Det4_2345_1345 - m[A11]*Det4_2345_0345
626  + m[A13]*Det4_2345_0145 - m[A14]*Det4_2345_0135 + m[A15]*Det4_2345_0134;
627  double Det5_12345_02345 = m[A10]*Det4_2345_2345 - m[A12]*Det4_2345_0345
628  + m[A13]*Det4_2345_0245 - m[A14]*Det4_2345_0235 + m[A15]*Det4_2345_0234;
629  double Det5_12345_12345 = m[A11]*Det4_2345_2345 - m[A12]*Det4_2345_1345
630  + m[A13]*Det4_2345_1245 - m[A14]*Det4_2345_1235 + m[A15]*Det4_2345_1234;
631 
632  // Find the determinant
633 
634  double det = m[A00]*Det5_12345_12345
635  - m[A01]*Det5_12345_02345
636  + m[A02]*Det5_12345_01345
637  - m[A03]*Det5_12345_01245
638  + m[A04]*Det5_12345_01235
639  - m[A05]*Det5_12345_01234;
640 
641  if ( det == 0 ) {
642 #ifdef SINGULAR_DIAGNOSTICS
643  std::cerr << "Kramer's rule inversion of a singular 6x6 matrix: "
644  << *this << "\n";
645 #endif
646  ifail = 1;
647  return;
648  }
649 
650  double oneOverDet = 1.0/det;
651  double mn1OverDet = - oneOverDet;
652 
653  m[A00] = Det5_12345_12345*oneOverDet;
654  m[A01] = Det5_12345_02345*mn1OverDet;
655  m[A02] = Det5_12345_01345*oneOverDet;
656  m[A03] = Det5_12345_01245*mn1OverDet;
657  m[A04] = Det5_12345_01235*oneOverDet;
658  m[A05] = Det5_12345_01234*mn1OverDet;
659 
660  m[A11] = Det5_02345_02345*oneOverDet;
661  m[A12] = Det5_02345_01345*mn1OverDet;
662  m[A13] = Det5_02345_01245*oneOverDet;
663  m[A14] = Det5_02345_01235*mn1OverDet;
664  m[A15] = Det5_02345_01234*oneOverDet;
665 
666  m[A22] = Det5_01345_01345*oneOverDet;
667  m[A23] = Det5_01345_01245*mn1OverDet;
668  m[A24] = Det5_01345_01235*oneOverDet;
669  m[A25] = Det5_01345_01234*mn1OverDet;
670 
671  m[A33] = Det5_01245_01245*oneOverDet;
672  m[A34] = Det5_01245_01235*mn1OverDet;
673  m[A35] = Det5_01245_01234*oneOverDet;
674 
675  m[A44] = Det5_01235_01235*oneOverDet;
676  m[A45] = Det5_01235_01234*mn1OverDet;
677 
678  m[A55] = Det5_01234_01234*oneOverDet;
679 
680  return;
681 }
682 
683 void HepSymMatrix::invertCholesky5 (int & ifail) {
684 
685 // Invert by
686 //
687 // a) decomposing M = G*G^T with G lower triangular
688 // (if M is not positive definite this will fail, leaving this unchanged)
689 // b) inverting G to form H
690 // c) multiplying H^T * H to get M^-1.
691 //
692 // If the matrix is pos. def. it is inverted and 1 is returned.
693 // If the matrix is not pos. def. it remains unaltered and 0 is returned.
694 
695  double h10; // below-diagonal elements of H
696  double h20, h21;
697  double h30, h31, h32;
698  double h40, h41, h42, h43;
699 
700  double h00, h11, h22, h33, h44; // 1/diagonal elements of G =
701  // diagonal elements of H
702 
703  double g10; // below-diagonal elements of G
704  double g20, g21;
705  double g30, g31, g32;
706  double g40, g41, g42, g43;
707 
708  ifail = 1; // We start by assuing we won't succeed...
709 
710 // Form G -- compute diagonal members of H directly rather than of G
711 //-------
712 
713 // Scale first column by 1/sqrt(A00)
714 
715  h00 = m[A00];
716  if (h00 <= 0) return;
717  h00 = 1.0 / sqrt(h00);
718 
719  g10 = m[A10] * h00;
720  g20 = m[A20] * h00;
721  g30 = m[A30] * h00;
722  g40 = m[A40] * h00;
723 
724 // Form G11 (actually, just h11)
725 
726  h11 = m[A11] - (g10 * g10);
727  if (h11 <= 0) return;
728  h11 = 1.0 / sqrt(h11);
729 
730 // Subtract inter-column column dot products from rest of column 1 and
731 // scale to get column 1 of G
732 
733  g21 = (m[A21] - (g10 * g20)) * h11;
734  g31 = (m[A31] - (g10 * g30)) * h11;
735  g41 = (m[A41] - (g10 * g40)) * h11;
736 
737 // Form G22 (actually, just h22)
738 
739  h22 = m[A22] - (g20 * g20) - (g21 * g21);
740  if (h22 <= 0) return;
741  h22 = 1.0 / sqrt(h22);
742 
743 // Subtract inter-column column dot products from rest of column 2 and
744 // scale to get column 2 of G
745 
746  g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22;
747  g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22;
748 
749 // Form G33 (actually, just h33)
750 
751  h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32);
752  if (h33 <= 0) return;
753  h33 = 1.0 / sqrt(h33);
754 
755 // Subtract inter-column column dot product from A43 and scale to get G43
756 
757  g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33;
758 
759 // Finally form h44 - if this is possible inversion succeeds
760 
761  h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43);
762  if (h44 <= 0) return;
763  h44 = 1.0 / sqrt(h44);
764 
765 // Form H = 1/G -- diagonal members of H are already correct
766 //-------------
767 
768 // The order here is dictated by speed considerations
769 
770  h43 = -h33 * g43 * h44;
771  h32 = -h22 * g32 * h33;
772  h42 = -h22 * (g32 * h43 + g42 * h44);
773  h21 = -h11 * g21 * h22;
774  h31 = -h11 * (g21 * h32 + g31 * h33);
775  h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44);
776  h10 = -h00 * g10 * h11;
777  h20 = -h00 * (g10 * h21 + g20 * h22);
778  h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33);
779  h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44);
780 
781 // Change this to its inverse = H^T*H
782 //------------------------------------
783 
784  m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40;
785  m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41;
786  m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41;
787  m[A02] = h20 * h22 + h30 * h32 + h40 * h42;
788  m[A12] = h21 * h22 + h31 * h32 + h41 * h42;
789  m[A22] = h22 * h22 + h32 * h32 + h42 * h42;
790  m[A03] = h30 * h33 + h40 * h43;
791  m[A13] = h31 * h33 + h41 * h43;
792  m[A23] = h32 * h33 + h42 * h43;
793  m[A33] = h33 * h33 + h43 * h43;
794  m[A04] = h40 * h44;
795  m[A14] = h41 * h44;
796  m[A24] = h42 * h44;
797  m[A34] = h43 * h44;
798  m[A44] = h44 * h44;
799 
800  ifail = 0;
801  return;
802 
803 }
804 
805 
806 void HepSymMatrix::invertCholesky6 (int & ifail) {
807 
808 // Invert by
809 //
810 // a) decomposing M = G*G^T with G lower triangular
811 // (if M is not positive definite this will fail, leaving this unchanged)
812 // b) inverting G to form H
813 // c) multiplying H^T * H to get M^-1.
814 //
815 // If the matrix is pos. def. it is inverted and 1 is returned.
816 // If the matrix is not pos. def. it remains unaltered and 0 is returned.
817 
818  double h10; // below-diagonal elements of H
819  double h20, h21;
820  double h30, h31, h32;
821  double h40, h41, h42, h43;
822  double h50, h51, h52, h53, h54;
823 
824  double h00, h11, h22, h33, h44, h55; // 1/diagonal elements of G =
825  // diagonal elements of H
826 
827  double g10; // below-diagonal elements of G
828  double g20, g21;
829  double g30, g31, g32;
830  double g40, g41, g42, g43;
831  double g50, g51, g52, g53, g54;
832 
833  ifail = 1; // We start by assuing we won't succeed...
834 
835 // Form G -- compute diagonal members of H directly rather than of G
836 //-------
837 
838 // Scale first column by 1/sqrt(A00)
839 
840  h00 = m[A00];
841  if (h00 <= 0) return;
842  h00 = 1.0 / sqrt(h00);
843 
844  g10 = m[A10] * h00;
845  g20 = m[A20] * h00;
846  g30 = m[A30] * h00;
847  g40 = m[A40] * h00;
848  g50 = m[A50] * h00;
849 
850 // Form G11 (actually, just h11)
851 
852  h11 = m[A11] - (g10 * g10);
853  if (h11 <= 0) return;
854  h11 = 1.0 / sqrt(h11);
855 
856 // Subtract inter-column column dot products from rest of column 1 and
857 // scale to get column 1 of G
858 
859  g21 = (m[A21] - (g10 * g20)) * h11;
860  g31 = (m[A31] - (g10 * g30)) * h11;
861  g41 = (m[A41] - (g10 * g40)) * h11;
862  g51 = (m[A51] - (g10 * g50)) * h11;
863 
864 // Form G22 (actually, just h22)
865 
866  h22 = m[A22] - (g20 * g20) - (g21 * g21);
867  if (h22 <= 0) return;
868  h22 = 1.0 / sqrt(h22);
869 
870 // Subtract inter-column column dot products from rest of column 2 and
871 // scale to get column 2 of G
872 
873  g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22;
874  g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22;
875  g52 = (m[A52] - (g20 * g50) - (g21 * g51)) * h22;
876 
877 // Form G33 (actually, just h33)
878 
879  h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32);
880  if (h33 <= 0) return;
881  h33 = 1.0 / sqrt(h33);
882 
883 // Subtract inter-column column dot products from rest of column 3 and
884 // scale to get column 3 of G
885 
886  g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33;
887  g53 = (m[A53] - (g30 * g50) - (g31 * g51) - (g32 * g52)) * h33;
888 
889 // Form G44 (actually, just h44)
890 
891  h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43);
892  if (h44 <= 0) return;
893  h44 = 1.0 / sqrt(h44);
894 
895 // Subtract inter-column column dot product from M54 and scale to get G54
896 
897  g54 = (m[A54] - (g40 * g50) - (g41 * g51) - (g42 * g52) - (g43 * g53)) * h44;
898 
899 // Finally form h55 - if this is possible inversion succeeds
900 
901  h55 = m[A55] - (g50*g50) - (g51*g51) - (g52*g52) - (g53*g53) - (g54*g54);
902  if (h55 <= 0) return;
903  h55 = 1.0 / sqrt(h55);
904 
905 // Form H = 1/G -- diagonal members of H are already correct
906 //-------------
907 
908 // The order here is dictated by speed considerations
909 
910  h54 = -h44 * g54 * h55;
911  h43 = -h33 * g43 * h44;
912  h53 = -h33 * (g43 * h54 + g53 * h55);
913  h32 = -h22 * g32 * h33;
914  h42 = -h22 * (g32 * h43 + g42 * h44);
915  h52 = -h22 * (g32 * h53 + g42 * h54 + g52 * h55);
916  h21 = -h11 * g21 * h22;
917  h31 = -h11 * (g21 * h32 + g31 * h33);
918  h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44);
919  h51 = -h11 * (g21 * h52 + g31 * h53 + g41 * h54 + g51 * h55);
920  h10 = -h00 * g10 * h11;
921  h20 = -h00 * (g10 * h21 + g20 * h22);
922  h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33);
923  h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44);
924  h50 = -h00 * (g10 * h51 + g20 * h52 + g30 * h53 + g40 * h54 + g50 * h55);
925 
926 // Change this to its inverse = H^T*H
927 //------------------------------------
928 
929  m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40 + h50*h50;
930  m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41 + h50 * h51;
931  m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41 + h51 * h51;
932  m[A02] = h20 * h22 + h30 * h32 + h40 * h42 + h50 * h52;
933  m[A12] = h21 * h22 + h31 * h32 + h41 * h42 + h51 * h52;
934  m[A22] = h22 * h22 + h32 * h32 + h42 * h42 + h52 * h52;
935  m[A03] = h30 * h33 + h40 * h43 + h50 * h53;
936  m[A13] = h31 * h33 + h41 * h43 + h51 * h53;
937  m[A23] = h32 * h33 + h42 * h43 + h52 * h53;
938  m[A33] = h33 * h33 + h43 * h43 + h53 * h53;
939  m[A04] = h40 * h44 + h50 * h54;
940  m[A14] = h41 * h44 + h51 * h54;
941  m[A24] = h42 * h44 + h52 * h54;
942  m[A34] = h43 * h44 + h53 * h54;
943  m[A44] = h44 * h44 + h54 * h54;
944  m[A05] = h50 * h55;
945  m[A15] = h51 * h55;
946  m[A25] = h52 * h55;
947  m[A35] = h53 * h55;
948  m[A45] = h54 * h55;
949  m[A55] = h55 * h55;
950 
951  ifail = 0;
952  return;
953 
954 }
955 
956 
957 void HepSymMatrix::invert4 (int & ifail) {
958 
959  ifail = 0;
960 
961  // Find all NECESSARY 2x2 dets: (14 of them)
962 
963  double Det2_12_01 = m[A10]*m[A21] - m[A11]*m[A20];
964  double Det2_12_02 = m[A10]*m[A22] - m[A12]*m[A20];
965  double Det2_12_12 = m[A11]*m[A22] - m[A12]*m[A21];
966  double Det2_13_01 = m[A10]*m[A31] - m[A11]*m[A30];
967  double Det2_13_02 = m[A10]*m[A32] - m[A12]*m[A30];
968  double Det2_13_03 = m[A10]*m[A33] - m[A13]*m[A30];
969  double Det2_13_12 = m[A11]*m[A32] - m[A12]*m[A31];
970  double Det2_13_13 = m[A11]*m[A33] - m[A13]*m[A31];
971  double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30];
972  double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30];
973  double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30];
974  double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31];
975  double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31];
976  double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32];
977 
978  // Find all NECESSARY 3x3 dets: (10 of them)
979 
980  double Det3_012_012 = m[A00]*Det2_12_12 - m[A01]*Det2_12_02
981  + m[A02]*Det2_12_01;
982  double Det3_013_012 = m[A00]*Det2_13_12 - m[A01]*Det2_13_02
983  + m[A02]*Det2_13_01;
984  double Det3_013_013 = m[A00]*Det2_13_13 - m[A01]*Det2_13_03
985  + m[A03]*Det2_13_01;
986  double Det3_023_012 = m[A00]*Det2_23_12 - m[A01]*Det2_23_02
987  + m[A02]*Det2_23_01;
988  double Det3_023_013 = m[A00]*Det2_23_13 - m[A01]*Det2_23_03
989  + m[A03]*Det2_23_01;
990  double Det3_023_023 = m[A00]*Det2_23_23 - m[A02]*Det2_23_03
991  + m[A03]*Det2_23_02;
992  double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02
993  + m[A12]*Det2_23_01;
994  double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03
995  + m[A13]*Det2_23_01;
996  double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03
997  + m[A13]*Det2_23_02;
998  double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13
999  + m[A13]*Det2_23_12;
1000 
1001  // Find the 4x4 det:
1002 
1003  double det = m[A00]*Det3_123_123
1004  - m[A01]*Det3_123_023
1005  + m[A02]*Det3_123_013
1006  - m[A03]*Det3_123_012;
1007 
1008  if ( det == 0 ) {
1009 #ifdef SINGULAR_DIAGNOSTICS
1010  std::cerr << "Kramer's rule inversion of a singular 4x4 matrix: "
1011  << *this << "\n";
1012 #endif
1013  ifail = 1;
1014  return;
1015  }
1016 
1017  double oneOverDet = 1.0/det;
1018  double mn1OverDet = - oneOverDet;
1019 
1020  m[A00] = Det3_123_123 * oneOverDet;
1021  m[A01] = Det3_123_023 * mn1OverDet;
1022  m[A02] = Det3_123_013 * oneOverDet;
1023  m[A03] = Det3_123_012 * mn1OverDet;
1024 
1025 
1026  m[A11] = Det3_023_023 * oneOverDet;
1027  m[A12] = Det3_023_013 * mn1OverDet;
1028  m[A13] = Det3_023_012 * oneOverDet;
1029 
1030  m[A22] = Det3_013_013 * oneOverDet;
1031  m[A23] = Det3_013_012 * mn1OverDet;
1032 
1033  m[A33] = Det3_012_012 * oneOverDet;
1034 
1035  return;
1036 }
1037 
1038 void HepSymMatrix::invertHaywood4 (int & ifail) {
1039  invert4(ifail); // For the 4x4 case, the method we use for invert is already
1040  // the Haywood method.
1041 }
1042 
1043 } // namespace CLHEP
1044 
A05
#define A05
Definition: SymMatrixInvert.cc:39
A02
#define A02
Definition: SymMatrixInvert.cc:36
A31
#define A31
Definition: SymMatrixInvert.cc:56
CLHEP::HepSymMatrix::invertCholesky6
void invertCholesky6(int &ifail)
Definition: SymMatrixInvert.cc:806
A42
#define A42
Definition: SymMatrixInvert.cc:64
CLHEP::HepSymMatrix::invertHaywood5
void invertHaywood5(int &ifail)
Definition: SymMatrixInvert.cc:124
A14
#define A14
Definition: SymMatrixInvert.cc:45
A52
#define A52
Definition: SymMatrixInvert.cc:71
A55
#define A55
Definition: SymMatrixInvert.cc:74
A22
#define A22
Definition: SymMatrixInvert.cc:50
A23
#define A23
Definition: SymMatrixInvert.cc:51
A32
#define A32
Definition: SymMatrixInvert.cc:57
A44
#define A44
Definition: SymMatrixInvert.cc:66
A12
#define A12
Definition: SymMatrixInvert.cc:43
A10
#define A10
Definition: SymMatrixInvert.cc:41
A03
#define A03
Definition: SymMatrixInvert.cc:37
A53
#define A53
Definition: SymMatrixInvert.cc:72
A21
#define A21
Definition: SymMatrixInvert.cc:49
A25
#define A25
Definition: SymMatrixInvert.cc:53
A20
#define A20
Definition: SymMatrixInvert.cc:48
A24
#define A24
Definition: SymMatrixInvert.cc:52
A00
#define A00
Definition: SymMatrixInvert.cc:34
A43
#define A43
Definition: SymMatrixInvert.cc:65
A34
#define A34
Definition: SymMatrixInvert.cc:59
A51
#define A51
Definition: SymMatrixInvert.cc:70
CLHEP
Definition: ClhepVersion.h:13
CLHEP::HepSymMatrix::invertHaywood4
void invertHaywood4(int &ifail)
Definition: SymMatrixInvert.cc:1038
A30
#define A30
Definition: SymMatrixInvert.cc:55
A13
#define A13
Definition: SymMatrixInvert.cc:44
A04
#define A04
Definition: SymMatrixInvert.cc:38
CLHEP::HepSymMatrix::invertCholesky5
void invertCholesky5(int &ifail)
Definition: SymMatrixInvert.cc:683
A41
#define A41
Definition: SymMatrixInvert.cc:63
A40
#define A40
Definition: SymMatrixInvert.cc:62
A50
#define A50
Definition: SymMatrixInvert.cc:69
A01
#define A01
Definition: SymMatrixInvert.cc:35
A35
#define A35
Definition: SymMatrixInvert.cc:60
A33
#define A33
Definition: SymMatrixInvert.cc:58
A15
#define A15
Definition: SymMatrixInvert.cc:46
CLHEP::HepSymMatrix::invertHaywood6
void invertHaywood6(int &ifail)
Definition: SymMatrixInvert.cc:295
A45
#define A45
Definition: SymMatrixInvert.cc:67
A54
#define A54
Definition: SymMatrixInvert.cc:73
A11
#define A11
Definition: SymMatrixInvert.cc:42