BOSS_7.0.2: /home/bes3soft/bes3soft/Boss/7.0.2/dist/7.0.2/Reconstruction/TrkReco/TrkReco-00-08-59-patch4-slc6tag/src/THelixFitter.cxx Source File

00001 //-----------------------------------------------------------------------------
00002 // $Id: THelixFitter.cxx,v 1.35.8.1 2012/10/29 03:46:45 xuqn Exp $
00003 //-----------------------------------------------------------------------------
00004 // Filename : THelixFitter.cc
00005 // Section  : Tracking
00006 // Owner    : Yoshi Iwasaki
00007 // Email    : yoshihito.iwasaki@kek.jp
00008 //-----------------------------------------------------------------------------
00009 // Description : A class to fit a TTrackBase object to a helix.
00010 //               See http://bsunsrv1.kek.jp/~yiwasaki/tracking/
00011 //-----------------------------------------------------------------------------
00012 
00013 /* for copysign */
00014 #if defined(__sparc)
00015 #  if defined(__EXTENSIONS__)
00016 #    include <cmath>
00017 #  else
00018 #    define __EXTENSIONS__
00019 #    include <cmath>
00020 #    undef __EXTENSIONS__
00021 #  endif
00022 #elif defined(__GNUC__)
00023 #  if defined(_XOPEN_SOURCE)
00024 #    include <cmath>
00025 #  else
00026 #    define _XOPEN_SOURCE
00027 #    include <cmath>
00028 #    undef _XOPEN_SOURCE
00029 #  endif
00030 #endif
00031 
00032 #define HEP_SHORT_NAMES
00033 #include <cfloat>
00034 //#include "panther/panther.h"
00035 //#include MDC_H
00036 #include "MdcTables/MdcTables.h"
00037 #include "CLHEP/Matrix/Vector.h"
00038 #include "CLHEP/Matrix/SymMatrix.h"
00039 #include "CLHEP/Matrix/DiagMatrix.h"
00040 #include "CLHEP/Matrix/Matrix.h"
00041 #include "TrkReco/THelixFitter.h"
00042 #include "TrkReco/TTrack.h"
00043 
00044 #include "MdcCalibFunSvc/IMdcCalibFunSvc.h"
00045 #include "MdcCalibFunSvc/MdcCalibFunSvc.h"
00046 
00047 #include "GaudiKernel/ISvcLocator.h"
00048 #include "GaudiKernel/Bootstrap.h"
00049 
00050 #include "GaudiKernel/StatusCode.h"
00051 #include "GaudiKernel/IInterface.h"
00052 #include "GaudiKernel/Kernel.h"
00053 #include "GaudiKernel/Service.h"
00054 #include "GaudiKernel/ISvcLocator.h"
00055 #include "GaudiKernel/SvcFactory.h"
00056 #include "GaudiKernel/IDataProviderSvc.h"
00057 #include "GaudiKernel/Bootstrap.h"
00058 #include "GaudiKernel/MsgStream.h"
00059 #include "GaudiKernel/SmartDataPtr.h"
00060 #include "GaudiKernel/IMessageSvc.h"
00061 
00062 
00063 #include "EventModel/EventModel.h"
00064 #include  "EvTimeEvent/RecEsTime.h"
00065 
00066 
00067 using CLHEP::HepVector;
00068 using CLHEP::HepSymMatrix;
00069 using CLHEP::HepDiagMatrix;
00070 using CLHEP::HepMatrix;
00071 
00072 // speed up option by j.tanaka (2001/04/14)
00073 #define OPTJT
00074 
00075 /*
00076 extern "C" 
00077 void
00078 calcdc_driftdist_(int *,
00079                   int *,
00080                   int *,
00081                   float[3],
00082                   float[3],
00083                   float *,
00084                   float *,
00085                   float *);
00086 extern "C"
00087 void 
00088 calcdc_driftdist3_(int *,
00089                    int *,                     
00090                    float[3],
00091                    float[3],
00092                    float *,
00093                    float[2],
00094                    float[2], 
00095                    float[2]);
00096 
00097 extern "C"
00098 void
00099 calcdc_tof2_(int *, float *, float *, float *);
00100 */
00101 
00102 #define NTrailMax 100
00103 #define Convergence 1.0e-5
00104 
00105 extern float
00106 TrkRecoHelixFitterChisqMax;
00107 
00108 #ifdef TRKRECO_DEBUG
00109 /*
00110 #include "tuple/BelleTupleManager.h"
00111 #include "TrkReco/TConformalFinder.h"
00112 #include "TrkReco/TMDCWireHitMC.h"
00113 BelleHistogram * _nCall[8];
00114 BelleHistogram * _nTrial[8];
00115 BelleHistogram * _pull[2][2][8];
00116 BelleHistogram * _nTrialNegative;
00117 BelleHistogram * _nTrialPositive;
00118 bool first = true;
00119 */
00120 #endif
00121 
00122 THelixFitter::THelixFitter(const std::string & name)
00123 : TMFitter(name),
00124   _fit2D(false),
00125   _freeT0(false),
00126   _sag(false),
00127   _propagation(false),
00128   _tof(false),
00129   _tanl(false),
00130   _pre_chi2(0.),
00131   _fitted_chi2(0.),
00132   m_pmgnIMF(0) {
00133 
00134     //yzhang delete
00135   //StatusCode scmgn = Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF); 
00136   //if(scmgn!=StatusCode::SUCCESS) { 
00137   //  std::cout<< name <<" Unable to open Magnetic field service"<<std::endl;
00138   //}else{
00139   //  std::cout<< name <<" open Magnetic field service"<<std::endl;
00140   //}
00141 
00142 }
00143 
00144 THelixFitter::~THelixFitter() {
00145 }
00146 
00147 #ifdef OPTJT
00148 // speed up
00149 int
00150 THelixFitter::main(TTrackBase & b, float t0Offset,
00151                    double *pre_chi2, double *fitted_chi2) const {
00152 #ifdef TRKRECO_DEBUG
00153 /*
00154         if (first) {
00155         first = false;
00156         extern BelleTupleManager * BASF_Histogram;
00157         BelleTupleManager * m = BASF_Histogram;
00158         _nCall[0] = m->histogram("HF nCall all", 1, 0., 1.);
00159         _nCall[1] = m->histogram("HF nCall conf f2d l0", 1, 0., 1.);
00160         _nCall[2] = m->histogram("HF nCall conf f3d l0", 1, 0., 1.);
00161         _nCall[3] = m->histogram("HF nCall conf f2d l1", 1, 0., 1.);
00162         _nCall[4] = m->histogram("HF nCall conf f3d l1", 1, 0., 1.);
00163         _nCall[5] = m->histogram("HF nCall conf s2d", 1, 0., 1.);
00164         _nCall[6] = m->histogram("HF nCall conf s3d", 1, 0., 1.);
00165         _nCall[7] = m->histogram("HF nCall other", 1, 0., 1.);
00166         _nTrial[0] = m->histogram("HF nTrial all", 100, 0., 100.);
00167         _nTrial[1] = m->histogram("HF nTrial conf f2d l0", 100, 0., 100.);
00168         _nTrial[2] = m->histogram("HF nTrial conf f3d l0", 100, 0., 100.);
00169         _nTrial[3] = m->histogram("HF nTrial conf f2d l1", 100, 0., 100.);
00170         _nTrial[4] = m->histogram("HF nTrial conf f3d l1", 100, 0., 100.);
00171         _nTrial[5] = m->histogram("HF nTrial conf s2d", 100, 0., 100.);
00172         _nTrial[6] = m->histogram("HF nTrial conf s3d", 100, 0., 100.);
00173         _nTrial[7] = m->histogram("HF nTrial other", 100, 0., 100.);
00174         _pull[0][0][0] = m->histogram("HF pull ax true all",
00175                                       100, 0., 5000.);
00176         _pull[0][0][2] = m->histogram("HF pull ax true conf f3d l0",
00177                                       100, 0., 5000.);
00178         _pull[0][0][4] = m->histogram("HF pull ax true conf f3d l1",
00179                                       100, 0., 5000.);
00180         _pull[0][0][6] = m->histogram("HF pull ax true conf s3d",
00181                                       100, 0., 5000.);
00182         _pull[0][0][7] = m->histogram("HF pull ax true other",
00183                                       100, 0., 5000.);
00184         _pull[1][0][0] = m->histogram("HF pull st true all",
00185                                       100, 0., 5000.);
00186         _pull[1][0][2] = m->histogram("HF pull st true conf f3d l0", 
00187                                       100, 0., 5000.);
00188         _pull[1][0][4] = m->histogram("HF pull st true conf f3d l1",
00189                                       100, 0., 5000.);
00190         _pull[1][0][6] = m->histogram("HF pull st true conf s3d",
00191                                       100, 0., 5000.);
00192         _pull[1][0][7] = m->histogram("HF pull st true other",
00193                                       100, 0., 5000.);
00194         _pull[0][1][0] = m->histogram("HF pull ax wrong all",
00195                                       100, 0., 5000.);
00196         _pull[0][1][2] = m->histogram("HF pull ax wrong conf f3d l0",
00197                                       100, 0., 5000.);
00198         _pull[0][1][4] = m->histogram("HF pull ax wrong conf f3d l1",
00199                                       100, 0., 5000.);
00200         _pull[0][1][6] = m->histogram("HF pull ax wrong conf s3d",
00201                                       100, 0., 5000.);
00202         _pull[0][1][7] = m->histogram("HF pull ax wrong other",
00203                                       100, 0., 5000.);
00204         _pull[1][1][0] = m->histogram("HF pull st wrong all",
00205                                       100, 0., 5000.);
00206         _pull[1][1][2] = m->histogram("HF pull st wrong conf f3d l0",
00207                                       100, 0., 5000.);
00208         _pull[1][1][4] = m->histogram("HF pull st wrong conf f3d l1",
00209                                       100, 0., 5000.);
00210         _pull[1][1][6] = m->histogram("HF pull st wrong conf s3d",
00211                                       100, 0., 5000.);
00212         _pull[1][1][7] = m->histogram("HF pull st wrong other",
00213                                       100, 0., 5000.);
00214         _nTrialPositive = m->histogram("HF nTrial +", 100, 0., 100.);
00215         _nTrialNegative = m->histogram("HF nTrial -", 100, 0., 100.);
00216     }
00217     _nCall[0]->accumulate(.5);
00218     if (TConformalFinder::_stage == ConformalOutside)
00219         _nCall[7]->accumulate(.5);
00220     else if (TConformalFinder::_stage == ConformalFast2DLevel0)
00221         _nCall[1]->accumulate(.5);
00222     else if (TConformalFinder::_stage == ConformalFast3DLevel0)
00223         _nCall[2]->accumulate(.5);
00224     else if (TConformalFinder::_stage == ConformalFast2DLevel1)
00225         _nCall[3]->accumulate(.5);
00226     else if (TConformalFinder::_stage == ConformalFast3DLevel1)
00227         _nCall[4]->accumulate(.5);
00228     else if (TConformalFinder::_stage == ConformalSlow2D)
00229         _nCall[5]->accumulate(.5);
00230     else if (TConformalFinder::_stage == ConformalSlow3D)
00231         _nCall[6]->accumulate(.5);
00232     bool posi = true;
00233     const TTrackHEP & hep = Links2HEP(b.links());
00234 */
00235 #endif
00236     //...Initialize
00237     _pre_chi2 = _fitted_chi2 = 0.;
00238     if(pre_chi2)*pre_chi2 = _pre_chi2;
00239     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
00240 
00241     //...Type check...
00242     if (b.objectType() != Track) return TFitUnavailable;
00243     TTrack & t = (TTrack &) b;
00244 
00245     //...Already fitted ?...
00246     if (t.fitted()) return TFitAlreadyFitted;
00247 
00248     //...Count # of hits...
00249     AList<TMLink> cores = t.cores();
00250 #ifdef TRKRECO_DEBUG
00251     cout<<__FILE__<<" cores in helix = "<<cores.length()<<endl;
00252 #endif
00253     if (_fit2D) cores = AxialHits(cores);
00254     unsigned nCores = cores.length();
00255     unsigned nStereoCores = NStereoHits(cores);
00256 
00257     //...2D or 3D...
00258     bool fitBy2D = _fit2D;
00259     if (! fitBy2D) fitBy2D = (! nStereoCores);
00260 
00261     //...Check # of hits...
00262     if (! fitBy2D) {
00263         if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
00264             return TFitErrorFewHits;
00265     }
00266     else {
00267         if (nCores < 3) return TFitErrorFewHits;
00268     }
00269 
00270     //...Setup...
00271     Vector a(5), da(5);
00272     a = t.helix().a();
00273     Vector dxda(5);
00274     Vector dyda(5);
00275     Vector dzda(5);
00276     Vector dDda(5);
00277     Vector dchi2da(5);
00278     SymMatrix d2chi2d2a(5, 0);
00279     static const SymMatrix zero5(5, 0);
00280     double chi2;
00281     double chi2Old = DBL_MAX;
00282     const double convergence = Convergence;
00283     bool allAxial = true;
00284     Matrix e(3, 3);
00285     Vector f(3);
00286     int err = 0;
00287     double factor = 1.0;//jtanaka0715
00288 
00289     //...For bad hit rejection...(by JT, 2001/04/12)...
00290     int flagBad = 0;
00291     if (TrkRecoHelixFitterChisqMax != 0.)
00292         flagBad = 1;
00293     AList<TMLink> initBadWires; 
00294     unsigned nInitBadWires = 0;
00295     Vector initBadDchi2da(5);
00296     SymMatrix initBadD2chi2d2a(5, 0);
00297     for (unsigned j = 0; j < 5; ++j) initBadDchi2da[j] = 0.;
00298     double initBadChi2 = 0.;
00299 
00300     //...Fitting loop...
00301     unsigned nTrial = 0;
00302     while (nTrial < NTrailMax) {
00303 
00304         //...Set up...
00305         chi2 = 0.;
00306         for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
00307         d2chi2d2a = zero5;
00308 
00309     //yuany
00310 #ifdef TRKRECO_DEBUG
00311         cout<<"helix fitter a5 "<<t.helix().a()<< " cores.length "<<cores.length()<<endl;
00312 #endif
00313         //...Loop with hits...
00314         unsigned i = 0;
00315         while (TMLink * l = cores[i++]) {
00316             const TMDCWireHit & h = * l->hit();
00317     //yuany
00318 #ifdef TRKRECO_DEBUG
00319             cout<<"trial "<<nTrial<<" wire name "<<l->wire()->name()<<"    L "<<(h.state()&WireHitPatternLeft)<<" R "<<(h.state()&WireHitPatternRight)<<" link "<<l->leftRight()<<endl;
00320 #endif
00321             //...Cal. closest points...
00322             t.approach(* l, _sag);
00323             double dPhi = l->dPhi();
00324             const HepPoint3D & onTrack = l->positionOnTrack();
00325             const HepPoint3D & onWire = l->positionOnWire();
00326             unsigned leftRight = (onWire.cross(onTrack).z() < 0.)
00327                 ? WireHitLeft : WireHitRight;
00328 
00329             //...Obtain drift distance and its error...
00330             double distance;
00331             double eDistance;
00332             drift(t, * l, t0Offset, distance, eDistance);
00333             l->drift(distance,0);
00334             l->drift(distance,1);
00335             l->dDrift(eDistance,0);
00336             l->dDrift(eDistance,1);
00337 
00338 
00339             double inv_eDistance2 = 1. / (eDistance * eDistance);
00340             
00341             //...Residual...
00342             HepVector3D v = onTrack - onWire;
00343             double vmag = v.mag();
00344 #ifdef TRKRECO_DEBUG
00345             std::cout<<"THelixFitter::eDistance-----"<<eDistance<<endl;
00346             cout<<"  vmag = "<<vmag<<"   distance = "<<distance<<"  eDistance = "<<eDistance<<endl;
00347 #endif
00348             double dDistance = vmag - distance;
00349             
00350             //...dxda...
00351             this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
00352 
00353             //...Chi2 related...
00354             // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
00355             // Vector3 vw = h.wire()->direction();
00356             double vw[3] = { h.wire()->direction().x(),
00357                              h.wire()->direction().y(),
00358                              h.wire()->direction().z() };
00359             double vwxy = vw[0]*vw[1];
00360             double vwyz = vw[1]*vw[2];
00361             double vwzx = vw[2]*vw[0];
00362             dDda = (vmag > 0.)
00363                 ? ((v.x() * (1. - vw[0] * vw[0]) -
00364                     v.y() * vwxy - v.z() * vwzx)
00365                    * dxda + 
00366                    (v.y() * (1. - vw[1] * vw[1]) -
00367                     v.z() * vwyz - v.x() * vwxy)
00368                    * dyda + 
00369                    (v.z() * (1. - vw[2] * vw[2]) -
00370                     v.x() * vwzx - v.y() * vwyz)
00371                    * dzda) / vmag
00372                 : Vector(5,0);
00373             if (vmag <= 0.0) {
00374                 std::cout << "    in fit " << onTrack << ", " << onWire;
00375                 h.dump();
00376             }
00377             double pChi2 = dDistance * dDistance * inv_eDistance2;
00378 #ifdef TRKRECO_DEBUG
00379             std::cout<<"THelixFitter::dDistance"<<dDistance<<" inv_eDistance2 "<<inv_eDistance2<<endl; 
00380             cout<<"Liuqg check ... .. pChi2 = "<<pChi2<<endl;
00381 #endif
00382 
00383             //...Bad hit rejection...
00384             if (flagBad && nTrial == 0) {
00385                 if (pChi2 > TrkRecoHelixFitterChisqMax) {
00386                     initBadWires.append(l);
00387                     initBadDchi2da += (dDistance * inv_eDistance2) * dDda;
00388                     initBadD2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
00389                     initBadChi2 += pChi2;
00390                 }
00391             }
00392             else {
00393                 dchi2da += (dDistance * inv_eDistance2) * dDda;
00394                 d2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
00395                 chi2 += pChi2;      
00396 
00397                 //...Store results...
00398                 l->update(onTrack, onWire, leftRight, pChi2);
00399             }
00400 
00401 #ifdef TRKRECO_DEBUG
00402 //          if ((! fitBy2D) && (nTrial == 0)) {
00403 //              unsigned as = 0;
00404 //              if (l->hit()->wire()->stereo()) as = 1;
00405 //              unsigned mt = 0;
00406 //              if (& hep != l->hit()->mc()->hep()) mt = 1;
00407 
00408 //              _pull[as][mt][0]->accumulate(pChi2);
00409 //              if (TConformalFinder::_stage == ConformalOutside)
00410 //                  _pull[as][mt][7]->accumulate(pChi2);
00411 //              else if (TConformalFinder::_stage == ConformalFast3DLevel0)
00412 //                  _pull[as][mt][2]->accumulate(pChi2);
00413 //              else if (TConformalFinder::_stage == ConformalFast3DLevel1)
00414 //                  _pull[as][mt][4]->accumulate(pChi2);
00415 //              else if (TConformalFinder::_stage == ConformalSlow3D)
00416 //                  _pull[as][mt][6]->accumulate(pChi2);
00417 //          }
00418 #endif
00419         }
00420 
00421         //...Bad hit rejection...
00422         if (flagBad && nTrial == 0) {
00423             if ((initBadWires.length() == 1 || initBadWires.length() == 2) &&
00424                 nCores >= 20 &&
00425                 chi2 / (double)(nCores - initBadWires.length()) < 10.) {
00426                 cores.remove(initBadWires);
00427                 nInitBadWires = initBadWires.length();
00428             }
00429             else if (initBadWires.length() != 0) {
00430                 dchi2da += initBadDchi2da;
00431                 d2chi2d2a += initBadD2chi2d2a;
00432                 chi2 += initBadChi2;
00433             }
00434         }
00435 
00436         //...Save chi2 information...
00437         if (nTrial == 0) {
00438             _pre_chi2 = chi2;
00439             _fitted_chi2 = chi2;
00440         }
00441         else {
00442             _fitted_chi2 = chi2;
00443         }
00444 
00445         //...Check condition...
00446         double change = chi2Old - chi2;
00447 #ifdef TRKRECO_DEBUG_DETAIL
00448         std::cout<<" chi2 change  "<<change <<" convergence  "<<convergence <<" break?  "<<(fabs(change) < convergence == true)<<std::endl;
00449 #endif
00450         if (fabs(change) < convergence) break;
00451         if (change < 0.) {
00452 //          a += factor * da;
00453 //          t._helix->a(a);
00454 //          break;
00455             factor = 0.5;
00456         }
00457         chi2Old = chi2;
00458 
00459         //...Cal. helix parameters for next loop...
00460         if (fitBy2D) {
00461             f = dchi2da.sub(1, 3);
00462             e = d2chi2d2a.sub(1, 3);
00463             f = solve(e, f);
00464             da[0] = f[0];
00465             da[1] = f[1];
00466             da[2] = f[2];
00467             da[3] = 0.;
00468             da[4] = 0.;
00469         }
00470         else {
00471             da = solve(d2chi2d2a, dchi2da);
00472         }
00473 
00474         a -= factor * da;
00475         t._helix->a(a);
00476         ++nTrial;
00477 
00478         // jtanaka 001008
00479         //if( fabs(a[3]) > 200. ){
00480         // yiwasaki 001010
00481         if( fabs(a[3]) > 1000. ){
00482           // stop "fit" and return error.
00483           //std::cout << "Stop Fit... " << a << std::endl;
00484           err = 1;
00485           break;
00486         }
00487 #ifdef TRKRECO_DEBUG_DETAIL
00488         std::cout << "            fit " << nTrial-1<< " : " << chi2 << " : "
00489                   << change << std::endl;
00490 #endif
00491     }
00492 
00493 #ifdef TRKRECO_DEBUG
00494 /*
00495     _nTrial[0]->accumulate(float(nTrial) + .5);
00496     if (TConformalFinder::_stage == ConformalOutside)
00497         _nTrial[7]->accumulate(float(nTrial) + .5);
00498     else if (TConformalFinder::_stage == ConformalFast2DLevel0)
00499         _nTrial[1]->accumulate(float(nTrial) + .5);
00500     else if (TConformalFinder::_stage == ConformalFast3DLevel0)
00501         _nTrial[2]->accumulate(float(nTrial) + .5);
00502     else if (TConformalFinder::_stage == ConformalFast2DLevel1)
00503         _nTrial[3]->accumulate(float(nTrial) + .5);
00504     else if (TConformalFinder::_stage == ConformalFast3DLevel1)
00505         _nTrial[4]->accumulate(float(nTrial) + .5);
00506     else if (TConformalFinder::_stage == ConformalSlow2D)
00507         _nTrial[5]->accumulate(float(nTrial) + .5);
00508     else if (TConformalFinder::_stage == ConformalSlow3D)
00509         _nTrial[6]->accumulate(float(nTrial) + .5);
00510 
00511     if (posi) _nTrialPositive->accumulate((float) nTrial + .5);
00512     else _nTrialNegative->accumulate((float) nTrial + .5);
00513 */
00514 #endif
00515 
00516     //...Cal. error matrix...
00517     SymMatrix Ea(5, 0);
00518     unsigned dim;
00519     if (fitBy2D) {
00520         dim = 3;
00521         SymMatrix Eb(3, 0), Ec(3, 0);
00522         Eb = d2chi2d2a.sub(1, 3);
00523         Ec = Eb.inverse(err);
00524         Ea[0][0] = Ec[0][0];
00525         Ea[0][1] = Ec[0][1];
00526         Ea[0][2] = Ec[0][2];
00527         Ea[1][1] = Ec[1][1];
00528         Ea[1][2] = Ec[1][2];
00529         Ea[2][2] = Ec[2][2];
00530     }
00531     else {
00532         dim = 5;
00533         Ea = d2chi2d2a.inverse(err);
00534     }
00535 
00536     //...Store information...
00537     if (! err) {
00538         t._helix->a(a);
00539         t._helix->Ea(Ea);
00540         t._fitted = true;
00541     }
00542     else {
00543         err = TFitFailed;
00544     }
00545 
00546     t._charge = copysign(1., a[2]);    
00547     t._ndf = nCores - dim -nInitBadWires;
00548     t._chi2 = chi2;
00549 
00550     //...Treatment for bad wires...
00551     if (nInitBadWires) {
00552         for  (unsigned i = 0; i < nInitBadWires; i++) {
00553             TMLink * l = initBadWires[i];
00554             t.approach(* l, _sag);
00555             double dPhi = l->dPhi();
00556             const HepPoint3D & onTrack = l->positionOnTrack();
00557             const HepPoint3D & onWire = l->positionOnWire();
00558             HepVector3D v = onTrack - onWire;
00559             double vmag = v.mag();
00560             unsigned leftRight = (onWire.cross(onTrack).z() < 0.)
00561                 ? WireHitLeft : WireHitRight;
00562             double distance;
00563             double eDistance;
00564             drift(t, * l, t0Offset, distance, eDistance);
00565             l->drift(distance,0);
00566             l->drift(distance,1);
00567             l->dDrift(eDistance,0);
00568             l->dDrift(eDistance,1);
00569 
00570 
00571             double inv_eDistance2 = 1. / (eDistance * eDistance);
00572             double dDistance = vmag - distance;
00573             double pChi2 = dDistance * dDistance * inv_eDistance2;
00574             l->update(onTrack, onWire, leftRight, pChi2);
00575         }
00576 #ifdef TRKRECO_DEBUG_DETAIL
00577         std::cout << "            HelixFitter : # of rejected hits="
00578                   << nInitBadWires << endl;
00579 #endif
00580     }
00581 
00582     if (pre_chi2) * pre_chi2 = _pre_chi2;
00583     if (fitted_chi2) * fitted_chi2 = _fitted_chi2;
00584 
00585     return err;
00586 }
00587 #else
00588 int
00589 THelixFitter::main(TTrackBase & b, float t0Offset,
00590                    double *pre_chi2, double *fitted_chi2) const {
00591 #ifdef TRKRECO_DEBUG_DETAIL
00592 /*
00593     if (first) {
00594         first = false;
00595         extern BelleTupleManager * BASF_Histogram;
00596         BelleTupleManager * m = BASF_Histogram;
00597         _nCall = m->histogram("HF nCall", 1, 0., 1.);
00598         _nTrial = m->histogram("HF nTrial", 100, 0., 100.);
00599         _nTrialPositive = m->histogram("HF nTrial +", 100, 0., 100.);
00600         _nTrialNegative = m->histogram("HF nTrial -", 100, 0., 100.);
00601     }
00602     _nCall->accumulate(1.);
00603     bool posi = true;
00604     std::cout << "        THelixFitter::fit ..." << std::endl;
00605 */
00606 #endif
00607     //...Initialize
00608     _pre_chi2 = _fitted_chi2 = 0.;
00609     if(pre_chi2)*pre_chi2 = _pre_chi2;
00610     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
00611 
00612     //...Type check...
00613     if (b.objectType() != Track) return TFitUnavailable;
00614     TTrack & t = (TTrack &) b;
00615 
00616     //...Already fitted ?...
00617     if (t.fitted()) return TFitAlreadyFitted;
00618 
00619     //...Count # of hits...
00620     AList<TMLink> cores = t.cores();
00621     if (_fit2D) cores = AxialHits(cores);
00622     unsigned nCores = cores.length();
00623     unsigned nStereoCores = NStereoHits(cores);
00624 
00625     //...2D or 3D...
00626     bool fitBy2D = _fit2D;
00627     if (! fitBy2D) fitBy2D = (! nStereoCores);
00628 
00629     //...Check # of hits...
00630     if (! fitBy2D) {
00631         if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
00632             return TFitErrorFewHits;
00633     }
00634     else {
00635         if (nCores < 3) return TFitErrorFewHits;
00636     }
00637 
00638     //...Setup...
00639     Vector a(5), da(5);
00640     a = t.helix().a();
00641     Vector dxda(5);
00642     Vector dyda(5);
00643     Vector dzda(5);
00644     Vector dDda(5);
00645     Vector dchi2da(5);
00646     SymMatrix d2chi2d2a(5, 0);
00647     static const SymMatrix zero5(5, 0);
00648     double chi2;
00649     double chi2Old = DBL_MAX;
00650     const double convergence = Convergence;
00651     bool allAxial = true;
00652     Matrix e(3, 3);
00653     Vector f(3);
00654     int err = 0;
00655     double factor = 1.0;//jtanaka0715
00656 
00657     int flagBad = 0; // 2001/04/12
00658     AList<TMLink> initBadWires; 
00659     unsigned nInitBadWires = 0;
00660     Vector initBadDchi2da(5);
00661     SymMatrix initBadD2chi2d2a(5, 0);
00662     for (unsigned j = 0; j < 5; ++j) initBadDchi2da[j] = 0.;
00663     double initBadChi2 = 0.;
00664 
00665     //...Fitting loop...
00666     unsigned nTrial = 0;
00667     while (nTrial < NTrailMax) {
00668 
00669         //...Set up...
00670         chi2 = 0.;
00671         for (unsigned j = 0; j < 5; j++) dchi2da[j] = 0.;
00672         d2chi2d2a = zero5;
00673 
00674         //...Loop with hits...
00675         unsigned i = 0;
00676         while (TMLink * l = cores[i++]) {
00677             const TMDCWireHit & h = * l->hit();
00678 
00679             //...Cal. closest points...
00680             t.approach(* l, _sag);
00681             double dPhi = l->dPhi();
00682             const HepPoint3D & onTrack = l->positionOnTrack();
00683             const HepPoint3D & onWire = l->positionOnWire();
00684             unsigned leftRight = WireHitRight;
00685             if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
00686 
00687             //...Obtain drift distance and its error...
00688             double distance;
00689             double eDistance;
00690             drift(t, * l, t0Offset, distance, eDistance);
00691            
00692 
00693             double eDistance2 = eDistance * eDistance;
00694 
00695             //...Residual...
00696             HepVector3D v = onTrack - onWire;
00697             double vmag = v.mag();
00698             double dDistance = vmag - distance;
00699 
00700             //...dxda...
00701             this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
00702 
00703             //...Chi2 related...
00704             // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
00705             HepVector3D vw = h.wire()->direction();
00706             dDda = (vmag > 0.)
00707                 ? ((v.x() * (1. - vw.x() * vw.x()) -
00708                     v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
00709                    * dxda + 
00710                    (v.y() * (1. - vw.y() * vw.y()) -
00711                     v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
00712                    * dyda + 
00713                    (v.z() * (1. - vw.z() * vw.z()) -
00714                     v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
00715                    * dzda) / vmag
00716                 : Vector(5,0);
00717             if (vmag <= 0.0) {
00718                 std::cout << "    in fit " << onTrack << ", " << onWire;
00719                 h.dump();
00720             }
00721             double pChi2 = dDistance * dDistance / eDistance2;
00722             if(flagBad){ // 2001/04/12
00723               if(nTrial == 0 && pChi2 > 1500.){
00724                 initBadWires.append(l);
00725                 initBadDchi2da += (dDistance / eDistance2) * dDda;
00726                 initBadD2chi2d2a += vT_times_v(dDda) / eDistance2;
00727                 initBadChi2 += pChi2;
00728               }else{
00729                 dchi2da += (dDistance / eDistance2) * dDda;
00730                 d2chi2d2a += vT_times_v(dDda) / eDistance2;
00731                 chi2 += pChi2;      
00732                 //...Store results...
00733                 l->update(onTrack, onWire, leftRight, pChi2);
00734               }
00735             }else{
00736               dchi2da += (dDistance / eDistance2) * dDda;
00737               d2chi2d2a += vT_times_v(dDda) / eDistance2;
00738               chi2 += pChi2;        
00739               //...Store results...
00740               l->update(onTrack, onWire, leftRight, pChi2);
00741             }
00742         }
00743 
00744         if(flagBad){ // 2001/04/12
00745           if(nTrial == 0 &&
00746              (initBadWires.length() == 1 ||
00747               initBadWires.length() == 2) &&
00748              nCores >= 20 &&
00749              chi2/(double)(nCores-initBadWires.length()) < 10.){
00750             cores.remove(initBadWires);
00751             nInitBadWires = initBadWires.length();
00752           }else if(nTrial == 0 && initBadWires.length() != 0){
00753             dchi2da += initBadDchi2da;
00754             d2chi2d2a += initBadD2chi2d2a;
00755             chi2 += initBadChi2;
00756           }
00757         }
00758 
00759         //...Save chi2 information...
00760         if(nTrial == 0){
00761           _pre_chi2 = chi2;
00762           _fitted_chi2 = chi2;
00763         }else _fitted_chi2 = chi2;
00764 
00765         //...Check condition...
00766         double change = chi2Old - chi2;
00767         if (fabs(change) < convergence) break;
00768         if (change < 0.) {
00769 //          a += factor * da;
00770 //          t._helix->a(a);
00771 //          break;
00772             factor = 0.5;
00773         }
00774         chi2Old = chi2;
00775 
00776         //...Cal. helix parameters for next loop...
00777         if (fitBy2D) {
00778             f = dchi2da.sub(1, 3);
00779             e = d2chi2d2a.sub(1, 3);
00780             f = solve(e, f);
00781             da[0] = f[0];
00782             da[1] = f[1];
00783             da[2] = f[2];
00784             da[3] = 0.;
00785             da[4] = 0.;
00786         }
00787         else {
00788             da = solve(d2chi2d2a, dchi2da);
00789         }
00790 
00791         a -= factor * da;
00792         t._helix->a(a);
00793         ++nTrial;
00794 
00795         // jtanaka 001008
00796         //if( fabs(a[3]) > 200. ){
00797         // yiwasaki 001010
00798         if( fabs(a[3]) > 1000. ){
00799           // stop "fit" and return error.
00800           //std::cout << "Stop Fit... " << a << std::endl;
00801           err = 1;
00802           break;
00803         }
00804 #ifdef TRKRECO_DEBUG_DETAIL
00805         std::cout << "            fit " << nTrial-1<< " : " << chi2 << " : "
00806                   << change << std::endl;
00807 #endif
00808     }
00809 
00810 #ifdef TRKRECO_DEBUG_DETAIL
00811 /*
00812     _nTrial->accumulate((float) nTrial + .5);
00813     if (posi) _nTrialPositive->accumulate((float) nTrial + .5);
00814     else _nTrialNegative->accumulate((float) nTrial + .5);
00815 */
00816 #endif
00817 
00818     //...Cal. error matrix...
00819     SymMatrix Ea(5, 0);
00820     unsigned dim;
00821     if (fitBy2D) {
00822         dim = 3;
00823         SymMatrix Eb(3, 0), Ec(3, 0);
00824         Eb = d2chi2d2a.sub(1, 3);
00825         Ec = Eb.inverse(err);
00826         Ea[0][0] = Ec[0][0];
00827         Ea[0][1] = Ec[0][1];
00828         Ea[0][2] = Ec[0][2];
00829         Ea[1][1] = Ec[1][1];
00830         Ea[1][2] = Ec[1][2];
00831         Ea[2][2] = Ec[2][2];
00832     }
00833     else {
00834         dim = 5;
00835         Ea = d2chi2d2a.inverse(err);
00836     }
00837 
00838     //...Store information...
00839     if (! err) {
00840         t._helix->a(a);
00841         t._helix->Ea(Ea);
00842         t._fitted = true;
00843     }
00844     else {
00845         err = TFitFailed;
00846     }
00847 
00848     t._charge = copysign(1., a[2]);    
00849     t._ndf = nCores - dim -nInitBadWires;
00850     t._chi2 = chi2;
00851 
00852     if(pre_chi2)*pre_chi2 = _pre_chi2;
00853     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
00854   
00855     return err;
00856 }
00857 #endif
00858 
00859 #ifdef OPTJT
00860 // speed up
00861 int
00862 THelixFitter::dxda(const TMLink & link,
00863                    const Helix & h,
00864                    double dPhi,
00865                    Vector & dxda,
00866                    Vector & dyda,
00867                    Vector & dzda) const {
00868    // std::cout << "1st: " << m_pmgnIMF<< std::endl;
00869    // maybe m_pmgnIMF == NULL here FIXME
00870    if(!m_pmgnIMF) {
00871     std::cout<<" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL  "<<std::endl;
00872      return 0;
00873    }
00874     //...Setup...
00875     const TMDCWire & w = * link.wire();
00876     const Vector & a = h.a();
00877     const double dRho  = a[0];
00878     const double phi0  = a[1];
00879     const double kappa = a[2];
00880    // const double rho   = Helix::ConstantAlpha / kappa;
00881    // const double rho   = 333.564095 / kappa;
00882     
00883     const double Bz = -1000*m_pmgnIMF->getReferField();
00884     const double rho = 333.564095/(kappa * Bz);    
00885     const double tanLambda = a[4];
00886 
00887     const double sinPhi0 = sin(phi0);
00888     const double cosPhi0 = cos(phi0);
00889     // double sinPhi0 = h.sinphi0();
00890     // double cosPhi0 = h.cosphi0();
00891     const double sinPhi0dPhi = sin(phi0+dPhi);
00892     const double cosPhi0dPhi = cos(phi0+dPhi);
00893     //Vector dphida(5);
00894     double dphida[5];
00895 
00896     //...Sag correction...
00897     HepPoint3D xw = w.xyPosition();
00898     HepPoint3D wireBackwardPosition = w.backwardPosition();
00899     HepVector3D v = w.direction();
00900     if (_sag)
00901         w.wirePosition(link.positionOnTrack().z(),
00902                        xw,
00903                        wireBackwardPosition,
00904                        v);
00905  //yzhang 2012-08-30
00906     //...Axial case...
00907 //    if (w.axial()) {
00908 //      const double d[3] = { h.center().x()-xw.x(),
00909 //                            h.center().y()-xw.y(),
00910 //                            h.center().z()-xw.z() };
00911 //      const double inv_dmag2 = 1./(d[0]*d[0]+d[1]*d[1]+d[2]*d[2]);
00912 //
00913 //      const double rho_per_kappa  = rho/kappa;
00914 //      const double dRho_plus_rho  = dRho+rho;
00915 //      const double rhoSinPhi0dPhi = rho*sinPhi0dPhi;
00916 //      const double rhoCosPhi0dPhi = rho*cosPhi0dPhi;
00917 //      const double rhoTanLambda   = rho*tanLambda;
00918 //      
00919 //      dphida[0] = (sinPhi0*d[0]-cosPhi0*d[1])*inv_dmag2;
00920 //      dphida[1] = dRho_plus_rho*(cosPhi0*d[0]+sinPhi0*d[1])*inv_dmag2-1.;
00921 //      dphida[2] = -rho_per_kappa*dphida[0];
00922 //      dphida[3] = 0.;
00923 //      dphida[4] = 0.;
00924 //      
00925 //      dxda[0] = cosPhi0+rhoSinPhi0dPhi*dphida[0];
00926 //      dxda[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi*(1.+dphida[1]);
00927 //      dxda[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi)+rhoSinPhi0dPhi*dphida[2];
00928 //      dxda[3] = 0.;
00929 //      dxda[4] = 0.;
00930 //      
00931 //      dyda[0] = sinPhi0-rhoCosPhi0dPhi*dphida[0];
00932 //      dyda[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi*(1.+dphida[1]);
00933 //      dyda[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi)-rhoCosPhi0dPhi*dphida[2];
00934 //      dyda[3] = 0.;
00935 //      dyda[4] = 0.;
00936 //      
00937 //      dzda[0] = -rhoTanLambda*dphida[0];
00938 //      dzda[1] = -rhoTanLambda*dphida[1];
00939 //      dzda[2] = rho_per_kappa*tanLambda*dPhi-rhoTanLambda*dphida[2];
00940 //      dzda[3] = 1.;
00941 //      dzda[4] = -rho*dPhi;
00942 //    }
00943 //
00944 //    //...Stereo case...
00945 //    else {
00946       const double v_dot_wireBackwardPosition = v.x()*wireBackwardPosition.x()
00947         +                                       v.y()*wireBackwardPosition.y()
00948         +                                       v.z()*wireBackwardPosition.z();
00949       const double c[3] = { w.backwardPosition().x()-v_dot_wireBackwardPosition*v.x(),
00950                             w.backwardPosition().y()-v_dot_wireBackwardPosition*v.y(),
00951                             w.backwardPosition().z()-v_dot_wireBackwardPosition*v.z() };
00952 
00953       const double x[3] = { link.positionOnTrack().x(), link.positionOnTrack().y(), link.positionOnTrack().z() };
00954       const double x_minus_c[3] = { x[0]-c[0], x[1]-c[1], x[2]-c[2] };
00955       
00956       //Vector dxdphi(3);
00957       const double dxdphi[3] = { rho*sinPhi0dPhi, -rho*cosPhi0dPhi, -rho*tanLambda };
00958       
00959       //Vector d2xdphi2(3);
00960       const double d2xdphi2[3] = { -dxdphi[1], dxdphi[0], 0. };
00961       
00962       double dxdphi_dot_v = (dxdphi[0] * v.x() +
00963                              dxdphi[1] * v.y() +
00964                              dxdphi[2] * v.z());
00965       double x_dot_v = x[0] * v.x() + x[1] * v.y() + x[2] * v.z();
00966       double inv_dfdphi = -1./(( dxdphi[0] - dxdphi_dot_v*v.x()) * dxdphi[0] 
00967                                +(dxdphi[1] - dxdphi_dot_v*v.y()) * dxdphi[1]
00968                                +(dxdphi[2] - dxdphi_dot_v*v.z()) * dxdphi[2]
00969                                +(x_minus_c[0] - x_dot_v*v.x()) * d2xdphi2[0]
00970                                +(x_minus_c[1] - x_dot_v*v.y()) * d2xdphi2[1]);
00971       /* +(x_minus_c[2] - x_dot_v*v.z()) * d2xdphi2[2];  = 0. */
00972       
00973 
00974       const double rho_per_kappa  =  rho/kappa;
00975       const double dRho_plus_rho  =  dRho+rho;
00976       const double &rhoSinPhi0dPhi =  dxdphi[0];
00977       const double rhoCosPhi0dPhi = -dxdphi[1];
00978       const double rhoTanLambda   = -dxdphi[2];
00979       
00980       //dxda_phi, dyda_phi, dzda_phi : phi is fixed
00981       //Vector dxda_phi(5);
00982       double dxda_phi[5];
00983       dxda_phi[0] = cosPhi0;
00984       dxda_phi[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi; 
00985       dxda_phi[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi);
00986       dxda_phi[3] = 0.;
00987       dxda_phi[4] = 0.;
00988       
00989       //Vector dyda_phi(5);
00990       double dyda_phi[5];
00991       dyda_phi[0] = sinPhi0;
00992       dyda_phi[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi;
00993       dyda_phi[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi);
00994       dyda_phi[3] = 0.;
00995       dyda_phi[4] = 0.;
00996       
00997       //Vector dzda_phi(5);
00998       double dzda_phi[5];
00999       dzda_phi[0] = 0.;
01000       dzda_phi[1] = 0.;
01001       dzda_phi[2] = rho_per_kappa*tanLambda*dPhi;
01002       dzda_phi[3] = 1.;
01003       dzda_phi[4] = -rho*dPhi;
01004       
01005       //Vector d2xdphida(5);
01006       double d2xdphida[5];
01007       d2xdphida[0] = 0.;
01008       d2xdphida[1] = rhoCosPhi0dPhi; 
01009       d2xdphida[2] = -rho_per_kappa*sinPhi0dPhi; 
01010       d2xdphida[3] = 0.;
01011       d2xdphida[4] = 0.;
01012       
01013       //Vector d2ydphida(5);
01014       double d2ydphida[5];
01015       d2ydphida[0] = 0.;
01016       d2ydphida[1] = rhoSinPhi0dPhi;
01017       d2ydphida[2] = rho_per_kappa*cosPhi0dPhi; 
01018       d2ydphida[3] = 0.;
01019       d2ydphida[4] = 0.;
01020       
01021       //Vector d2zdphida(5);
01022       double d2zdphida[5];
01023       d2zdphida[0] = 0.;
01024       d2zdphida[1] = 0.;
01025       d2zdphida[2] = rho_per_kappa*tanLambda;
01026       d2zdphida[3] = 0.;
01027       d2zdphida[4] = -rho;
01028       
01029       //Vector dfda(5);
01030       double dfda[5];
01031       for(int i = 0; i < 5; ++i) {
01032         double d_dot_v = (v.x() * dxda_phi[i] +
01033                           v.y() * dyda_phi[i] +
01034                           v.z() * dzda_phi[i]);
01035         dfda[i] = (- (dxda_phi[i] - d_dot_v * v.x()) * dxdphi[0]
01036                    - (dyda_phi[i] - d_dot_v * v.y()) * dxdphi[1]
01037                    - (dzda_phi[i] - d_dot_v * v.z()) * dxdphi[2]
01038                    - (x_minus_c[0] - x_dot_v * v.x()) * d2xdphida[i]
01039                    - (x_minus_c[1] - x_dot_v * v.y()) * d2ydphida[i]
01040                    - (x_minus_c[2] - x_dot_v * v.z()) * d2zdphida[i]);
01041         dphida[i] = -dfda[i]*inv_dfdphi;
01042       }
01043       
01044       dxda[0] = cosPhi0+rhoSinPhi0dPhi*dphida[0];
01045       dxda[1] = -dRho_plus_rho*sinPhi0+rhoSinPhi0dPhi*(1.+dphida[1]);
01046       dxda[2] = -rho_per_kappa*(cosPhi0-cosPhi0dPhi)+rhoSinPhi0dPhi*dphida[2];
01047       dxda[3] = rhoSinPhi0dPhi*dphida[3];
01048       dxda[4] = rhoSinPhi0dPhi*dphida[4];
01049       
01050       dyda[0] = sinPhi0-rhoCosPhi0dPhi*dphida[0];
01051       dyda[1] = dRho_plus_rho*cosPhi0-rhoCosPhi0dPhi*(1.+dphida[1]);
01052       dyda[2] = -rho_per_kappa*(sinPhi0-sinPhi0dPhi)-rhoCosPhi0dPhi*dphida[2];
01053       dyda[3] = -rhoCosPhi0dPhi*dphida[3];
01054       dyda[4] = -rhoCosPhi0dPhi*dphida[4];
01055       
01056       dzda[0] = -rhoTanLambda*dphida[0];
01057       dzda[1] = -rhoTanLambda*dphida[1];
01058       dzda[2] = rho_per_kappa*tanLambda*dPhi-rhoTanLambda*dphida[2];
01059       dzda[3] = 1.-rhoTanLambda*dphida[3];
01060       dzda[4] = -rho*dPhi-rhoTanLambda*dphida[4];
01061     //}
01062 
01063     //std::cout << dxda << std::endl;
01064     //std::cout << dyda << std::endl;
01065     //std::cout << dzda << std::endl;
01066     
01067     return 0;
01068 }
01069 #else
01070 int
01071 THelixFitter::dxda(const TMLink & link,
01072                    const Helix & h,
01073                    double dPhi,
01074                    Vector & dxda,
01075                    Vector & dyda,
01076                    Vector & dzda) const {
01077 
01078   //    std::cout << "2nd: " << m_pmgnIMF<< std::endl;
01079     //...Setup...
01080     const TMDCWire & w = * link.wire();
01081     const Vector & a = h.a();
01082     double dRho  = a[0];
01083     double phi0  = a[1];
01084     double kappa = a[2];
01085    // double rho   = Helix::ConstantAlpha / kappa;
01086    // double rho   = 333.564095 / kappa;
01087    
01088    // maybe m_pmgnIMF == NULL here FIXME
01089     if(!m_pmgnIMF) {
01090       std::cout<<" ERROR: THelixFitter::dxda 1st m_pmgnIMF == NULL  "<<std::endl;
01091       return 0;
01092     }
01093     double rho   = 333.564095/(-1000*(m_pmgnIMF->getReferField())) / kappa;    
01094 
01095     double tanLambda = a[4];
01096 
01097     double sinPhi0 = sin(phi0);
01098     double cosPhi0 = cos(phi0);
01099     // double sinPhi0 = h.sinphi0();
01100     // double cosPhi0 = h.cosphi0();
01101     double sinPhi0dPhi = sin(phi0 + dPhi);
01102     double cosPhi0dPhi = cos(phi0 + dPhi);
01103     Vector dphida(5);
01104 
01105     //...Sag correction...
01106     HepPoint3D xw = w.xyPosition();
01107     HepPoint3D wireBackwardPosition = w.backwardPosition();
01108     HepVector3D v = w.direction();
01109     if (_sag)
01110         w.wirePosition(link.positionOnTrack().z(),
01111                        xw,
01112                        wireBackwardPosition,
01113                        v);
01114  //yzhang 2012-08-30
01115     //...Axial case...
01116 //    if (w.axial()) {
01117 //      HepPoint3D d = h.center() - xw;
01118 //      double dmag2 = d.mag2();
01119 //
01120 //      dphida[0] = (sinPhi0 * d.x() - cosPhi0 * d.y()) / dmag2;
01121 //      dphida[1] = (dRho + rho)    * (cosPhi0 * d.x() + sinPhi0 * d.y())
01122 //          / dmag2 - 1.;
01123 //      dphida[2] = (- rho / kappa) * (sinPhi0 * d.x() - cosPhi0 * d.y())
01124 //          / dmag2;
01125 //      dphida[3] = 0.;
01126 //      dphida[4] = 0.;
01127 //    }
01128 //
01129 //    //...Stereo case...
01130 //    else {
01131       //temp    HepPoint3D onTrack = h.x(dPhi);
01132         //temp
01133         HepPoint3D onTrack = link.positionOnTrack();
01134         //        std::cout << "ontrack =" << onTrack  << std::endl;
01135         //        std::cout << "ontrackp=" << onTrackp << std::endl;
01136         //temp
01137 
01138         Vector c(3);
01139         c = HepPoint3D(w.backwardPosition() - (v * wireBackwardPosition) * v);
01140 
01141         Vector x(3);
01142         x = onTrack;
01143 
01144         Vector dxdphi(3);
01145         dxdphi[0] =   rho * sinPhi0dPhi;
01146         dxdphi[1] = - rho * cosPhi0dPhi;
01147         dxdphi[2] = - rho * tanLambda;
01148 
01149         Vector d2xdphi2(3);
01150         d2xdphi2[0] = rho * cosPhi0dPhi;
01151         d2xdphi2[1] = rho * sinPhi0dPhi;
01152         d2xdphi2[2] = 0.;
01153 
01154         double dxdphi_dot_v = (dxdphi[0] * v.x() +
01155                                dxdphi[1] * v.y() +
01156                                dxdphi[2] * v.z());
01157         double x_dot_v = x[0] * v.x() + x[1] * v.y() + x[2] * v.z();
01158         double dfdphi = - (dxdphi[0] - dxdphi_dot_v*v.x()) * dxdphi[0] 
01159                         - (dxdphi[1] - dxdphi_dot_v*v.y()) * dxdphi[1]
01160                         - (dxdphi[2] - dxdphi_dot_v*v.z()) * dxdphi[2]
01161                         - (x[0] - c[0] - x_dot_v*v.x()) * d2xdphi2[0]
01162                         - (x[1] - c[1] - x_dot_v*v.y()) * d2xdphi2[1];
01163                     /*  - (x[2] - c[2] - x_dot_v*v.z()) * d2xdphi2[2];  = 0. */
01164 
01165 
01166         //dxda_phi, dyda_phi, dzda_phi : phi is fixed
01167         Vector dxda_phi(5);
01168         dxda_phi[0] = cosPhi0;
01169         dxda_phi[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi; 
01170         dxda_phi[2] = - (rho / kappa) * (cosPhi0 - cosPhi0dPhi);
01171         dxda_phi[3] = 0.;
01172         dxda_phi[4] = 0.;
01173 
01174         Vector dyda_phi(5);
01175         dyda_phi[0] = sinPhi0;
01176         dyda_phi[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi;
01177         dyda_phi[2] = - (rho / kappa) * (sinPhi0 - sinPhi0dPhi);
01178         dyda_phi[3] = 0.;
01179         dyda_phi[4] = 0.;
01180         
01181         Vector dzda_phi(5);
01182         dzda_phi[0] = 0.;
01183         dzda_phi[1] = 0.;
01184         dzda_phi[2] = (rho / kappa) * tanLambda * dPhi;
01185         dzda_phi[3] = 1.;
01186         dzda_phi[4] = - rho * dPhi;
01187 
01188         Vector d2xdphida(5);
01189         d2xdphida[0] = 0.;
01190         d2xdphida[1] = rho * cosPhi0dPhi; 
01191         d2xdphida[2] = - (rho / kappa) * sinPhi0dPhi; 
01192         d2xdphida[3] = 0.;
01193         d2xdphida[4] = 0.;
01194 
01195         Vector d2ydphida(5);
01196         d2ydphida[0] = 0.;
01197         d2ydphida[1] = rho * sinPhi0dPhi;
01198         d2ydphida[2] = (rho / kappa) * cosPhi0dPhi; 
01199         d2ydphida[3] = 0.;
01200         d2ydphida[4] = 0.;
01201 
01202         Vector d2zdphida(5);
01203         d2zdphida[0] = 0.;
01204         d2zdphida[1] = 0.;
01205         d2zdphida[2] = (rho / kappa) * tanLambda;
01206         d2zdphida[3] = 0.;
01207         d2zdphida[4] = - rho;
01208  
01209         Vector dfda(5);
01210         for(int i = 0; i < 5; i++) {
01211             double d_dot_v = (v.x() * dxda_phi[i] +
01212                               v.y() * dyda_phi[i] +
01213                               v.z() * dzda_phi[i]);
01214             dfda[i] = (- (dxda_phi[i] - d_dot_v * v.x()) * dxdphi[0]
01215                        - (dyda_phi[i] - d_dot_v * v.y()) * dxdphi[1]
01216                        - (dzda_phi[i] - d_dot_v * v.z()) * dxdphi[2]
01217                        - (x[0] - c[0] - x_dot_v * v.x()) * d2xdphida[i]
01218                        - (x[1] - c[1] - x_dot_v * v.y()) * d2ydphida[i]
01219                        - (x[2] - c[2] - x_dot_v * v.z()) * d2zdphida[i]);
01220             dphida[i] = - dfda[i] / dfdphi;
01221         }
01222     //}
01223 
01224     dxda[0] = cosPhi0 + rho * sinPhi0dPhi * dphida[0];
01225     dxda[1] = - (dRho + rho) * sinPhi0 + rho * sinPhi0dPhi * (1. + dphida[1]);
01226     dxda[2] = - rho / kappa * (cosPhi0 - cosPhi0dPhi)
01227               + rho * sinPhi0dPhi * dphida[2];
01228     dxda[3] = rho * sinPhi0dPhi * dphida[3];
01229     dxda[4] = rho * sinPhi0dPhi * dphida[4];
01230 
01231     dyda[0] = sinPhi0 - rho * cosPhi0dPhi * dphida[0];
01232     dyda[1] = (dRho + rho) * cosPhi0 - rho * cosPhi0dPhi * (1. + dphida[1]);
01233     dyda[2] = - rho / kappa * (sinPhi0 - sinPhi0dPhi)
01234               - rho * cosPhi0dPhi * dphida[2];
01235     dyda[3] = - rho * cosPhi0dPhi * dphida[3];
01236     dyda[4] = - rho * cosPhi0dPhi * dphida[4];
01237 
01238     dzda[0] = - rho * tanLambda * dphida[0];
01239     dzda[1] = - rho * tanLambda * dphida[1];
01240     dzda[2] = rho / kappa * tanLambda * dPhi - rho * tanLambda * dphida[2];
01241     dzda[3] = 1. - rho * tanLambda * dphida[3];
01242     dzda[4] = - rho * dPhi - rho * tanLambda * dphida[4];
01243 
01244     //std::cout << dxda << std::endl;
01245     //std::cout << dyda << std::endl;
01246     //std::cout << dzda << std::endl;
01247     
01248     return 0;
01249 }
01250 #endif
01251 
01252 void
01253 THelixFitter::drift(const TTrack & t,
01254                     TMLink & l,
01255                     float t0Offset,
01256                     double & distance,
01257                     double & err) const {
01258   //be cautious here
01259     const TMDCWireHit & h = * l.hit();
01260     const HepPoint3D & onTrack = l.positionOnTrack();
01261     const HepPoint3D & onWire = l.positionOnWire();
01262     unsigned leftRight = WireHitRight;
01263     //yzhang delete TEMP
01264     if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
01265     int charge = (t.helix().a()[2]>0) ? 1: -1;//track charge//yzhang 2012-05-03 TEMP
01266     //m_pmgnIMF NULL set mutable 2012-05-04 
01267     if(NULL == m_pmgnIMF) {
01268       Gaudi::svcLocator()->service("MagneticFieldSvc",m_pmgnIMF);
01269       if(NULL == m_pmgnIMF) {
01270         std::cout<< __FILE__<<" "<<__LINE__<<" Unable to open Magnetic field service"<<std::endl;
01271       }
01272     }
01273     const double Bz = -1000*m_pmgnIMF->getReferField();
01274 #ifdef TRKRECO_DEBUG
01275     std::cout << " orignal ambig "<<leftRight<<" charge "<< charge <<" Bz "<<Bz<<std::endl;
01276 #endif
01277     //if(charge * Bz <0){
01278     //  leftRight = (onWire.cross(onTrack).z() < 0.)
01279     //    ? WireHitLeft : WireHitRight;
01280     //}else{
01281     //  leftRight = (onWire.cross(onTrack).z() < 0.)
01282     //    ? WireHitRight : WireHitLeft;
01283     //}
01284 #ifdef TRKRECO_DEBUG
01285     std::cout << " new ambig "<<leftRight<<std::endl;
01286 #endif
01287     //zhangy
01288 
01289     //...No correction...
01290     if ((t0Offset == 0.) && (! _propagation) && (! _tof)) {
01291         distance = h.drift(leftRight);
01292         err = h.dDrift(leftRight);
01293         return;
01294     }
01295 
01296 //    float x[3]={ onWire.x(), onWire.y(), onWire.z()};
01297 //    float Tcenter = 0;
01298 
01299     //...TOF correction...
01300     float tof = 0.;
01301     if (_tof) {
01302         int imass = 3;
01303         float tl = t.helix().a()[4];
01304         float f = sqrt(1. + tl * tl);
01305         float s = fabs(t.helix().curv()) * fabs(l.dPhi()) * f;
01306         float p = f / fabs(t.helix().a()[2]);
01307 //zsl   calcdc_tof2_(& imass, & p, & s, & tof);
01308         float Mass[5] = {0.000511,0.105,0.140,0.493677,0.98327};
01309         tof = s/29.98*sqrt(1.0+(Mass[imass-1]/p)*(Mass[imass-1]/p));
01310 
01311 /*      float x[3]={ onWire.x(), onWire.y(), onWire.z()};
01312       float Rad = 81; // cm
01313       float dRho = t.helix().a()[0];
01314       float Phi0 = t.helix().a()[1];
01315       float a2 = (dRho*cos(Phi0))*(dRho*cos(Phi0));
01316       float b2 = (dRho*sin(Phi0))*(dRho*sin(Phi0));
01317       float Lproj = sqrt(Rad*Rad - a2 - b2);
01318       Tcenter = Lproj/29.98; //approxiate... cm/ns
01319       tof = s/29.98;  //cosmic
01320       if (x[1]>0) tof = -tof;
01321       */
01322     }
01323 
01324     //...T0 and propagation corrections...
01325     int wire = h.wire()->localId();
01326     int layerId = h.wire()->layerId();
01327     int side = leftRight;
01328 //    if (side == 0) side = -1;
01329 //    HepVector3D tp = t.helix().momentum(l.dPhi());
01330 //    float p[3] = {tp.x(), tp.y(), tp.z()};
01331 //    float x[3] = {onWire.x(), onWire.y(), onWire.z()};
01332 //    float time = h.reccdc()->tdc + t0Offset - tof;
01333 //    float dist;
01334 //    float edist;
01335     double dist;
01336     double edist;
01337     int prop = _propagation;
01338 
01339     const double x0 = t.helix().pivot().x();
01340     const double y0 = t.helix().pivot().y();
01341     Hep3Vector pivot_helix(x0,y0,0);
01342     const double dr    =  t.helix().a()[0];
01343     const double phi0  =  t.helix().a()[1];
01344     const double kappa =  t.helix().a()[2];
01345     const double dz    =  t.helix().a()[3];
01346     const double tanl  =  t.helix().a()[4];
01347 
01348     //yzhang 2012-05-03 delete
01349     //const double alpha = 333.564095;
01350     //yzhang add
01351     const double alpha= 333.564095/(-1000*(m_pmgnIMF->getReferField()));
01352     //zhangy
01353 
01354     const double cox = x0 + dr*cos(phi0) + alpha*cos(phi0)/kappa;
01355     const double coy = y0 + dr*sin(phi0) + alpha*sin(phi0)/kappa;
01356 
01357 
01358     unsigned nHits = t.links().length();
01359     unsigned nStereos = 0;
01360     unsigned firstLyr = 44;
01361     unsigned lastLyr = 0;
01362 
01363 
01364     HepPoint3D ontrack = l.positionOnTrack();
01365     HepPoint3D onwire = l.positionOnWire();
01366     HepPoint3D dir(ontrack.y()-coy,cox-ontrack.x(),0);           
01367     double pos_phi=onwire.phi();
01368     double dir_phi=dir.phi();
01369     while(pos_phi>pi){pos_phi-=pi;}
01370     while(pos_phi<0){pos_phi+=pi;}
01371     while(dir_phi>pi){dir_phi-=pi;}
01372     while(dir_phi<0){dir_phi+=pi;}
01373     double entrangle=dir_phi-pos_phi;
01374     while(entrangle>pi/2)entrangle-=pi;
01375     while(entrangle<(-1)*pi/2)entrangle+=pi; 
01377     double zhit = onWire.z();
01378     
01379 #ifdef TRKRECO_DEBUG 
01380     std::cout<<" onWire: "<<onWire<<std::endl;
01381     std::cout<<" zhit: "<<zhit<<std::endl;
01382 #endif
01383 
01384     const double vinner = 220.0e8; // unit is cm/s
01385     const double vouter = 240.0e8; 
01386     double vprop = 300.0e9;
01387 //    double tprop = 0.;
01388     
01389     
01390     if(layerId<8) {
01391       vprop = vinner;
01392     }
01393     else {
01394       vprop = vouter;
01395     }
01396   
01397     
01398     double EsT0 = -1.;
01399     IMessageSvc *msgSvc;
01400     Gaudi::svcLocator()->service("MessageSvc", msgSvc);
01401     MsgStream log(msgSvc, "TCosmicFitter");
01402 
01403     IDataProviderSvc* eventSvc = NULL;
01404     Gaudi::svcLocator()->service("EventDataSvc", eventSvc);
01405 
01406     SmartDataPtr<RecEsTimeCol> aevtimeCol (eventSvc,"/Event/Recon/RecEsTimeCol");
01407     if (aevtimeCol) {
01408       RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
01409       EsT0 = (*iter_evt)->getTest();
01410     }else{
01411       log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
01412     }
01413    
01414     double rawTime = 0.;
01415     rawTime = h.reccdc()->tdc;
01416     double rawadc = 0.;
01417     rawadc =h.reccdc()->adc;
01418     //    double timewalk = CalibFunSvc_->getTimeWalk(layerid, 0.0);
01419     //    double timeDrift = rawTime - tof - tprop - EsTo - toWalk; 
01420     
01421 
01422     //----cal drift----//
01423     IMdcCalibFunSvc* l_mdcCalFunSvc;
01424     Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
01425 
01426     double tprop = l_mdcCalFunSvc->getTprop(layerId,zhit*10.);
01427 
01428     double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
01429 
01430     double T0 = l_mdcCalFunSvc->getT0(layerId,wire);
01431     double drifttime = rawTime - tof - tprop - timeWalk -T0;
01432     l.setDriftTime(drifttime);
01433 
01434 #ifdef TRKRECO_DEBUG
01435     std::cout<<"timewalk is : "<< timeWalk << std::endl ;
01436     std::cout<<"T0 is : "<< T0 << std::endl ;
01437     std::cout<<"EsT0 is : "<< EsT0 << std::endl ;
01438     std::cout<<"tprop is : "<< tprop << std::endl ;
01439     std::cout<<"tof is : "<< tof << std::endl ;
01440     std::cout<<"rawTime is : "<< rawTime << std::endl ;
01441     std::cout<<"driftTime is : "<< drifttime << std::endl ;
01442 #endif
01443 //    dist = l_mdcCalFunSvc->rawTimeNoTOFToDist(h.reccdc()->tdc - tof - tprop-timeWalk, layerId, wire, side, entrangle); //default entranceangle is 0
01444     dist = l_mdcCalFunSvc->driftTimeToDist(drifttime,layerId, wire, side, entrangle);//by liucy 2010/05/12
01445     edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, entrangle,tanl,zhit*10,rawadc);
01446     dist = dist/10.0; //mm->cm
01447     edist = edist/10.0;
01448 
01449 //zsl    calcdc_driftdist_(& prop,
01450 //                    & wire,
01451 //                    & side,
01452 //                    p,
01453 //                    x,
01454 //                    & time,
01455 //                    & dist,
01456 //                    & edist);
01457 
01458 //      dist = (h.reccdc()->tdc-tof) * 40./10000;
01459 
01460 //    distance = (double) dist;
01461 //    err = (double) edist;
01462     distance = dist;
01463     err = edist;
01464 
01465     return;
01466 }
01467 
01468 #ifdef OPTJT
01469 //=====================================================================
01470 int
01471 THelixFitter::main(TTrackBase & b, float & tev, float & tev_err,
01472                    double *pre_chi2, double *fitted_chi2) const {
01473 //=====================================================================
01474     //...Initialize
01475     _pre_chi2 = _fitted_chi2 = 0.;
01476     if(pre_chi2)*pre_chi2 = _pre_chi2;
01477     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
01478 
01479     //...Type check...
01480     if (b.objectType() != Track) return TFitUnavailable;
01481     TTrack & t = (TTrack &) b;
01482 
01483     //...Already fitted ?...
01484     if (t.fitted()) return TFitAlreadyFitted;
01485 
01486     //...Count # of hits...
01487     AList<TMLink> cores = t.cores();
01488     if (_fit2D) cores = AxialHits(cores);
01489     unsigned nCores = cores.length();
01490     unsigned nStereoCores = NStereoHits(cores);
01491 
01492     //...2D or 3D...
01493     bool fitBy2D = _fit2D;
01494     if (! fitBy2D) fitBy2D = (! nStereoCores);
01495 
01496     //...Check # of hits...
01497     if (! fitBy2D) {
01498         if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
01499             return TFitErrorFewHits;
01500     }
01501     else {
01502         if (nCores < 3) return TFitErrorFewHits;
01503     }
01504 
01505     //...Setup...
01506     Vector a(6), da(6);
01507     Vector a_5dim(5);
01508     for (unsigned j = 0; j < 5; j++) a[j] = t.helix().a()[j];
01509     a[5] = tev;
01510     Vector dxda(5);
01511     Vector dyda(5);
01512     Vector dzda(5);
01513     Vector dDda(6);
01514     Vector dDda_5dim(5);
01515     Vector dchi2da(6);
01516     SymMatrix d2chi2d2a(6, 0);
01517     static const SymMatrix zero6(6, 0);
01518     double chi2;
01519     double chi2Old = DBL_MAX;
01520 //    const double convergence = Convergence;
01521     const double convergence = 1.0e-4;   //Liuqg
01522     bool allAxial = true;
01523     Matrix e(3, 3);
01524     Vector f(3);
01525     int err = 0;
01526     double factor = 1.0;//jtanaka0715
01527 
01528     //...Fitting loop...
01529     unsigned nTrial = 0;
01530     while (nTrial < NTrailMax) {
01531 
01532         //...Set up...
01533         chi2 = 0.;
01534         for (unsigned j = 0; j < 6; j++) dchi2da[j] = 0.;
01535         d2chi2d2a = zero6;
01536 
01537         //...Loop with hits...
01538         unsigned i = 0;
01539         while (TMLink * l = cores[i++]) {
01540             const TMDCWireHit & h = * l->hit();
01541 
01542             //...Cal. closest points...
01543             t.approach(* l, _sag);
01544             double dPhi = l->dPhi();
01545             const HepPoint3D & onTrack = l->positionOnTrack();
01546             const HepPoint3D & onWire = l->positionOnWire();
01547             unsigned leftRight = (onWire.cross(onTrack).z() < 0.) ? WireHitLeft : WireHitRight;
01548 
01549             //...Obtain drift distance and its error...
01550             double distance;
01551             double eDistance;
01552             double dddt;
01553             drift(t, * l, tev, distance, eDistance, dddt);
01554 //          l->drift(distance,0);
01555 //          l->drift(distance,1);
01556 //          l->dDrift(distance,0);
01557 //          l->dDrift(distance,1);
01558            
01559 
01560             double inv_eDistance2 = 1./(eDistance * eDistance);
01561 
01562 
01563             //...Residual...
01564             HepVector3D v = onTrack - onWire;
01565             double vmag = v.mag();
01566             double dDistance = vmag - distance;
01567 
01568             //...dxda...
01569             this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
01570 
01571             //...Chi2 related...
01572             // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
01573             double vw[3] = { h.wire()->direction().x(),
01574                              h.wire()->direction().y(),
01575                              h.wire()->direction().z() };
01576             double vwxy = vw[0]*vw[1];
01577             double vwyz = vw[1]*vw[2];
01578             double vwzx = vw[2]*vw[0];
01579             dDda_5dim = (vmag > 0.)
01580                 ? ((v.x() * (1. - vw[0] * vw[0]) -
01581                     v.y() * vwxy - v.z() * vwzx)
01582                    * dxda + 
01583                    (v.y() * (1. - vw[1] * vw[1]) -
01584                     v.z() * vwyz - v.x() * vwxy)
01585                    * dyda + 
01586                    (v.z() * (1. - vw[2] * vw[2]) -
01587                     v.x() * vwzx - v.y() * vwyz)
01588                    * dzda) / vmag
01589                 : Vector(5,0);
01590             if (vmag <= 0.0) {
01591                 std::cout << "    in fit " << onTrack << ", " << onWire;
01592                 h.dump();
01593             }
01594             //      for (unsigned j = 0; j < 5; j++) dDda[j] = dDda_5dim[j];
01595             dDda[0] = dDda_5dim[0];
01596             dDda[1] = dDda_5dim[1];
01597             dDda[2] = dDda_5dim[2];
01598             dDda[3] = dDda_5dim[3];
01599             dDda[4] = dDda_5dim[4];
01600             dDda[5] = -dddt;
01601 
01602             dchi2da += (dDistance * inv_eDistance2) * dDda;
01603             d2chi2d2a += vT_times_v(dDda) * inv_eDistance2;
01604             double pChi2 = dDistance * dDistance * inv_eDistance2;
01605             chi2 += pChi2;
01606 
01607             //...Store results...
01608             l->update(onTrack, onWire, leftRight, pChi2);
01609         }
01610 
01611         //...Save chi2 information...
01612         if(nTrial == 0){
01613           _pre_chi2 = chi2;
01614           _fitted_chi2 = chi2;
01615         }else _fitted_chi2 = chi2;
01616 
01617         //...Check condition...
01618         double change = chi2Old - chi2;
01619         if (fabs(change) < convergence) break;
01620         //temp
01621                 factor = 1.0;
01622         //temp
01623         if (change < 0.) {
01624 //          a += factor * da;
01625 //          t._helix->a(a);
01626 //          break;
01627             factor = 0.5;
01628         }
01629 
01630         chi2Old = chi2;
01631 
01632         //...Cal. helix parameters for next loop...
01633         if (fitBy2D) {
01634             f = dchi2da.sub(1, 4);
01635             e = d2chi2d2a.sub(1, 4);
01636             f = solve(e, f);
01637             da[0] = f[0];
01638             da[1] = f[1];
01639             da[2] = f[2];
01640             da[3] = f[3];
01641             da[4] = 0.;
01642             da[5] = 0.;
01643         }
01644         else {
01645             da = solve(d2chi2d2a, dchi2da);
01646         }
01647 
01648         a -= factor * da;
01649 
01650         //      for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
01651         a_5dim[0] = a[0];
01652         a_5dim[1] = a[1];
01653         a_5dim[2] = a[2];
01654         a_5dim[3] = a[3];
01655         a_5dim[4] = a[4];
01656         t._helix->a(a_5dim);
01657         tev = a[5];
01658         //temp
01659         //      if(nTrial == 0) std::cout << "initial chi2=" <<chi2 << std::endl;
01660         //temp
01661         ++nTrial;
01662 
01663 #ifdef TRKRECO_DEBUG
01664         std::cout << "fit " << nTrial-1<< " : " << chi2 << " : " << change << std::endl;
01665 #endif
01666     }
01667 
01668     //...Cal. error matrix...
01669     SymMatrix Ea(6, 0);
01670     unsigned dim;
01671     if (fitBy2D) {
01672         dim = 4;
01673         SymMatrix Eb(4, 0), Ec(4, 0);
01674         Eb = d2chi2d2a.sub(1, 4);
01675         Ec = Eb.inverse(err);
01676         Ea[0][0] = Ec[0][0];
01677         Ea[0][1] = Ec[0][1];
01678         Ea[0][2] = Ec[0][2];
01679         Ea[0][3] = Ec[0][3];
01680         Ea[1][1] = Ec[1][1];
01681         Ea[1][2] = Ec[1][2];
01682         Ea[1][3] = Ec[1][3];
01683         Ea[2][2] = Ec[2][2];
01684         Ea[2][3] = Ec[2][3];
01685         Ea[3][3] = Ec[3][3];
01686     }
01687     else {
01688         dim = 6;
01689         Ea = d2chi2d2a.inverse(err);
01690         //        std::cout << "err flg=" << err << std::endl;
01691     }
01692 
01693 
01694     //...Store information...
01695     if (! err) {
01696         for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
01697         SymMatrix Ea_5dim(5, 0);
01698         Ea_5dim = Ea.sub(1, 5);
01699         t._helix->a(a_5dim);
01700         t._helix->Ea(Ea_5dim);
01701         tev = a[5];
01702         tev_err = sqrt(Ea[5][5]);
01703         //temp
01704         //      std::cout << "nTrial=" << nTrial << std::endl;
01705         //      std::cout << "chi2="   << chi2   << std::endl;
01706         //      std::cout << "pt:" << fabs(1./a_5dim[2])<< std::endl;
01707         //      std::cout << "tev,tev_err="<<tev<<" "<<tev_err<<std::endl;
01708         //temp
01709 
01710         t._fitted = true;
01711     }
01712     else {
01713         err = TFitFailed;
01714     }
01715 
01716     t._ndf = nCores - dim;
01717     t._chi2 = chi2;
01718 
01719     if(pre_chi2)*pre_chi2 = _pre_chi2;
01720     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
01721 
01722     return err;
01723 }
01724 #else
01725 //=====================================================================
01726 int
01727 THelixFitter::main(TTrackBase & b, float & tev, float & tev_err,
01728                    double *pre_chi2, double *fitted_chi2) const {
01729 //=====================================================================
01730     //...Initialize
01731     _pre_chi2 = _fitted_chi2 = 0.;
01732     if(pre_chi2)*pre_chi2 = _pre_chi2;
01733     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
01734 
01735     //...Type check...
01736     if (b.objectType() != Track) return TFitUnavailable;
01737     TTrack & t = (TTrack &) b;
01738 
01739     //...Already fitted ?...
01740     if (t.fitted()) return TFitAlreadyFitted;
01741 
01742     //...Count # of hits...
01743     AList<TMLink> cores = t.cores();
01744     if (_fit2D) cores = AxialHits(cores);
01745     unsigned nCores = cores.length();
01746     unsigned nStereoCores = NStereoHits(cores);
01747 
01748     //...2D or 3D...
01749     bool fitBy2D = _fit2D;
01750     if (! fitBy2D) fitBy2D = (! nStereoCores);
01751 
01752     //...Check # of hits...
01753     if (! fitBy2D) {
01754         if ((nStereoCores < 2) || (nCores - nStereoCores < 3))
01755             return TFitErrorFewHits;
01756     }
01757     else {
01758         if (nCores < 3) return TFitErrorFewHits;
01759     }
01760 
01761     //...Setup...
01762     Vector a(6), da(6);
01763     Vector a_5dim(5);
01764     for (unsigned j = 0; j < 5; j++) a[j] = t.helix().a()[j];
01765     a[5] = tev;
01766     Vector dxda(5);
01767     Vector dyda(5);
01768     Vector dzda(5);
01769     Vector dDda(6);
01770     Vector dDda_5dim(5);
01771     Vector dchi2da(6);
01772     SymMatrix d2chi2d2a(6, 0);
01773     static const SymMatrix zero6(6, 0);
01774     double chi2;
01775     double chi2Old = DBL_MAX;
01776     //temp    const double convergence = Convergence;
01777     const double convergence = 1.0e-4;
01778     bool allAxial = true;
01779     Matrix e(3, 3);
01780     Vector f(3);
01781     int err = 0;
01782     double factor = 1.0;//jtanaka0715
01783 
01784     //...Fitting loop...
01785     unsigned nTrial = 0;
01786     while (nTrial < NTrailMax) {
01787 
01788         //...Set up...
01789         chi2 = 0.;
01790         for (unsigned j = 0; j < 6; j++) dchi2da[j] = 0.;
01791         d2chi2d2a = zero6;
01792 
01793         //...Loop with hits...
01794         unsigned i = 0;
01795         while (TMLink * l = cores[i++]) {
01796             const TMDCWireHit & h = * l->hit();
01797 
01798             //...Cal. closest points...
01799             t.approach(* l, _sag);
01800             double dPhi = l->dPhi();
01801             const HepPoint3D & onTrack = l->positionOnTrack();
01802             const HepPoint3D & onWire = l->positionOnWire();
01803             unsigned leftRight = WireHitRight;
01804             if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
01805 
01806             //...Obtain drift distance and its error...
01807             double distance;
01808             double eDistance;
01809             double dddt;
01810             drift(t, * l, tev, distance, eDistance, dddt);
01811 
01812 
01813             double eDistance2 = eDistance * eDistance;
01814 
01815             //...Residual...
01816             HepVector3D v = onTrack - onWire;
01817             double vmag = v.mag();
01818             double dDistance = vmag - distance;
01819 
01820             //...dxda...
01821             this->dxda(* l, t.helix(), dPhi, dxda, dyda, dzda);
01822 
01823             //...Chi2 related...
01824             // dDda = (v.x() * dxda + v.y() * dyda + v.z() * dzda) / vmag;
01825             HepVector3D vw = h.wire()->direction();
01826             dDda_5dim = (vmag > 0.)
01827                 ? ((v.x() * (1. - vw.x() * vw.x()) -
01828                     v.y() * vw.x() * vw.y() - v.z() * vw.x() * vw.z())
01829                    * dxda + 
01830                    (v.y() * (1. - vw.y() * vw.y()) -
01831                     v.z() * vw.y() * vw.z() - v.x() * vw.y() * vw.x())
01832                    * dyda + 
01833                    (v.z() * (1. - vw.z() * vw.z()) -
01834                     v.x() * vw.z() * vw.x() - v.y() * vw.z() * vw.y())
01835                    * dzda) / vmag
01836                 : Vector(5,0);
01837             if (vmag <= 0.0) {
01838                 std::cout << "    in fit " << onTrack << ", " << onWire;
01839                 h.dump();
01840             }
01841             //      for (unsigned j = 0; j < 5; j++) dDda[j] = dDda_5dim[j];
01842             dDda[0] = dDda_5dim[0];
01843             dDda[1] = dDda_5dim[1];
01844             dDda[2] = dDda_5dim[2];
01845             dDda[3] = dDda_5dim[3];
01846             dDda[4] = dDda_5dim[4];
01847             dDda[5] = -dddt;
01848 
01849             dchi2da += (dDistance / eDistance2) * dDda;
01850             d2chi2d2a += vT_times_v(dDda) / eDistance2;
01851             double pChi2 = dDistance * dDistance / eDistance2;
01852             chi2 += pChi2;
01853 
01854             //...Store results...
01855             l->update(onTrack, onWire, leftRight, pChi2);
01856         }
01857 
01858         //...Save chi2 information...
01859         if(nTrial == 0){
01860           _pre_chi2 = chi2;
01861           _fitted_chi2 = chi2;
01862         }else _fitted_chi2 = chi2;
01863 
01864         //...Check condition...
01865         double change = chi2Old - chi2;
01866         if (fabs(change) < convergence) break;
01867         //temp
01868                 factor = 1.0;
01869         //temp
01870         if (change < 0.) {
01871 //          a += factor * da;
01872 //          t._helix->a(a);
01873 //          break;
01874             factor = 0.5;
01875         }
01876         chi2Old = chi2;
01877 
01878         //...Cal. helix parameters for next loop...
01879         if (fitBy2D) {
01880             f = dchi2da.sub(1, 4);
01881             e = d2chi2d2a.sub(1, 4);
01882             f = solve(e, f);
01883             da[0] = f[0];
01884             da[1] = f[1];
01885             da[2] = f[2];
01886             da[3] = f[3];
01887             da[4] = 0.;
01888             da[5] = 0.;
01889         }
01890         else {
01891             da = solve(d2chi2d2a, dchi2da);
01892         }
01893 
01894         a -= factor * da;
01895 
01896         //      for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
01897         a_5dim[0] = a[0];
01898         a_5dim[1] = a[1];
01899         a_5dim[2] = a[2];
01900         a_5dim[3] = a[3];
01901         a_5dim[4] = a[4];
01902         t._helix->a(a_5dim);
01903         tev = a[5];
01904         //temp
01905         //      if(nTrial == 0) std::cout << "initial chi2=" <<chi2 << std::endl;
01906         //temp
01907         ++nTrial;
01908 
01909 #ifdef TRKRECO_DEBUG
01910         std::cout << "fit " << nTrial-1<< " : " << chi2 << " : " << change << std::endl;
01911 #endif
01912     }
01913 
01914     //...Cal. error matrix...
01915     SymMatrix Ea(6, 0);
01916     unsigned dim;
01917     if (fitBy2D) {
01918         dim = 4;
01919         SymMatrix Eb(4, 0), Ec(4, 0);
01920         Eb = d2chi2d2a.sub(1, 4);
01921         Ec = Eb.inverse(err);
01922         Ea[0][0] = Ec[0][0];
01923         Ea[0][1] = Ec[0][1];
01924         Ea[0][2] = Ec[0][2];
01925         Ea[0][3] = Ec[0][3];
01926         Ea[1][1] = Ec[1][1];
01927         Ea[1][2] = Ec[1][2];
01928         Ea[1][3] = Ec[1][3];
01929         Ea[2][2] = Ec[2][2];
01930         Ea[2][3] = Ec[2][3];
01931         Ea[3][3] = Ec[3][3];
01932     }
01933     else {
01934         dim = 6;
01935         Ea = d2chi2d2a.inverse(err);
01936         //        std::cout << "err flg=" << err << std::endl;
01937     }
01938 
01939 
01940     //...Store information...
01941     if (! err) {
01942         for (unsigned j = 0; j < 5; j++) a_5dim[j] = a[j];
01943         SymMatrix Ea_5dim(5, 0);
01944         Ea_5dim = Ea.sub(1, 5);
01945         t._helix->a(a_5dim);
01946         t._helix->Ea(Ea_5dim);
01947         tev = a[5];
01948         tev_err = sqrt(Ea[5][5]);
01949         //temp
01950         //      std::cout << "nTrial=" << nTrial << std::endl;
01951         //      std::cout << "chi2="   << chi2   << std::endl;
01952         //      std::cout << "tev,tev_err="<<tev<<" "<<tev_err<<std::endl;
01953         //temp
01954 
01955         t._fitted = true;
01956     }
01957     else {
01958         err = TFitFailed;
01959     }
01960 
01961     t._ndf = nCores - dim;
01962     t._chi2 = chi2;
01963 
01964     if(pre_chi2)*pre_chi2 = _pre_chi2;
01965     if(fitted_chi2)*fitted_chi2 = _fitted_chi2;
01966 
01967     return err;
01968 }
01969 #endif
01970 
01971 //=========================================
01972 void
01973 THelixFitter::drift(const TTrack & t,
01974                     TMLink & l,                    
01975                     float tev,
01976                     double & distance,
01977                     double & err,
01978                     double & dddt) const {
01979 //=========================================
01980 // be cautious here
01981     const TMDCWireHit & h = * l.hit();
01982     const HepPoint3D & onTrack = l.positionOnTrack();
01983     const HepPoint3D & onWire = l.positionOnWire();
01984     unsigned leftRight = WireHitRight;
01985     if (onWire.cross(onTrack).z() < 0.) leftRight = WireHitLeft;
01986 
01987     //...No correction...
01988     if ((tev == 0.) && (! _propagation) && (! _tof)) {
01989         distance = h.drift(leftRight);
01990         err = h.dDrift(leftRight);
01991         return;
01992     }
01993 
01994     //...TOF correction...
01995     float tof = 0.;
01996     if (_tof) {
01997         int imass = 3;
01998         float tl = t.helix().a()[4];
01999         float f = sqrt(1. + tl * tl);
02000         float s = fabs(t.helix().curv()) * fabs(l.dPhi()) * f;
02001         float p = f / fabs(t.helix().a()[2]);
02002 //zsl   calcdc_tof2_(& imass, & p, & s, & tof);
02003         float Mass[5] = {0.000511,0.105,0.140,0.493677,0.98327};
02004         tof = s/29.98*sqrt(1.0+(Mass[imass-1]/p)*(Mass[imass-1]/p));
02005         //cout<<"tof:"<<tof<<endl;
02006     }
02007 
02008     //...T0 and propagation corrections...
02009     int wire = h.wire()->localId();
02010     int layerId = h.wire()->layerId();
02011 //    int wire = h.wire()->id();
02012     int side = leftRight;
02013 
02014 
02016     const double zhit = onWire.z();
02017     const double vinner = 220.0e8; // unit is cm/s
02018     const double vouter = 240.0e8;
02019     
02020     double tprop = 0.;
02021     const double vprop = (layerId<8) ? vinner : vouter;
02022     if (0 == layerId%2){
02024       tprop = fabs((zhit - h.wire()->backwardPosition()[2]))/vprop; 
02025     }else{ 
02027       tprop = fabs(( zhit - h.wire()->forwardPosition()[2]))/vprop;
02028     }
02029 
02030 
02031 //    if (side==0) side = -1;
02032 //    HepVector3D tp = t.helix().momentum(l.dPhi());
02033 //    float p[3] = {tp.x(),tp.y(),tp.z()};
02034 //    float x[3] = {onWire.x(), onWire.y(), onWire.z()};
02035     float time = h.reccdc()->tdc + tev - tof - 1.0e9*tprop;
02036     //    float dist_p;
02037     //    float dist_m;
02038 //    float dist;
02039 //    float edist;
02040 
02041 //  cout<<"tdc: "<<h.reccdc()->tdc<<"  tev: "<<tev<<"  tof: "<<tof<<endl;
02042 
02043 
02044     double EsT0 = -1.;
02045     IMessageSvc *msgSvc;
02046     Gaudi::svcLocator()->service("MessageSvc", msgSvc);
02047     MsgStream log(msgSvc, "TCosmicFitter");
02048 
02049     IDataProviderSvc* eventSvc = NULL;
02050     Gaudi::svcLocator()->service("EventDataSvc", eventSvc);
02051 
02052     SmartDataPtr<RecEsTimeCol> aevtimeCol (eventSvc,"/Event/Recon/RecEsTimeCol");
02053     if (aevtimeCol) {
02054       RecEsTimeCol::iterator iter_evt = aevtimeCol->begin();
02055       EsT0 = (*iter_evt)->getTest();
02056     }else{
02057       log << MSG::WARNING << "Could not find RecEsTimeCol" << endreq;
02058     }
02059 
02060     double rawTime = 0.;
02061     rawTime = h.reccdc()->tdc;
02062     double rawadc = 0.;
02063     rawadc =h.reccdc()->adc;
02064 //    double timewalk = CalibFunSvc_->getTimeWalk(layerid, 0.0);
02065 //    double timeDrift = rawTime - tof - tprop - EsTo - toWalk; 
02066 
02067 
02068 //----cal drift----//
02069     IMdcCalibFunSvc* l_mdcCalFunSvc;
02070     Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
02071 
02072     double timeWalk = l_mdcCalFunSvc->getTimeWalk(layerId, rawadc);
02073     double drifttime = rawTime - tof - 1.0e9*tprop - EsT0 - timeWalk;
02074 
02075     l.setDriftTime(drifttime);
02076 
02077 
02078     double dist;
02079     double edist;
02080     int prop = _propagation;
02081 
02082     //    //calculate derivative w.r.t. time in blute force way; need update 
02083     //    //in future to speed up
02084     //    float time_p = time + 0.1;
02085     //    calcdc_driftdist_(& prop,
02086     //                        & wire,
02087     //                        & side,
02088     //                        p,
02089     //                        x,
02090     //                        & time_p,
02091     //                        & dist_p,
02092     //                        & edist);
02093     //
02094     //        float time_m = time - 0.1;
02095     //        calcdc_driftdist_(& prop,
02096     //                        & wire,                 
02097     //                      & side,
02098     //                        p,
02099     //                        x,
02100     //                        & time_m,
02101     //                        & dist_m,
02102     //                        & edist);
02104     //    dddt = (dist_p - dist_m)*5.;
02105     //               std::cout << "side=" << side << std::endl;
02106     //               std::cout << "dddt=" << dddt << std::endl;
02107 
02108 //    float dist2[2];
02109 //    float sigma_d2[2];
02110 //    float deriv2[2];
02111     float time_tmp = time;
02112 
02113     //----cal drift----//
02114 //    IMdcCalibFunSvc* l_mdcCalFunSvc;
02115     Gaudi::svcLocator() -> service("MdcCalibFunSvc", l_mdcCalFunSvc);
02116   //  dist = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_tmp, layerId, wire, side, 0.0);
02117     dist =  l_mdcCalFunSvc->driftTimeToDist(time_tmp,layerId, wire, side,0.0);//by liucy 2010/05/12
02118     edist = l_mdcCalFunSvc->getSigma(layerId, side, dist, 0.0);
02119     dist = dist/10.0; //mm->cm
02120     edist = edist/10.0;
02121 
02122     distance = dist;
02123     err = edist;
02124 
02125 //    cout<<"dist: "<<dist<<" edist: "<<edist<<endl;
02126 
02127     float time_p = time - 0.1;
02128     float time_m = time + 0.1;
02129 //    float dist_p = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_p, layerId, wire, side, 0.0);
02130 //    float dist_m = l_mdcCalFunSvc->rawTimeNoTOFToDist(time_m, layerId, wire, side, 0.0);
02131 //    dist_p = dist_p/10.;
02132 //    dist_m = dist_m/10.;
02133 //    dddt = (dist_m - dist_p)/0.2;
02134 //    dddt = 0;
02135 //    dddt = 0.004;
02136 
02137 //    float dist2[2] = {dist, dist};
02138 //    float sigma_d2[2] = {edist, edist};
02139 //    float deriv2[2] = {dddt_tmp, dddt_tmp}; // cm/ns
02140 //    float deriv2[2] = {0.00, 0.00};
02141 //    float deriv2[2] = {0.004, 0.004};
02142 //cout<<"dddt_tmp: "<<dddt_tmp<<endl;
02143 
02144 //zsl    calcdc_driftdist3_(& prop,
02145 //                     & wire,                
02146 //                     p,
02147 //                     x,
02148 //                     & time_tmp,
02149 //                     dist2,
02150 //                     sigma_d2, 
02151 //                     deriv2);
02152     //n.b. input and output time are slightly different because of prop. 
02153     //delay corr. in driftdist3.
02154     //    calcdc_driftdist_(& prop,
02155     //                        & wire,
02156     //                        & side,
02157     //                        p,
02158     //                        x,
02159     //                        & time,
02160     //                        & dist,
02161     //                        & edist);
02162 
02163 /*    if (side==-1) {
02164       //            std::cout << " " << std::endl;
02165       //            std::cout << dist << " " << dist2[0] << " " <<dist2[1] << std::endl;
02166       //            std::cout << edist << " " << sigma_d2[0] << " " << sigma_d2[1] << std::endl;
02167       //            std::cout << dddt  << " " << 0.001*deriv2[0] << std::endl;
02168       dist  = dist2[0];
02169       edist = sigma_d2[0];
02170       //dddt = 0.001*deriv2[0];
02171       dddt = deriv2[0];
02172     } else if(side== 1) {
02173       //            std::cout << " " << std::endl;
02174       //            std::cout << dist << " " << dist2[1] << " " <<dist2[0] << std::endl;
02175       //            std::cout << edist << " " << sigma_d2[1] << " " << sigma_d2[0] << std::endl;
02176       //            std::cout << dddt  << " " << 0.001*deriv2[1] << std::endl;
02177       dist  = dist2[1];
02178       edist = sigma_d2[1];
02179       //dddt = 0.001*deriv2[1];
02180       dddt = deriv2[1];
02181     }  
02182 
02183     distance = (double) dist;
02184 //    std::cout << "time,distance,dddt="<<time<<" "<<distance<<"  "<<dddt<<std::endl;
02185     err = (double) edist;
02186 */
02187     return;
02188 }
02189 
02190 
02191 //add by jialk, to return drift time after prop correction
02192 /*
02193 //=========================================
02194 void
02195 THelixFitter::drift(const TTrack & t,
02196                     const TMLink & l,
02197                     float tev,
02198                     double & distance,
02199                     double & err,
02200                     double & dddt) const {
02201 //=========================================
02202 */