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