00001
00002
00003 #include <iostream>
00004 #include <iomanip>
00005 #include "cstdlib"
00006 #include "math.h"
00007
00008
00009 #include "MdcAlignAlg/Millepede.h"
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 using namespace std;
00020
00021
00022
00023
00024 Millepede::Millepede()
00025 {}
00026
00027
00028
00029 Millepede::~Millepede() {}
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 bool Millepede::InitMille(bool DOF[], double Sigm[], int nglo
00047 , int nloc, double startfact, int nstd
00048 , double res_cut, double res_cut_init)
00049 {
00050
00051 cout << " " << endl;
00052 cout << " * o o o " << endl;
00053 cout << " o o o " << endl;
00054 cout << " o ooooo o o o oo ooo oo ooo oo " << endl;
00055 cout << " o o o o o o o o o o o o o o o o " << endl;
00056 cout << " o o o o o o oooo o o oooo o o oooo " << endl;
00057 cout << " o o o o o o o ooo o o o o " << endl;
00058 cout << " o o o o o o oo o oo ooo oo ++ starts" << endl;
00059 cout << " " << endl;
00060
00061 if (debug_mode) cout << "" << endl;
00062 if (debug_mode) cout << "----------------------------------------------------" << endl;
00063 if (debug_mode) cout << "" << endl;
00064 if (debug_mode) cout << " Entering InitMille" << endl;
00065 if (debug_mode) cout << "" << endl;
00066 if (debug_mode) cout << "-----------------------------------------------------" << endl;
00067 if (debug_mode) cout << "" << endl;
00068
00069 ncs = 0;
00070 loctot = 0;
00071 locrej = 0;
00072 cfactref = 1.0;
00073
00074 Millepede::SetTrackNumber(0);
00075
00076 m_residual_cut = res_cut;
00077 m_residual_cut_init = res_cut_init;
00078
00079
00080 nagb = 3*nglo;
00081 nalc = nloc;
00082 nstdev = nstd;
00083
00084 if (debug_mode) cout << "Number of global parameters : " << nagb << endl;
00085 if (debug_mode) cout << "Number of local parameters : " << nalc << endl;
00086 if (debug_mode) cout << "Number of standard deviations : " << nstdev << endl;
00087
00088 if (nagb>mglobl || nalc>mlocal)
00089 {
00090 if (debug_mode) cout << "Two many parameters !!!!!" << endl;
00091 return false;
00092 }
00093
00094
00095
00096 for (int i=0; i<nagb; i++)
00097 {
00098 bgvec[i]=0.;
00099 pparm[i]=0.;
00100 dparm[i]=0.;
00101 psigm[i]=-1.;
00102 indnz[i]=-1;
00103 indbk[i]=-1;
00104 nlnpa[i]=0;
00105
00106 for (int j=0; j<nagb;j++)
00107 {
00108 cgmat[i][j]=0.;
00109 }
00110 }
00111
00112
00113
00114 for (int i=0; i<nalc; i++)
00115 {
00116 blvec[i]=0.;
00117
00118 for (int j=0; j<nalc;j++)
00119 {
00120 clmat[i][j]=0.;
00121 }
00122 }
00123
00124
00125
00126 for (int j=0; j<nagb; j++) {Millepede::ParSig(j,0.0);}
00127
00128
00129
00130
00131 for (int i=0; i<3; i++)
00132 {
00133 if (verbose_mode) cout << "GetDOF(" << i << ")= " << DOF[i] << endl;
00134
00135 if (DOF[i])
00136 {
00137 for (int j=i*nglo; j<(i+1)*nglo; j++)
00138 {Millepede::ParSig(j,Sigm[i]);}
00139 }
00140 }
00141
00142
00143
00144 itert = 0;
00145 cfactr = 500.0;
00146 if (m_iteration) Millepede::InitUn(startfact);
00147
00148 arest.clear();
00149 arenl.clear();
00150 indst.clear();
00151
00152 storeind.clear();
00153 storeare.clear();
00154 storenl.clear();
00155 storeplace.clear();
00156
00157 if (debug_mode) cout << "" << endl;
00158 if (debug_mode) cout << "----------------------------------------------------" << endl;
00159 if (debug_mode) cout << "" << endl;
00160 if (debug_mode) cout << " InitMille has been successfully called!" << endl;
00161 if (debug_mode) cout << "" << endl;
00162 if (debug_mode) cout << "-----------------------------------------------------" << endl;
00163 if (debug_mode) cout << "" << endl;
00164
00165 return true;
00166 }
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182 bool Millepede::ParGlo(int index, double param)
00183 {
00184 if (index<0 || index>=nagb)
00185 {return false;}
00186 else
00187 {pparm[index] = param;}
00188
00189 return true;
00190 }
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209 bool Millepede::ParSig(int index, double sigma)
00210 {
00211 if (index>=nagb)
00212 {return false;}
00213 else
00214 {psigm[index] = sigma;}
00215
00216 return true;
00217 }
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241 bool Millepede::InitUn(double cutfac)
00242 {
00243 cfactr = std::max(1.0, cutfac);
00244
00245 cout << "Initial cut factor is " << cfactr << endl;
00246 itert = 1;
00247 return true;
00248 }
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 bool Millepede::ConstF(double dercs[], double rhs)
00264 {
00265 if (ncs>=mcs)
00266 {
00267 cout << "Too many constraints !!!" << endl;
00268 return false;
00269 }
00270
00271 for (int i=0; i<nagb; i++) {adercs[ncs][i] = dercs[i];}
00272
00273 arhs[ncs] = rhs;
00274 ncs++ ;
00275 cout << "Number of constraints increased to " << ncs << endl;
00276 return true;
00277 }
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293 bool Millepede::EquLoc(double dergb[], double derlc[], double dernl[], double rmeas, double sigma)
00294 {
00295
00296 if (sigma<=0.0)
00297 {
00298 for (int i=0; i<nalc; i++)
00299 {
00300 derlc[i] = 0.0;
00301 }
00302 for (int i=0; i<nagb; i++)
00303 {
00304 dergb[i] = 0.0;
00305 }
00306 return true;
00307 }
00308
00309
00310
00311 double wght = 1.0/(sigma*sigma);
00312 int nonzer = 0;
00313 int ialc = -1;
00314 int iblc = -1;
00315 int iagb = -1;
00316 int ibgb = -1;
00317
00318 for (int i=0; i<nalc; i++)
00319 {
00320 if (derlc[i]!=0.0)
00321 {
00322 nonzer++;
00323 if (ialc == -1) ialc=i;
00324 iblc = i;
00325 }
00326 }
00327
00328 if (verbose_mode) cout << ialc << " / " << iblc << endl;
00329
00330 for (int i=0; i<nagb; i++)
00331 {
00332 if (dergb[i]!=0.0)
00333 {
00334 nonzer++;
00335 if (iagb == -1) iagb=i;
00336 ibgb = i;
00337 }
00338 }
00339
00340 if (verbose_mode) cout << iagb << " / " << ibgb << endl;
00341
00342 indst.push_back(-1);
00343 arest.push_back(rmeas);
00344 arenl.push_back(0.);
00345
00346 for (int i=ialc; i<=iblc; i++)
00347 {
00348 if (derlc[i]!=0.0)
00349 {
00350 indst.push_back(i);
00351 arest.push_back(derlc[i]);
00352 arenl.push_back(0.0);
00353 derlc[i] = 0.0;
00354 }
00355 }
00356
00357 indst.push_back(-1);
00358 arest.push_back(wght);
00359 arenl.push_back(0.0);
00360
00361 for (int i=iagb; i<=ibgb; i++)
00362 {
00363 if (dergb[i]!=0.0)
00364 {
00365 indst.push_back(i);
00366 arest.push_back(dergb[i]);
00367 arenl.push_back(dernl[i]);
00368 dergb[i] = 0.0;
00369 }
00370 }
00371
00372 if (verbose_mode) cout << "Out Equloc -- NST = " << arest.size() << endl;
00373
00374 return true;
00375 }
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388 bool Millepede::ZerLoc(double dergb[], double derlc[], double dernl[])
00389 {
00390 for(int i=0; i<nalc; i++) {derlc[i] = 0.0;}
00391 for(int i=0; i<nagb; i++) {dergb[i] = 0.0;}
00392 for(int i=0; i<nagb; i++) {dernl[i] = 0.0;}
00393
00394 return true;
00395 }
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418 bool Millepede::FitLoc(int n, double track_params[], int single_fit)
00419 {
00420
00421
00422 int i, j, k, ik, ij, ist, nderlc, ndergl, ndf;
00423 int ja = -1;
00424 int jb = 0;
00425 int nagbn = 0;
00426
00427 double rmeas, wght, rms, cutval;
00428
00429 double summ = 0.0;
00430 int nsum = 0;
00431 nst = 0;
00432 nst = arest.size();
00433
00434
00435
00436
00437 if (itert < 2 && single_fit != 1)
00438 {
00439 if (debug_mode) cout << "Store equation no: " << n << endl;
00440
00441 for (i=0; i<nst; i++)
00442 {
00443 storeind.push_back(indst[i]);
00444 storeare.push_back(arest[i]);
00445 storenl.push_back(arenl[i]);
00446
00447 if (arenl[i] != 0.) arest[i] = 0.0;
00448 }
00449
00450 arenl.clear();
00451
00452 storeplace.push_back(storeind.size());
00453
00454 if (verbose_mode) cout << "StorePlace size = " << storeplace[n] << endl;
00455 if (verbose_mode) cout << "StoreInd size = " << storeind.size() << endl;
00456 }
00457
00458
00459 for (i=0; i<nalc; i++)
00460 {
00461 blvec[i] = 0.0;
00462
00463 for (j=0; j<nalc; j++) {clmat[i][j] = 0.0;}
00464 }
00465
00466 for (i=0; i<nagb; i++) {indnz[i] = -1;}
00467
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489
00490
00491
00492
00493
00494
00495
00496
00497 ist = 0;
00498 indst.push_back(-1);
00499
00500 while (ist <= nst)
00501 {
00502 if (indst[ist] == -1)
00503 {
00504 if (ja == -1) {ja = ist;}
00505 else if (jb == 0) {jb = ist;}
00506 else
00507 {
00508 rmeas = arest[ja];
00509 wght = arest[jb];
00510 if (verbose_mode) cout << "rmeas = " << rmeas << endl ;
00511 if (verbose_mode) cout << "wght = " << wght << endl ;
00512
00513 for (i=(jb+1); i<ist; i++)
00514
00515 {
00516 j = indst[i];
00517 if (verbose_mode) cout << "dparm[" << j << "] = " << dparm[j] << endl;
00518 if (verbose_mode) cout << "Starting misalignment = " << pparm[j] << endl;
00519 rmeas -= arest[i]*(pparm[j]+dparm[j]);
00520 }
00521
00522 if (verbose_mode) cout << "rmeas after global stuff removal = " << rmeas << endl ;
00523
00524 for (i=(ja+1); i<jb; i++)
00525 {
00526 j = indst[i];
00527 blvec[j] += wght*rmeas*arest[i];
00528
00529 if (verbose_mode) cout << "blvec[" << j << "] = " << blvec[j] << endl ;
00530
00531 for (k=(ja+1); k<=i ; k++)
00532 {
00533 ik = indst[k];
00534 clmat[j][ik] += wght*arest[i]*arest[k];
00535
00536 if (verbose_mode) cout << "clmat[" << j << "][" << ik << "] = " << clmat[j][ik] << endl;
00537 }
00538 }
00539 ja = -1;
00540 jb = 0;
00541 ist--;
00542 }
00543 }
00544 ist++;
00545 }
00546
00547
00548
00549
00550
00551
00552 nrank = 0;
00553 nrank = Millepede::SpmInv(clmat, blvec, nalc, scdiag, scflag);
00554
00555 if (debug_mode) cout << "" << endl;
00556 if (debug_mode) cout << " __________________________________________________" << endl;
00557 if (debug_mode) cout << " Printout of local fit (FITLOC) with rank= "<< nrank << endl;
00558 if (debug_mode) cout << " Result of local fit : (index/parameter/error)" << endl;
00559
00560 for (i=0; i<nalc; i++)
00561 {
00562 if (debug_mode) cout << std::setprecision(4) << std::fixed;
00563 if (debug_mode) cout << std::setw(20) << i << " / " << std::setw(10)
00564 << blvec[i] << " / " << sqrt(clmat[i][i]) << endl;
00565 }
00566
00567
00568
00569
00570 for (i=0; i<nalc; i++)
00571 {
00572 track_params[2*i] = blvec[i];
00573 track_params[2*i+1] = sqrt(fabs(clmat[i][i]));
00574 }
00575
00576
00577
00578
00579
00580
00581 ist = 0;
00582 ja = -1;
00583 jb = 0;
00584
00585 while (ist <= nst)
00586 {
00587 if (indst[ist] == -1)
00588 {
00589 if (ja == -1) {ja = ist;}
00590 else if (jb == 0) {jb = ist;}
00591 else
00592 {
00593 rmeas = arest[ja];
00594 wght = arest[jb];
00595
00596 nderlc = jb-ja-1;
00597 ndergl = ist-jb-1;
00598
00599
00600
00601 if (verbose_mode) cout << "" << endl;
00602 if (verbose_mode) cout << std::setprecision(4) << std::fixed;
00603 if (verbose_mode) cout << ". equation: measured value " << std::setw(13)
00604 << rmeas << " +/- " << std::setw(13) << 1.0/sqrt(wght) << endl;
00605 if (verbose_mode) cout << "Number of derivatives (global, local): "
00606 << ndergl << ", " << nderlc << endl;
00607 if (verbose_mode) cout << "Global derivatives are: (index/derivative/parvalue) " << endl;
00608
00609 for (i=(jb+1); i<ist; i++)
00610 {if (verbose_mode) cout << indst[i] << " / " << arest[i]
00611 << " / " << pparm[indst[i]] << endl;}
00612
00613 if (verbose_mode) cout << "Local derivatives are: (index/derivative) " << endl;
00614
00615 for (i=(ja+1); i<jb; i++) {if (verbose_mode) cout << indst[i] << " / " << arest[i] << endl;}
00616
00617
00618
00619 for (i=(ja+1); i<jb; i++)
00620 {
00621 j = indst[i];
00622 rmeas -= arest[i]*blvec[j];
00623 }
00624
00625 for (i=(jb+1); i<ist; i++)
00626 {
00627 j = indst[i];
00628 rmeas -= arest[i]*(pparm[j]+dparm[j]);
00629 }
00630
00631
00632
00633 if (verbose_reject) cout << "Residual value : "<< rmeas << endl;
00634
00635
00636 if (fabs(rmeas) >= m_residual_cut_init && itert <= 1)
00637 {
00638
00639 if (verbose_reject) cout << "Rejected track !!!!!" << endl;
00640 locrej++;
00641 indst.clear();
00642 arest.clear();
00643 return false;
00644 }
00645
00646 if (fabs(rmeas) >= m_residual_cut && itert > 1)
00647 {
00648
00649 if (verbose_reject) cout << "Rejected track !!!!!" << endl;
00650 locrej++;
00651 indst.clear();
00652 arest.clear();
00653 return false;
00654 }
00655
00656 summ += wght*rmeas*rmeas ;
00657 nsum++;
00658 ja = -1;
00659 jb = 0;
00660 ist--;
00661 }
00662 }
00663 ist++;
00664 }
00665
00666 ndf = nsum-nrank;
00667 rms = 0.0;
00668
00669 if (debug_mode) cout << "Final chi square / degrees of freedom "<< summ << " / " << ndf << endl;
00670
00671 if (ndf > 0) rms = summ/float(ndf);
00672
00673 if (single_fit == 0) loctot++;
00674
00675 if (nstdev != 0 && ndf > 0 && single_fit != 1)
00676 {
00677 cutval = Millepede::chindl(nstdev, ndf)*cfactr;
00678
00679 if (debug_mode) cout << "Reject if Chisq/Ndf = " << rms << " > " << cutval << endl;
00680
00681 if (rms > cutval)
00682 {
00683 if (verbose_mode) cout << "Rejected track !!!!!" << endl;
00684 if (single_fit == 0) locrej++;
00685 indst.clear();
00686 arest.clear();
00687 return false;
00688 }
00689 }
00690
00691 if (single_fit == 1)
00692 {
00693 indst.clear();
00694 arest.clear();
00695 return true;
00696 }
00697
00698
00699
00700
00701
00702
00703 ist = 0;
00704 ja = -1;
00705 jb = 0;
00706
00707 while (ist <= nst)
00708 {
00709 if (indst[ist] == -1)
00710 {
00711 if (ja == -1) {ja = ist;}
00712 else if (jb == 0) {jb = ist;}
00713 else
00714 {
00715 rmeas = arest[ja];
00716 wght = arest[jb];
00717
00718 for (i=(jb+1); i<ist; i++)
00719 {
00720 j = indst[i];
00721 rmeas -= arest[i]*(pparm[j]+dparm[j]);
00722 }
00723
00724
00725
00726 for (i=(jb+1); i<ist; i++)
00727 {
00728 j = indst[i];
00729
00730 bgvec[j] += wght*rmeas*arest[i];
00731 if (verbose_mode) cout << "bgvec[" << j << "] = " << bgvec[j] << endl ;
00732
00733 for (k=(jb+1); k<ist ; k++)
00734 {
00735 ik = indst[k];
00736 cgmat[j][ik] += wght*arest[i]*arest[k];
00737 if (verbose_mode) cout << "cgmat[" << j << "][" << ik << "] = " << cgmat[j][ik] << endl;
00738 }
00739 }
00740
00741
00742
00743 for (i=(jb+1); i<ist; i++)
00744 {
00745 j = indst[i];
00746 ik = indnz[j];
00747
00748 if (ik == -1)
00749 {
00750 for (k=0; k<nalc; k++) {clcmat[nagbn][k] = 0.0;}
00751
00752 indnz[j] = nagbn;
00753 indbk[nagbn] = j;
00754 ik = nagbn;
00755 nagbn++;
00756 }
00757
00758 for (k=(ja+1); k<jb ; k++)
00759 {
00760 ij = indst[k];
00761 clcmat[ik][ij] += wght*arest[i]*arest[k];
00762 if (verbose_mode) cout << "clcmat[" << ik << "][" << ij << "] = " << clcmat[ik][ij] << endl;
00763 }
00764 }
00765 ja = -1;
00766 jb = 0;
00767 ist--;
00768 }
00769 }
00770 ist++;
00771 }
00772
00773
00774
00775 Millepede::SpAVAt(clmat, clcmat, corrm, nalc, nagbn);
00776 Millepede::SpAX(clcmat, blvec, corrv, nalc, nagbn);
00777
00778 for (i=0; i<nagbn; i++)
00779 {
00780 j = indbk[i];
00781 bgvec[j] -= corrv[i];
00782
00783 for (k=0; k<nagbn; k++)
00784 {
00785 ik = indbk[k];
00786 cgmat[j][ik] -= corrm[i][k];
00787 }
00788 }
00789
00790 indst.clear();
00791 arest.clear();
00792
00793 return true;
00794 }
00795
00796
00797
00798
00799
00800
00801
00802
00803
00804
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814 bool Millepede::MakeGlobalFit(double par[], double error[], double pull[])
00815 {
00816 int i, j, nf, nvar;
00817 int itelim = 0;
00818
00819 int nstillgood;
00820
00821 double sum;
00822
00823 double step[150];
00824
00825 double trackpars[2*mlocal];
00826
00827 int ntotal_start, ntotal;
00828
00829 cout << "..... Making global fit ....." << endl;
00830
00831 ntotal_start = Millepede::GetTrackNumber();
00832
00833 std::vector<double> track_slopes;
00834
00835 track_slopes.resize(2*ntotal_start);
00836
00837 for (i=0; i<2*ntotal_start; i++) track_slopes[i] = 0.;
00838
00839 if (itert <= 1) itelim=10;
00840
00841 while (itert < itelim)
00842 {
00843 if (debug_mode) cout << "ITERATION --> " << itert << endl;
00844
00845 ntotal = Millepede::GetTrackNumber();
00846 cout << "...using " << ntotal << " tracks..." << endl;
00847
00848
00849
00850 for (i=0; i<nagb; i++) {diag[i] = cgmat[i][i];}
00851
00852
00853
00854 nf = 0;
00855
00856 for (i=0; i<nagb; i++)
00857 {
00858 if (psigm[i] <= 0.0)
00859 {
00860 nf++;
00861
00862 for (j=0; j<nagb; j++)
00863 {
00864 cgmat[i][j] = 0.0;
00865 cgmat[j][i] = 0.0;
00866 }
00867 }
00868 else if (psigm[i] > 0.0)
00869 {
00870 cgmat[i][i] += 1.0/(psigm[i]*psigm[i]);
00871 }
00872 }
00873
00874 nvar = nagb;
00875 if (debug_mode) cout << "Number of constraint equations : " << ncs << endl;
00876
00877 if (ncs > 0)
00878 {
00879 for (i=0; i<ncs; i++)
00880 {
00881 sum = arhs[i];
00882 for (j=0; j<nagb; j++)
00883 {
00884 cgmat[nvar][j] = float(ntotal)*adercs[i][j];
00885 cgmat[j][nvar] = float(ntotal)*adercs[i][j];
00886 sum -= adercs[i][j]*(pparm[j]+dparm[j]);
00887 }
00888
00889 cgmat[nvar][nvar] = 0.0;
00890 bgvec[nvar] = float(ntotal)*sum;
00891 nvar++;
00892 }
00893 }
00894
00895
00896
00897
00898 double final_cor = 0.0;
00899
00900 if (itert > 1)
00901 {
00902 for (j=0; j<nagb; j++)
00903 {
00904 for (i=0; i<nagb; i++)
00905 {
00906 if (psigm[i] > 0.0)
00907 {
00908 final_cor += step[j]*cgmat[j][i]*step[i];
00909 if (i == j) final_cor -= step[i]*step[i]/(psigm[i]*psigm[i]);
00910 }
00911 }
00912 }
00913 }
00914
00915 cout << " Final coeff is " << final_cor << endl;
00916 cout << " Final NDOFs = " << nagb << endl;
00917
00918
00919
00920 nrank = Millepede::SpmInv(cgmat, bgvec, nvar, scdiag, scflag);
00921
00922 for (i=0; i<nagb; i++)
00923 {
00924 dparm[i] += bgvec[i];
00925 if (verbose_mode) cout << "dparm[" << i << "] = " << dparm[i] << endl;
00926 if (verbose_mode) cout << "cgmat[" << i << "][" << i << "] = " << cgmat[i][i] << endl;
00927 if (verbose_mode) cout << "err = " << sqrt(fabs(cgmat[i][i])) << endl;
00928
00929 step[i] = bgvec[i];
00930
00931 if (itert == 1) error[i] = cgmat[i][i];
00932 }
00933
00934 cout << "" << endl;
00935 cout << "The rank defect of the symmetric " << nvar << " by " << nvar
00936 << " matrix is " << nvar-nf-nrank << " (bad if non 0)" << endl;
00937
00938 if (itert == 0) break;
00939 itert++;
00940
00941 cout << "" << endl;
00942 cout << "Total : " << loctot << " local fits, "
00943 << locrej << " rejected." << endl;
00944
00945
00946
00947 loctot = 0;
00948 locrej = 0;
00949
00950 if (cfactr != cfactref && sqrt(cfactr) > 1.2*cfactref)
00951 {
00952 cfactr = sqrt(cfactr);
00953 }
00954 else
00955 {
00956 cfactr = cfactref;
00957
00958 }
00959
00960 if (itert == itelim) break;
00961
00962 cout << "Iteration " << itert << " with cut factor " << cfactr << endl;
00963
00964
00965
00966 for (i=0; i<nvar; i++)
00967 {
00968 bgvec[i] = 0.0;
00969 for (j=0; j<nvar; j++)
00970 {
00971 cgmat[i][j] = 0.0;
00972 }
00973 }
00974
00975
00976
00977
00978
00979
00980
00981 nstillgood = 0;
00982
00983 for (i=0; i<ntotal_start; i++)
00984 {
00985 int rank_i = 0;
00986 int rank_f = 0;
00987
00988 (i>0) ? rank_i = abs(storeplace[i-1]) : rank_i = 0;
00989 rank_f = storeplace[i];
00990
00991 if (verbose_mode) cout << "Track " << i << " : " << endl;
00992 if (verbose_mode) cout << "Starts at " << rank_i << endl;
00993 if (verbose_mode) cout << "Ends at " << rank_f << endl;
00994
00995 if (rank_f >= 0)
00996 {
00997
00998 if (itert > 3)
00999 {
01000 indst.clear();
01001 arest.clear();
01002
01003 for (j=rank_i; j<rank_f; j++)
01004 {
01005 indst.push_back(storeind[j]);
01006
01007 if (storenl[j] == 0) arest.push_back(storeare[j]);
01008 if (storenl[j] == 1) arest.push_back(track_slopes[2*i]*storeare[j]);
01009 if (storenl[j] == 2) arest.push_back(track_slopes[2*i+1]*storeare[j]);
01010 }
01011
01012 for (j=0; j<2*nalc; j++) {trackpars[j] = 0.;}
01013
01014 Millepede::FitLoc(i,trackpars,1);
01015
01016 track_slopes[2*i] = trackpars[2];
01017 track_slopes[2*i+1] = trackpars[6];
01018 }
01019
01020 indst.clear();
01021 arest.clear();
01022
01023 for (j=rank_i; j<rank_f; j++)
01024 {
01025 indst.push_back(storeind[j]);
01026
01027 if (storenl[j] == 0) arest.push_back(storeare[j]);
01028 if (storenl[j] == 1) arest.push_back(track_slopes[2*i]*storeare[j]);
01029 if (storenl[j] == 2) arest.push_back(track_slopes[2*i+1]*storeare[j]);
01030 }
01031
01032 for (j=0; j<2*nalc; j++) {trackpars[j] = 0.;}
01033
01034 bool sc = Millepede::FitLoc(i,trackpars,0);
01035
01036 (sc)
01037 ? nstillgood++
01038 : storeplace[i] = -rank_f;
01039 }
01040 }
01041
01042 Millepede::SetTrackNumber(nstillgood);
01043
01044 }
01045
01046 Millepede::PrtGlo();
01047
01048 for (j=0; j<nagb; j++)
01049 {
01050 par[j] = dparm[j];
01051 dparm[j] = 0.;
01052 pull[j] = par[j]/sqrt(psigm[j]*psigm[j]-cgmat[j][j]);
01053 error[j] = sqrt(fabs(cgmat[j][j]));
01054 }
01055
01056 cout << std::setw(10) << " " << endl;
01057 cout << std::setw(10) << " * o o o " << endl;
01058 cout << std::setw(10) << " o o o " << endl;
01059 cout << std::setw(10) << " o ooooo o o o oo ooo oo ooo oo " << endl;
01060 cout << std::setw(10) << " o o o o o o o o o o o o o o o o " << endl;
01061 cout << std::setw(10) << " o o o o o o oooo o o oooo o o oooo " << endl;
01062 cout << std::setw(10) << " o o o o o o o ooo o o o o " << endl;
01063 cout << std::setw(10) << " o o o o o o oo o oo ooo oo ++ ends." << endl;
01064 cout << std::setw(10) << " o " << endl;
01065
01066 return true;
01067 }
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084 int Millepede::SpmInv(double v[][mgl], double b[], int n, double diag[], bool flag[])
01085 {
01086 int i, j, jj, k;
01087 double vkk, *temp;
01088 double *r, *c;
01089 double eps = 0.00000000000001;
01090
01091 r = new double[n];
01092 c = new double[n];
01093
01094 temp = new double[n];
01095
01096 for (i=0; i<n; i++)
01097 {
01098 r[i] = 0.0;
01099 c[i] = 0.0;
01100 flag[i] = true;
01101
01102 for (j=0; j<=i; j++) {if (v[j][i] == 0) {v[j][i] = v[i][j];}}
01103 }
01104
01105
01106
01107 for (i=0; i<n; i++)
01108 {
01109 for (j=0; j<n; j++)
01110 {
01111 if (fabs(v[i][j]) >= r[i]) r[i] = fabs(v[i][j]);
01112 if (fabs(v[j][i]) >= c[i]) c[i] = fabs(v[j][i]);
01113 }
01114 }
01115
01116 for (i=0; i<n; i++)
01117 {
01118 if (0.0 != r[i]) r[i] = 1./r[i];
01119 if (0.0 != c[i]) c[i] = 1./c[i];
01120
01121
01122
01123 }
01124
01125 for (i=0; i<n; i++)
01126 {
01127 for (j=0; j<n; j++) {v[i][j] = sqrt(r[i])*v[i][j]*sqrt(c[j]);}
01128 }
01129
01130 nrank = 0;
01131
01132
01133 for (i=0; i<n; i++) {diag[i] = fabs(v[i][i]);}
01134
01135 for (i=0; i<n; i++)
01136 {
01137 vkk = 0.0;
01138 k = -1;
01139
01140 for (j=0; j<n; j++)
01141 {
01142 if (flag[j] && (fabs(v[j][j])>std::max(fabs(vkk),eps*diag[j])))
01143 {
01144 vkk = v[j][j];
01145 k = j;
01146 }
01147 }
01148
01149 if (k >= 0)
01150 {
01151 if (verbose_mode) cout << "Pivot value :" << vkk << endl;
01152 nrank++;
01153 flag[k] = false;
01154 vkk = 1.0/vkk;
01155 v[k][k] = -vkk;
01156
01157 for (j=0; j<n; j++)
01158 {
01159 for (jj=0; jj<n; jj++)
01160 {
01161 if (j != k && jj != k)
01162 {
01163 v[j][jj] = v[j][jj] - vkk*v[j][k]*v[k][jj];
01164 }
01165 }
01166 }
01167
01168 for (j=0; j<n; j++)
01169 {
01170 if (j != k)
01171 {
01172 v[j][k] = (v[j][k])*vkk;
01173 v[k][j] = (v[k][j])*vkk;
01174 }
01175 }
01176 }
01177 else
01178 {
01179 for (j=0; j<n; j++)
01180 {
01181 if (flag[j])
01182 {
01183 b[j] = 0.0;
01184
01185 for (k=0; k<n; k++)
01186 {
01187 v[j][k] = 0.0;
01188 v[k][j] = 0.0;
01189 }
01190 }
01191 }
01192
01193 break;
01194 }
01195 }
01196
01197 for (i=0; i<n; i++)
01198 {
01199 for (j=0; j<n; j++) {v[i][j] = sqrt(c[i])*v[i][j]*sqrt(r[j]);}
01200 }
01201
01202 for (j=0; j<n; j++)
01203 {
01204 temp[j] = 0.0;
01205
01206 for (jj=0; jj<n; jj++)
01207 {
01208 v[j][jj] = -v[j][jj];
01209 temp[j] += v[j][jj]*b[jj];
01210 }
01211 }
01212
01213 for (j=0; j<n; j++) {b[j] = temp[j];}
01214
01215 delete temp;
01216 delete r;
01217 delete c;
01218
01219 return nrank;
01220 }
01221
01222
01223
01224
01225
01226
01227 int Millepede::SpmInv(double v[][mlocal], double b[], int n, double diag[], bool flag[])
01228 {
01229 int i, j, jj, k;
01230 double vkk, *temp;
01231 double eps = 0.0000000000001;
01232
01233 temp = new double[n];
01234
01235 for (i=0; i<n; i++)
01236 {
01237 flag[i] = true;
01238 diag[i] = fabs(v[i][i]);
01239
01240 for (j=0; j<=i; j++)
01241 {
01242 v[j][i] = v[i][j] ;
01243 }
01244 }
01245
01246 nrank = 0;
01247
01248 for (i=0; i<n; i++)
01249 {
01250 vkk = 0.0;
01251 k = -1;
01252
01253 for (j=0; j<n; j++)
01254 {
01255 if (flag[j] && (fabs(v[j][j])>std::max(fabs(vkk),eps*diag[j])))
01256 {
01257 vkk = v[j][j];
01258 k = j;
01259 }
01260 }
01261
01262 if (k >= 0)
01263 {
01264 nrank++;
01265 flag[k] = false;
01266 vkk = 1.0/vkk;
01267 v[k][k] = -vkk;
01268
01269 for (j=0; j<n; j++)
01270 {
01271 for (jj=0; jj<n; jj++)
01272 {
01273 if (j != k && jj != k)
01274 {
01275 v[j][jj] = v[j][jj] - vkk*v[j][k]*v[k][jj];
01276 }
01277 }
01278 }
01279
01280 for (j=0; j<n; j++)
01281 {
01282 if (j != k)
01283 {
01284 v[j][k] = (v[j][k])*vkk;
01285 v[k][j] = (v[k][j])*vkk;
01286 }
01287 }
01288 }
01289 else
01290 {
01291 for (j=0; j<n; j++)
01292 {
01293 if (flag[j])
01294 {
01295 b[j] = 0.0;
01296
01297 for (k=0; k<n; k++)
01298 {
01299 v[j][k] = 0.0;
01300 }
01301 }
01302 }
01303
01304 break;
01305 }
01306 }
01307
01308 for (j=0; j<n; j++)
01309 {
01310 temp[j] = 0.0;
01311
01312 for (jj=0; jj<n; jj++)
01313 {
01314 v[j][jj] = -v[j][jj];
01315 temp[j] += v[j][jj]*b[jj];
01316 }
01317 }
01318
01319 for (j=0; j<n; j++)
01320 {
01321 b[j] = temp[j];
01322 }
01323
01324 delete temp;
01325
01326 return nrank;
01327 }
01328
01329
01330
01331
01332
01333
01334
01335
01336
01337
01338
01339
01340
01341
01342
01343
01344
01345
01346
01347
01348
01349
01350 bool Millepede::SpAVAt(double v[][mlocal], double a[][mlocal], double w[][mglobl], int n, int m)
01351 {
01352 int i,j, k, l;
01353
01354 for (i=0; i<m; i++)
01355 {
01356 for (j=0; j<m; j++)
01357 {
01358 w[i][j] = 0.0;
01359
01360 for (k=0; k<n; k++)
01361 {
01362 for (l=0; l<n; l++)
01363 {
01364 w[i][j] += a[i][k]*v[k][l]*a[j][l];
01365 }
01366 }
01367 }
01368 }
01369
01370 return true;
01371 }
01372
01373
01374
01375
01376
01377
01378
01379
01380
01381
01382
01383
01384
01385
01386
01387
01388
01389
01390 bool Millepede::SpAX(double a[][mlocal], double x[], double y[], int n, int m)
01391 {
01392 int i,j;
01393
01394 for (i=0; i<m; i++)
01395 {
01396 y[i] = 0.0;
01397
01398 for (j=0; j<n; j++)
01399 {
01400 y[i] += a[i][j]*x[j];
01401 }
01402 }
01403
01404 return true;
01405 }
01406
01407
01408
01409
01410
01411
01412
01413
01414
01415
01416
01417
01418
01419 bool Millepede::PrtGlo()
01420 {
01421 double err, gcor;
01422
01423 cout << "" << endl;
01424 cout << " Result of fit for global parameters" << endl;
01425 cout << " ===================================" << endl;
01426 cout << " I initial final differ "
01427 << " lastcor Error gcor" << endl;
01428 cout << "-----------------------------------------"
01429 << "------------------------------------------" << endl;
01430
01431 for (int i=0; i<nagb; i++)
01432 {
01433 err = sqrt(fabs(cgmat[i][i]));
01434 if (cgmat[i][i] < 0.0) err = -err;
01435 gcor = 0.0;
01436
01437 if (i%(nagb/6) == 0)
01438 {
01439 cout << "-----------------------------------------"
01440 << "------------------------------------------" << endl;
01441 }
01442
01443
01444
01445 if (fabs(cgmat[i][i]*diag[i]) > 0)
01446 {
01447 cout << std::setprecision(4) << std::fixed;
01448 gcor = sqrt(fabs(1.0-1.0/(cgmat[i][i]*diag[i])));
01449 cout << std::setw(4) << i << " / " << std::setw(10) << pparm[i]
01450 << " / " << std::setw(10) << pparm[i]+ dparm[i]
01451 << " / " << std::setw(13) << dparm[i]
01452 << " / " << std::setw(13) << bgvec[i] << " / " << std::setw(10)
01453 << std::setprecision(5) << err << " / " << std::setw(10) << gcor << endl;
01454 }
01455 else
01456 {
01457 cout << std::setw(4) << i << " / " << std::setw(10) << "OFF"
01458 << " / " << std::setw(10) << "OFF"
01459 << " / " << std::setw(13) << "OFF"
01460 << " / " << std::setw(13) << "OFF"
01461 << " / " << std::setw(10) << "OFF"
01462 << " / " << std::setw(10) << "OFF" << endl;
01463 }
01464 }
01465 return true;
01466 }
01467
01468
01469
01470
01471
01472
01473
01474
01475
01476
01477
01478 double Millepede::chindl(int n, int nd)
01479 {
01480 int m;
01481 double sn[3] = {0.47523, 1.690140, 2.782170};
01482 double table[3][30] = {{1.0000, 1.1479, 1.1753, 1.1798, 1.1775, 1.1730, 1.1680, 1.1630,
01483 1.1581, 1.1536, 1.1493, 1.1454, 1.1417, 1.1383, 1.1351, 1.1321,
01484 1.1293, 1.1266, 1.1242, 1.1218, 1.1196, 1.1175, 1.1155, 1.1136,
01485 1.1119, 1.1101, 1.1085, 1.1070, 1.1055, 1.1040},
01486 {4.0000, 3.0900, 2.6750, 2.4290, 2.2628, 2.1415, 2.0481, 1.9736,
01487 1.9124, 1.8610, 1.8171, 1.7791, 1.7457, 1.7161, 1.6897, 1.6658,
01488 1.6442, 1.6246, 1.6065, 1.5899, 1.5745, 1.5603, 1.5470, 1.5346,
01489 1.5230, 1.5120, 1.5017, 1.4920, 1.4829, 1.4742},
01490 {9.0000, 5.9146, 4.7184, 4.0628, 3.6410, 3.3436, 3.1209, 2.9468,
01491 2.8063, 2.6902, 2.5922, 2.5082, 2.4352, 2.3711, 2.3143, 2.2635,
01492 2.2178, 2.1764, 2.1386, 2.1040, 2.0722, 2.0428, 2.0155, 1.9901,
01493 1.9665, 1.9443, 1.9235, 1.9040, 1.8855, 1.8681}};
01494
01495 if (nd < 1)
01496 {
01497 return 0.0;
01498 }
01499 else
01500 {
01501 m = std::max(1,std::min(n,3));
01502
01503 if (nd <= 30)
01504 {
01505 return table[m-1][nd-1];
01506 }
01507 else
01508 {
01509 return ((sn[m-1]+sqrt(float(2*nd-3)))*(sn[m-1]+sqrt(float(2*nd-3))))/float(2*nd-2);
01510 }
01511 }
01512 }
01513
01514
01515 int Millepede::GetTrackNumber() {return m_track_number;}
01516 void Millepede::SetTrackNumber(int value) {m_track_number = value;}