CLHEP Home Page CLHEP Documentation CLHEP Bug Reports
|
|
CLHEP VERSION Reference Documentation
CLHEP Home Page CLHEP Documentation CLHEP Bug Reports |
00001 // -*- C++ -*- 00002 // 00003 // This file is a part of the CLHEP - a Class Library for High Energy Physics. 00004 // 00005 // This software written by Mark Fischler and Steven Haywood 00006 // 00007 00008 #ifdef GNUPRAGMA 00009 #pragma implementation 00010 #endif 00011 00012 #include <string.h> 00013 #include <float.h> // for DBL_EPSILON 00014 #include <cmath> 00015 00016 #include "CLHEP/Matrix/defs.h" 00017 #include "CLHEP/Matrix/SymMatrix.h" 00018 00019 namespace CLHEP { 00020 00021 double HepSymMatrix::posDefFraction5x5 = 1.0; 00022 double HepSymMatrix::posDefFraction6x6 = 1.0; 00023 double HepSymMatrix::adjustment5x5 = 0.0; 00024 double HepSymMatrix::adjustment6x6 = 0.0; 00025 const double HepSymMatrix::CHOLESKY_THRESHOLD_5x5 = .5; 00026 const double HepSymMatrix::CHOLESKY_THRESHOLD_6x6 = .2; 00027 const double HepSymMatrix::CHOLESKY_CREEP_5x5 = .005; 00028 const double HepSymMatrix::CHOLESKY_CREEP_6x6 = .002; 00029 00030 // Aij are indices for a 6x6 symmetric matrix. 00031 // The indices for 5x5 or 4x4 symmetric matrices are the same, 00032 // ignoring all combinations with an index which is inapplicable. 00033 00034 #define A00 0 00035 #define A01 1 00036 #define A02 3 00037 #define A03 6 00038 #define A04 10 00039 #define A05 15 00040 00041 #define A10 1 00042 #define A11 2 00043 #define A12 4 00044 #define A13 7 00045 #define A14 11 00046 #define A15 16 00047 00048 #define A20 3 00049 #define A21 4 00050 #define A22 5 00051 #define A23 8 00052 #define A24 12 00053 #define A25 17 00054 00055 #define A30 6 00056 #define A31 7 00057 #define A32 8 00058 #define A33 9 00059 #define A34 13 00060 #define A35 18 00061 00062 #define A40 10 00063 #define A41 11 00064 #define A42 12 00065 #define A43 13 00066 #define A44 14 00067 #define A45 19 00068 00069 #define A50 15 00070 #define A51 16 00071 #define A52 17 00072 #define A53 18 00073 #define A54 19 00074 #define A55 20 00075 00076 00077 void HepSymMatrix::invert5(int & ifail) { 00078 if (posDefFraction5x5 >= CHOLESKY_THRESHOLD_5x5) { 00079 invertCholesky5(ifail); 00080 posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail); 00081 if (ifail!=0) { // Cholesky failed -- invert using Haywood 00082 invertHaywood5(ifail); 00083 } 00084 } else { 00085 if (posDefFraction5x5 + adjustment5x5 >= CHOLESKY_THRESHOLD_5x5) { 00086 invertCholesky5(ifail); 00087 posDefFraction5x5 = .9*posDefFraction5x5 + .1*(1-ifail); 00088 if (ifail!=0) { // Cholesky failed -- invert using Haywood 00089 invertHaywood5(ifail); 00090 adjustment5x5 = 0; 00091 } 00092 } else { 00093 invertHaywood5(ifail); 00094 adjustment5x5 += CHOLESKY_CREEP_5x5; 00095 } 00096 } 00097 return; 00098 } 00099 00100 void HepSymMatrix::invert6(int & ifail) { 00101 if (posDefFraction6x6 >= CHOLESKY_THRESHOLD_6x6) { 00102 invertCholesky6(ifail); 00103 posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail); 00104 if (ifail!=0) { // Cholesky failed -- invert using Haywood 00105 invertHaywood6(ifail); 00106 } 00107 } else { 00108 if (posDefFraction6x6 + adjustment6x6 >= CHOLESKY_THRESHOLD_6x6) { 00109 invertCholesky6(ifail); 00110 posDefFraction6x6 = .9*posDefFraction6x6 + .1*(1-ifail); 00111 if (ifail!=0) { // Cholesky failed -- invert using Haywood 00112 invertHaywood6(ifail); 00113 adjustment6x6 = 0; 00114 } 00115 } else { 00116 invertHaywood6(ifail); 00117 adjustment6x6 += CHOLESKY_CREEP_6x6; 00118 } 00119 } 00120 return; 00121 } 00122 00123 00124 void HepSymMatrix::invertHaywood5 (int & ifail) { 00125 00126 ifail = 0; 00127 00128 // Find all NECESSARY 2x2 dets: (25 of them) 00129 00130 double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30]; 00131 double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30]; 00132 double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30]; 00133 double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31]; 00134 double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31]; 00135 double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32]; 00136 double Det2_24_01 = m[A20]*m[A41] - m[A21]*m[A40]; 00137 double Det2_24_02 = m[A20]*m[A42] - m[A22]*m[A40]; 00138 double Det2_24_03 = m[A20]*m[A43] - m[A23]*m[A40]; 00139 double Det2_24_04 = m[A20]*m[A44] - m[A24]*m[A40]; 00140 double Det2_24_12 = m[A21]*m[A42] - m[A22]*m[A41]; 00141 double Det2_24_13 = m[A21]*m[A43] - m[A23]*m[A41]; 00142 double Det2_24_14 = m[A21]*m[A44] - m[A24]*m[A41]; 00143 double Det2_24_23 = m[A22]*m[A43] - m[A23]*m[A42]; 00144 double Det2_24_24 = m[A22]*m[A44] - m[A24]*m[A42]; 00145 double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40]; 00146 double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40]; 00147 double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40]; 00148 double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40]; 00149 double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41]; 00150 double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41]; 00151 double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41]; 00152 double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42]; 00153 double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42]; 00154 double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43]; 00155 00156 // Find all NECESSARY 3x3 dets: (30 of them) 00157 00158 double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02 00159 + m[A12]*Det2_23_01; 00160 double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03 00161 + m[A13]*Det2_23_01; 00162 double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03 00163 + m[A13]*Det2_23_02; 00164 double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13 00165 + m[A13]*Det2_23_12; 00166 double Det3_124_012 = m[A10]*Det2_24_12 - m[A11]*Det2_24_02 00167 + m[A12]*Det2_24_01; 00168 double Det3_124_013 = m[A10]*Det2_24_13 - m[A11]*Det2_24_03 00169 + m[A13]*Det2_24_01; 00170 double Det3_124_014 = m[A10]*Det2_24_14 - m[A11]*Det2_24_04 00171 + m[A14]*Det2_24_01; 00172 double Det3_124_023 = m[A10]*Det2_24_23 - m[A12]*Det2_24_03 00173 + m[A13]*Det2_24_02; 00174 double Det3_124_024 = m[A10]*Det2_24_24 - m[A12]*Det2_24_04 00175 + m[A14]*Det2_24_02; 00176 double Det3_124_123 = m[A11]*Det2_24_23 - m[A12]*Det2_24_13 00177 + m[A13]*Det2_24_12; 00178 double Det3_124_124 = m[A11]*Det2_24_24 - m[A12]*Det2_24_14 00179 + m[A14]*Det2_24_12; 00180 double Det3_134_012 = m[A10]*Det2_34_12 - m[A11]*Det2_34_02 00181 + m[A12]*Det2_34_01; 00182 double Det3_134_013 = m[A10]*Det2_34_13 - m[A11]*Det2_34_03 00183 + m[A13]*Det2_34_01; 00184 double Det3_134_014 = m[A10]*Det2_34_14 - m[A11]*Det2_34_04 00185 + m[A14]*Det2_34_01; 00186 double Det3_134_023 = m[A10]*Det2_34_23 - m[A12]*Det2_34_03 00187 + m[A13]*Det2_34_02; 00188 double Det3_134_024 = m[A10]*Det2_34_24 - m[A12]*Det2_34_04 00189 + m[A14]*Det2_34_02; 00190 double Det3_134_034 = m[A10]*Det2_34_34 - m[A13]*Det2_34_04 00191 + m[A14]*Det2_34_03; 00192 double Det3_134_123 = m[A11]*Det2_34_23 - m[A12]*Det2_34_13 00193 + m[A13]*Det2_34_12; 00194 double Det3_134_124 = m[A11]*Det2_34_24 - m[A12]*Det2_34_14 00195 + m[A14]*Det2_34_12; 00196 double Det3_134_134 = m[A11]*Det2_34_34 - m[A13]*Det2_34_14 00197 + m[A14]*Det2_34_13; 00198 double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02 00199 + m[A22]*Det2_34_01; 00200 double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03 00201 + m[A23]*Det2_34_01; 00202 double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04 00203 + m[A24]*Det2_34_01; 00204 double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03 00205 + m[A23]*Det2_34_02; 00206 double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04 00207 + m[A24]*Det2_34_02; 00208 double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04 00209 + m[A24]*Det2_34_03; 00210 double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13 00211 + m[A23]*Det2_34_12; 00212 double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14 00213 + m[A24]*Det2_34_12; 00214 double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14 00215 + m[A24]*Det2_34_13; 00216 double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24 00217 + m[A24]*Det2_34_23; 00218 00219 // Find all NECESSARY 4x4 dets: (15 of them) 00220 00221 double Det4_0123_0123 = m[A00]*Det3_123_123 - m[A01]*Det3_123_023 00222 + m[A02]*Det3_123_013 - m[A03]*Det3_123_012; 00223 double Det4_0124_0123 = m[A00]*Det3_124_123 - m[A01]*Det3_124_023 00224 + m[A02]*Det3_124_013 - m[A03]*Det3_124_012; 00225 double Det4_0124_0124 = m[A00]*Det3_124_124 - m[A01]*Det3_124_024 00226 + m[A02]*Det3_124_014 - m[A04]*Det3_124_012; 00227 double Det4_0134_0123 = m[A00]*Det3_134_123 - m[A01]*Det3_134_023 00228 + m[A02]*Det3_134_013 - m[A03]*Det3_134_012; 00229 double Det4_0134_0124 = m[A00]*Det3_134_124 - m[A01]*Det3_134_024 00230 + m[A02]*Det3_134_014 - m[A04]*Det3_134_012; 00231 double Det4_0134_0134 = m[A00]*Det3_134_134 - m[A01]*Det3_134_034 00232 + m[A03]*Det3_134_014 - m[A04]*Det3_134_013; 00233 double Det4_0234_0123 = m[A00]*Det3_234_123 - m[A01]*Det3_234_023 00234 + m[A02]*Det3_234_013 - m[A03]*Det3_234_012; 00235 double Det4_0234_0124 = m[A00]*Det3_234_124 - m[A01]*Det3_234_024 00236 + m[A02]*Det3_234_014 - m[A04]*Det3_234_012; 00237 double Det4_0234_0134 = m[A00]*Det3_234_134 - m[A01]*Det3_234_034 00238 + m[A03]*Det3_234_014 - m[A04]*Det3_234_013; 00239 double Det4_0234_0234 = m[A00]*Det3_234_234 - m[A02]*Det3_234_034 00240 + m[A03]*Det3_234_024 - m[A04]*Det3_234_023; 00241 double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023 00242 + m[A12]*Det3_234_013 - m[A13]*Det3_234_012; 00243 double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024 00244 + m[A12]*Det3_234_014 - m[A14]*Det3_234_012; 00245 double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034 00246 + m[A13]*Det3_234_014 - m[A14]*Det3_234_013; 00247 double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034 00248 + m[A13]*Det3_234_024 - m[A14]*Det3_234_023; 00249 double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134 00250 + m[A13]*Det3_234_124 - m[A14]*Det3_234_123; 00251 00252 // Find the 5x5 det: 00253 00254 double det = m[A00]*Det4_1234_1234 00255 - m[A01]*Det4_1234_0234 00256 + m[A02]*Det4_1234_0134 00257 - m[A03]*Det4_1234_0124 00258 + m[A04]*Det4_1234_0123; 00259 00260 if ( det == 0 ) { 00261 #ifdef SINGULAR_DIAGNOSTICS 00262 std::cerr << "Kramer's rule inversion of a singular 5x5 matrix: " 00263 << *this << "\n"; 00264 #endif 00265 ifail = 1; 00266 return; 00267 } 00268 00269 double oneOverDet = 1.0/det; 00270 double mn1OverDet = - oneOverDet; 00271 00272 m[A00] = Det4_1234_1234 * oneOverDet; 00273 m[A01] = Det4_1234_0234 * mn1OverDet; 00274 m[A02] = Det4_1234_0134 * oneOverDet; 00275 m[A03] = Det4_1234_0124 * mn1OverDet; 00276 m[A04] = Det4_1234_0123 * oneOverDet; 00277 00278 m[A11] = Det4_0234_0234 * oneOverDet; 00279 m[A12] = Det4_0234_0134 * mn1OverDet; 00280 m[A13] = Det4_0234_0124 * oneOverDet; 00281 m[A14] = Det4_0234_0123 * mn1OverDet; 00282 00283 m[A22] = Det4_0134_0134 * oneOverDet; 00284 m[A23] = Det4_0134_0124 * mn1OverDet; 00285 m[A24] = Det4_0134_0123 * oneOverDet; 00286 00287 m[A33] = Det4_0124_0124 * oneOverDet; 00288 m[A34] = Det4_0124_0123 * mn1OverDet; 00289 00290 m[A44] = Det4_0123_0123 * oneOverDet; 00291 00292 return; 00293 } 00294 00295 void HepSymMatrix::invertHaywood6 (int & ifail) { 00296 00297 ifail = 0; 00298 00299 // Find all NECESSARY 2x2 dets: (39 of them) 00300 00301 double Det2_34_01 = m[A30]*m[A41] - m[A31]*m[A40]; 00302 double Det2_34_02 = m[A30]*m[A42] - m[A32]*m[A40]; 00303 double Det2_34_03 = m[A30]*m[A43] - m[A33]*m[A40]; 00304 double Det2_34_04 = m[A30]*m[A44] - m[A34]*m[A40]; 00305 double Det2_34_12 = m[A31]*m[A42] - m[A32]*m[A41]; 00306 double Det2_34_13 = m[A31]*m[A43] - m[A33]*m[A41]; 00307 double Det2_34_14 = m[A31]*m[A44] - m[A34]*m[A41]; 00308 double Det2_34_23 = m[A32]*m[A43] - m[A33]*m[A42]; 00309 double Det2_34_24 = m[A32]*m[A44] - m[A34]*m[A42]; 00310 double Det2_34_34 = m[A33]*m[A44] - m[A34]*m[A43]; 00311 double Det2_35_01 = m[A30]*m[A51] - m[A31]*m[A50]; 00312 double Det2_35_02 = m[A30]*m[A52] - m[A32]*m[A50]; 00313 double Det2_35_03 = m[A30]*m[A53] - m[A33]*m[A50]; 00314 double Det2_35_04 = m[A30]*m[A54] - m[A34]*m[A50]; 00315 double Det2_35_05 = m[A30]*m[A55] - m[A35]*m[A50]; 00316 double Det2_35_12 = m[A31]*m[A52] - m[A32]*m[A51]; 00317 double Det2_35_13 = m[A31]*m[A53] - m[A33]*m[A51]; 00318 double Det2_35_14 = m[A31]*m[A54] - m[A34]*m[A51]; 00319 double Det2_35_15 = m[A31]*m[A55] - m[A35]*m[A51]; 00320 double Det2_35_23 = m[A32]*m[A53] - m[A33]*m[A52]; 00321 double Det2_35_24 = m[A32]*m[A54] - m[A34]*m[A52]; 00322 double Det2_35_25 = m[A32]*m[A55] - m[A35]*m[A52]; 00323 double Det2_35_34 = m[A33]*m[A54] - m[A34]*m[A53]; 00324 double Det2_35_35 = m[A33]*m[A55] - m[A35]*m[A53]; 00325 double Det2_45_01 = m[A40]*m[A51] - m[A41]*m[A50]; 00326 double Det2_45_02 = m[A40]*m[A52] - m[A42]*m[A50]; 00327 double Det2_45_03 = m[A40]*m[A53] - m[A43]*m[A50]; 00328 double Det2_45_04 = m[A40]*m[A54] - m[A44]*m[A50]; 00329 double Det2_45_05 = m[A40]*m[A55] - m[A45]*m[A50]; 00330 double Det2_45_12 = m[A41]*m[A52] - m[A42]*m[A51]; 00331 double Det2_45_13 = m[A41]*m[A53] - m[A43]*m[A51]; 00332 double Det2_45_14 = m[A41]*m[A54] - m[A44]*m[A51]; 00333 double Det2_45_15 = m[A41]*m[A55] - m[A45]*m[A51]; 00334 double Det2_45_23 = m[A42]*m[A53] - m[A43]*m[A52]; 00335 double Det2_45_24 = m[A42]*m[A54] - m[A44]*m[A52]; 00336 double Det2_45_25 = m[A42]*m[A55] - m[A45]*m[A52]; 00337 double Det2_45_34 = m[A43]*m[A54] - m[A44]*m[A53]; 00338 double Det2_45_35 = m[A43]*m[A55] - m[A45]*m[A53]; 00339 double Det2_45_45 = m[A44]*m[A55] - m[A45]*m[A54]; 00340 00341 // Find all NECESSARY 3x3 dets: (65 of them) 00342 00343 double Det3_234_012 = m[A20]*Det2_34_12 - m[A21]*Det2_34_02 00344 + m[A22]*Det2_34_01; 00345 double Det3_234_013 = m[A20]*Det2_34_13 - m[A21]*Det2_34_03 00346 + m[A23]*Det2_34_01; 00347 double Det3_234_014 = m[A20]*Det2_34_14 - m[A21]*Det2_34_04 00348 + m[A24]*Det2_34_01; 00349 double Det3_234_023 = m[A20]*Det2_34_23 - m[A22]*Det2_34_03 00350 + m[A23]*Det2_34_02; 00351 double Det3_234_024 = m[A20]*Det2_34_24 - m[A22]*Det2_34_04 00352 + m[A24]*Det2_34_02; 00353 double Det3_234_034 = m[A20]*Det2_34_34 - m[A23]*Det2_34_04 00354 + m[A24]*Det2_34_03; 00355 double Det3_234_123 = m[A21]*Det2_34_23 - m[A22]*Det2_34_13 00356 + m[A23]*Det2_34_12; 00357 double Det3_234_124 = m[A21]*Det2_34_24 - m[A22]*Det2_34_14 00358 + m[A24]*Det2_34_12; 00359 double Det3_234_134 = m[A21]*Det2_34_34 - m[A23]*Det2_34_14 00360 + m[A24]*Det2_34_13; 00361 double Det3_234_234 = m[A22]*Det2_34_34 - m[A23]*Det2_34_24 00362 + m[A24]*Det2_34_23; 00363 double Det3_235_012 = m[A20]*Det2_35_12 - m[A21]*Det2_35_02 00364 + m[A22]*Det2_35_01; 00365 double Det3_235_013 = m[A20]*Det2_35_13 - m[A21]*Det2_35_03 00366 + m[A23]*Det2_35_01; 00367 double Det3_235_014 = m[A20]*Det2_35_14 - m[A21]*Det2_35_04 00368 + m[A24]*Det2_35_01; 00369 double Det3_235_015 = m[A20]*Det2_35_15 - m[A21]*Det2_35_05 00370 + m[A25]*Det2_35_01; 00371 double Det3_235_023 = m[A20]*Det2_35_23 - m[A22]*Det2_35_03 00372 + m[A23]*Det2_35_02; 00373 double Det3_235_024 = m[A20]*Det2_35_24 - m[A22]*Det2_35_04 00374 + m[A24]*Det2_35_02; 00375 double Det3_235_025 = m[A20]*Det2_35_25 - m[A22]*Det2_35_05 00376 + m[A25]*Det2_35_02; 00377 double Det3_235_034 = m[A20]*Det2_35_34 - m[A23]*Det2_35_04 00378 + m[A24]*Det2_35_03; 00379 double Det3_235_035 = m[A20]*Det2_35_35 - m[A23]*Det2_35_05 00380 + m[A25]*Det2_35_03; 00381 double Det3_235_123 = m[A21]*Det2_35_23 - m[A22]*Det2_35_13 00382 + m[A23]*Det2_35_12; 00383 double Det3_235_124 = m[A21]*Det2_35_24 - m[A22]*Det2_35_14 00384 + m[A24]*Det2_35_12; 00385 double Det3_235_125 = m[A21]*Det2_35_25 - m[A22]*Det2_35_15 00386 + m[A25]*Det2_35_12; 00387 double Det3_235_134 = m[A21]*Det2_35_34 - m[A23]*Det2_35_14 00388 + m[A24]*Det2_35_13; 00389 double Det3_235_135 = m[A21]*Det2_35_35 - m[A23]*Det2_35_15 00390 + m[A25]*Det2_35_13; 00391 double Det3_235_234 = m[A22]*Det2_35_34 - m[A23]*Det2_35_24 00392 + m[A24]*Det2_35_23; 00393 double Det3_235_235 = m[A22]*Det2_35_35 - m[A23]*Det2_35_25 00394 + m[A25]*Det2_35_23; 00395 double Det3_245_012 = m[A20]*Det2_45_12 - m[A21]*Det2_45_02 00396 + m[A22]*Det2_45_01; 00397 double Det3_245_013 = m[A20]*Det2_45_13 - m[A21]*Det2_45_03 00398 + m[A23]*Det2_45_01; 00399 double Det3_245_014 = m[A20]*Det2_45_14 - m[A21]*Det2_45_04 00400 + m[A24]*Det2_45_01; 00401 double Det3_245_015 = m[A20]*Det2_45_15 - m[A21]*Det2_45_05 00402 + m[A25]*Det2_45_01; 00403 double Det3_245_023 = m[A20]*Det2_45_23 - m[A22]*Det2_45_03 00404 + m[A23]*Det2_45_02; 00405 double Det3_245_024 = m[A20]*Det2_45_24 - m[A22]*Det2_45_04 00406 + m[A24]*Det2_45_02; 00407 double Det3_245_025 = m[A20]*Det2_45_25 - m[A22]*Det2_45_05 00408 + m[A25]*Det2_45_02; 00409 double Det3_245_034 = m[A20]*Det2_45_34 - m[A23]*Det2_45_04 00410 + m[A24]*Det2_45_03; 00411 double Det3_245_035 = m[A20]*Det2_45_35 - m[A23]*Det2_45_05 00412 + m[A25]*Det2_45_03; 00413 double Det3_245_045 = m[A20]*Det2_45_45 - m[A24]*Det2_45_05 00414 + m[A25]*Det2_45_04; 00415 double Det3_245_123 = m[A21]*Det2_45_23 - m[A22]*Det2_45_13 00416 + m[A23]*Det2_45_12; 00417 double Det3_245_124 = m[A21]*Det2_45_24 - m[A22]*Det2_45_14 00418 + m[A24]*Det2_45_12; 00419 double Det3_245_125 = m[A21]*Det2_45_25 - m[A22]*Det2_45_15 00420 + m[A25]*Det2_45_12; 00421 double Det3_245_134 = m[A21]*Det2_45_34 - m[A23]*Det2_45_14 00422 + m[A24]*Det2_45_13; 00423 double Det3_245_135 = m[A21]*Det2_45_35 - m[A23]*Det2_45_15 00424 + m[A25]*Det2_45_13; 00425 double Det3_245_145 = m[A21]*Det2_45_45 - m[A24]*Det2_45_15 00426 + m[A25]*Det2_45_14; 00427 double Det3_245_234 = m[A22]*Det2_45_34 - m[A23]*Det2_45_24 00428 + m[A24]*Det2_45_23; 00429 double Det3_245_235 = m[A22]*Det2_45_35 - m[A23]*Det2_45_25 00430 + m[A25]*Det2_45_23; 00431 double Det3_245_245 = m[A22]*Det2_45_45 - m[A24]*Det2_45_25 00432 + m[A25]*Det2_45_24; 00433 double Det3_345_012 = m[A30]*Det2_45_12 - m[A31]*Det2_45_02 00434 + m[A32]*Det2_45_01; 00435 double Det3_345_013 = m[A30]*Det2_45_13 - m[A31]*Det2_45_03 00436 + m[A33]*Det2_45_01; 00437 double Det3_345_014 = m[A30]*Det2_45_14 - m[A31]*Det2_45_04 00438 + m[A34]*Det2_45_01; 00439 double Det3_345_015 = m[A30]*Det2_45_15 - m[A31]*Det2_45_05 00440 + m[A35]*Det2_45_01; 00441 double Det3_345_023 = m[A30]*Det2_45_23 - m[A32]*Det2_45_03 00442 + m[A33]*Det2_45_02; 00443 double Det3_345_024 = m[A30]*Det2_45_24 - m[A32]*Det2_45_04 00444 + m[A34]*Det2_45_02; 00445 double Det3_345_025 = m[A30]*Det2_45_25 - m[A32]*Det2_45_05 00446 + m[A35]*Det2_45_02; 00447 double Det3_345_034 = m[A30]*Det2_45_34 - m[A33]*Det2_45_04 00448 + m[A34]*Det2_45_03; 00449 double Det3_345_035 = m[A30]*Det2_45_35 - m[A33]*Det2_45_05 00450 + m[A35]*Det2_45_03; 00451 double Det3_345_045 = m[A30]*Det2_45_45 - m[A34]*Det2_45_05 00452 + m[A35]*Det2_45_04; 00453 double Det3_345_123 = m[A31]*Det2_45_23 - m[A32]*Det2_45_13 00454 + m[A33]*Det2_45_12; 00455 double Det3_345_124 = m[A31]*Det2_45_24 - m[A32]*Det2_45_14 00456 + m[A34]*Det2_45_12; 00457 double Det3_345_125 = m[A31]*Det2_45_25 - m[A32]*Det2_45_15 00458 + m[A35]*Det2_45_12; 00459 double Det3_345_134 = m[A31]*Det2_45_34 - m[A33]*Det2_45_14 00460 + m[A34]*Det2_45_13; 00461 double Det3_345_135 = m[A31]*Det2_45_35 - m[A33]*Det2_45_15 00462 + m[A35]*Det2_45_13; 00463 double Det3_345_145 = m[A31]*Det2_45_45 - m[A34]*Det2_45_15 00464 + m[A35]*Det2_45_14; 00465 double Det3_345_234 = m[A32]*Det2_45_34 - m[A33]*Det2_45_24 00466 + m[A34]*Det2_45_23; 00467 double Det3_345_235 = m[A32]*Det2_45_35 - m[A33]*Det2_45_25 00468 + m[A35]*Det2_45_23; 00469 double Det3_345_245 = m[A32]*Det2_45_45 - m[A34]*Det2_45_25 00470 + m[A35]*Det2_45_24; 00471 double Det3_345_345 = m[A33]*Det2_45_45 - m[A34]*Det2_45_35 00472 + m[A35]*Det2_45_34; 00473 00474 // Find all NECESSARY 4x4 dets: (55 of them) 00475 00476 double Det4_1234_0123 = m[A10]*Det3_234_123 - m[A11]*Det3_234_023 00477 + m[A12]*Det3_234_013 - m[A13]*Det3_234_012; 00478 double Det4_1234_0124 = m[A10]*Det3_234_124 - m[A11]*Det3_234_024 00479 + m[A12]*Det3_234_014 - m[A14]*Det3_234_012; 00480 double Det4_1234_0134 = m[A10]*Det3_234_134 - m[A11]*Det3_234_034 00481 + m[A13]*Det3_234_014 - m[A14]*Det3_234_013; 00482 double Det4_1234_0234 = m[A10]*Det3_234_234 - m[A12]*Det3_234_034 00483 + m[A13]*Det3_234_024 - m[A14]*Det3_234_023; 00484 double Det4_1234_1234 = m[A11]*Det3_234_234 - m[A12]*Det3_234_134 00485 + m[A13]*Det3_234_124 - m[A14]*Det3_234_123; 00486 double Det4_1235_0123 = m[A10]*Det3_235_123 - m[A11]*Det3_235_023 00487 + m[A12]*Det3_235_013 - m[A13]*Det3_235_012; 00488 double Det4_1235_0124 = m[A10]*Det3_235_124 - m[A11]*Det3_235_024 00489 + m[A12]*Det3_235_014 - m[A14]*Det3_235_012; 00490 double Det4_1235_0125 = m[A10]*Det3_235_125 - m[A11]*Det3_235_025 00491 + m[A12]*Det3_235_015 - m[A15]*Det3_235_012; 00492 double Det4_1235_0134 = m[A10]*Det3_235_134 - m[A11]*Det3_235_034 00493 + m[A13]*Det3_235_014 - m[A14]*Det3_235_013; 00494 double Det4_1235_0135 = m[A10]*Det3_235_135 - m[A11]*Det3_235_035 00495 + m[A13]*Det3_235_015 - m[A15]*Det3_235_013; 00496 double Det4_1235_0234 = m[A10]*Det3_235_234 - m[A12]*Det3_235_034 00497 + m[A13]*Det3_235_024 - m[A14]*Det3_235_023; 00498 double Det4_1235_0235 = m[A10]*Det3_235_235 - m[A12]*Det3_235_035 00499 + m[A13]*Det3_235_025 - m[A15]*Det3_235_023; 00500 double Det4_1235_1234 = m[A11]*Det3_235_234 - m[A12]*Det3_235_134 00501 + m[A13]*Det3_235_124 - m[A14]*Det3_235_123; 00502 double Det4_1235_1235 = m[A11]*Det3_235_235 - m[A12]*Det3_235_135 00503 + m[A13]*Det3_235_125 - m[A15]*Det3_235_123; 00504 double Det4_1245_0123 = m[A10]*Det3_245_123 - m[A11]*Det3_245_023 00505 + m[A12]*Det3_245_013 - m[A13]*Det3_245_012; 00506 double Det4_1245_0124 = m[A10]*Det3_245_124 - m[A11]*Det3_245_024 00507 + m[A12]*Det3_245_014 - m[A14]*Det3_245_012; 00508 double Det4_1245_0125 = m[A10]*Det3_245_125 - m[A11]*Det3_245_025 00509 + m[A12]*Det3_245_015 - m[A15]*Det3_245_012; 00510 double Det4_1245_0134 = m[A10]*Det3_245_134 - m[A11]*Det3_245_034 00511 + m[A13]*Det3_245_014 - m[A14]*Det3_245_013; 00512 double Det4_1245_0135 = m[A10]*Det3_245_135 - m[A11]*Det3_245_035 00513 + m[A13]*Det3_245_015 - m[A15]*Det3_245_013; 00514 double Det4_1245_0145 = m[A10]*Det3_245_145 - m[A11]*Det3_245_045 00515 + m[A14]*Det3_245_015 - m[A15]*Det3_245_014; 00516 double Det4_1245_0234 = m[A10]*Det3_245_234 - m[A12]*Det3_245_034 00517 + m[A13]*Det3_245_024 - m[A14]*Det3_245_023; 00518 double Det4_1245_0235 = m[A10]*Det3_245_235 - m[A12]*Det3_245_035 00519 + m[A13]*Det3_245_025 - m[A15]*Det3_245_023; 00520 double Det4_1245_0245 = m[A10]*Det3_245_245 - m[A12]*Det3_245_045 00521 + m[A14]*Det3_245_025 - m[A15]*Det3_245_024; 00522 double Det4_1245_1234 = m[A11]*Det3_245_234 - m[A12]*Det3_245_134 00523 + m[A13]*Det3_245_124 - m[A14]*Det3_245_123; 00524 double Det4_1245_1235 = m[A11]*Det3_245_235 - m[A12]*Det3_245_135 00525 + m[A13]*Det3_245_125 - m[A15]*Det3_245_123; 00526 double Det4_1245_1245 = m[A11]*Det3_245_245 - m[A12]*Det3_245_145 00527 + m[A14]*Det3_245_125 - m[A15]*Det3_245_124; 00528 double Det4_1345_0123 = m[A10]*Det3_345_123 - m[A11]*Det3_345_023 00529 + m[A12]*Det3_345_013 - m[A13]*Det3_345_012; 00530 double Det4_1345_0124 = m[A10]*Det3_345_124 - m[A11]*Det3_345_024 00531 + m[A12]*Det3_345_014 - m[A14]*Det3_345_012; 00532 double Det4_1345_0125 = m[A10]*Det3_345_125 - m[A11]*Det3_345_025 00533 + m[A12]*Det3_345_015 - m[A15]*Det3_345_012; 00534 double Det4_1345_0134 = m[A10]*Det3_345_134 - m[A11]*Det3_345_034 00535 + m[A13]*Det3_345_014 - m[A14]*Det3_345_013; 00536 double Det4_1345_0135 = m[A10]*Det3_345_135 - m[A11]*Det3_345_035 00537 + m[A13]*Det3_345_015 - m[A15]*Det3_345_013; 00538 double Det4_1345_0145 = m[A10]*Det3_345_145 - m[A11]*Det3_345_045 00539 + m[A14]*Det3_345_015 - m[A15]*Det3_345_014; 00540 double Det4_1345_0234 = m[A10]*Det3_345_234 - m[A12]*Det3_345_034 00541 + m[A13]*Det3_345_024 - m[A14]*Det3_345_023; 00542 double Det4_1345_0235 = m[A10]*Det3_345_235 - m[A12]*Det3_345_035 00543 + m[A13]*Det3_345_025 - m[A15]*Det3_345_023; 00544 double Det4_1345_0245 = m[A10]*Det3_345_245 - m[A12]*Det3_345_045 00545 + m[A14]*Det3_345_025 - m[A15]*Det3_345_024; 00546 double Det4_1345_0345 = m[A10]*Det3_345_345 - m[A13]*Det3_345_045 00547 + m[A14]*Det3_345_035 - m[A15]*Det3_345_034; 00548 double Det4_1345_1234 = m[A11]*Det3_345_234 - m[A12]*Det3_345_134 00549 + m[A13]*Det3_345_124 - m[A14]*Det3_345_123; 00550 double Det4_1345_1235 = m[A11]*Det3_345_235 - m[A12]*Det3_345_135 00551 + m[A13]*Det3_345_125 - m[A15]*Det3_345_123; 00552 double Det4_1345_1245 = m[A11]*Det3_345_245 - m[A12]*Det3_345_145 00553 + m[A14]*Det3_345_125 - m[A15]*Det3_345_124; 00554 double Det4_1345_1345 = m[A11]*Det3_345_345 - m[A13]*Det3_345_145 00555 + m[A14]*Det3_345_135 - m[A15]*Det3_345_134; 00556 double Det4_2345_0123 = m[A20]*Det3_345_123 - m[A21]*Det3_345_023 00557 + m[A22]*Det3_345_013 - m[A23]*Det3_345_012; 00558 double Det4_2345_0124 = m[A20]*Det3_345_124 - m[A21]*Det3_345_024 00559 + m[A22]*Det3_345_014 - m[A24]*Det3_345_012; 00560 double Det4_2345_0125 = m[A20]*Det3_345_125 - m[A21]*Det3_345_025 00561 + m[A22]*Det3_345_015 - m[A25]*Det3_345_012; 00562 double Det4_2345_0134 = m[A20]*Det3_345_134 - m[A21]*Det3_345_034 00563 + m[A23]*Det3_345_014 - m[A24]*Det3_345_013; 00564 double Det4_2345_0135 = m[A20]*Det3_345_135 - m[A21]*Det3_345_035 00565 + m[A23]*Det3_345_015 - m[A25]*Det3_345_013; 00566 double Det4_2345_0145 = m[A20]*Det3_345_145 - m[A21]*Det3_345_045 00567 + m[A24]*Det3_345_015 - m[A25]*Det3_345_014; 00568 double Det4_2345_0234 = m[A20]*Det3_345_234 - m[A22]*Det3_345_034 00569 + m[A23]*Det3_345_024 - m[A24]*Det3_345_023; 00570 double Det4_2345_0235 = m[A20]*Det3_345_235 - m[A22]*Det3_345_035 00571 + m[A23]*Det3_345_025 - m[A25]*Det3_345_023; 00572 double Det4_2345_0245 = m[A20]*Det3_345_245 - m[A22]*Det3_345_045 00573 + m[A24]*Det3_345_025 - m[A25]*Det3_345_024; 00574 double Det4_2345_0345 = m[A20]*Det3_345_345 - m[A23]*Det3_345_045 00575 + m[A24]*Det3_345_035 - m[A25]*Det3_345_034; 00576 double Det4_2345_1234 = m[A21]*Det3_345_234 - m[A22]*Det3_345_134 00577 + m[A23]*Det3_345_124 - m[A24]*Det3_345_123; 00578 double Det4_2345_1235 = m[A21]*Det3_345_235 - m[A22]*Det3_345_135 00579 + m[A23]*Det3_345_125 - m[A25]*Det3_345_123; 00580 double Det4_2345_1245 = m[A21]*Det3_345_245 - m[A22]*Det3_345_145 00581 + m[A24]*Det3_345_125 - m[A25]*Det3_345_124; 00582 double Det4_2345_1345 = m[A21]*Det3_345_345 - m[A23]*Det3_345_145 00583 + m[A24]*Det3_345_135 - m[A25]*Det3_345_134; 00584 double Det4_2345_2345 = m[A22]*Det3_345_345 - m[A23]*Det3_345_245 00585 + m[A24]*Det3_345_235 - m[A25]*Det3_345_234; 00586 00587 // Find all NECESSARY 5x5 dets: (19 of them) 00588 00589 double Det5_01234_01234 = m[A00]*Det4_1234_1234 - m[A01]*Det4_1234_0234 00590 + m[A02]*Det4_1234_0134 - m[A03]*Det4_1234_0124 + m[A04]*Det4_1234_0123; 00591 double Det5_01235_01234 = m[A00]*Det4_1235_1234 - m[A01]*Det4_1235_0234 00592 + m[A02]*Det4_1235_0134 - m[A03]*Det4_1235_0124 + m[A04]*Det4_1235_0123; 00593 double Det5_01235_01235 = m[A00]*Det4_1235_1235 - m[A01]*Det4_1235_0235 00594 + m[A02]*Det4_1235_0135 - m[A03]*Det4_1235_0125 + m[A05]*Det4_1235_0123; 00595 double Det5_01245_01234 = m[A00]*Det4_1245_1234 - m[A01]*Det4_1245_0234 00596 + m[A02]*Det4_1245_0134 - m[A03]*Det4_1245_0124 + m[A04]*Det4_1245_0123; 00597 double Det5_01245_01235 = m[A00]*Det4_1245_1235 - m[A01]*Det4_1245_0235 00598 + m[A02]*Det4_1245_0135 - m[A03]*Det4_1245_0125 + m[A05]*Det4_1245_0123; 00599 double Det5_01245_01245 = m[A00]*Det4_1245_1245 - m[A01]*Det4_1245_0245 00600 + m[A02]*Det4_1245_0145 - m[A04]*Det4_1245_0125 + m[A05]*Det4_1245_0124; 00601 double Det5_01345_01234 = m[A00]*Det4_1345_1234 - m[A01]*Det4_1345_0234 00602 + m[A02]*Det4_1345_0134 - m[A03]*Det4_1345_0124 + m[A04]*Det4_1345_0123; 00603 double Det5_01345_01235 = m[A00]*Det4_1345_1235 - m[A01]*Det4_1345_0235 00604 + m[A02]*Det4_1345_0135 - m[A03]*Det4_1345_0125 + m[A05]*Det4_1345_0123; 00605 double Det5_01345_01245 = m[A00]*Det4_1345_1245 - m[A01]*Det4_1345_0245 00606 + m[A02]*Det4_1345_0145 - m[A04]*Det4_1345_0125 + m[A05]*Det4_1345_0124; 00607 double Det5_01345_01345 = m[A00]*Det4_1345_1345 - m[A01]*Det4_1345_0345 00608 + m[A03]*Det4_1345_0145 - m[A04]*Det4_1345_0135 + m[A05]*Det4_1345_0134; 00609 double Det5_02345_01234 = m[A00]*Det4_2345_1234 - m[A01]*Det4_2345_0234 00610 + m[A02]*Det4_2345_0134 - m[A03]*Det4_2345_0124 + m[A04]*Det4_2345_0123; 00611 double Det5_02345_01235 = m[A00]*Det4_2345_1235 - m[A01]*Det4_2345_0235 00612 + m[A02]*Det4_2345_0135 - m[A03]*Det4_2345_0125 + m[A05]*Det4_2345_0123; 00613 double Det5_02345_01245 = m[A00]*Det4_2345_1245 - m[A01]*Det4_2345_0245 00614 + m[A02]*Det4_2345_0145 - m[A04]*Det4_2345_0125 + m[A05]*Det4_2345_0124; 00615 double Det5_02345_01345 = m[A00]*Det4_2345_1345 - m[A01]*Det4_2345_0345 00616 + m[A03]*Det4_2345_0145 - m[A04]*Det4_2345_0135 + m[A05]*Det4_2345_0134; 00617 double Det5_02345_02345 = m[A00]*Det4_2345_2345 - m[A02]*Det4_2345_0345 00618 + m[A03]*Det4_2345_0245 - m[A04]*Det4_2345_0235 + m[A05]*Det4_2345_0234; 00619 double Det5_12345_01234 = m[A10]*Det4_2345_1234 - m[A11]*Det4_2345_0234 00620 + m[A12]*Det4_2345_0134 - m[A13]*Det4_2345_0124 + m[A14]*Det4_2345_0123; 00621 double Det5_12345_01235 = m[A10]*Det4_2345_1235 - m[A11]*Det4_2345_0235 00622 + m[A12]*Det4_2345_0135 - m[A13]*Det4_2345_0125 + m[A15]*Det4_2345_0123; 00623 double Det5_12345_01245 = m[A10]*Det4_2345_1245 - m[A11]*Det4_2345_0245 00624 + m[A12]*Det4_2345_0145 - m[A14]*Det4_2345_0125 + m[A15]*Det4_2345_0124; 00625 double Det5_12345_01345 = m[A10]*Det4_2345_1345 - m[A11]*Det4_2345_0345 00626 + m[A13]*Det4_2345_0145 - m[A14]*Det4_2345_0135 + m[A15]*Det4_2345_0134; 00627 double Det5_12345_02345 = m[A10]*Det4_2345_2345 - m[A12]*Det4_2345_0345 00628 + m[A13]*Det4_2345_0245 - m[A14]*Det4_2345_0235 + m[A15]*Det4_2345_0234; 00629 double Det5_12345_12345 = m[A11]*Det4_2345_2345 - m[A12]*Det4_2345_1345 00630 + m[A13]*Det4_2345_1245 - m[A14]*Det4_2345_1235 + m[A15]*Det4_2345_1234; 00631 00632 // Find the determinant 00633 00634 double det = m[A00]*Det5_12345_12345 00635 - m[A01]*Det5_12345_02345 00636 + m[A02]*Det5_12345_01345 00637 - m[A03]*Det5_12345_01245 00638 + m[A04]*Det5_12345_01235 00639 - m[A05]*Det5_12345_01234; 00640 00641 if ( det == 0 ) { 00642 #ifdef SINGULAR_DIAGNOSTICS 00643 std::cerr << "Kramer's rule inversion of a singular 6x6 matrix: " 00644 << *this << "\n"; 00645 #endif 00646 ifail = 1; 00647 return; 00648 } 00649 00650 double oneOverDet = 1.0/det; 00651 double mn1OverDet = - oneOverDet; 00652 00653 m[A00] = Det5_12345_12345*oneOverDet; 00654 m[A01] = Det5_12345_02345*mn1OverDet; 00655 m[A02] = Det5_12345_01345*oneOverDet; 00656 m[A03] = Det5_12345_01245*mn1OverDet; 00657 m[A04] = Det5_12345_01235*oneOverDet; 00658 m[A05] = Det5_12345_01234*mn1OverDet; 00659 00660 m[A11] = Det5_02345_02345*oneOverDet; 00661 m[A12] = Det5_02345_01345*mn1OverDet; 00662 m[A13] = Det5_02345_01245*oneOverDet; 00663 m[A14] = Det5_02345_01235*mn1OverDet; 00664 m[A15] = Det5_02345_01234*oneOverDet; 00665 00666 m[A22] = Det5_01345_01345*oneOverDet; 00667 m[A23] = Det5_01345_01245*mn1OverDet; 00668 m[A24] = Det5_01345_01235*oneOverDet; 00669 m[A25] = Det5_01345_01234*mn1OverDet; 00670 00671 m[A33] = Det5_01245_01245*oneOverDet; 00672 m[A34] = Det5_01245_01235*mn1OverDet; 00673 m[A35] = Det5_01245_01234*oneOverDet; 00674 00675 m[A44] = Det5_01235_01235*oneOverDet; 00676 m[A45] = Det5_01235_01234*mn1OverDet; 00677 00678 m[A55] = Det5_01234_01234*oneOverDet; 00679 00680 return; 00681 } 00682 00683 void HepSymMatrix::invertCholesky5 (int & ifail) { 00684 00685 // Invert by 00686 // 00687 // a) decomposing M = G*G^T with G lower triangular 00688 // (if M is not positive definite this will fail, leaving this unchanged) 00689 // b) inverting G to form H 00690 // c) multiplying H^T * H to get M^-1. 00691 // 00692 // If the matrix is pos. def. it is inverted and 1 is returned. 00693 // If the matrix is not pos. def. it remains unaltered and 0 is returned. 00694 00695 double h10; // below-diagonal elements of H 00696 double h20, h21; 00697 double h30, h31, h32; 00698 double h40, h41, h42, h43; 00699 00700 double h00, h11, h22, h33, h44; // 1/diagonal elements of G = 00701 // diagonal elements of H 00702 00703 double g10; // below-diagonal elements of G 00704 double g20, g21; 00705 double g30, g31, g32; 00706 double g40, g41, g42, g43; 00707 00708 ifail = 1; // We start by assuing we won't succeed... 00709 00710 // Form G -- compute diagonal members of H directly rather than of G 00711 //------- 00712 00713 // Scale first column by 1/sqrt(A00) 00714 00715 h00 = m[A00]; 00716 if (h00 <= 0) return; 00717 h00 = 1.0 / sqrt(h00); 00718 00719 g10 = m[A10] * h00; 00720 g20 = m[A20] * h00; 00721 g30 = m[A30] * h00; 00722 g40 = m[A40] * h00; 00723 00724 // Form G11 (actually, just h11) 00725 00726 h11 = m[A11] - (g10 * g10); 00727 if (h11 <= 0) return; 00728 h11 = 1.0 / sqrt(h11); 00729 00730 // Subtract inter-column column dot products from rest of column 1 and 00731 // scale to get column 1 of G 00732 00733 g21 = (m[A21] - (g10 * g20)) * h11; 00734 g31 = (m[A31] - (g10 * g30)) * h11; 00735 g41 = (m[A41] - (g10 * g40)) * h11; 00736 00737 // Form G22 (actually, just h22) 00738 00739 h22 = m[A22] - (g20 * g20) - (g21 * g21); 00740 if (h22 <= 0) return; 00741 h22 = 1.0 / sqrt(h22); 00742 00743 // Subtract inter-column column dot products from rest of column 2 and 00744 // scale to get column 2 of G 00745 00746 g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22; 00747 g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22; 00748 00749 // Form G33 (actually, just h33) 00750 00751 h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32); 00752 if (h33 <= 0) return; 00753 h33 = 1.0 / sqrt(h33); 00754 00755 // Subtract inter-column column dot product from A43 and scale to get G43 00756 00757 g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33; 00758 00759 // Finally form h44 - if this is possible inversion succeeds 00760 00761 h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43); 00762 if (h44 <= 0) return; 00763 h44 = 1.0 / sqrt(h44); 00764 00765 // Form H = 1/G -- diagonal members of H are already correct 00766 //------------- 00767 00768 // The order here is dictated by speed considerations 00769 00770 h43 = -h33 * g43 * h44; 00771 h32 = -h22 * g32 * h33; 00772 h42 = -h22 * (g32 * h43 + g42 * h44); 00773 h21 = -h11 * g21 * h22; 00774 h31 = -h11 * (g21 * h32 + g31 * h33); 00775 h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44); 00776 h10 = -h00 * g10 * h11; 00777 h20 = -h00 * (g10 * h21 + g20 * h22); 00778 h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33); 00779 h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44); 00780 00781 // Change this to its inverse = H^T*H 00782 //------------------------------------ 00783 00784 m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40; 00785 m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41; 00786 m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41; 00787 m[A02] = h20 * h22 + h30 * h32 + h40 * h42; 00788 m[A12] = h21 * h22 + h31 * h32 + h41 * h42; 00789 m[A22] = h22 * h22 + h32 * h32 + h42 * h42; 00790 m[A03] = h30 * h33 + h40 * h43; 00791 m[A13] = h31 * h33 + h41 * h43; 00792 m[A23] = h32 * h33 + h42 * h43; 00793 m[A33] = h33 * h33 + h43 * h43; 00794 m[A04] = h40 * h44; 00795 m[A14] = h41 * h44; 00796 m[A24] = h42 * h44; 00797 m[A34] = h43 * h44; 00798 m[A44] = h44 * h44; 00799 00800 ifail = 0; 00801 return; 00802 00803 } 00804 00805 00806 void HepSymMatrix::invertCholesky6 (int & ifail) { 00807 00808 // Invert by 00809 // 00810 // a) decomposing M = G*G^T with G lower triangular 00811 // (if M is not positive definite this will fail, leaving this unchanged) 00812 // b) inverting G to form H 00813 // c) multiplying H^T * H to get M^-1. 00814 // 00815 // If the matrix is pos. def. it is inverted and 1 is returned. 00816 // If the matrix is not pos. def. it remains unaltered and 0 is returned. 00817 00818 double h10; // below-diagonal elements of H 00819 double h20, h21; 00820 double h30, h31, h32; 00821 double h40, h41, h42, h43; 00822 double h50, h51, h52, h53, h54; 00823 00824 double h00, h11, h22, h33, h44, h55; // 1/diagonal elements of G = 00825 // diagonal elements of H 00826 00827 double g10; // below-diagonal elements of G 00828 double g20, g21; 00829 double g30, g31, g32; 00830 double g40, g41, g42, g43; 00831 double g50, g51, g52, g53, g54; 00832 00833 ifail = 1; // We start by assuing we won't succeed... 00834 00835 // Form G -- compute diagonal members of H directly rather than of G 00836 //------- 00837 00838 // Scale first column by 1/sqrt(A00) 00839 00840 h00 = m[A00]; 00841 if (h00 <= 0) return; 00842 h00 = 1.0 / sqrt(h00); 00843 00844 g10 = m[A10] * h00; 00845 g20 = m[A20] * h00; 00846 g30 = m[A30] * h00; 00847 g40 = m[A40] * h00; 00848 g50 = m[A50] * h00; 00849 00850 // Form G11 (actually, just h11) 00851 00852 h11 = m[A11] - (g10 * g10); 00853 if (h11 <= 0) return; 00854 h11 = 1.0 / sqrt(h11); 00855 00856 // Subtract inter-column column dot products from rest of column 1 and 00857 // scale to get column 1 of G 00858 00859 g21 = (m[A21] - (g10 * g20)) * h11; 00860 g31 = (m[A31] - (g10 * g30)) * h11; 00861 g41 = (m[A41] - (g10 * g40)) * h11; 00862 g51 = (m[A51] - (g10 * g50)) * h11; 00863 00864 // Form G22 (actually, just h22) 00865 00866 h22 = m[A22] - (g20 * g20) - (g21 * g21); 00867 if (h22 <= 0) return; 00868 h22 = 1.0 / sqrt(h22); 00869 00870 // Subtract inter-column column dot products from rest of column 2 and 00871 // scale to get column 2 of G 00872 00873 g32 = (m[A32] - (g20 * g30) - (g21 * g31)) * h22; 00874 g42 = (m[A42] - (g20 * g40) - (g21 * g41)) * h22; 00875 g52 = (m[A52] - (g20 * g50) - (g21 * g51)) * h22; 00876 00877 // Form G33 (actually, just h33) 00878 00879 h33 = m[A33] - (g30 * g30) - (g31 * g31) - (g32 * g32); 00880 if (h33 <= 0) return; 00881 h33 = 1.0 / sqrt(h33); 00882 00883 // Subtract inter-column column dot products from rest of column 3 and 00884 // scale to get column 3 of G 00885 00886 g43 = (m[A43] - (g30 * g40) - (g31 * g41) - (g32 * g42)) * h33; 00887 g53 = (m[A53] - (g30 * g50) - (g31 * g51) - (g32 * g52)) * h33; 00888 00889 // Form G44 (actually, just h44) 00890 00891 h44 = m[A44] - (g40 * g40) - (g41 * g41) - (g42 * g42) - (g43 * g43); 00892 if (h44 <= 0) return; 00893 h44 = 1.0 / sqrt(h44); 00894 00895 // Subtract inter-column column dot product from M54 and scale to get G54 00896 00897 g54 = (m[A54] - (g40 * g50) - (g41 * g51) - (g42 * g52) - (g43 * g53)) * h44; 00898 00899 // Finally form h55 - if this is possible inversion succeeds 00900 00901 h55 = m[A55] - (g50*g50) - (g51*g51) - (g52*g52) - (g53*g53) - (g54*g54); 00902 if (h55 <= 0) return; 00903 h55 = 1.0 / sqrt(h55); 00904 00905 // Form H = 1/G -- diagonal members of H are already correct 00906 //------------- 00907 00908 // The order here is dictated by speed considerations 00909 00910 h54 = -h44 * g54 * h55; 00911 h43 = -h33 * g43 * h44; 00912 h53 = -h33 * (g43 * h54 + g53 * h55); 00913 h32 = -h22 * g32 * h33; 00914 h42 = -h22 * (g32 * h43 + g42 * h44); 00915 h52 = -h22 * (g32 * h53 + g42 * h54 + g52 * h55); 00916 h21 = -h11 * g21 * h22; 00917 h31 = -h11 * (g21 * h32 + g31 * h33); 00918 h41 = -h11 * (g21 * h42 + g31 * h43 + g41 * h44); 00919 h51 = -h11 * (g21 * h52 + g31 * h53 + g41 * h54 + g51 * h55); 00920 h10 = -h00 * g10 * h11; 00921 h20 = -h00 * (g10 * h21 + g20 * h22); 00922 h30 = -h00 * (g10 * h31 + g20 * h32 + g30 * h33); 00923 h40 = -h00 * (g10 * h41 + g20 * h42 + g30 * h43 + g40 * h44); 00924 h50 = -h00 * (g10 * h51 + g20 * h52 + g30 * h53 + g40 * h54 + g50 * h55); 00925 00926 // Change this to its inverse = H^T*H 00927 //------------------------------------ 00928 00929 m[A00] = h00 * h00 + h10 * h10 + h20 * h20 + h30 * h30 + h40 * h40 + h50*h50; 00930 m[A01] = h10 * h11 + h20 * h21 + h30 * h31 + h40 * h41 + h50 * h51; 00931 m[A11] = h11 * h11 + h21 * h21 + h31 * h31 + h41 * h41 + h51 * h51; 00932 m[A02] = h20 * h22 + h30 * h32 + h40 * h42 + h50 * h52; 00933 m[A12] = h21 * h22 + h31 * h32 + h41 * h42 + h51 * h52; 00934 m[A22] = h22 * h22 + h32 * h32 + h42 * h42 + h52 * h52; 00935 m[A03] = h30 * h33 + h40 * h43 + h50 * h53; 00936 m[A13] = h31 * h33 + h41 * h43 + h51 * h53; 00937 m[A23] = h32 * h33 + h42 * h43 + h52 * h53; 00938 m[A33] = h33 * h33 + h43 * h43 + h53 * h53; 00939 m[A04] = h40 * h44 + h50 * h54; 00940 m[A14] = h41 * h44 + h51 * h54; 00941 m[A24] = h42 * h44 + h52 * h54; 00942 m[A34] = h43 * h44 + h53 * h54; 00943 m[A44] = h44 * h44 + h54 * h54; 00944 m[A05] = h50 * h55; 00945 m[A15] = h51 * h55; 00946 m[A25] = h52 * h55; 00947 m[A35] = h53 * h55; 00948 m[A45] = h54 * h55; 00949 m[A55] = h55 * h55; 00950 00951 ifail = 0; 00952 return; 00953 00954 } 00955 00956 00957 void HepSymMatrix::invert4 (int & ifail) { 00958 00959 ifail = 0; 00960 00961 // Find all NECESSARY 2x2 dets: (14 of them) 00962 00963 double Det2_12_01 = m[A10]*m[A21] - m[A11]*m[A20]; 00964 double Det2_12_02 = m[A10]*m[A22] - m[A12]*m[A20]; 00965 double Det2_12_12 = m[A11]*m[A22] - m[A12]*m[A21]; 00966 double Det2_13_01 = m[A10]*m[A31] - m[A11]*m[A30]; 00967 double Det2_13_02 = m[A10]*m[A32] - m[A12]*m[A30]; 00968 double Det2_13_03 = m[A10]*m[A33] - m[A13]*m[A30]; 00969 double Det2_13_12 = m[A11]*m[A32] - m[A12]*m[A31]; 00970 double Det2_13_13 = m[A11]*m[A33] - m[A13]*m[A31]; 00971 double Det2_23_01 = m[A20]*m[A31] - m[A21]*m[A30]; 00972 double Det2_23_02 = m[A20]*m[A32] - m[A22]*m[A30]; 00973 double Det2_23_03 = m[A20]*m[A33] - m[A23]*m[A30]; 00974 double Det2_23_12 = m[A21]*m[A32] - m[A22]*m[A31]; 00975 double Det2_23_13 = m[A21]*m[A33] - m[A23]*m[A31]; 00976 double Det2_23_23 = m[A22]*m[A33] - m[A23]*m[A32]; 00977 00978 // Find all NECESSARY 3x3 dets: (10 of them) 00979 00980 double Det3_012_012 = m[A00]*Det2_12_12 - m[A01]*Det2_12_02 00981 + m[A02]*Det2_12_01; 00982 double Det3_013_012 = m[A00]*Det2_13_12 - m[A01]*Det2_13_02 00983 + m[A02]*Det2_13_01; 00984 double Det3_013_013 = m[A00]*Det2_13_13 - m[A01]*Det2_13_03 00985 + m[A03]*Det2_13_01; 00986 double Det3_023_012 = m[A00]*Det2_23_12 - m[A01]*Det2_23_02 00987 + m[A02]*Det2_23_01; 00988 double Det3_023_013 = m[A00]*Det2_23_13 - m[A01]*Det2_23_03 00989 + m[A03]*Det2_23_01; 00990 double Det3_023_023 = m[A00]*Det2_23_23 - m[A02]*Det2_23_03 00991 + m[A03]*Det2_23_02; 00992 double Det3_123_012 = m[A10]*Det2_23_12 - m[A11]*Det2_23_02 00993 + m[A12]*Det2_23_01; 00994 double Det3_123_013 = m[A10]*Det2_23_13 - m[A11]*Det2_23_03 00995 + m[A13]*Det2_23_01; 00996 double Det3_123_023 = m[A10]*Det2_23_23 - m[A12]*Det2_23_03 00997 + m[A13]*Det2_23_02; 00998 double Det3_123_123 = m[A11]*Det2_23_23 - m[A12]*Det2_23_13 00999 + m[A13]*Det2_23_12; 01000 01001 // Find the 4x4 det: 01002 01003 double det = m[A00]*Det3_123_123 01004 - m[A01]*Det3_123_023 01005 + m[A02]*Det3_123_013 01006 - m[A03]*Det3_123_012; 01007 01008 if ( det == 0 ) { 01009 #ifdef SINGULAR_DIAGNOSTICS 01010 std::cerr << "Kramer's rule inversion of a singular 4x4 matrix: " 01011 << *this << "\n"; 01012 #endif 01013 ifail = 1; 01014 return; 01015 } 01016 01017 double oneOverDet = 1.0/det; 01018 double mn1OverDet = - oneOverDet; 01019 01020 m[A00] = Det3_123_123 * oneOverDet; 01021 m[A01] = Det3_123_023 * mn1OverDet; 01022 m[A02] = Det3_123_013 * oneOverDet; 01023 m[A03] = Det3_123_012 * mn1OverDet; 01024 01025 01026 m[A11] = Det3_023_023 * oneOverDet; 01027 m[A12] = Det3_023_013 * mn1OverDet; 01028 m[A13] = Det3_023_012 * oneOverDet; 01029 01030 m[A22] = Det3_013_013 * oneOverDet; 01031 m[A23] = Det3_013_012 * mn1OverDet; 01032 01033 m[A33] = Det3_012_012 * oneOverDet; 01034 01035 return; 01036 } 01037 01038 void HepSymMatrix::invertHaywood4 (int & ifail) { 01039 invert4(ifail); // For the 4x4 case, the method we use for invert is already 01040 // the Haywood method. 01041 } 01042 01043 } // namespace CLHEP 01044
1.4.7