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
19namespace CLHEP {
20
21double HepSymMatrix::posDefFraction5x5 = 1.0;
22double HepSymMatrix::posDefFraction6x6 = 1.0;
23double HepSymMatrix::adjustment5x5 = 0.0;
24double HepSymMatrix::adjustment6x6 = 0.0;
25const double HepSymMatrix::CHOLESKY_THRESHOLD_5x5 = .5;
26const double HepSymMatrix::CHOLESKY_THRESHOLD_6x6 = .2;
27const double HepSymMatrix::CHOLESKY_CREEP_5x5 = .005;
28const 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
77void 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
100void 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
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
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
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
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
957void 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
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
#define A41
#define A20
#define A01
#define A53
#define A23
#define A45
#define A13
#define A00
#define A54
#define A40
#define A25
#define A02
#define A24
#define A22
#define A04
#define A30
#define A12
#define A55
#define A35
#define A50
#define A03
#define A14
#define A51
#define A21
#define A11
#define A10
#define A44
#define A05
#define A32
#define A31
#define A33
#define A42
#define A52
#define A15
#define A34
#define A43
void invertHaywood5(int &ifail)
void invertCholesky6(int &ifail)
void invertHaywood4(int &ifail)
void invertCholesky5(int &ifail)
void invertHaywood6(int &ifail)