StRoot: Sti/StiKalmanTrackNode.cxx Source File

00001 //StiKalmanTrack.cxx
00002 /*
00003  * $Id: StiKalmanTrackNode.cxx,v 2.133 2011/11/21 17:05:26 fisyak Exp $
00004  *
00005  * /author Claude Pruneau
00006  *
00007  * $Log: StiKalmanTrackNode.cxx,v $
00008  * Revision 2.133  2011/11/21 17:05:26  fisyak
00009  * Correct no. of possible point for CA case
00010  *
00011  * Revision 2.132  2010/09/07 18:37:31  fisyak
00012  * Restore Sti logic before TPCCATracker
00013  *
00014  * Revision 2.131  2010/09/06 18:20:48  fisyak
00015  * Add TPCCATracker
00016  *
00017  * Revision 1.4  2010/08/23 21:56:28  ikulakov
00018  * Fix - alinghment bag.
00019  *
00020  * Revision 1.3  2010/08/04 19:46:43  ikulakov
00021  * Fix - edge (don't cut 2 cm from detector)
00022  *
00023  * Revision 1.2  2010/07/29 16:19:11  fisyak
00024  * GSI CA tracker
00025  *
00026  * Revision 2.130  2010/07/14 18:45:10  perev
00027  * MoreComments
00028  *
00029  * Revision 2.129  2010/04/03 04:02:29  perev
00030  * Account field=0
00031  *
00032  * Revision 2.128  2010/04/01 20:25:34  perev
00033  * Remove test to small error _cPP
00034  *
00035  * Revision 2.127  2010/04/01 00:27:33  perev
00036  * Zero fied = 2e-6
00037  *
00038  * Revision 2.126  2009/11/05 17:37:52  fine
00039  * remove the compilation warnings
00040  *
00041  * Revision 2.125  2009/10/18 22:48:58  perev
00042  * remove STAR LOG in print()
00043  *
00044  * Revision 2.124  2009/08/19 21:19:37  perev
00045  * getTime() is a const now
00046  *
00047  * Revision 2.123  2009/04/23 02:39:03  perev
00048  * GetTime defence sin <1 added
00049  *
00050  * Revision 2.122  2009/04/01 19:20:17  perev
00051  * Replace asserts to error condition
00052  *
00053  * Revision 2.121  2009/03/16 13:50:15  fisyak
00054  * Move out all Sti Chairs into StDetectorDb
00055  *
00056  * Revision 2.120  2008/12/26 15:18:00  fisyak
00057  * Enlarge fitting volume from 200 => 250 cm
00058  *
00059  * Revision 2.119  2008/06/11 22:04:37  fisyak
00060  * Add dead material
00061  *
00062  * Revision 2.118  2008/06/09 20:12:09  perev
00063  * BigStepBack
00064  *
00065  * Revision 2.115  2008/04/03 20:03:36  fisyak
00066  * Straighten out DB access via chairs
00067  *
00068  * Revision 2.114  2008/03/25 18:02:53  perev
00069  * remove field field from everythere
00070  *
00071  * Revision 2.113  2007/09/10 21:26:52  perev
00072  * getPt non positive bug fix. introduces 3 month ago
00073  *
00074  * Revision 2.112  2007/08/30 19:13:27  fine
00075  * replace the repmaining cout with LOG_DEBUG
00076  *
00077  * Revision 2.111  2007/08/16 20:21:24  fine
00078  * replace printf with logger
00079  *
00080  * Revision 2.110  2007/07/12 00:21:00  perev
00081  * Normal radius instead of layer one
00082  *
00083  * Revision 2.109  2007/06/25 19:31:52  perev
00084  * Init of _sinCA and _cosCA non zeros now
00085  *
00086  * Revision 2.108  2007/06/07 20:13:42  perev
00087  * BugFix in getPt()
00088  *
00089  * Revision 2.107  2007/06/06 04:03:03  perev
00090  * getTime() cleanup
00091  *
00092  * Revision 2.106  2007/04/30 19:53:16  fisyak
00093  * Correct time of flight calculation, add time of flight corrrection for Laser
00094  *
00095  * Revision 2.105  2007/03/21 17:47:24  fisyak
00096  * Time of Flight
00097  *
00098  * Revision 2.104  2006/12/24 02:16:36  perev
00099  * _inf=0 added in copy constructor
00100  *
00101  * Revision 2.103  2006/12/18 01:17:41  perev
00102  * Info block added and filled for pulls
00103  *
00104  * Revision 2.102  2006/10/16 20:29:35  fisyak
00105  * Clean up useless classes
00106  *
00107  * Revision 2.101  2006/10/09 15:47:07  fisyak
00108  * take out Central represantation, remove StiDedxCalculator
00109  *
00110  * Revision 2.100  2006/05/31 03:58:06  fisyak
00111  * Add Victor's dca track parameters, clean up
00112  *
00113  * Revision 2.99  2006/04/15 23:12:10  perev
00114  * Supress printout
00115  *
00116  * Revision 2.98  2006/04/07 18:01:56  perev
00117  * Back to the latest Sti
00118  *
00119  * Revision 2.96  2006/02/16 20:44:50  perev
00120  * assert changed back
00121  *
00122  * Revision 2.95  2006/02/15 19:07:18  perev
00123  * assrt in nudge cos < 1
00124  *
00125  * Revision 2.94  2006/02/14 18:10:41  perev
00126  * getGlobalHitErrors added+CleanUp
00127  *
00128  * Revision 2.93  2005/12/31 01:37:12  perev
00129  * Primary node perpendicular to track
00130  *
00131  * Revision 2.92  2005/12/20 00:41:21  perev
00132  * unassigned sind fixed(thanxYF)
00133  *
00134  * Revision 2.91  2005/12/18 23:41:46  perev
00135  * Dependency from StiKalmanTrackNode removed
00136  *
00137  * Revision 2.90  2005/12/08 22:05:45  perev
00138  * nudge assert replaced by print. But very strangeStiKalmanTrackNode.cxx
00139  *
00140  * Revision 2.89  2005/12/07 22:29:27  perev
00141  * Big Cleanup
00142  *
00143  * Revision 2.88  2005/08/09 14:55:34  perev
00144  * extend()/reduce() of node
00145  *
00146  * Revision 2.87  2005/08/04 03:52:54  perev
00147  * Cleanup
00148  *
00149  * Revision 2.86  2005/07/20 17:24:25  perev
00150  * Nudge actions in evaluateChi2 added
00151  *
00152  * Revision 2.85  2005/06/14 22:22:46  perev
00153  * Replace assert to error return
00154  *
00155  * Revision 2.84  2005/06/03 19:57:04  perev
00156  * Bug fix, violation of array size
00157  *
00158  * Revision 2.83  2005/06/02 17:27:41  perev
00159  * More weak assert in nudge()
00160  *
00161  * Revision 2.82  2005/05/31 16:47:56  perev
00162  * technical reorganization
00163  *
00164  * Revision 2.81  2005/05/12 18:10:04  perev
00165  * dL/dCurv more accurate
00166  *
00167  * Revision 2.80  2005/05/04 19:33:00  perev
00168  * Supress assert
00169  *
00170  * Revision 2.79  2005/04/30 20:45:18  perev
00171  * Less strong test for assert in propagateError
00172  *
00173  * Revision 2.78  2005/04/25 20:20:25  fisyak
00174  * replace assert by print out
00175  *
00176  * Revision 2.77  2005/04/12 14:35:39  fisyak
00177  * Add print out for dE/dx
00178  *
00179  * Revision 2.76  2005/04/11 22:48:30  perev
00180  * assert removed
00181  *
00182  * Revision 2.75  2005/04/11 17:33:55  perev
00183  * Wrong sorting accounted, check for accuracy inctreased
00184  *
00185  * Revision 2.74  2005/04/11 14:32:18  fisyak
00186  * Use gdrelx from GEANT for dE/dx calculation with accouning density effect
00187  *
00188  * Revision 2.73  2005/03/30 21:01:43  perev
00189  * asserts replaced to prints
00190  *
00191  * Revision 2.72  2005/03/28 05:52:40  perev
00192  * Reorganization of node container
00193  *
00194  * Revision 2.71  2005/03/24 19:28:35  perev
00195  * Switch off DerivTest
00196  *
00197  * Revision 2.70  2005/03/24 18:05:07  perev
00198  * Derivatives and their test fixed to eta==Psi model
00199  *
00200  * Revision 2.69  2005/03/19 00:20:33  perev
00201  * Assert for zero determinant ==> print
00202  *
00203  * Revision 2.68  2005/03/18 17:35:38  perev
00204  * some asserts removed
00205  *
00206  * Revision 2.67  2005/03/18 17:13:07  perev
00207  * assert in rotate fix
00208  *
00209  * Revision 2.66  2005/03/17 06:24:52  perev
00210  * A lot of changes. _eta now is Psi
00211  *
00212  * Revision 2.65  2005/02/25 17:05:41  perev
00213  * Scaling for errors added
00214  *
00215  * Revision 2.64  2005/02/19 20:23:37  perev
00216  * Cleanup
00217  *
00218  * Revision 2.63  2005/02/18 19:02:55  fisyak
00219  * Add debug print out for extendToVertex
00220  *
00221  * Revision 2.62  2005/02/17 23:19:02  perev
00222  * NormalRefangle + Error reseting
00223  *
00224  * Revision 2.61  2005/02/17 19:58:06  fisyak
00225  * Add debug print out flags
00226  *
00227  * Revision 2.60  2005/02/16 17:47:16  perev
00228  * assert in nudge 1==>5
00229  *
00230  * Revision 2.59  2005/02/07 18:33:42  fisyak
00231  * Add VMC dead material
00232  *
00233  * Revision 2.58  2005/01/20 16:51:32  perev
00234  * Remove redundant print
00235  *
00236  * Revision 2.57  2005/01/17 01:31:25  perev
00237  * New parameter model
00238  *
00239  * Revision 2.56  2005/01/06 00:59:41  perev
00240  * Initial errors tuned
00241  *
00242  * Revision 2.55  2005/01/04 01:37:47  perev
00243  * minor bug fix
00244  *
00245  * Revision 2.54  2004/12/23 18:15:46  perev
00246  * Cut for -ve cosCA added
00247  *
00248  * Revision 2.53  2004/12/14 17:10:17  perev
00249  * Propagate for 0 not called
00250  *
00251  * Revision 2.52  2004/12/13 22:52:23  perev
00252  * Off testError
00253  *
00254  * Revision 2.51  2004/12/13 20:01:38  perev
00255  * old version of testError temporary activated
00256  *
00257  * Revision 2.50  2004/12/12 01:34:24  perev
00258  * More smart testError, partial error reset
00259  *
00260  * Revision 2.49  2004/12/11 22:17:49  pruneau
00261  * new eloss calculation
00262  *
00263  * Revision 2.48  2004/12/11 04:31:36  perev
00264  * set of bus fixed
00265  *
00266  * Revision 2.47  2004/12/10 15:51:44  fisyak
00267  * Remove fudge factor from eloss calculation, add more debug printout and tests, reorder calculation of cov. matrix for low triangular form
00268  *
00269  * Revision 2.46  2004/12/08 16:56:16  fisyak
00270  * Fix sign in dE/dx; move from upper to lower triangular matrix convention (StEvent) for px,py,pz
00271  *
00272  * Revision 2.45  2004/12/05 00:39:07  fisyak
00273  * Add test suit for matrix manipulation debugging under overall CPPFLAGS=-DSti_DEBUG
00274  *
00275  * Revision 2.44  2004/12/01 14:04:57  pruneau
00276  * z propagation fix
00277  *
00278  * Revision 2.43  2004/11/24 17:59:26  fisyak
00279  * Set ionization potential for Ar in eloss calculateion instead 5
00280  *
00281  * Revision 2.42  2004/11/22 19:43:06  pruneau
00282  * commented out offending cout statement
00283  *
00284  * Revision 2.41  2004/11/22 19:23:20  pruneau
00285  * minor changes
00286  *
00287  * Revision 2.40  2004/11/10 21:46:02  pruneau
00288  * added extrapolation function; minor change to updateNode function
00289  *
00290  * Revision 2.39  2004/11/08 15:32:54  pruneau
00291  * 3 sets of modifications
00292  * (1) Changed the StiPlacement class to hold keys to both the radial and angle placement. Propagated the use
00293  * of those keys in StiSvt StiTpc StiSsd and all relevant Sti classes.
00294  * (2) Changed the StiKalmanTrackFinder::find(StiTrack*) function's algorithm for the navigation of the
00295  * detector volumes. The new code uses an iterator to visit all relevant volumes. The code is now more robust and compact
00296  * as well as much easier to read and maintain.
00297  * (3) Changed the chi2 calculation in StiKalmanTrack::getChi2 and propagated the effects of this change
00298  * in both StiTrackingPlots and StiStEventFiller classes.
00299  *
00300  * Revision 2.38  2004/10/27 03:25:49  perev
00301  * Version V3V
00302  *
00303  * Revision 2.37  2004/10/26 21:53:23  pruneau
00304  * No truncation but bad hits dropped
00305  *
00306  * Revision 2.36  2004/10/26 06:45:37  perev
00307  * version V2V
00308  *
00309  * Revision 2.35  2004/10/25 14:15:56  pruneau
00310  * various changes to improve track quality.
00311  *
00312  * Revision 2.34  2004/03/24 22:01:07  pruneau
00313  * Removed calls to center representation and replaced by normal representation
00314  *
00315  * Revision 2.33  2004/03/17 21:01:53  andrewar
00316  * Trapping for negative track error (^2) values _cYY and _cZZ. This should
00317  * be a temporary fix until the root of the problem is found. Problem seems
00318  * localized to trackNodes without hits.
00319  * Also trapping for asin(x), x>1 in ::length; point to point cord length
00320  * on the helix is greater than twice radius of curvature. This should also be
00321  * resovled.
00322  *
00323  * Revision 2.32  2004/01/30 21:40:21  pruneau
00324  * some clean up of the infinite checks
00325  *
00326  * Revision 2.31  2003/09/02 17:59:41  perev
00327  * gcc 3.2 updates + WarnOff
00328  *
00329  * Revision 2.30  2003/08/13 21:04:21  pruneau
00330  * transfered relevant tracking pars to detector builders
00331  *
00332  * Revision 2.29  2003/08/02 08:23:10  pruneau
00333  * best performance so far
00334  *
00335  * Revision 2.28  2003/07/30 19:18:58  pruneau
00336  * sigh
00337  *
00338  * Revision 2.26  2003/07/15 13:56:19  andrewar
00339  * Revert to previous version to remove bug.
00340  *
00341  * Revision 2.24  2003/05/22 18:42:33  andrewar
00342  * Changed max eloss correction from 1% to 10%.
00343  *
00344  * Revision 2.23  2003/05/09 22:07:57  pruneau
00345  * Added protection to avoid 90deg tracks and ill defined eloss
00346  *
00347  * Revision 2.22  2003/05/09 14:57:20  pruneau
00348  * Synching
00349  *
00350  * Revision 2.21  2003/05/08 18:49:09  pruneau
00351  * fudge=1
00352  *
00353  * Revision 2.20  2003/05/07 03:01:39  pruneau
00354  * *** empty log message ***
00355  *
00356  * Revision 2.19  2003/05/03 14:37:22  pruneau
00357  * *** empty log message ***
00358  *
00359  * Revision 2.18  2003/05/01 20:46:47  pruneau
00360  * changed error parametrization
00361  *
00362  * Revision 2.17  2003/04/22 21:20:17  pruneau
00363  * Added hit filter
00364  * Tuning og finder pars
00365  * Tuning of KalmanTrackNode
00366  *
00367  * Revision 2.16  2003/04/17 22:49:36  andrewar
00368  * Fixed getPhase function to conform to StHelixModel convention.
00369  *
00370  * Revision 2.15  2003/03/31 17:18:56  pruneau
00371  * various
00372  *
00373  * Revision 2.14  2003/03/13 21:21:27  pruneau
00374  * getPhase() fixed. MUST inclde -helicity()*pi/2
00375  *
00376  * Revision 2.13  2003/03/13 18:59:13  pruneau
00377  * various updates
00378  *
00379  * Revision 2.12  2003/03/12 17:57:31  pruneau
00380  * Elss calc updated.
00381  *
00382  * Revision 2.11  2003/03/04 21:31:05  pruneau
00383  * Added getX0() and getGasX0() conveninence methods.
00384  *
00385  * Revision 2.10  2003/03/04 18:41:27  pruneau
00386  * Fixed StiHit to use global coordinates as well as locals.
00387  * Fixed Logic Bug in StiKalmanTrackFinder
00388  *
00389  * Revision 2.9  2003/03/04 15:25:48  andrewar
00390  * Added several functions for radlength calculation.
00391  *
00392  */
00393 
00394 #include <Stiostream.h>
00395 #include <stdexcept>
00396 #include <math.h>
00397 #include <stdio.h>
00398 using namespace std;
00399 
00400 #include "StiHit.h"
00401 #include "StiDetector.h"
00402 #include "StiPlacement.h"
00403 #include "StiMaterial.h"
00404 #include "StiShape.h"
00405 #include "StiPlanarShape.h"
00406 #include "StiCylindricalShape.h"
00407 #include "StiKalmanTrackNode.h"
00408 #include "StiElossCalculator.h"
00409 #include "StDetectorDbMaker/StiTrackingParameters.h"
00410 #include "StDetectorDbMaker/StiKalmanTrackFinderParameters.h"
00411 #include "StDetectorDbMaker/StiHitErrorCalculator.h"
00412 #include "StiTrackNodeHelper.h"
00413 #include "StiFactory.h"
00414 #include "TString.h"
00415 #if ROOT_VERSION_CODE < 331013
00416 #include "TCL.h"
00417 #else
00418 #include "TCernLib.h"
00419 #endif
00420 #include "THelixTrack.h"
00421 #include "StThreeVector.hh"
00422 #include "StThreeVectorF.hh"
00423 
00424 #include "TRMatrix.h"
00425 #include "TRVector.h"
00426 #include "StarMagField.h"
00427 #include "TMath.h"
00428 #include "StMessMgr.h"
00429 
00430 #define PrP(A)    { LOG_INFO << "\t" << (#A) << " = \t" << ( A ) }
00431 #define PrPP(A,B) {LOG_INFO  << "=== StiKalmanTrackNode::" << (#A); PrP((B)); LOG_INFO << endm;}
00432 // Local Track Model
00433 //
00434 // x[0] = y  coordinate
00435 // x[1] = z  position along beam axis
00436 // x[2] = (Psi)
00437 // x[3] = C  (local) curvature of the track
00438 // x[4] = tan(l) 
00439 
00440 static const double kMaxEta = 1.25; // 72 degrees for laser tracks
00441 static const double kMaxSinEta = sin(kMaxEta);
00442 static const double kMaxCur = 0.2;
00443 static const double kFarFromBeam = 10.;
00444 static const Double_t kMaxZ = 250;
00445 static const Double_t kMaxR = 250;
00446 StiNodeStat StiKalmanTrackNode::mgP; 
00447 
00448 
00449 static const int    idx33[3][3] = {{0,1,3},{1,2,4},{3,4,5}};
00450 static const int    idx55[5][5] = 
00451   {{0,1,3,6,10},{1,2,4,7,11},{3,4,5, 8,12},{6,7, 8, 9,13},{10,11,12,13,14}};
00452 static const int    idx55tpt[5][5] = 
00453   {{0,1,2,3, 4},{1,5,6,7, 8},{2,6,9,10,11},{3,7,10,12,13},{ 4, 8,11,13,14}};
00454 
00455 static const int    idx66[6][6] =
00456   {{ 0, 1, 3, 6,10,15},{ 1, 2, 4, 7,11,16},{ 3, 4, 5, 8,12,17}
00457   ,{ 6, 7, 8, 9,13,18},{10,11,12,13,14,19},{15,16,17,18,19,20}};
00458 
00459 bool StiKalmanTrackNode::useCalculatedHitError = true;
00460 TString StiKalmanTrackNode::comment("Legend: \tE - extapolation\tM Multiple scattering\tV at Vertex\tB at beam\tR at Radius\tU Updated\n");
00461 TString StiKalmanTrackNode::commentdEdx(""); 
00462 //debug vars
00463 //#define STI_ERROR_TEST
00464 //#define STI_DERIV_TEST
00465 #ifdef STI_DERIV_TEST
00466 int    StiKalmanTrackNode::fDerivTestOn=0;   
00467 #endif
00468 #ifndef STI_DERIV_TEST
00469 int    StiKalmanTrackNode::fDerivTestOn=-10;   
00470 #endif
00471 
00472 double StiKalmanTrackNode::fDerivTest[kNPars][kNPars];   
00473 int gCurrShape=0;
00474 
00475 void StiKalmanTrackNode::Break(int kase)
00476 {
00477 static int myBreak=-2005;
00478 if (kase!=myBreak) return;
00479   LOG_DEBUG << Form("*** Break(%d) ***",kase)<< endm;
00480 }               
00481 /* bit mask for debug printout  
00482    0   => 1 - covariance and propagate matrices 
00483    1   => 2 - hit associated with the node
00484    2   => 4 - test matrix manipulation
00485    3   => 8 - test locate
00486  */
00487 int StiKalmanTrackNode::_debug = 0;
00488 int StiKalmanTrackNode::_laser = 0;
00489 
00490 //______________________________________________________________________________
00491 void StiKalmanTrackNode::reset()
00492 { 
00493 static int myCount=0;
00494   StiTrackNode::reset();
00495   memset(_beg,0,_end-_beg+1);
00496   _ext=0; _inf=0;
00497   mId = ++myCount; 
00498   mHz = 999;
00499   Break(mId);
00500 }
00501 //______________________________________________________________________________
00502 void StiKalmanTrackNode::unset()
00503 { 
00504   reduce();
00505   if (_inf) BFactory::Free(_inf); _inf=0;
00506 }
00507 //______________________________________________________________________________
00508 void StiKalmanTrackNode::resetError(double fak)
00509 { 
00510 static const double DY=0.3,DZ=0.3,DEta=0.03,DPti=1.,DTan=0.05;
00511 
00512   if (!fak) {
00513     mFE.reset();
00514     mFE._cYY=DY*DY;
00515     mFE._cZZ=DZ*DZ;
00516     mFE._cEE=DEta*DEta;
00517     mFE._cPP=DPti*DPti;
00518     mFE._cTT=DTan*DTan;
00519   } else {
00520     for (int i=0;i<kNErrs;i++) mFE.A[i] *=fak;
00521   }  
00522   mPE() = mFE;
00523 }
00524 //_____________________________________________________________
00529 //______________________________________________________________________________
00530 void StiKalmanTrackNode::setState(const StiKalmanTrackNode * n)
00531 {
00532   _state   = n->_state;
00533   _alpha    = n->_alpha;
00534   mFP = n->mFP;
00535   mFE = n->mFE;
00536   mFP.hz()=0;
00537   nullCount = n->nullCount;
00538   contiguousHitCount = n->contiguousHitCount;
00539   contiguousNullCount = n->contiguousNullCount;
00540   setChi2(1e62);  
00541 }
00542 
00551 //______________________________________________________________________________
00552 void StiKalmanTrackNode::get(double& alpha,
00553                              double& xRef,
00554                              double  x[kNPars], 
00555                              double  e[kNErrs], 
00556                              double& chi2)
00557 {
00558   alpha = _alpha;
00559   xRef  = getRefPosition();
00560   memcpy(x,mFP.P,kNPars*sizeof(mFP.x()));
00561   memcpy(e,mFE.A,sizeof(mFE));
00562   chi2 = getChi2();
00563 }
00564 
00565 //______________________________________________________________________________
00573 //______________________________________________________________________________
00574 double StiKalmanTrackNode::getPt() const
00575 {
00576   return (fabs(mFP.ptin())<1e-3) ? 1e3: 1./fabs(mFP.ptin());
00577 }
00578 //______________________________________________________________________________
00579 void StiKalmanTrackNode::propagateCurv(const StiKalmanTrackNode *parent)
00580 {
00581    mFP.ptin()=parent->mFP.ptin();
00582    mFP.curv()=getHz()*mFP.ptin();
00583 } 
00584 //______________________________________________________________________________
00591 //______________________________________________________________________________
00592 double StiKalmanTrackNode::getHz() const
00593 {
00594   
00595 static const double EC = 2.99792458e-4,ZEROHZ = 2e-6;
00596    if (mHz<999) return mHz;
00597    double h[3];
00598    StarMagField::Instance()->BField(&(getGlobalPoint().x()),h);
00599    h[2] = EC*h[2];
00600    if (fabs(h[2]) < ZEROHZ) h[2]=0;
00601    mHz = h[2];
00602    return mHz;
00603 }
00604 //______________________________________________________________________________
00634 //______________________________________________________________________________
00635 void StiKalmanTrackNode::getMomentum(double p[3], double e[6]) const
00636 {       
00637 //      keep in mind that _eta == CA
00638 //      keep in mind that pt == SomeCoef/rho
00639 enum {jX=0,jY,jZ,jE,jP,jT};
00640 
00641   double pt = getPt();
00642   p[0] = pt*mFP._cosCA;
00643   p[1] = pt*mFP._sinCA;
00644   p[2] = pt*mFP.tanl();
00645 
00646 //              if e==0, error calculation is not needed, then return
00647   if (!e) return;
00648 
00649   double F[3][kNPars]; memset(F,0,sizeof(F));
00650   double dPtdPi = -pt*pt; if (mFP.ptin()<0) dPtdPi=-dPtdPi;
00651   F[jX][jE] = -pt    *mFP._sinCA;
00652   F[jX][jP] =  dPtdPi*mFP._cosCA;
00653   F[jX][jT] =  0;
00654 
00655   F[jY][jE] =  pt    *mFP._cosCA;
00656   F[jY][jP] =  dPtdPi*mFP._sinCA;
00657   F[jY][jT] =  0;
00658   
00659   F[jZ][jE] =  0;
00660   F[jZ][jP] =  dPtdPi*mFP.tanl();
00661   F[jZ][jT] =  pt;
00662   
00663   
00664   TCL::trasat(F[0],mFE.A,e,3,kNPars);  
00665 }
00666 //______________________________________________________________________________
00685 //______________________________________________________________________________
00686 void StiKalmanTrackNode::getGlobalRadial(double  x[6],double  e[15])
00687 {
00688   enum {jRad=0,jPhi,jZ,jTan,jPsi,jCur, kX=0,kY,kZ,kE,kC,kT};
00689   double alpha,xRef,chi2;
00690   double xx[kNPars],ee[kNErrs];
00691 
00692   get(alpha,xRef,xx,ee,chi2);
00693   
00694   x[jRad] = sqrt(pow(xx[kX],2)+pow(xx[kY],2));
00695   x[jPhi] = atan2(xx[kY],xx[kX]) + alpha;
00696   x[jZ  ] = xx[kZ];
00697   x[jTan] = xx[kT];
00698   x[jPsi] = xx[kE] + alpha;
00699   x[jCur] = xx[kC];
00700   if (!e) return;
00701 
00702   double F[kNErrs][kNErrs]; memset(F,0,sizeof(F));
00703   F[jPhi][kX] = -1e5;
00704   F[jPhi][kY] =  1e5;
00705   if (fabs(xx[kY])>1e-5)  F[jPhi][kX] = -1./(xx[kY]);
00706   if (fabs(xx[kX])>1e-5)  F[jPhi][kY] =  1./(xx[kX]);
00707   F[jZ][kZ]   = 1.;
00708   F[jTan][kT] = 1;
00709   F[jPsi][kE] = 1;
00710   F[jCur][kC] = 1;
00711   memset(e,0,sizeof(*e)*15);
00712   for (int k1=0;k1<kNPars;k1++) {
00713   for (int k2=0;k2<kNPars;k2++) {
00714     double cc = mFE.A[idx66[k1][k2]];    
00715     for (int j1=jPhi;j1<= 5;j1++){
00716     for (int j2=jPhi;j2<=j1;j2++){
00717       e[idx55[j1-1][j2-1]]+= cc*F[j1][k1]*F[j2][k2];
00718   }}}}    
00719   
00720 }
00721 //______________________________________________________________________________
00754 //______________________________________________________________________________
00755 void StiKalmanTrackNode::getGlobalTpt(float  x[6],float  e[15])
00756 {
00757   enum {jRad=0,jPhi,jZ,jTan,jPsi,jCur,jPt=jCur};
00758 static const double DEG = 180./M_PI;
00759 static       double fak[6] = {1,0,1,1,DEG,0};
00760 
00761   double xx[6],ee[15];
00762   getGlobalRadial(xx,ee);
00763   double pt = getPt();
00764   fak[jPhi] = DEG*xx[jRad];
00765   fak[jPt] = (double(getCharge())/pt)/xx[jCur];
00766 
00767   for (int i=0;i<6;i++) {x[i] = (float)(fak[i]*xx[i]);}
00768   if (!e) return;
00769 
00770   for (int j1=jPhi;j1<= 5;j1++){
00771   for (int j2=jPhi;j2<=j1;j2++){
00772     e[idx55tpt[j1-1][j2-1]] = (float)fak[j1]*fak[j2]*ee[idx55[j1-1][j2-1]];
00773   }}
00774 
00775 }
00776 //______________________________________________________________________________
00777 double StiKalmanTrackNode::getPhase() const
00778 {
00782   return getPsi()-getHelicity()*M_PI/2;
00783 
00784 }
00785 //______________________________________________________________________________
00786 double StiKalmanTrackNode::getPsi() const
00787 {
00788   return nice(mFP.eta()+_alpha);
00789 }
00790 
00791 //______________________________________________________________________________
00805 
00806 //______________________________________________________________________________
00807 void StiKalmanTrackNode::getGlobalMomentum(double p[3], double e[6]) const
00808 {       
00809   // first get p & e in the local ref frame
00810   enum {jXX=0,jXY,jYY};
00811   
00812   getMomentum(p,e);
00813   // now rotate the p & e in the global ref frame
00814   // for the time being, assume an azimuthal rotation 
00815   // by alpha is sufficient.
00816   // transformation matrix - needs to be set
00817   double px=p[0];
00818   double py=p[1];
00819   double cosAlpha = cos(_alpha);
00820   double sinAlpha = sin(_alpha);
00821   p[0] = cosAlpha*px - sinAlpha*py;
00822   p[1] = sinAlpha*px + cosAlpha*py;
00823   if (e==0) return;
00824 
00825     // original error matrix
00826 
00827   double cXX = e[jXX];
00828   double cXY = e[jXY];
00829   double cYY = e[jYY];
00830   double cc = cosAlpha*cosAlpha;
00831   double ss = sinAlpha*sinAlpha;
00832   double cs = cosAlpha*sinAlpha;
00833   e[jXX] = cc*cXX -   2.*cs*cXY + ss*cYY;
00834   e[jYY] = ss*cXX +   2.*cs*cXY + cc*cYY;
00835   e[jXY] = cs*cXX + (cc-ss)*cXY - cs*cYY;
00836 }
00837 
00838 
00839 //______________________________________________________________________________
00854 //______________________________________________________________________________
00855 int StiKalmanTrackNode::propagate(StiKalmanTrackNode *pNode, 
00856                                   const StiDetector * tDet,int dir)
00857 {
00858 static int nCall=0; nCall++;
00859 Break(nCall);
00860   int position = 0;
00861   setState(pNode);
00862   setDetector(tDet);
00863   if (debug()) ResetComment(::Form("%30s ",tDet->getName().c_str()));
00864 
00865   StiPlacement * place = tDet->getPlacement();
00866 //double nLayerRadius  = place->getLayerRadius ();
00867   double nNormalRadius = place->getNormalRadius();
00868 
00869   StiShape * sh = tDet->getShape();
00870   int shapeCode = sh->getShapeCode();
00871   double endVal,dAlpha;
00872   switch (shapeCode) {
00873 
00874   case kPlanar: endVal = nNormalRadius;
00875     { //flat volume
00876       dAlpha = place->getNormalRefAngle();
00877       dAlpha = nice(dAlpha - _alpha);
00878       // bail out if the rotation fails...
00879       position = rotate(dAlpha);
00880       if (position)                     return -10;
00881     }
00882                                         break;
00883   case kDisk:                                                   
00884   case kCylindrical: endVal = nNormalRadius;
00885     {
00886       double xy[4];
00887       position = cylCross(endVal,&mFP._cosCA,mFP.curv(),xy);
00888       if (position)                     return -11;
00889       dAlpha = atan2(xy[1],xy[0]);
00890       position = rotate(dAlpha);
00891       if (position)                     return -11;
00892     }
00893                                         break;
00894   default: assert(0);
00895   }
00896 
00897   position = propagate(endVal,shapeCode,dir); 
00898 
00899   if (position>kEdgeZplus || position<0) return position;
00900   propagateError();
00901   if (debug() & 8) { PrintpT("E");}
00902 
00903   // Multiple scattering
00904   if (StiKalmanTrackFinderParameters::instance()->mcsCalculated() && getHz())  
00905     propagateMCS(pNode,tDet);
00906   if (debug() & 8) { PrintpT("M");}
00907   return position;
00908 }
00909 
00910 //______________________________________________________________________________
00921 bool StiKalmanTrackNode::propagate(const StiKalmanTrackNode *parentNode, StiHit * vertex,int dir)
00922 {
00923 static int nCall=0; nCall++;
00924 Break(nCall);
00925   
00926   setState(parentNode);
00927   TCircle tc(&mFP.x(),&mFP._cosCA,mFP.curv());
00928   double xy[2]; xy[0]=vertex->x(),xy[1]=vertex->y();
00929   double s = tc.Path(xy); tc.Move(s);
00930   double ang = atan2(tc.Dir()[1],tc.Dir()[0]);
00931   vertex->rotate(ang);
00932   rotate(ang);
00933   if (debug()) ResetComment(::Form("Vtx:%8.3f %8.3f %8.3f",vertex->x(),vertex->y(),vertex->z()));
00934   if (propagate(vertex->x(),kPlanar,dir))    return false; // track does not reach vertex "plane"
00935   propagateError();
00936   if (debug() & 8) { PrintpT("V");}
00937   setHit(vertex);
00938   setDetector(0);
00939   return true;
00940 }
00941 
00942 //______________________________________________________________________________
00945 bool StiKalmanTrackNode::propagateToBeam(const StiKalmanTrackNode *parentNode,int dir)
00946 {
00947   setState(parentNode);
00948   if (debug()) {
00949     if (parentNode->getDetector()) 
00950       ResetComment(::Form("%30s ",parentNode->getDetector()->getName().c_str()));
00951     else ResetComment("Unknown Detector");
00952   }
00953   if (propagate(0., kPlanar,dir) < 0) return false; // track does not reach vertex "plane"
00954   propagateError();
00955   if (debug() & 8) { PrintpT("B");}
00956   setHit(0);
00957   setDetector(0);
00958   return true;
00959 }
00960 
00961 //______________________________________________________________________________
00964 int StiKalmanTrackNode::propagateToRadius(StiKalmanTrackNode *pNode, double radius,int dir)
00965 {
00966   int position = 0;
00967   setState(pNode);
00968   if (debug()) ResetComment(::Form("%30s ",pNode->getDetector()->getName().c_str()));
00969   position = propagate(radius,kCylindrical,dir);
00970   if (position<0) return position;
00971   propagateError();
00972   if (debug() & 8) { PrintpT("R");}
00973   _detector = 0;
00974   return position;
00975 }
00976 
00977 
00978 //______________________________________________________________________________
00987 //______________________________________________________________________________
00988 int  StiKalmanTrackNode::propagate(double xk, int option,int dir)
00989 {
00990   static int nCalls=0;
00991   nCalls++;
00992   assert(fDerivTestOn!=-10 || _state==kTNRotEnd ||_state>=kTNReady);
00993   _state = kTNProBeg;
00994 //  numeDeriv(xk,1,option,dir);
00995   mgP.x1=mFP.x();  mgP.y1=mFP.y(); mgP.cosCA1 =mFP._cosCA; mgP.sinCA1 =mFP._sinCA;
00996   double rho = mFP.curv();
00997   mgP.x2 = xk;
00998 
00999   mgP.dx=mgP.x2-mgP.x1;  
01000   double test = (dir)? mgP.dx:-mgP.dx;  
01001 //      if track is coming back stop tracking
01002 //VP  if (test<0) return -3; //Unfortunatelly correct order not garanteed
01003 
01004   double dsin = mFP.curv()*mgP.dx;
01005   mgP.sinCA2=mgP.sinCA1 + dsin; 
01006 //      Orientation is bad. Fit is non reliable
01007   if (fabs(mgP.sinCA2)>kMaxSinEta)                              return -4;
01008   mgP.cosCA2   = ::sqrt((1.-mgP.sinCA2)*(1.+mgP.sinCA2));
01009 //      Check what sign of cosCA2 must be
01010   test = (2*dir-1)*mgP.dx*mgP.cosCA1;
01011   if (test<0) mgP.cosCA2 = -mgP.cosCA2;
01012 
01013   int nIt = (mgP.cosCA2 <0)? 2:1;
01014   int ians = 0;
01015   StiNodePars save = mFP;
01016   for (int iIt=0; iIt<nIt; iIt++) {//try 2 cases, +ve and -ve cosCA
01017     ians = -1;
01018     mFP = save;
01019     mgP.cosCA2 = (!iIt)? fabs(mgP.cosCA2):-fabs(mgP.cosCA2);
01020     mgP.sumSin   = mgP.sinCA1+mgP.sinCA2;
01021     mgP.sumCos   = mgP.cosCA1+mgP.cosCA2;
01022     if (fabs(mgP.sumCos)<1e-6) continue;
01023     mgP.dy = mgP.dx*(mgP.sumSin/mgP.sumCos);
01024     mgP.y2 = mgP.y1+mgP.dy;
01025 
01026 
01027     mgP.dl0 = mgP.cosCA1*mgP.dx+mgP.sinCA1*mgP.dy;
01028     double sind = mgP.dl0*rho;
01029   
01030     if (fabs(dsin) < 0.02 ) { //tiny angle
01031       mgP.dl = mgP.dl0*(1.+sind*sind/6);
01032       
01033     } else {
01034       double cosd = mgP.cosCA2*mgP.cosCA1+mgP.sinCA2*mgP.sinCA1;
01035       mgP.dl = atan2(sind,cosd)/rho;
01036     }
01037 
01038     mFP.z() += mgP.dl*mFP.tanl();
01039     mFP.y() = mgP.y2;
01040     mFP.eta() = nice(mFP.eta()+rho*mgP.dl);                                     /*VP*/
01041     mFP.x()       = mgP.x2;
01042     mFP._sinCA   = mgP.sinCA2;
01043     mFP._cosCA   = mgP.cosCA2;
01044     ians = locate();
01045     if (ians<=kEdgeZplus && ians>=0) break;
01046   }
01047   if (ians>kEdgeZplus || ians<0)                return ians;
01048   if (mFP.x()> kFarFromBeam) {
01049     if (fabs(mFP.eta())>kMaxEta)                return kEnded;
01050     if (mFP.x()*mgP.cosCA2+mFP.y()*mgP.sinCA2<=0)       return kEnded; 
01051   }
01052   mFP.hz()      = getHz();
01053   if (fabs(mFP.hz()) > 1e-10)   { mFP.curv() = mFP.hz()*mFP.ptin();}
01054   else                          { mFP.curv() = 1e-6 ;}
01055 
01056   mPP() = mFP;
01057   _state = kTNProEnd;
01058   return ians;
01059 }
01060 
01061 //______________________________________________________________________________
01062 int StiKalmanTrackNode::nudge(StiHit *hitp)
01063 {
01064   StiHit *hit = hitp;
01065   if (!hit) hit = getHit();
01066   double deltaX = 0;
01067   if (hit) { deltaX = hit->x()-mFP.x();}
01068   else     { if (_detector) deltaX = _detector->getPlacement()->getNormalRadius()-mFP.x();}
01069   if(fabs(deltaX)>5)            return -1;
01070   if (fabs(deltaX) <1.e-3)      return  0;
01071   double deltaS = mFP.curv()*(deltaX);
01072   double sCA2 = mFP._sinCA + deltaS;
01073   if (fabs(sCA2)>0.99)          return -2;
01074   double cCA2,deltaY,deltaL,sind;
01075   if (fabs(deltaS) < 1e-3 && fabs(mFP.eta())<1) { //Small angle approx
01076     cCA2= mFP._cosCA - mFP._sinCA/mFP._cosCA*deltaS;
01077     if (cCA2> 1) cCA2= 1;
01078     if (cCA2<-1) cCA2=-1;
01079     deltaY = deltaX*(mFP._sinCA+sCA2)/(mFP._cosCA+cCA2);
01080     deltaL = deltaX*mFP._cosCA + deltaY*mFP._sinCA;
01081     sind = deltaL*mFP.curv();
01082     deltaL = deltaL*(1.+sind*sind/6);
01083   } else {
01084     cCA2= sqrt((1.-sCA2)*(1.+sCA2));
01085     if (mFP._cosCA <0) cCA2 = -cCA2;
01086     deltaY = deltaX*(mFP._sinCA+sCA2)/(mFP._cosCA+cCA2);
01087     deltaL = deltaX*mFP._cosCA + deltaY*mFP._sinCA;
01088     sind = deltaL*mFP.curv();
01089     deltaL = asin(sind)/mFP.curv();
01090   }
01091   double deltaZ = mFP.tanl()*(deltaL);
01092   mFP._sinCA    = mgP.sinCA2 = sCA2;
01093   mFP._cosCA    = mgP.cosCA2 = cCA2;
01094   mgP.sumSin   = mgP.sinCA1+mgP.sinCA2;
01095   mgP.sumCos   = mgP.cosCA1+mgP.cosCA2;
01096   mFP.x()   += deltaX;
01097   mFP.y()   += deltaY;
01098   mFP.z()   += deltaZ;
01099   mFP.eta() += deltaL*mFP.curv();
01100   mgP.dx   += deltaX;
01101   mgP.dy   += deltaY;
01102   mgP.dl0  += deltaL;
01103   mgP.dl   += deltaL;
01104 
01105 
01106 //  assert(fabs(mFP._sinCA) <  1.);
01107   if (fabs(mFP._sinCA)>=1) {
01108     LOG_DEBUG << Form("StiKalmanTrackNode::nudge WRONG WRONG WRONG sinCA=%g",mFP._sinCA)
01109     << endm;          
01110     mFP.print();
01111     return -13;
01112   }
01113   assert(fabs(mFP._cosCA) <= 1.);
01114   mPP() = mFP;
01115   return 0;
01116 }
01117 //______________________________________________________________________________
01120 void StiKalmanTrackNode::propagateMtx()
01121 {  
01122 //      fYE == dY/dEta
01123   double fYE= mgP.dx*(1.+mgP.cosCA1*mgP.cosCA2+mgP.sinCA1*mgP.sinCA2)/(mgP.sumCos*mgP.cosCA2);
01124 //      fEC == dEta/dRho
01125   double  fEC = mgP.dx/mgP.cosCA2;
01126 //      fYC == dY/dRho
01127   double fYC=(mgP.dy*mgP.sinCA2+mgP.dx*mgP.cosCA2)/mgP.sumCos*fEC;
01128 //      fZC == dZ/dRho
01129   double dang = mgP.dl*mFP.curv();
01130   double C2LDX = mgP.dl*mgP.dl*(
01131                    0.5*mgP.sinCA2*pow((1+pow(dang/2,2)*sinX(dang/2)),2) +
01132                    mgP.cosCA2*dang*sinX(dang));
01133 
01134   double fZC = mFP.tanl()*C2LDX/mgP.cosCA2;
01135 //      fZE == dZ/dEta
01136   double dLdEta = mgP.dy/mgP.cosCA2;
01137   double fZE =  mFP.tanl()*dLdEta;
01138 
01139 //      fZT == dZ/dTanL; 
01140   double fZT= mgP.dl; 
01141   double hz = getHz(); fEC *=hz; fYC*=hz; fZC*=hz;
01142 
01143   double ca =1, sa=0;
01144   if (mMtx().A[0][0]) { ca = mMtx().A[0][0]+1.;sa = mMtx().A[0][1];}
01145   mMtx().reset();
01146 //  X related derivatives
01147   mMtx().A[0][0] = -1;
01148   mMtx().A[1][0] = -mgP.sinCA2/mgP.cosCA2; 
01149   mMtx().A[2][0] = -mFP.tanl() /mgP.cosCA2;
01150   mMtx().A[3][0] = -mFP.curv() /mgP.cosCA2;
01151 
01152   mMtx().A[1][3]=fYE; mMtx().A[1][4]=fYC; mMtx().A[2][3]=fZE;
01153   mMtx().A[2][4]=fZC; mMtx().A[2][5]=fZT; mMtx().A[3][4]=fEC;
01154   if (sa) {
01155     double fYX = mMtx().A[1][0]; 
01156     mMtx().A[1][0] = fYX*ca-sa;
01157     mMtx().A[1][1] = fYX*sa+ca-1;
01158   }
01159 }
01160 
01161 
01162 
01163 //______________________________________________________________________________
01166 void StiKalmanTrackNode::propagateError()
01167 {  
01168   static int nCall=0; nCall++;
01169   Break(nCall);
01170   assert(fDerivTestOn!=-10 || _state==kTNProEnd);
01171   
01172   if (debug() & 1) 
01173     {
01174       LOG_DEBUG << "Prior Error:"
01175            << "cYY:"<<mFE._cYY<<endm;
01176            LOG_DEBUG << "cZY:"<<mFE._cZY<<" cZZ:"<<mFE._cZZ<<endm;
01177       LOG_DEBUG << "cEY:"<<mFE._cEY<<" cEZ:"<<mFE._cEZ<<endm;
01178       LOG_DEBUG << "cPY:"<<mFE._cPY<<" cPZ:"<<mFE._cPZ<<endm;
01179       LOG_DEBUG << "cTY:"<<mFE._cTY<<" cTZ:"<<mFE._cTZ<<endm;
01180     }
01181   propagateMtx();
01182   errPropag6(mFE.A,mMtx().A,kNPars);
01183   int smallErr = !(mFE._cYY>1e-20 && mFE._cZZ>1e-20 && mFE._cEE>1e-20 && mFE._cTT>1.e-20);
01184   if (smallErr) {
01185     LOG_INFO << Form("***SmallErr: cYY=%g cZZ=%g cEE=%g cCC=%g cTT=%g"
01186           ,mFE._cYY,mFE._cZZ,mFE._cEE,mFE._cPP,mFE._cTT) << endm;
01187     assert(mFE._cYY>0 && mFE._cZZ>0 && mFE._cEE>0 && mFE._cPP>=0 && mFE._cTT>0);
01188   }
01189   assert(fabs(mFE._cXX)<1.e-6);
01190   assert(mFE._cYY*mFE._cZZ-mFE._cZY*mFE._cZY>0);
01191   mFE._cXX = mFE._cYX= mFE._cZX = mFE._cEX = mFE._cPX = mFE._cTX = 0;
01192   mFE.recov();
01193   mFE.check("StiKalmanTrackNode::propagateError");
01194 
01195 #ifdef Sti_DEBUG
01196   if (debug() & 4) {
01197     PrPP(propagateError,C);
01198     TRMatrix F(kNPars,kNPars,f[0]); PrPP(propagateError,F);
01199     // C^k-1_k = F_k * C_k-1 * F_kT + Q_k
01200     C = TRSymMatrix(F,TRArray::kAxSxAT,C); PrPP(propagateError,C);
01201     TRSymMatrix C1(kNPars,mFE.A);   PrPP(propagateError,C1);
01202     C1.Verify(C);//,1e-7,2);
01203   }
01204 #endif
01205   if (debug() & 1) 
01206     {
01207       LOG_DEBUG << "Post Error:"
01208            << "cYY:"<<mFE._cYY<<endm;
01209            LOG_DEBUG << "cZY:"<<mFE._cZY<<" cZZ:"<<mFE._cZZ<<endm;
01210            LOG_DEBUG << "cEY:"<<mFE._cEY<<" cEZ:"<<mFE._cEZ<<endm;
01211            LOG_DEBUG << "cCY:"<<mFE._cPY<<" cCZ:"<<mFE._cPZ<<endm;
01212            LOG_DEBUG << "cTY:"<<mFE._cTY<<" cTZ:"<<mFE._cTZ<<endm;
01213     }
01214 // now set hiterrors
01215    if (_hit) setHitErrors();
01216 
01217 // set state node is ready
01218   mPE() = mFE;
01219   _state = kTNReady;
01220 }
01221 
01222 //______________________________________________________________________________
01238 //delta(mgP.dx,dy,dz) = here - there
01239 double StiKalmanTrackNode::pathLToNode(const StiKalmanTrackNode * const oNode)
01240 {
01241   const StThreeVector<double> delta = 
01242     getGlobalPoint() - oNode->getGlobalPoint();
01243   double R = getCurvature();
01244   // s = 2c * asin( t/(2c)); t=::sqrt(mgP.dx^2+dy^2+dz^2)
01245   return length(delta, R);
01246 }
01247 
01248 //______________________________________________________________________________
01249 inline double StiKalmanTrackNode::length(const StThreeVector<double>& delta, double curv)
01250 {
01251   
01252   double m = delta.perp();
01253   double as = 0.5*m*curv;
01254   double lxy=0;
01255   if (fabs(as)<0.01) { lxy = m*(1.+as*as/24);}
01256   else               { lxy = 2.*asin(as)/curv;}
01257   return sqrt(lxy*lxy+delta.z()*delta.z());
01258 }
01259 
01260 //______________________________________________________________________________
01274 double StiKalmanTrackNode::evaluateChi2(const StiHit * hit) 
01275 {
01276   double r00, r01,r11;
01277   //If required, recalculate the errors of the detector hits.
01278   //Do not attempt this calculation for the main vertex.
01279   double dsin =mFP.curv()*(hit->x()-mFP.x());
01280   if (fabs(mFP._sinCA+dsin)>0.99   )    return 1e41;
01281   if (fabs(mFP.eta())       >kMaxEta)   return 1e41;
01282   if (fabs(mFP.curv())      >kMaxCur)    return 1e41;
01283   if (mHrr.hYY>1000*mFE._cYY
01284    && mHrr.hZZ>1000*mFE._cZZ)           return 1e41;
01285   setHitErrors(hit);
01286   r00=mHrr.hYY+mFE._cYY;
01287   r01=mHrr.hZY+mFE._cZY;  
01288   r11=mHrr.hZZ+mFE._cZZ;
01289 
01290 #ifdef Sti_DEBUG
01291   TRSymMatrix R(2,
01292                 r00,
01293                 r01, r11);
01294 #endif
01295   _det=r00*r11 - r01*r01;
01296   if (_det<r00*r11*1.e-5) {
01297     LOG_DEBUG << Form("StiKalmanTrackNode::evalChi2 *** zero determinant %g",_det)<< endm;
01298     return 1e60;
01299   }
01300   double tmp=r00; r00=r11; r11=tmp; r01=-r01;  
01301   double deltaX = hit->x()-mFP.x();
01302   double deltaL = deltaX/mFP._cosCA;
01303   double deltaY = mFP._sinCA *deltaL;
01304   double deltaZ = mFP.tanl()  *deltaL;
01305   double dyt=(mFP.y()-hit->y()) + deltaY;
01306   double dzt=(mFP.z()-hit->z()) + deltaZ;
01307   double cc= (dyt*r00*dyt + 2*r01*dyt*dzt + dzt*r11*dzt)/_det;
01308 
01309 #ifdef Sti_DEBUG
01310   if (debug() & 4) {
01311     TRSymMatrix G(R,TRArray::kInverted);
01312     TRVector r(2,hit->y()-mFP.y(),hit->z()-mFP.z());
01313     Double_t chisq = G.Product(r,TRArray::kATxSxA);
01314     Double_t diff = chisq - cc;
01315     Double_t sum  = chisq + cc;
01316     if (diff > 1e-7 || (sum > 2. && (2 * diff ) / sum > 1e-7)) {
01317       LOG_DEBUG << "Failed:\t" << chisq << "\t" << cc << "\tdiff\t" << diff << endm;
01318     }
01319   }
01320 #endif
01321   if (debug() & 8) {comment += Form(" chi2 = %6.2f",cc);}
01322   return cc;
01323 }
01324 //______________________________________________________________________________
01325 int StiKalmanTrackNode::isEnded() const
01326 {
01327 
01328    if(fabs(mFP.eta() )<=kMaxEta) return 0;
01329    return 1;   
01330 }               
01331 //______________________________________________________________________________
01332 int StiKalmanTrackNode::isDca() const
01333 {
01334    return (fabs(mFP.x())<=0);   
01335 }               
01336                 
01337 //______________________________________________________________________________
01346 void StiKalmanTrackNode::propagateMCS(StiKalmanTrackNode * previousNode, const StiDetector * tDet)
01347 {  
01348   propagateCurv(previousNode);
01349   double pt = getPt();
01350 #if 1
01351   if (pt>=1e3) {
01352     mPP() = mFP; mPE() = mFE;
01353     return;
01354   }
01355 #endif
01356   double relRadThickness;
01357   // Half path length in previous node
01358   double pL1,pL2,pL3,d1,d2,d3,dxEloss=0;
01359   pL1=previousNode->pathlength()/2.;
01360   // Half path length in this node
01361   pL3=pathlength()/2.;
01362   // Gap path length
01363   pL2= pathLToNode(previousNode);
01364   if (pL1<0) pL1=0;
01365   if (pL2<0) pL2=0;
01366   if (pL3<0) pL3=0;
01367   double x0p =-1;
01368   double x0Gas=-1;
01369   double x0=-1;
01370   d1    = previousNode->getDensity();
01371   x0p   = previousNode->getX0();
01372   d3    = tDet->getMaterial()->getDensity();
01373   x0    = tDet->getMaterial()->getX0();
01374 
01375 
01376   if (pL2> (pL1+pL3)) 
01377     {
01378       pL2=pL2-pL1-pL3;
01379       if (mgP.dx>0)
01380                                 {
01381                                         x0Gas = tDet->getGas()->getX0();
01382                                         d2    = tDet->getGas()->getDensity();
01383                                 }
01384       else
01385                                 {
01386                                         x0Gas = previousNode->getGasX0(); 
01387                                         d2    = previousNode->getGasDensity();
01388                                 }
01389       relRadThickness = 0.;
01390       dxEloss = 0;
01391       if (x0p>0.) 
01392         {
01393           relRadThickness += pL1/x0p;
01394           dxEloss += d1*pL1;
01395         }
01396       if (x0Gas>0.)
01397                                 {
01398                                         relRadThickness += pL2/x0Gas;
01399                                         dxEloss += d2*pL2;
01400                                 }
01401       if (x0>0.)
01402                                 {
01403                                         relRadThickness += pL3/x0;
01404                                         dxEloss += d3*pL3;
01405                                 }
01406     }
01407   else 
01408     {
01409       relRadThickness = 0.; 
01410       dxEloss = 0;
01411       if (x0p>0.) 
01412                                 {
01413                                         relRadThickness += pL1/x0p;
01414                                         dxEloss += d1*pL1;
01415                                 }
01416       if (x0>0.)
01417                                 {
01418                                         relRadThickness += pL3/x0;
01419                                         dxEloss += d3*pL3;
01420                                 }
01421     }
01422   if (pt > 0.350 && TMath::Abs(getHz()) < 1e-3) pt = 0.350;
01423   double p2=(1.+mFP.tanl()*mFP.tanl())*pt*pt;
01424   double m=StiKalmanTrackFinderParameters::instance()->massHypothesis();
01425   double m2=m*m;
01426   double e2=p2+m2;
01427   double beta2=p2/e2;
01428   //cout << " m2:"<<m2<<" p2:"<<p2<<" beta2:"<<beta2;
01429   double theta2=mcs2(relRadThickness,beta2,p2);
01430   //cout << " theta2:"<<theta2;
01431  double pti = mFP.ptin(), tanl = mFP.tanl(); 
01432 
01433  double cos2Li = (1.+ tanl*tanl);  // 1/cos(lamda)**2
01434  
01435  mFE._cEE += cos2Li             *theta2;
01436  mFE._cPP += tanl*tanl*pti*pti  *theta2;
01437  mFE._cTP += pti*tanl*cos2Li    *theta2;
01438  mFE._cTT += cos2Li*cos2Li      *theta2;
01439 
01440 #ifdef STI_ERROR_TEST
01441   testError(mFE.A,1);
01442 #endif // STI_ERROR_TEST
01443   double dE=0;
01444   double sign = (mgP.dx>0)? 1:-1;
01445 
01446 //  const static double I2Ar = (15.8*18) * (15.8*18) * 1e-18; // GeV**2
01447   StiElossCalculator * calculator = tDet->getElossCalculator();
01448   double eloss = calculator->calculate(1.,m, beta2);
01449   dE = sign*dxEloss*eloss;
01450   if (TMath::Abs(dE)>0)
01451     {
01452       if (debug()) {
01453         commentdEdx  = Form("%6.3g cm(%5.2f) %6.3g keV %6.3f GeV",mgP.dx,100*relRadThickness,1e6*dE,TMath::Sqrt(e2)-m); 
01454       }
01455       double correction =1. + ::sqrt(e2)*dE/p2;
01456       if (correction>1.1) correction = 1.1;
01457       else if (correction<0.9) correction = 0.9;
01458       mFP.curv() = mFP.curv()*correction;
01459       mFP.ptin() = mFP.ptin()*correction;
01460     }
01461     mPP() = mFP; mPE() = mFE;
01462 
01463 }
01464 
01465 //______________________________________________________________________________
01485 //______________________________________________________________________________
01486 int StiKalmanTrackNode::updateNode() 
01487 {
01488 static int nCall=0; nCall++;
01489   assert(fDerivTestOn!=-10 || _state>=kTNReady);
01490   _state = kTNFitBeg;
01491 #ifdef STI_ERROR_TEST
01492   testError(mFE.A,0);
01493 #endif //STI_ERROR_TEST
01494   assert(mFE._cXX<1e-8);
01495   double r00,r01,r11;
01496   r00 = mHrr.hYY + mFE._cYY;
01497   r01 = mHrr.hZY + mFE._cZY;
01498   r11 = mHrr.hZZ + mFE._cZZ;
01499 #ifdef Sti_DEBUG
01500   TRSymMatrix V(2,mHrr.hYY,
01501                   mHrr.hZY, mHrr.hZZ);  
01502   TRSymMatrix R1(2,r00,
01503                    r01, r11);
01504   static const TRMatrix H(2,5, 1., 0., 0., 0., 0.,
01505                                0., 1., 0., 0., 0.);
01506 #endif
01507   _det=r00*r11 - r01*r01;
01508   if (!finite(_det) || _det<(r00*r11)*1.e-5) {
01509     LOG_DEBUG << Form("StiKalmanTrackNode::updateNode *** zero determinant %g",_det)
01510     << endm;
01511     return -11;
01512   }
01513   // inverse matrix
01514   double tmp=r00; r00=r11/_det; r11=tmp/_det; r01=-r01/_det;
01515   // update error matrix
01516   double k00=mFE._cYY*r00+mFE._cZY*r01, k01=mFE._cYY*r01+mFE._cZY*r11;
01517   double k10=mFE._cZY*r00+mFE._cZZ*r01, k11=mFE._cZY*r01+mFE._cZZ*r11;
01518   double k20=mFE._cEY*r00+mFE._cEZ*r01, k21=mFE._cEY*r01+mFE._cEZ*r11;
01519   double k30=mFE._cPY*r00+mFE._cPZ*r01, k31=mFE._cPY*r01+mFE._cPZ*r11;
01520   double k40=mFE._cTY*r00+mFE._cTZ*r01, k41=mFE._cTY*r01+mFE._cTZ*r11;
01521   double dyt  = getHit()->y() - mFP.y();
01522   double dzt  = getHit()->z() - mFP.z();
01523   double dp3  = k30*dyt + k31*dzt;
01524   double dp2  = k20*dyt + k21*dzt;
01525   double dp4  = k40*dyt + k41*dzt;
01526 #ifdef Sti_DEBUG
01527   double dp0  = k00*dyt + k01*dz;
01528   double dp1  = k10*dyt + k11*dz;
01529   if (debug() & 4) {
01530     PrPP(updateNode,R1);
01531     PrPP(updateNode,V);
01532   }
01533   TRSymMatrix C(kNPars,mFE.A);  
01534   TRSymMatrix R(H,TRArray::kAxSxAT,C);
01535   R += V;
01536   TRSymMatrix G(R,TRArray::kInverted); 
01537   if (debug() & 4) {
01538     PrPP(updateNode,C);
01539     PrPP(updateNode,R);
01540     PrPP(updateNode,G);
01541   }
01542   // K = C * HT * G
01543   TRMatrix T(C,TRArray::kSxAT,H); 
01544   TRMatrix K(T,TRArray::kAxS,G);  
01545   TRMatrix K1(5,2,
01546               k00, k01,
01547               k10, k11,
01548               k20, k21,
01549               k30, k31,
01550               k40, k41);   
01551   if (debug() & 4) {
01552     PrPP(updateNode,T);
01553     PrPP(updateNode,K1);
01554     PrPP(updateNode,K);
01555     K1.Verify(K);
01556   }
01557   TRVector dR(2,dyt, dzt);
01558   TRVector dP1(5, dp0, dp1, dp2, dp3, dp4);
01559   TRVector dP(K,TRArray::kAxB,dR);
01560   if (debug() & 4) dP1.Verify(dP);//,1e-7,2);
01561 #endif
01562   double eta  = nice(mFP.eta() + dp2);
01563   if (fabs(eta)>kMaxEta) return -14;
01564   double pti  = mFP.ptin() + dp3;
01565   if (fabs(pti) < 1e-3) pti=1e-3;
01566   double cur  = pti*getHz();
01567   if (fabs(cur)>kMaxCur) return -16;
01568   assert(finite(cur));
01569   double tanl = mFP.tanl() + dp4;
01570   // update Kalman state
01571    double p0 = mFP.y() + k00*dyt + k01*dzt;
01572 //VP  mFP.y() += k00*dy + k01*dz;
01573   if (fabs(p0)>kMaxR) 
01574     {
01575       LOG_DEBUG << "updateNode()[1] -W- _y:"<<mFP.y()<<" _z:"<<mFP.z()<<endm;
01576       return -12;
01577     }
01578   double p1 = mFP.z() + k10*dyt + k11*dzt;
01579   if (fabs(p1)>kMaxZ) 
01580     {
01581       LOG_DEBUG << "updateNode()[2] -W- _y:"<<mFP.y()<<" _z:"<<mFP.z()<<endm;
01582       return -13;
01583     }
01584   //mFP.tanl() += k40*dyt + k41*dzt;
01585   double sinCA  =  sin(eta);
01586   // The following test introduces a track propagation error but happens
01587   // only when the track should be aborted so we don't care...
01588   mFP.y()  = p0;
01589   mFP.z()  = p1;
01590   mFP.eta()   = eta;
01591   mFP.ptin()  = pti;
01592   mFP.curv()  = cur;
01593   mFP.tanl()  = tanl;
01594   mFP._sinCA = sinCA;
01595   mFP._cosCA = ::sqrt((1.-mFP._sinCA)*(1.+mFP._sinCA)); 
01596   mFP.hz() = getHz();
01597 
01598 // update error matrix
01599   double c00=mFE._cYY;                       
01600   double c10=mFE._cZY, c11=mFE._cZZ;                 
01601   double c20=mFE._cEY, c21=mFE._cEZ;//, c22=mFE._cEE;           
01602   double c30=mFE._cPY, c31=mFE._cPZ;//, c32=mFE._cPE, c33=mFE._cPP;     
01603   double c40=mFE._cTY, c41=mFE._cTZ;//, c42=mFE._cTE, c43=mFE._cTP, c44=mFE._cTT;
01604   mFE._cYY-=k00*c00+k01*c10;
01605   mFE._cZY-=k10*c00+k11*c10;mFE._cZZ-=k10*c10+k11*c11;
01606   mFE._cEY-=k20*c00+k21*c10;mFE._cEZ-=k20*c10+k21*c11;mFE._cEE-=k20*c20+k21*c21;
01607   mFE._cPY-=k30*c00+k31*c10;mFE._cPZ-=k30*c10+k31*c11;mFE._cPE-=k30*c20+k31*c21;mFE._cPP-=k30*c30+k31*c31;
01608   mFE._cTY-=k40*c00+k41*c10;mFE._cTZ-=k40*c10+k41*c11;mFE._cTE-=k40*c20+k41*c21;mFE._cTP-=k40*c30+k41*c31;mFE._cTT-=k40*c40+k41*c41;
01609 
01610   if (mFE._cYY >= mHrr.hYY || mFE._cZZ >= mHrr.hZZ) {
01611     LOG_DEBUG << Form("StiKalmanTrackNode::updateNode *** _cYY >= hYY || _cZZ >= hZZ %g %g %g %g"
01612           ,mFE._cYY,mHrr.hYY,mFE._cZZ,mHrr.hZZ)<< endm;
01613     return -14;
01614   }
01615   if (mFE.check()) return -14;
01616 
01617 #ifdef Sti_DEBUG
01618   TRSymMatrix W(H,TRArray::kATxSxA,G); 
01619   TRSymMatrix C0(C);
01620   C0 -= TRSymMatrix(C,TRArray::kRxSxR,W);
01621   TRSymMatrix C1(kNPars,mFE.A);  
01622   if (debug() & 4) {
01623     PrPP(updateNode,W); 
01624     PrPP(updateNode,C0);
01625     PrPP(updateNode,C1);
01626     C1.Verify(C0);
01627   }
01628   //   update of the covariance matrix:
01629   //    C_k = (I - K_k * H_k) * C^k-1_k * (I - K_k * H_k)T + K_k * V_k * KT_k
01630   // P* C^k-1_k * PT
01631   TRMatrix A(K,TRArray::kAxB,H);
01632   TRMatrix P(TRArray::kUnit,kNPars);
01633   P -= A;
01634   TRSymMatrix C2(P,TRArray::kAxSxAT,C); 
01635   TRSymMatrix Y(K,TRArray::kAxSxAT,V);  
01636   C2 += Y;  
01637   if (debug() & 4) {
01638     PrPP(updateNode,C2); PrPP(updateNode,Y); PrPP(updateNode,C2);
01639     C2.Verify(C0);
01640     C2.Verify(C1);
01641   }
01642 #endif
01643   if (debug() & 8) PrintpT("U");
01644   _state = kTNFitEnd;
01645   return 0; 
01646 }
01647 
01648 //______________________________________________________________________________
01660 int StiKalmanTrackNode::rotate (double alpha) //throw ( Exception)
01661 {
01662   assert(fDerivTestOn!=-10 || _state>=kTNReady);
01663   mMtx().A[0][0]=0;
01664   if (fabs(alpha)<1.e-6) return 0;
01665   _state = kTNRotBeg;
01666   _alpha += alpha;
01667   _alpha = nice(_alpha);
01668     //cout << "    new  _alpha:"<< 180.*_alpha/3.1415927<<endl;
01669 
01670   double xt1=mFP.x(); 
01671   double yt1=mFP.y(); 
01672   mgP.sinCA1 = mFP._sinCA;
01673   mgP.cosCA1 = mFP._cosCA;
01674   double ca = cos(alpha);
01675   double sa = sin(alpha);
01676   mFP.x() = xt1*ca + yt1*sa;
01677   mFP.y()= -xt1*sa + yt1*ca;
01678   mFP._cosCA =  mgP.cosCA1*ca+mgP.sinCA1*sa;
01679   mFP._sinCA = -mgP.cosCA1*sa+mgP.sinCA1*ca;
01680    double nor = 0.5*(mFP._sinCA*mFP._sinCA+mFP._cosCA*mFP._cosCA +1);
01681   mFP._cosCA /= nor;
01682   mFP._sinCA /= nor;
01683 
01684   mFP.eta()= nice(mFP.eta()-alpha); /*VP*/
01685   mFP._sinCA = sin(mFP.eta());
01686   mFP._cosCA = cos(mFP.eta());
01687 #ifdef Sti_DEBUG  
01688   TRSymMatrix C(kNPars,mFE.A);
01689   if (debug() & 4) {PrPP(rotate,C);}
01690 #endif
01691 //cout << " mFP._sinCA:"<<mFP._sinCA<<endl;
01692   assert(fabs(mFP._sinCA)<=1.);
01693   assert(fabs(mFP._cosCA)<=1.);
01694   memset(mMtx().A,0,sizeof(mMtx()));
01695   mMtx().A[0][0]= ca-1;
01696   mMtx().A[0][1]= sa;
01697   mMtx().A[1][0]=-sa;
01698   mMtx().A[1][1]= ca-1;
01699 
01700   _state = kTNRotEnd;
01701   mPP() = mFP;
01702   return 0;
01703 }
01704 
01705 
01706 //_____________________________________________________________________________
01709 ostream& operator<<(ostream& os, const StiKalmanTrackNode& n)
01710 {
01711   const StiDetector *det = n.getDetector();
01712   if (det) os  <<"Det:"<<n.getDetector()->getName();
01713   else     os << "Det:UNknown";
01714   os << " a:" << 180*n._alpha/M_PI<<" degs"
01715      << "\tx:" << n.mFP.x()
01716      << " p0:" << n.mFP.y() 
01717      << " p1:" << n.mFP.z() 
01718      << " p2:" << n.mFP.eta() 
01719      << " p3:" << n.mFP.ptin() 
01720      << " p4:" << n.mFP.tanl()
01721      << " c00:" <<n.mFE._cYY<< " c11:"<<n.mFE._cZZ 
01722      << " pT:" << n.getPt() << endl;
01723   if (n.debug() & 2) {
01724     StiHit * hit = n.getHit();
01725     if (hit) os << "\thas hit with chi2 = " << n.getChi2()
01726                 << " n:"<<n.hitCount
01727                 << " null:"<<n.nullCount
01728                 << endl<<"\t hit:"<<*hit;
01729   }
01730   else os << endl;
01731   return os;
01732 }
01733 
01734 //______________________________________________________________________________
01735 double StiKalmanTrackNode::getWindowY()
01736 {         
01737   const StiDetector * detector = getDetector();
01738   const StiTrackingParameters * parameters = detector->getTrackingParameters();
01739   double searchWindowScale = parameters->getSearchWindowScale();
01740   double minSearchWindow   = parameters->getMinSearchWindow();
01741   double maxSearchWindow   = parameters->getMaxSearchWindow();
01742 
01743   const StiHitErrorCalculator * calc = detector->getHitErrorCalculator();
01744   double myEyy,myEzz;
01745   calc->calculateError(&mFP,myEyy,myEzz);
01746   double window = searchWindowScale*::sqrt(mFE._cYY+myEyy);
01747   if      (window<minSearchWindow) window = minSearchWindow;
01748   else if (window>maxSearchWindow) window = maxSearchWindow;
01749   return window;
01750 }
01751 
01752 //_____________________________________________________________________________
01753 double StiKalmanTrackNode::getWindowZ()
01754 {        
01755   const StiDetector * detector = getDetector();
01756   const StiTrackingParameters * parameters = detector->getTrackingParameters();
01757   double searchWindowScale = parameters->getSearchWindowScale();
01758   double minSearchWindow   = parameters->getMinSearchWindow();
01759   double maxSearchWindow   = parameters->getMaxSearchWindow();
01760 
01761   const StiHitErrorCalculator * calc = detector->getHitErrorCalculator();
01762   double myEyy,myEzz;
01763   calc->calculateError(&mFP,myEyy,myEzz);
01764   
01765   double window = searchWindowScale*::sqrt(mFE._cZZ+myEzz); 
01766   if      (window<minSearchWindow) window = minSearchWindow;
01767   else if (window>maxSearchWindow) window = maxSearchWindow;
01768   return window;
01769 }
01770 
01771 //______________________________________________________________________________
01772 StThreeVector<double> StiKalmanTrackNode::getHelixCenter() const
01773 {
01774   if (mFP.curv()==0) throw logic_error("StiKalmanTrackNode::getHelixCenter() -F- _curv==0 ");
01775   double xt0 = mFP.x()-mFP._sinCA/mFP.curv();   /*VP*/
01776   double yt0 = mFP.y()+mFP._cosCA/(mFP.curv());
01777   double zt0 = mFP.z()+mFP.tanl()*asin(mFP._sinCA)/mFP.curv();
01778   double cosAlpha = cos(_alpha);
01779   double sinAlpha = sin(_alpha);
01780   return (StThreeVector<double>(cosAlpha*xt0-sinAlpha*yt0,sinAlpha*xt0+cosAlpha*yt0,zt0));
01781 }
01782 //______________________________________________________________________________
01783 int StiKalmanTrackNode::locate()
01784 {
01785   enum {kNStd = 5};
01786   int position;
01787   double yOff, yAbsOff, detHW, detHD,edge,innerY, outerY, innerZ, outerZ, zOff, zAbsOff;
01788   //fast way out for projections going out of fiducial volume
01789   const StiDetector *tDet = getDetector();
01790   if (!tDet) return 0;
01791   const StiPlacement *place = tDet->getPlacement();
01792   const StiShape     *sh    = tDet->getShape();
01793 
01794   if (fabs(mFP.z())>kMaxZ || fabs(mFP.y())> kMaxR) return -1;
01795   
01796 #ifndef DO_TPCCATRACKER // insensible region on a detector plane
01797   edge  = 2.;
01798   if (mFP.x()<50.)      edge  = 0.3;
01799 #else /* DO_TPCCATRACKER */
01800   edge = 0.;
01801 #endif /* !DO_TPCCATRACKER */
01802   
01803   //YF edge is tolerance when we consider that detector is hit. //  edge = 0; //VP the meaning of edge is not clear
01804   Int_t shapeCode  = sh->getShapeCode();
01805   switch (shapeCode) {
01806   case kDisk:
01807   case kCylindrical: // cylinder
01808     yOff    = nice(_alpha - place->getLayerAngle());
01809     yAbsOff = fabs(yOff);
01810     yAbsOff -=kNStd*sqrt((mFE._cXX+mFE._cYY)/(mFP.x()*mFP.x()+mFP.y()*mFP.y()));
01811     if (yAbsOff<0) yAbsOff=0;
01812     detHW = ((StiCylindricalShape *) sh)->getOpeningAngle()/2.;
01813     innerY = outerY = detHW;
01814     break;
01815   case kPlanar: 
01816   default:
01817     yOff = mFP.y() - place->getNormalYoffset();
01818     yAbsOff = fabs(yOff) - kNStd*sqrt(mFE._cYY);
01819     if (yAbsOff<0) yAbsOff=0;
01820     detHW = sh->getHalfWidth();
01821     innerY = detHW - edge;
01822     //outerY = innerY + 2*edge;
01823     //outerZ = innerZ + 2*edge;
01824     outerY = innerY + edge;
01825     break;
01826   }
01827   zOff = mFP.z() - place->getZcenter();
01828   zAbsOff = fabs(zOff);
01829   detHD = sh->getHalfDepth();
01830   innerZ = detHD - edge;
01831   outerZ = innerZ + edge;
01832   if (yAbsOff<innerY && zAbsOff<innerZ)
01833     position = kHit; 
01834   else if (yAbsOff>outerY && (yAbsOff-outerY)>(zAbsOff-outerZ))
01835     // outside detector to positive or negative y (phi)
01836     // if the track is essentially tangent to the plane, terminate it.
01837       position = yOff>0 ? kMissPhiPlus : kMissPhiMinus;
01838   else if (zAbsOff>outerZ && (zAbsOff-outerZ)>(yAbsOff-outerY))
01839     // outside detector to positive or negative z (west or east)
01840     position = zOff>0 ? kMissZplus : kMissZminus;
01841   else if ((yAbsOff-innerY)>(zAbsOff-innerZ))
01842     // positive or negative phi edge
01843     position = yOff>0 ? kEdgePhiPlus : kEdgePhiMinus;
01844   else
01845     // positive or negative z edge
01846     position = zOff>0 ? kEdgeZplus : kEdgeZminus;
01847   if (debug()&8) {
01848     comment += ::Form("R %8.3f y/z %8.3f/%8.3f", 
01849                       mFP.x(), mFP.y(), mFP.z());
01850     if (position>kEdgeZplus || position<0)  
01851       comment += ::Form(" missed %2d y0/z0 %8.3f/%8.3f dY/dZ %8.3f/%8.3f",
01852                         position, yOff, zOff, detHW, detHD);
01853   }
01854   return position;
01855  }
01856 //______________________________________________________________________________
01857 void StiKalmanTrackNode::initialize(StiHit *h)
01858 {
01859   reset();
01860   setHit(h);
01861   _detector = h->detector();
01862   _alpha   = _detector->getPlacement()->getNormalRefAngle(); 
01863   mFP._sinCA = 0.6;
01864   mFP._cosCA = 0.8;
01865   mFP.x() = h->x();
01866   mFP.y() = h->y();
01867   mFP.z() = h->z();
01868   mFP.hz() = getHz();
01869   resetError();
01870   mPP() = mFP;
01871   setHitErrors();
01872   _state = kTNInit;
01873   setChi2(0.1);
01874 }
01875 //______________________________________________________________________________
01876 void StiKalmanTrackNode::initialize(StiDetector *d)
01877 {
01878   reset();
01879   _detector = d;
01880   _alpha   = _detector->getPlacement()->getNormalRefAngle(); 
01881   _state = kTNInit;
01882   setChi2(1e10);
01883 }
01884 //______________________________________________________________________________
01885 StiKalmanTrackNode::StiKalmanTrackNode(const StiKalmanTrackNode &n)
01886 {
01887    _ext=0; _inf=0 ; *this = n;
01888 }
01889 //______________________________________________________________________________
01890 const StiKalmanTrackNode& StiKalmanTrackNode::operator=(const StiKalmanTrackNode &n)
01891 {
01892   StiTrackNode::operator=(n);
01893   memcpy(_beg,n._beg,_end-_beg+1);
01894   if (n._ext) { extend();*_ext = *n._ext;}
01895   else        { if(_ext) _ext->reset();  }
01896   if (n._inf) { extinf();*_inf = *n._inf;}
01897   else        { if(_inf) {BFactory::Free(_inf); _inf=0;}}
01898   return *this;
01899 }
01900 //______________________________________________________________________________
01901 void StiKalmanTrackNode::setHitErrors(const StiHit *hit)
01902 {
01903   if (!hit) hit = _hit;
01904   assert(hit);
01905   StiTrackNodeHelper::getHitErrors(hit,&mFP,&mHrr);
01906 }
01907 //______________________________________________________________________________
01908 StiHitErrs StiKalmanTrackNode::getGlobalHitErrs(const StiHit *hit) const
01909 {
01910    StiHitErrs hr;
01911    StiTrackNodeHelper::getHitErrors(hit,&mFP,&hr);
01912    hr.rotate(_alpha);
01913    return hr;
01914 }
01915 #if 1
01916 //______________________________________________________________________________
01917 int StiKalmanTrackNode::testError(double *emx, int begend)
01918 {
01919 // Test and correct error matrix. Output : number of fixes
01920 // DO NOT IMPROVE weird if() here. This accounts NaN
01921 
01922 
01923   static int nCall=0; nCall++;
01924   static const double dia[6] = { 1000.,1000., 1000.,1000.,1000,1000.};
01925   static double emxBeg[kNErrs];
01926 //return 0;
01927   if (!begend) { memcpy(emxBeg,emx,sizeof(emxBeg));}
01928   int ians=0,j1,j2,jj;
01929   for (j1=0; j1<5;j1++){
01930     jj = idx55[j1][j1];
01931     if (!(emx[jj]>0)) {;
01932       LOG_DEBUG << Form("<StiKalmanTrackNode::testError> Negative diag %g[%d][%d]",emx[jj],j1,j1)
01933       << endm;
01934         continue;}
01935     if (emx[jj]<=10*dia[j1] )   continue;
01936     assert(finite(emx[jj]));
01937     LOG_DEBUG << Form("<StiKalmanTrackNode::testError> Huge diag %g[%d][%d]",emx[jj],j1,j1)
01938     << endm;
01939                                 continue;
01940 //    ians++; emx[jj]=dia[j1];
01941 //    for (j2=0; j2<5;j2++){if (j1!=j2) emx[idx55[j1][j2]]=0;}
01942   }
01943   for (j1=0; j1< 5;j1++){
01944   for (j2=0; j2<j1;j2++){
01945     jj = idx55[j1][j2];
01946     assert(finite(emx[jj]));
01947     double cormax = emx[idx55[j1][j1]]*emx[idx55[j2][j2]];
01948     if (emx[jj]*emx[jj]<cormax) continue;
01949     cormax= sqrt(cormax);
01950 //    ians++;emx[jj]= (emx[jj]<0) ? -cormax:cormax;
01951   }}
01952   return ians;
01953 }
01954 #endif//0
01955 //______________________________________________________________________________
01956 void StiKalmanTrackNode::numeDeriv(double val,int kind,int shape,int dir)
01957 {
01958    double maxStep[kNPars]={0.1,0.1,0.1,0.01,0.001,0.01};
01959    if (fDerivTestOn<0) return;
01960    gCurrShape = shape;
01961    fDerivTestOn=-1;
01962    double save[20];
01963    StiKalmanTrackNode myNode;
01964    double *pars = &myNode.mFP.x();
01965    int state=0;
01966    saveStatics(save);
01967    if (fabs(mFP.curv())> 0.02) goto FAIL;
01968    int ipar;
01969    for (ipar=1;ipar<kNPars;ipar++)
01970    {
01971      for (int is=-1;is<=1;is+=2) {
01972        myNode = *this;
01973        backStatics(save);
01974        double step = 0.1*sqrt((mFE.A)[idx66[ipar][ipar]]);
01975        if (step>maxStep[ipar]) step = maxStep[ipar];
01976 //       if (step>0.1*fabs(pars[ipar])) step = 0.1*pars[ipar];
01977 //       if (fabs(step)<1.e-7) step = 1.e-7;
01978        pars[ipar] +=step*is;
01979 //              Update sinCA & cosCA       
01980        myNode.mFP._sinCA = sin(myNode.mFP.eta());
01981        if (fabs(myNode.mFP._sinCA) > 0.9) goto FAIL;
01982        myNode.mFP._cosCA = cos(myNode.mFP.eta());
01983        
01984        switch (kind) {
01985          case 1: //propagate
01986            state = myNode.propagate(val,shape,dir); break;
01987          case 2: //rotate
01988            state = myNode.rotate(val);break;
01989          default: assert(0);  
01990        }  
01991        if(state  ) goto FAIL;
01992 
01993        for (int jpar=1;jpar<kNPars;jpar++) {
01994          if (is<0) {
01995            fDerivTest[jpar][ipar]= pars[jpar];
01996          } else      {
01997            double tmp = fDerivTest[jpar][ipar];
01998            fDerivTest[jpar][ipar] = (pars[jpar]-tmp)/(2.*step);
01999            if (ipar==jpar) fDerivTest[jpar][ipar]-=1.;
02000          }
02001        }
02002      }
02003    }
02004    fDerivTestOn=1;backStatics(save);return;
02005 FAIL: 
02006    fDerivTestOn=0;backStatics(save);return;
02007 }
02008 //______________________________________________________________________________
02009 int StiKalmanTrackNode::testDeriv(double *der)
02010 {
02011    if (fDerivTestOn!=1) return 0;
02012    double *num = fDerivTest[0];
02013    int nerr = 0;
02014    for (int i=1;i<kNErrs;i++) {
02015      int ipar = i/kNPars;
02016      int jpar = i%kNPars;
02017      if (ipar==jpar)                                    continue;
02018      if (ipar==0)                                       continue;
02019      if (jpar==0)                                       continue;
02020      double dif = fabs(num[i]-der[i]);
02021      if (fabs(dif) <= 1.e-5)                            continue;
02022      if (fabs(dif) <= 0.2*0.5*fabs(num[i]+der[i]))      continue;
02023      nerr++;
02024      LOG_DEBUG << Form("***Wrong deriv [%d][%d] = %g %g %g",ipar,jpar,num[i],der[i],dif)
02025      << endm;
02026   }
02027   fDerivTestOn=0;
02028   return nerr;
02029 }
02030 //______________________________________________________________________________
02031 void StiKalmanTrackNode::saveStatics(double *sav)
02032 {  
02033   sav[ 0]=mgP.x1;
02034   sav[ 1]=mgP.x2; 
02035   sav[ 2]=mgP.y1; 
02036   sav[ 3]=mgP.y2; 
02037   sav[ 5]=mgP.dx; 
02038   sav[ 6]=mgP.cosCA1; 
02039   sav[ 7]=mgP.sinCA1; 
02040   sav[ 8]=mgP.cosCA2; 
02041   sav[ 9]=mgP.sinCA2; 
02042   sav[10]=mgP.sumSin; 
02043   sav[11]=mgP.sumCos; 
02044   sav[14]=mgP.dl; 
02045   sav[15]=mgP.dl0; 
02046   sav[16]=mgP.dy; 
02047 }  
02048 //______________________________________________________________________________
02049 void StiKalmanTrackNode::backStatics(double *sav)
02050 {  
02051   mgP.x1=             sav[ 0];
02052   mgP.x2=                 sav[ 1]; 
02053   mgP.y1=                 sav[ 2]; 
02054   mgP.y2=                 sav[ 3]; 
02055   mgP.dx=         sav[ 5]; 
02056   mgP.cosCA1=     sav[ 6]; 
02057   mgP.sinCA1=     sav[ 7]; 
02058   mgP.cosCA2=     sav[ 8]; 
02059   mgP.sinCA2=     sav[ 9]; 
02060   mgP.sumSin=     sav[10]; 
02061   mgP.sumCos=     sav[11]; 
02062   mgP.dl=             sav[14];
02063   mgP.dl0=            sav[15];
02064   mgP.dy=             sav[16];
02065 }
02066 //________________________________________________________________________________
02067 void   StiKalmanTrackNode::PrintpT(const Char_t *opt) const {
02068   // opt = "E" extapolation
02069   //       "M" Multiple scattering
02070   //       "V" at Vertex
02071   //       "B" at beam
02072   //       "R" at Radius
02073   //       "U" Updated
02074   //       mFP fit parameters
02075   //       mFE fit errors
02076   //       _ext->mPP 
02077   //       _ext->mPE
02078   //       _ext->mMtx
02079   Double_t dpTOverpT = 100*TMath::Sqrt(mFE._cPP/(mFP.ptin()*mFP.ptin()));
02080   if (dpTOverpT > 9999.9) dpTOverpT = 9999.9;
02081   comment += ::Form(" %s pT %8.3f+-%6.1f sy %6.4f",opt,getPt(),dpTOverpT,TMath::Sqrt(mFE._cYY));
02082 }
02083 //________________________________________________________________________________
02084 void StiKalmanTrackNode::PrintStep() {
02085   LOG_INFO << comment << "\t" << commentdEdx << endm;
02086   ResetComment();
02087 }
02088 //________________________________________________________________________________
02089 int   StiKalmanTrackNode::print(const char *opt) const
02090 {
02091 static const char *txt = "xyzeptchXYZEPTCH";
02092 static const char *hhh = "uvwUVW";
02093 static const char *HHH = "xyzXYZ";
02094   if (!opt || !opt[0]) opt = "2xh";
02095   StiHit *hit = getHit();
02096   if (strchr(opt,'h') && !hit)  return 0;
02097   TString ts;
02098   if (!isValid()) ts+="*";
02099   if (hit) {ts+=(getChi2()>1e3)? "h":"H";}
02100   printf("%p(%s)",(void*)this,ts.Data());
02101   if (strchr(opt,'2')) printf("\t%s=%g","ch2",getChi2());
02102 
02103   for (int i=0;txt[i];i++) {
02104     if (!strchr(opt,txt[i])) continue;
02105     int j = i%8;
02106     double val=0,err=0;
02107     if (i<=8 || i>11) { //local coord
02108       val = mFP[j];
02109       int jj = idx66[j][j];
02110       err=0;
02111       if (j<6) {err = sqrt(fabs(mFE.A[jj])); if (mFE.A[jj]<0) err*= -1;}
02112     } else {//global coordinate
02113       switch (j) {
02114         case 0: val = x_g();    break;
02115         case 1: val = y_g();    break;
02116         case 2: val = z_g();    break;
02117         case 3: val = getPsi(); break;
02118     } }
02119     printf("\t%c=%g",txt[i],val);
02120     if (err) printf("(%6.1g)",err);
02121   }//end for i
02122 
02123   for (int i=0;hit && hhh[i];i++) {
02124     if (!strchr(opt,hhh[i])) continue;
02125     double val=0,err=0;
02126     switch(i) {
02127       case 0:val = hit->x();    break;
02128       case 1:val = hit->y();    err = ::sqrt(getEyy());break;
02129       case 2:val = hit->z();    err = ::sqrt(getEzz());break;
02130       case 3:val = hit->x_g();  break;
02131       case 4:val = hit->y_g();  break;
02132       case 5:val = hit->z_g();  err = ::sqrt(getEzz());break;
02133     }
02134     printf("\th%c=%g",HHH[i],val);
02135     if (err) printf("(%6.1g)",err);
02136   }
02137   printf("\n");
02138   return 1;
02139 }    
02140 //________________________________________________________________________________
02141 StiNodeExt *StiKalmanTrackNode::nodeExtInstance()
02142 {    
02143 static StiFactory<StiNodeExt,StiNodeExt> *extFactory=0;
02144   if (!extFactory) {
02145     extFactory = StiFactory<StiNodeExt,StiNodeExt>::myInstance();
02146     extFactory->setMaxIncrementCount(400000);
02147     extFactory->setFastDelete();
02148   }
02149   return extFactory->getInstance();
02150 }    
02151 //________________________________________________________________________________
02152 StiNodeInf *StiKalmanTrackNode::nodeInfInstance()
02153 {    
02154 static StiFactory<StiNodeInf,StiNodeInf> *infFactory=0;
02155   if (!infFactory) {
02156     infFactory = StiFactory<StiNodeInf,StiNodeInf>::myInstance();
02157     infFactory->setMaxIncrementCount(40000);
02158     infFactory->setFastDelete();
02159   }
02160   return infFactory->getInstance();
02161 }    
02162 //________________________________________________________________________________
02163 void StiKalmanTrackNode::extend()
02164 {
02165   if(_ext) return;
02166   _ext =  nodeExtInstance();
02167 }
02168 //________________________________________________________________________________
02169 void StiKalmanTrackNode::extinf()
02170 {
02171   if(_inf) return;
02172   _inf =  nodeInfInstance();
02173 }
02174 //________________________________________________________________________________
02175 void StiKalmanTrackNode::saveInfo(int kase)
02176 {
02177   if (kase){};
02178   extinf();
02179   _inf->mPP = mPP();
02180   _inf->mPE = mPE();
02181   _inf->mHrr = mHrr;
02182 }
02183 
02184 //________________________________________________________________________________
02185 void StiKalmanTrackNode::reduce()
02186 {
02187   if(!_ext) return;
02188   BFactory::Free(_ext); _ext=0;
02189 }
02190 
02191 
02192 //________________________________________________________________________________
02193 StThreeVector<double> StiKalmanTrackNode::getPoint() const
02194 {
02195   return StThreeVector<double>(mFP.x(),mFP.y(),mFP.z());
02196 }
02197 
02198 //________________________________________________________________________________
02199 StThreeVector<double> StiKalmanTrackNode::getGlobalPoint() const
02200 {
02201   double ca = cos(_alpha),sa=sin(_alpha);
02202   return StThreeVector<double>(ca*mFP.x()-sa*mFP.y(), sa*mFP.x()+ca*mFP.y(), mFP.z());
02203 }
02204 
02205 //________________________________________________________________________________
02206 double StiKalmanTrackNode::x_g() const
02207 {
02208   return cos(_alpha)*mFP.x()-sin(_alpha)*mFP.y();
02209 }
02210 
02211 //________________________________________________________________________________
02212 double StiKalmanTrackNode::y_g() const
02213 {
02214   return sin(_alpha)*mFP.x()+cos(_alpha)*mFP.y();
02215 }
02216 
02217 //________________________________________________________________________________
02218 double StiKalmanTrackNode::z_g() const
02219 {
02220   return mFP.z();
02221 }
02222 
02223 //________________________________________________________________________________
02224 void StiKalmanTrackNode::setUntouched() 
02225 {
02226   mUnTouch.set(mPP(),mPE());
02227 }
02228 //________________________________________________________________________________
02229 double StiKalmanTrackNode::getTime() const
02230 {
02231   static const double smax = 1e3; 
02232   double time = 0;
02233   if (! _laser) {
02234     double d = sqrt(mFP.x()*mFP.x()+mFP.y()*mFP.y());
02235     double sn = fabs(mFP._cosCA*mFP.y() - mFP._sinCA*mFP.x())/d;
02236     if (sn> 0.99) sn =  0.99;
02237     if (sn<0.2) {
02238       d *= (1.+sn*sn/6);
02239     } else {
02240       d *= asin(sn)/sn;
02241     }
02242     d *= sqrt(1.+mFP.tanl()*mFP.tanl());
02243     double beta = 1;   
02244     double pt = fabs(mFP.ptin());
02245     if (pt>0.1) {
02246       pt = 1./pt;
02247       double p2=(1.+mFP.tanl()*mFP.tanl())*pt*pt;
02248       double m=StiKalmanTrackFinderParameters::instance()->massHypothesis();
02249       double m2=m*m;
02250       double e2=p2+m2;
02251       double beta2=p2/e2;
02252       beta = TMath::Sqrt(beta2);
02253     }
02254     time = d/(TMath::Ccgs()*beta*1e-6); // mksec  
02255   } else {
02256     if (TMath::Abs(mFP.z()) > 20.0) {
02257 static const double Radius = 197.;
02258 static const int    nsurf  = 6;
02259 static const double surf[6] = {-Radius*Radius, 0, 0, 0, 1, 1};
02260       double dir[3] = {mFP._cosCA,mFP._sinCA,mFP.tanl()};
02261       THelixTrack tc(mFP.P,dir,mFP.curv());
02262       double s = tc.Step(smax, surf, nsurf,0,0,1);
02263       if (TMath::Abs(s) < smax) 
02264         time = TMath::Abs(s)/(TMath::Ccgs()*1e-6); // mksec
02265     }
02266   }
02267   return time;
02268 }
02269