From 2cafa38fb992052660f4bbd1548b6ab28d40ce0f Mon Sep 17 00:00:00 2001
From: Chengdong Fu <fucd@ihep.ac.cn>
Date: Sun, 19 Jul 2020 15:25:26 +0800
Subject: [PATCH] first import MarlinTrk as service of CEPCSW
---
Service/TrackSystemSvc/CMakeLists.txt | 36 +
.../TrackSystemSvc/ConfigFlags.h | 94 +
.../TrackSystemSvc/TrackSystemSvc/HelixFit.h | 57 +
.../TrackSystemSvc/HelixTrack.h | 62 +
.../TrackSystemSvc/IMarlinTrack.h | 232 +++
.../TrackSystemSvc/IMarlinTrkSystem.h | 105 +
.../TrackSystemSvc/ITrackSystemSvc.h | 21 +
.../LCIOTrackPropagators.h.remove | 42 +
.../MarlinTrkDiagnostics.h.remove | 31 +
.../TrackSystemSvc/MarlinTrkUtils.h | 86 +
.../src/DiagnosticsController.cc.remove | 883 +++++++++
.../src/DiagnosticsController.h.remove | 90 +
Service/TrackSystemSvc/src/HelixFit.cc | 577 ++++++
Service/TrackSystemSvc/src/HelixTrack.cc | 117 ++
Service/TrackSystemSvc/src/IMarlinTrack.cc | 38 +
.../TrackSystemSvc/src/IMarlinTrkSystem.cc | 33 +
.../src/LCIOTrackPropagators.cc.remove | 489 +++++
Service/TrackSystemSvc/src/MarlinKalTest.cc | 520 +++++
Service/TrackSystemSvc/src/MarlinKalTest.h | 104 +
.../TrackSystemSvc/src/MarlinKalTestTrack.cc | 1711 +++++++++++++++++
.../TrackSystemSvc/src/MarlinKalTestTrack.h | 360 ++++
.../src/MarlinTrkDiagnostics.cc.remove | 63 +
Service/TrackSystemSvc/src/MarlinTrkUtils.cc | 784 ++++++++
Service/TrackSystemSvc/src/TrackSystemSvc.cpp | 72 +
Service/TrackSystemSvc/src/TrackSystemSvc.h | 22 +
25 files changed, 6629 insertions(+)
create mode 100644 Service/TrackSystemSvc/CMakeLists.txt
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/ConfigFlags.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/HelixFit.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/HelixTrack.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrack.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrkSystem.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/ITrackSystemSvc.h
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/LCIOTrackPropagators.h.remove
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkDiagnostics.h.remove
create mode 100644 Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkUtils.h
create mode 100644 Service/TrackSystemSvc/src/DiagnosticsController.cc.remove
create mode 100644 Service/TrackSystemSvc/src/DiagnosticsController.h.remove
create mode 100644 Service/TrackSystemSvc/src/HelixFit.cc
create mode 100644 Service/TrackSystemSvc/src/HelixTrack.cc
create mode 100644 Service/TrackSystemSvc/src/IMarlinTrack.cc
create mode 100644 Service/TrackSystemSvc/src/IMarlinTrkSystem.cc
create mode 100644 Service/TrackSystemSvc/src/LCIOTrackPropagators.cc.remove
create mode 100644 Service/TrackSystemSvc/src/MarlinKalTest.cc
create mode 100644 Service/TrackSystemSvc/src/MarlinKalTest.h
create mode 100644 Service/TrackSystemSvc/src/MarlinKalTestTrack.cc
create mode 100644 Service/TrackSystemSvc/src/MarlinKalTestTrack.h
create mode 100644 Service/TrackSystemSvc/src/MarlinTrkDiagnostics.cc.remove
create mode 100644 Service/TrackSystemSvc/src/MarlinTrkUtils.cc
create mode 100644 Service/TrackSystemSvc/src/TrackSystemSvc.cpp
create mode 100644 Service/TrackSystemSvc/src/TrackSystemSvc.h
diff --git a/Service/TrackSystemSvc/CMakeLists.txt b/Service/TrackSystemSvc/CMakeLists.txt
new file mode 100644
index 00000000..f441f576
--- /dev/null
+++ b/Service/TrackSystemSvc/CMakeLists.txt
@@ -0,0 +1,36 @@
+gaudi_subdir(TrackSystemSvc v0r0)
+
+find_package(ROOT 6.14 REQUIRED COMPONENTS Matrix Physics)
+find_package(CLHEP REQUIRED)
+find_package(GEAR REQUIRED)
+find_package(LCIO REQUIRED)
+find_package(EDM4HEP REQUIRED)
+find_package(KalTest REQUIRED)
+find_package(KalDet REQUIRED)
+#find_package(streamlog REQUIRED)
+
+gaudi_depends_on_subdirs(Service/GearSvc Detector/DetInterface)
+
+set(TrackSystemSvc_srcs src/*.cpp)
+set(TrackSystemSvcLib_srcs src/*.cc)
+
+
+gaudi_install_headers(TrackSystemSvc)
+
+message( "${KalDet_INCLUDE_DIRS}" )
+message( "${KalDet_LIBRARIES}" )
+
+gaudi_add_library(TrackSystemSvcLib ${TrackSystemSvcLib_srcs}
+ PUBLIC_HEADERS TrackSystemSvc
+ INCLUDE_DIRS GaudiKernel ROOT CLHEP gear ${LCIO_INCLUDE_DIRS} ${KalDet_INCLUDE_DIRS} ${KalTest_INCLUDE_DIRS} ${EDM4HEP_INCLUDE_DIRS}
+ LINK_LIBRARIES GaudiKernel ROOT CLHEP $ENV{GEAR}/lib/libgear.so $ENV{GEAR}/lib/libgearsurf.so ${LCIO_LIBRARIES} ${KalDet_LIBRARIES} ${KalTest_LIBRARIES} -Wl,--no-as-needed
+ EDM4HEP::edm4hep EDM4HEP::edm4hepDict
+ -Wl,--as-needed
+)
+
+gaudi_add_module(TrackSystemSvc ${TrackSystemSvc_srcs}
+ INCLUDE_DIRS GaudiKernel ROOT CLHEP gear ${LCIO_INCLUDE_DIRS} ${KalDet_INCLUDE_DIRS} ${KalTest_INCLUDE_DIRS} ${EDM4HEP_INCLUDE_DIRS}
+ LINK_LIBRARIES TrackSystemSvcLib GaudiKernel ROOT CLHEP $ENV{GEAR}/lib/libgear.so $ENV{GEAR}/lib/libgearsurf.so ${LCIO_LIBRARIES} ${KalDet_LIBRARIES} ${KalTest_LIBRARIES} -Wl,--no-as-needed
+ EDM4HEP::edm4hep EDM4HEP::edm4hepDict
+ -Wl,--as-needed
+)
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/ConfigFlags.h b/Service/TrackSystemSvc/TrackSystemSvc/ConfigFlags.h
new file mode 100644
index 00000000..bba19752
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/ConfigFlags.h
@@ -0,0 +1,94 @@
+#ifndef ConfigFlags_h
+#define ConfigFlags_h
+
+#include <string>
+#include <iostream>
+#include <exception>
+#include <map>
+
+namespace MarlinTrk {
+
+ class ConfigFlags ;
+
+ inline std::ostream& operator<<(std::ostream& os, const ConfigFlags& cf) ;
+
+ class ConfigFlags{
+
+ friend std::ostream& operator<<(std::ostream& os, const ConfigFlags& flags) ;
+
+ typedef std::pair<std::string, bool> Flag ;
+ typedef std::map< unsigned, Flag > Map ;
+
+
+ public:
+
+ /** Helper class that holds a number of boolean properties for configuration.
+ * The property keys (type: unsigned) have to be defined in the class using
+ * this class.
+ */
+ ConfigFlags() {}
+
+ ~ConfigFlags(){}
+
+
+ void registerOption( unsigned key, const std::string& name, bool defaultValue=false ){
+
+ _map[ key ] = std::make_pair( name , defaultValue ) ;
+ }
+
+ bool option(unsigned key) const {
+
+ Map::const_iterator it = _map.find( key ) ;
+
+ if( it == _map.end() )
+ return false ;
+
+ return it->second.second ;
+ }
+
+ bool operator[](unsigned key) const {
+ return option( key ) ;
+ }
+
+ void setOption(unsigned key , bool val) {
+
+ Map::iterator it = _map.find( key ) ;
+
+ if( it !=_map.end() )
+ it->second.second = val ;
+ }
+
+
+ std::string& optionName(unsigned key) {
+
+ static std::string empty("UNKNOWN") ;
+
+ Map::iterator it = _map.find( key ) ;
+
+ if( it == _map.end() )
+ return empty ;
+
+ return it->second.first ;
+ }
+
+ protected:
+ Map _map ;
+
+ };
+
+
+ inline std::ostream& operator<<(std::ostream& os, const MarlinTrk::ConfigFlags& cf) {
+
+ for( ConfigFlags::Map::const_iterator it = cf._map.begin(); it != cf._map.end() ; ++it){
+
+ os << " option: " << it->second.first << "\t: " << it->second.second << std::endl ;
+ }
+
+ return os ;
+ }
+
+} // namespace
+
+
+#endif
+
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/HelixFit.h b/Service/TrackSystemSvc/TrackSystemSvc/HelixFit.h
new file mode 100644
index 00000000..6497531b
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/HelixFit.h
@@ -0,0 +1,57 @@
+#ifndef __HELIXFIT__
+#define __HELIXFIT__
+//
+// HelixFit.h
+// MarlinTrk
+//
+
+/**
+ // Created by Steve Aplin on 9/16/11.
+ // DESY
+ //
+ // C++ rewrite of the aleph Fortran routine TFITHL
+ //
+ //! Fast helix fit
+ //
+ //
+ // Input: NPT Number of 3-D points to be fit
+ // xf Array of X-values of points to be fit
+ // yf Array of Y-values of points to be fit
+ // zf Array of Z-values of points to be fit
+ // wf Array of 1/(sig(rphi))**2 for each point
+ // wzf Array of 1/(sig(z))**2 for each point
+ // iopt < 3 : error matrix calculated
+ // = 3 : 3-dimensional iteration
+ //
+ // OUTPUT: vv0 = Helix parameter in perigee form
+ // ee0 = INVERSE OF ERROR MATRIX IN TRIANG. FORM
+ // chi2ph = CHI SQUARED = SUM (PHI DEVIATIONS/ERRORS)**2
+ // CH2Z = CHI SQUARED = SUM (Z DEVIATIONS/ERRORS)**2
+ // NOTE: DEGREES OF FREEDOM = 2*NPT-5
+ //----------------------------------------------------------------
+ // BASED ON SUBROUTINE CIRCLE
+ // REFERENCE: COMPUTER PHYSICS COMMUNICATIONS VOL 33,P329
+ //
+ // AUTHORS: N. CHERNOV, G. OSOSKOV & M. POPPE
+ // Modified by: Fred Weber, 8 Jun 1989
+ // Modified by: M.Cattaneo, 27-Jan-1998
+ // Protect against arg SIN > 1.0
+ //
+ //-----------------------------------------------------------------
+ */
+
+namespace MarlinTrk {
+
+ class HelixFit {
+
+
+ public:
+
+ int fastHelixFit(int npt, double* xf, double* yf, float* rf, float* pf, double* wf, float* zf , float* wzf, int iopt,
+ float* vv0, float* ee0, float& ch2ph, float& ch2z);
+
+ };
+
+}
+
+#endif
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/HelixTrack.h b/Service/TrackSystemSvc/TrackSystemSvc/HelixTrack.h
new file mode 100644
index 00000000..0f049621
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/HelixTrack.h
@@ -0,0 +1,62 @@
+#ifndef HelixTrack_h
+#define HelixTrack_h
+
+#include <cmath>
+
+namespace edm4hep{
+ class Vector3d;
+}
+
+class HelixTrack {
+
+public:
+
+ HelixTrack( double ref_point_x, double ref_point_y, double ref_point_z, double d0, double z0, double phi0, double omega, double tanLambda )
+ : _ref_point_x(ref_point_x), _ref_point_y(ref_point_y), _ref_point_z(ref_point_z), _d0(d0), _z0(z0), _phi0(phi0), _omega(omega), _tanLambda(tanLambda)
+ {
+ while ( _phi0 < -M_PI ) _phi0 += 2.0*M_PI ;
+ while ( _phi0 >= M_PI ) _phi0 -= 2.0*M_PI;
+ }
+
+ HelixTrack( const edm4hep::Vector3d& x1, const edm4hep::Vector3d& x2, const edm4hep::Vector3d& x3, double Bz, bool direction );
+
+ HelixTrack( const edm4hep::Vector3d& position, const edm4hep::Vector3d& p, double charge, double Bz ) ;
+
+ double moveRefPoint( double x, double y, double z) ;
+
+ double getRefPointX() const { return _ref_point_x ; }
+ double getRefPointY() const { return _ref_point_y ; }
+ double getRefPointZ() const { return _ref_point_z ; }
+ double getD0() const { return _d0 ; }
+ double getZ0() const { return _z0 ; }
+ double getPhi0() const { return _phi0; }
+ double getOmega() const { return _omega ; }
+ double getTanLambda() const { return _tanLambda ; }
+
+ // defines if s of the helix increases in the direction of x2 to x3
+ static bool forwards;
+
+private:
+
+ double _ref_point_x ;
+ double _ref_point_y ;
+ double _ref_point_z ;
+ double _d0 ;
+ double _z0 ;
+ double _phi0 ;
+ double _omega ;
+ double _tanLambda ;
+
+ /** helper function to restrict the range of the azimuthal angle to ]-pi,pi]*/
+ inline double toBaseRange( double phi) const {
+ while( phi <= -M_PI ){ phi += 2. * M_PI ; }
+ while( phi > M_PI ){ phi -= 2. * M_PI ; }
+ return phi ;
+ }
+
+} ;
+
+
+
+
+#endif
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrack.h b/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrack.h
new file mode 100644
index 00000000..2abcdd52
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrack.h
@@ -0,0 +1,232 @@
+#ifndef IMarlinTrack_h
+#define IMarlinTrack_h
+
+#include <cfloat>
+
+//#include "lcio.h"
+
+#include "edm4hep/TrackerHit.h"
+#include "edm4hep/TrackState.h"
+
+//#include "gearimpl/Vector3D.h"
+//#include "plcio/DoubleThree.h"
+#include "edm4hep/Vector3d.h"
+
+#include <exception>
+
+ /** Interface for generic tracks in MarlinTrk. The interface should provide the functionality to
+ * perform track finding and fitting. It is asssumed that the underlying implemetation will by
+ * a Kalman Filter or a similar algorithm.
+ *
+ * @version $Id: IMarlinTrack.h 3989 2012-09-04 07:16:44Z aplin $
+ * @author S.Aplin, F. Gaede DESY
+ */
+namespace MarlinTrk{
+ class IMarlinTrack {
+
+ public:
+
+ /** boolean constant for defining backward direction - to be used for intitialise */
+ static const bool backward ;
+
+ /** boolean constant for defining backward direction - to be used for intitialise */
+ static const bool forward ;
+
+
+
+ static const int modeBackward ;
+ static const int modeClosest ;
+ static const int modeForward ;
+
+
+ static const int success ; // no error
+ static const int error ;
+ static const int bad_intputs ;
+ static const int no_intersection ; // no intersection found
+ static const int site_discarded ; // measurement discarded by the fitter
+ static const int site_fails_chi2_cut ; // measurement discarded by the fitter due to chi2 cut
+ static const int all_sites_fail_fit ; // no single measurement added to the fit
+
+
+ /**default d'tor*/
+ virtual ~IMarlinTrack() {};
+
+ /** add hit to track - the hits have to be added ordered in time ( i.e. typically outgoing )
+ * this order will define the direction of the energy loss used in the fit
+ */
+ virtual int addHit(edm4hep::TrackerHit* hit) = 0 ;
+
+ /** initialise the fit using the hits added up to this point -
+ * the fit direction has to be specified using IMarlinTrack::backward or IMarlinTrack::forward.
+ * this is the order wrt the order used in addHit() that will be used in the fit()
+ */
+ virtual int initialise( bool fitDirection ) = 0 ;
+
+ /** initialise the fit with a track state, and z component of the B field in Tesla.
+ * the fit direction has to be specified using IMarlinTrack::backward or IMarlinTrack::forward.
+ * this is the order that will be used in the fit().
+ * it is the users responsibility that the track state is consistent with the order
+ * of the hits used in addHit() ( i.e. the direction of energy loss )
+ */
+ virtual int initialise( const edm4hep::TrackState& ts, double bfield_z, bool fitDirection ) = 0 ;
+
+
+ /** perform the fit of all current hits, returns error code ( IMarlinTrack::success if no error ) .
+ * the fit will be performed in the order specified at initialise() wrt the order used in addHit(), i.e.
+ * IMarlinTrack::backward implies fitting from the outside to the inside for tracks comming from the IP.
+ */
+ virtual int fit( double maxChi2Increment=DBL_MAX ) = 0 ;
+
+
+ /** update the current fit using the supplied hit, return code via int. Provides the Chi2 increment to the fit from adding the hit via reference.
+ * the given hit will not be added if chi2increment > maxChi2Increment.
+ */
+ virtual int addAndFit( edm4hep::TrackerHit* hit, double& chi2increment, double maxChi2Increment=DBL_MAX ) = 0 ;
+
+
+ /** obtain the chi2 increment which would result in adding the hit to the fit. This method will not alter the current fit, and the hit will not be stored in the list of hits or outliers
+ */
+ virtual int testChi2Increment( edm4hep::TrackerHit* hit, double& chi2increment ) = 0 ;
+
+
+ /** smooth all track states
+ */
+ virtual int smooth() = 0 ;
+
+
+ /** smooth track states from the last filtered hit back to the measurement site associated with the given hit
+ */
+ virtual int smooth( edm4hep::TrackerHit* hit ) = 0 ;
+
+
+
+ // Track State Accessesors
+
+ /** get track state, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int getTrackState( edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+
+ /** get track state at measurement associated with the given hit, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int getTrackState( edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+ /** get the list of hits included in the fit, together with the chi2 contributions of the hits.
+ * Pointers to the hits together with their chi2 contribution will be filled into a vector of
+ * pairs consitining of the pointer as the first part of the pair and the chi2 contribution as
+ * the second.
+ */
+ virtual int getHitsInFit( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) = 0 ;
+
+ /** get the list of hits which have been rejected by from the fit due to the a chi2 increment greater than threshold,
+ * Pointers to the hits together with their chi2 contribution will be filled into a vector of
+ * pairs consitining of the pointer as the first part of the pair and the chi2 contribution as
+ * the second.
+ */
+ virtual int getOutliers( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) = 0 ;
+
+ /** get the current number of degrees of freedom for the fit.
+ */
+ virtual int getNDF( int& ndf ) = 0 ;
+
+ /** get TrackeHit at which fit became constrained, i.e. ndf >= 0
+ */
+ virtual int getTrackerHitAtPositiveNDF( edm4hep::TrackerHit*& trkhit ) = 0 ;
+
+ // PROPAGATORS
+
+ /** propagate the fit to the point of closest approach to the given point, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int propagate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+
+ /** propagate the fit at the measurement site associated with the given hit, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ virtual int propagate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+
+ /** propagate fit to numbered sensitive layer, returning TrackState, chi2, ndf and integer ID of the intersected sensitive detector element via reference
+ */
+ virtual int propagateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) = 0 ;
+
+ /** propagate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of the intersected sensitive detector element via reference
+ */
+ virtual int propagateToLayer(int layerID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) = 0;
+
+ /** propagate the fit to sensitive detector element, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int propagateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) = 0 ;
+
+ /** propagate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ virtual int propagateToDetElement( int detEementID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) = 0 ;
+
+
+
+ // EXTRAPOLATORS
+
+ /** extrapolate the fit to the point of closest approach to the given point, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ virtual int extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) = 0 ;
+
+ /** extrapolate the fit to numbered sensitive layer, returning TrackState, chi2, ndf and integer ID of the intersected sensitive detector element via reference
+ */
+ virtual int extrapolateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) = 0 ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of the intersected sensitive detector element via reference
+ */
+ virtual int extrapolateToLayer( int layerID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) = 0 ;
+
+ /** extrapolate the fit to sensitive detector element, returning TrackState, chi2 and ndf via reference
+ */
+ virtual int extrapolateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) = 0 ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ virtual int extrapolateToDetElement( int detEementID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) = 0 ;
+
+
+ // INTERSECTORS
+
+ /** extrapolate the fit to numbered sensitive layer, returning intersection point in global coordinates and integer ID of the
+ * intersected sensitive detector element via reference
+ */
+ virtual int intersectionWithLayer( int layerID, edm4hep::Vector3d& point, int& detElementID, int mode=modeClosest ) = 0 ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning intersection point in global coordinates and integer ID of the intersected sensitive detector element via reference
+ */
+ virtual int intersectionWithLayer( int layerID, edm4hep::TrackerHit* hit, edm4hep::Vector3d& point, int& detElementID, int mode=modeClosest ) = 0 ;
+
+
+ /** extrapolate the fit to numbered sensitive detector element, returning intersection point in global coordinates via reference
+ */
+ virtual int intersectionWithDetElement( int detElementID, edm4hep::Vector3d& point, int mode=modeClosest ) = 0 ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning intersection point in global coordinates via reference
+ */
+ virtual int intersectionWithDetElement( int detEementID, edm4hep::TrackerHit* hit, edm4hep::Vector3d& point, int mode=modeClosest ) = 0 ;
+
+
+ protected:
+
+ private:
+
+ IMarlinTrack& operator=( const IMarlinTrack&) ; // disallow assignment operator
+
+ } ;
+ std::string errorCode( int error );
+}
+#endif
+
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrkSystem.h b/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrkSystem.h
new file mode 100644
index 00000000..2c19b1e4
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/IMarlinTrkSystem.h
@@ -0,0 +1,105 @@
+#ifndef IMarlinTrkSystem_h
+#define IMarlinTrkSystem_h
+
+//#include "MarlinTrkDiagnostics.h"
+
+#include <exception>
+#include "ConfigFlags.h"
+
+namespace MarlinTrk{
+class IMarlinTrack ;
+
+/** Exception thrown in IMarlinTrk namespace (implemetations of IMarlinTrkSystem
+ * and IMarlinTrack).
+ */
+class Exception : public std::exception {
+
+ protected:
+ std::string message ;
+
+ Exception(){ /*no_op*/ ; }
+
+ public:
+ virtual ~Exception() throw() { /*no_op*/; }
+
+ Exception( const std::string& text ){
+ message = "MarlinTrk::Exception: " + text ;
+ }
+
+ virtual const char* what() const throw() { return message.c_str() ; }
+
+};
+
+//----------------------------------------------------------------------------------------------------
+
+/** Base class for tracking system implementations in MarlinTrk.
+ *
+ * @version $Id: IMarlinTrkSystem.h 3288 2012-04-03 09:25:12Z aplin $
+ * @author S.Aplin, F. Gaede DESY
+ */
+
+class IMarlinTrkSystem {
+
+ public:
+
+ /** 'Enums' for configuration options to be used with setOption().
+ */
+ struct CFG {
+ /** Use multiple scattering in the track fits. */
+ static const unsigned useQMS = 1 ;
+ /** Use multiple scattering in the track fits. */
+ static const unsigned usedEdx = 2 ;
+ /** Use smoothing when calling fit( bool fitDirection ) */
+ static const unsigned useSmoothing = 3 ;
+ //---
+ static const unsigned size = 4 ;
+
+ } ;
+
+
+ /** D'tor - cleans up any allocated resources.*/
+ virtual ~IMarlinTrkSystem() {};
+
+
+ /** Sets the specified option ( one of the constants defined in IMarlinTrkSystem::CFG )
+ * to the given value.
+ */
+ void setOption(unsigned CFGOption, bool val) ;
+
+ /** Return the option's current value - false if option not defined.
+ */
+ bool getOption( unsigned CFGOption) ;
+
+ /** String with all configuration options and their current values.
+ */
+ std::string getOptions() ;
+
+
+ /** Initialise tracking system - to be called after configuration with setOption() -
+ * IMarlinTrkSystem cannot be used before a call to init().
+ */
+ virtual void init() = 0 ;
+
+
+ /** Return an instance of IMarlinTrack corresponding to the current implementation.
+ */
+ virtual IMarlinTrack* createTrack() = 0 ;
+
+ protected:
+
+ MarlinTrk::ConfigFlags _cfg ;
+
+ /** Register the possible configuration options
+ */
+ void registerOptions() ;
+
+
+
+ private:
+
+ IMarlinTrkSystem& operator=( const IMarlinTrkSystem&) ; // disallow assignment operator
+
+};
+}
+#endif
+
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/ITrackSystemSvc.h b/Service/TrackSystemSvc/TrackSystemSvc/ITrackSystemSvc.h
new file mode 100644
index 00000000..699d26ba
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/ITrackSystemSvc.h
@@ -0,0 +1,21 @@
+#ifndef ITrackSystemSvc_h
+#define ITrackSystemSvc_h
+
+#include "GaudiKernel/IService.h"
+#include "IMarlinTrkSystem.h"
+
+// ITrackSystemSvc is the interface between Gaudi and Track.
+
+class ITrackSystemSvc: virtual public IService {
+ public:
+ DeclareInterfaceID(ITrackSystemSvc, 0, 1); // major/minor version
+
+ virtual ~ITrackSystemSvc() = default;
+
+ //Get the track manager
+ virtual MarlinTrk::IMarlinTrkSystem* getTrackSystem() = 0;
+
+ virtual void removeTrackSystem() = 0;
+};
+
+#endif
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/LCIOTrackPropagators.h.remove b/Service/TrackSystemSvc/TrackSystemSvc/LCIOTrackPropagators.h.remove
new file mode 100644
index 00000000..a697cbd3
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/LCIOTrackPropagators.h.remove
@@ -0,0 +1,42 @@
+#ifndef LCIOTrackPropagators_h
+#define LCIOTrackPropagators_h
+
+namespace EVENT{
+ class TrackState ;
+}
+
+namespace IMPL{
+ class TrackStateImpl ;
+}
+
+
+namespace LCIOTrackPropagators{
+
+ /** Propagate trackstate to a new reference point
+ */
+ int PropagateLCIOToNewRef( IMPL::TrackStateImpl& ts, double xref, double yref, double zref) ;
+
+ /** Propagate trackstate to a new reference point taken as its crossing point with a cylinder of infinite length centered at x0,y0, parallel to the z axis.
+ For direction== 0 the closest crossing point will be taken
+ For direction== 1 the first crossing traversing in positive s will be taken
+ For direction==-1 the first crossing traversing in negative s will be taken
+ */
+ int PropagateLCIOToCylinder( IMPL::TrackStateImpl& ts, float r, float x0, float y0, int direction=0, double epsilon=1.0e-8) ;
+
+
+ /** Propagate trackstate to a new reference point taken as its crossing point with an infinite plane located at z, perpendicular to the z axis
+ */
+ int PropagateLCIOToZPlane( IMPL::TrackStateImpl& ts, float z) ;
+
+ /** Propagate trackstate to a new reference point taken as its crossing point with a plane parallel to the z axis, containing points x1,x2 and y1,y2. Tolerance for intersection determined by epsilon.
+ For direction== 0 the closest crossing point will be taken
+ For direction== 1 the first crossing traversing in positive s will be taken
+ For direction==-1 the first crossing traversing in negative s will be taken
+ */
+ int PropagateLCIOToPlaneParralelToZ( IMPL::TrackStateImpl& ts, float x1, float y1, float x2, float y2, int direction=0, double epsilon=1.0e-8) ;
+
+
+
+}
+
+#endif
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkDiagnostics.h.remove b/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkDiagnostics.h.remove
new file mode 100644
index 00000000..ffa97ef3
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkDiagnostics.h.remove
@@ -0,0 +1,31 @@
+#ifndef MarlinTrkDiagnostics_h
+#define MarlinTrkDiagnostics_h
+
+//// switch to turn on diagnostics code
+//#define MARLINTRK_DIAGNOSTICS_ON
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+#include "lcio.h"
+#include "EVENT/SimTrackerHit.h"
+#include "EVENT/TrackerHit.h"
+
+
+namespace MarlinTrk{
+
+
+ // LCIO Extension creating a pointer to the simhit for trackerhits
+ struct MCTruth4HitExtStruct{
+ MCTruth4HitExtStruct() : simhit(0) {}
+ EVENT::SimTrackerHit* simhit;
+ } ;
+ struct MCTruth4HitExt : lcio::LCOwnedExtension<MCTruth4HitExt, MCTruth4HitExtStruct> {} ;
+
+ // fills a vector of MCParticle pointers with the MCParticles assosiated with the provided tracker hit using MCTruth4HitExtStruct
+ void getMCParticlesForTrackerHit(EVENT::TrackerHit* trkhit, std::vector<EVENT::MCParticle*>& mcps) ;
+
+}
+
+#endif
+
+#endif
diff --git a/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkUtils.h b/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkUtils.h
new file mode 100644
index 00000000..73826c71
--- /dev/null
+++ b/Service/TrackSystemSvc/TrackSystemSvc/MarlinTrkUtils.h
@@ -0,0 +1,86 @@
+#ifndef INCLUDE_MarlinTrkUtils
+#define INCLUDE_MarlinTrkUtils 1
+
+#include <vector>
+#include <array>
+#include <cfloat>
+
+#include <LCIOSTLTypes.h>
+
+namespace edm4hep{
+ class Track;
+ class TrackerHit;
+ class TrackState;
+}
+
+namespace UTIL {
+ class BitField64;
+}
+
+
+namespace MarlinTrk{
+ class IMarlinTrack ;
+}
+
+
+namespace MarlinTrk{
+ enum Location{
+ AtOther = 0,
+ AtIP = 1,
+ AtFirstHit = 2,
+ AtLastHit = 3,
+ AtCalorimeter = 4,
+ AtVertex = 5,
+ LastLocation = 6
+ };
+ /** Takes a list of hits and uses the IMarlinTrack inferface to fit them using a supplied prefit containing
+ * a covariance matrix for the initialisation. The TrackImpl will have the 4 trackstates added to
+ * it @IP, @First_Hit, @Last_Hit and @CaloFace */
+ int createFinalisedLCIOTrack(
+ IMarlinTrack* marlinTrk,
+ std::vector<edm4hep::TrackerHit*>& hit_list,
+ edm4hep::Track* track,
+ bool fit_backwards,
+ edm4hep::TrackState* pre_fit,
+ float bfield_z,
+ double maxChi2Increment=DBL_MAX);
+
+ /** Takes a list of hits and uses the IMarlinTrack inferface to fit them using a supplied covariance matrix
+ * for the initialisation. The TrackImpl will have the 4 trackstates added to
+ * it @IP, @First_Hit, @Last_Hit and @CaloFace */
+ int createFinalisedLCIOTrack(
+ IMarlinTrack* marlinTrk,
+ std::vector<edm4hep::TrackerHit*>& hit_list,
+ edm4hep::Track* track,
+ bool fit_backwards,
+ const std::array<float,15>& initial_cov_for_prefit,
+ float bfield_z,
+ double maxChi2Increment=DBL_MAX);
+
+ /** Provides the values of a track state from the first, middle and last hits in the hit_list. */
+ int createPrefit( std::vector<edm4hep::TrackerHit*>& hit_list, edm4hep::TrackState* pre_fit, float bfield_z, bool fit_backwards );
+
+ /** Takes a list of hits and uses the IMarlinTrack inferface to fit them using a supplied prefit containing a covariance matrix for the initialisation. */
+ int createFit( std::vector<edm4hep::TrackerHit*>& hit_list, IMarlinTrack* marlinTrk, edm4hep::TrackState* pre_fit, float bfield_z, bool fit_backwards, double maxChi2Increment=DBL_MAX );
+
+ /** Takes a fitted MarlinTrack, TrackImpl to record the fit and the hits which have been added to the fit.
+ * The TrackImpl will have the 4 trackstates added to it @IP, @First_Hit, @Last_Hit and @CaloFace.
+ * Note: the hit list is needed as the IMarlinTrack only contains the hits used in the fit, not the spacepoints
+ * (if any have been included) so as the strip hits cannot point to the space points we need to have the list so
+ * that they can be recorded in the LCIO TrackImpl */
+ int finaliseLCIOTrack(
+ IMarlinTrack* marlinTrk,
+ edm4hep::Track* track,
+ std::vector<edm4hep::TrackerHit*>& hit_list,
+ edm4hep::TrackState* atLastHit=0,
+ edm4hep::TrackState* atCaloFace=0);
+
+ /** Set the subdetector hit numbers for the TrackImpl */
+ void addHitNumbersToTrack(edm4hep::Track* track, std::vector<edm4hep::TrackerHit*>& hit_list, bool hits_in_fit, UTIL::BitField64& cellID_encoder);
+
+ /** Set the subdetector hit numbers for the TrackImpl */
+ void addHitNumbersToTrack(edm4hep::Track* track, std::vector<std::pair<edm4hep::TrackerHit* , double> >& hit_list, bool hits_in_fit, UTIL::BitField64& cellID_encoder);
+
+}
+
+#endif
diff --git a/Service/TrackSystemSvc/src/DiagnosticsController.cc.remove b/Service/TrackSystemSvc/src/DiagnosticsController.cc.remove
new file mode 100644
index 00000000..cbdf6b64
--- /dev/null
+++ b/Service/TrackSystemSvc/src/DiagnosticsController.cc.remove
@@ -0,0 +1,883 @@
+
+#include "MarlinTrkDiagnostics.h"
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+#include "DiagnosticsController.h"
+
+#define MarlinTrkNtuple_cxx
+#include "MarlinTrkNtuple.h"
+
+#include "MarlinKalTestTrack.h"
+
+#include "streamlog/streamlog.h"
+
+#include "kaldet/ILDVTrackHit.h"
+#include "kaltest/TKalTrackSite.h"
+#include "kaltest/TKalTrack.h"
+
+#include "TFile.h"
+#include "TTree.h"
+
+//#include "MarlinTrk/MarlinTrkNtuple.h"
+
+#include "HelixTrack.h"
+
+#include "EVENT/MCParticle.h"
+#include <UTIL/BitField64.h>
+#include <UTIL/ILDConf.h>
+
+
+namespace MarlinTrk{
+
+ DiagnosticsController::DiagnosticsController() {
+
+ _initialised = false;
+ _recording_on = false; // recording is set to off by default so that processors do not have to perform any action if they are not interested in diagnostics, i.e. no need to call init
+ _current_track = 0;
+ _currentMCP = 0;
+ _mcpInfoStored=false;
+ _skip_track = false;
+ _ntracks_skipped = 0;
+ _ntracks_written = 0;
+ _track_record = 0;
+ _tree = 0;
+ _root_file = 0;
+
+ } // constructor
+
+ DiagnosticsController::~DiagnosticsController(){
+ if(_track_record) { delete _track_record; _track_record = 0; }
+ }
+
+ void DiagnosticsController::init(std::string root_file_name, std::string root_tree_name, bool recording_on){
+
+ streamlog_out(DEBUG4) << " DiagnosticsController::init called " << "\n"
+ << "\t root file name = " << root_file_name << "\n"
+ << "\t root tree name = " << root_tree_name << "\n"
+ << "\t recording on = " << recording_on << "\n"
+ << std::endl;
+
+ _recording_on = recording_on;
+
+ if ( _recording_on == false ) {
+ _initialised = true;
+ return;
+ }
+
+ _root_file_name = root_file_name+".root";
+ _root_tree_name = root_tree_name;
+
+ _root_file = new TFile(_root_file_name.c_str(),"RECREATE");
+
+ _tree = new TTree( _root_tree_name.c_str(), _root_tree_name.c_str());
+
+ _track_record = new MarlinTrkNtuple();
+
+ _track_record->CreateBranches(_tree);
+
+ _initialised = true;
+
+ }
+
+
+ void DiagnosticsController::skip_current_track(){
+
+ streamlog_out(DEBUG) << " DiagnosticsController::skip_current_track called " << std::endl;
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ _skip_track = true ;
+ this->clear_track_record();
+ }
+
+ void DiagnosticsController::clear_track_record(){
+
+ streamlog_out(DEBUG) << " DiagnosticsController::clear_track_record called " << std::endl;
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ _track_record->error_code = 0 ;
+
+ _track_record->nsites = 0 ;
+
+ _track_record->nsites_vxd = 0 ;
+ _track_record->nsites_sit = 0 ;
+ _track_record->nsites_ftd = 0 ;
+ _track_record->nsites_tpc = 0 ;
+ _track_record->nsites_set = 0 ;
+
+ _track_record->x_mcp = 0 ;
+ _track_record->y_mcp = 0 ;
+ _track_record->z_mcp = 0 ;
+
+ _track_record->px_mcp = 0 ;
+ _track_record->py_mcp = 0 ;
+ _track_record->pz_mcp = 0 ;
+ _track_record->p_mcp = 0 ;
+ _track_record->theta_mcp = 0 ;
+ _track_record->phi_mcp = 0 ;
+ _track_record->pdg_mcp = 0 ;
+
+ _track_record->d0_mcp = 0 ;
+ _track_record->phi0_mcp = 0 ;
+ _track_record->omega_mcp = 0 ;
+ _track_record->z0_mcp = 0 ;
+ _track_record->tanL_mcp = 0 ;
+
+ _track_record->ndf = 0 ;
+ _track_record->chi2 = 0 ;
+ _track_record->prob = 0 ;
+
+ _track_record->d0_seed = 0 ;
+ _track_record->phi0_seed = 0 ;
+ _track_record->omega_seed = 0 ;
+ _track_record->z0_seed = 0 ;
+ _track_record->tanL_seed = 0 ;
+
+ _track_record->seed_ref_point_x = 0 ;
+ _track_record->seed_ref_point_y = 0 ;
+ _track_record->seed_ref_point_z = 0 ;
+
+ _track_record->cov_seed_d0d0 = 0 ;
+ _track_record->cov_seed_phi0d0 = 0 ;
+ _track_record->cov_seed_phi0phi0 = 0 ;
+ _track_record->cov_seed_kappad0 = 0 ;
+ _track_record->cov_seed_kappaphi0 = 0 ;
+ _track_record->cov_seed_kappakappa = 0 ;
+ _track_record->cov_seed_z0d0 = 0 ;
+ _track_record->cov_seed_z0phi0 = 0 ;
+ _track_record->cov_seed_z0kappa = 0 ;
+ _track_record->cov_seed_z0z0 = 0 ;
+ _track_record->cov_seed_tanLd0 = 0 ;
+ _track_record->cov_seed_tanLphi0 = 0 ;
+ _track_record->cov_seed_tanLkappa = 0 ;
+ _track_record->cov_seed_tanLz0 = 0 ;
+ _track_record->cov_seed_tanLtanL = 0 ;
+
+
+ _track_record->d0_ip = 0 ;
+ _track_record->phi0_ip = 0 ;
+ _track_record->omega_ip = 0 ;
+ _track_record->z0_ip = 0 ;
+ _track_record->tanL_ip = 0 ;
+
+ _track_record->cov_ip_d0d0 = 0 ;
+ _track_record->cov_ip_phi0d0 = 0 ;
+ _track_record->cov_ip_phi0phi0 = 0 ;
+ _track_record->cov_ip_omegad0 = 0 ;
+ _track_record->cov_ip_omegaphi0 = 0 ;
+ _track_record->cov_ip_omegaomega = 0 ;
+ _track_record->cov_ip_z0d0 = 0 ;
+ _track_record->cov_ip_z0phi0 = 0 ;
+ _track_record->cov_ip_z0omega = 0 ;
+ _track_record->cov_ip_z0z0 = 0 ;
+ _track_record->cov_ip_tanLd0 = 0 ;
+ _track_record->cov_ip_tanLphi0 = 0 ;
+ _track_record->cov_ip_tanLomega = 0 ;
+ _track_record->cov_ip_tanLz0 = 0 ;
+ _track_record->cov_ip_tanLtanL = 0 ;
+
+ for ( int i = 0 ; i<MAX_SITES; ++i) {
+
+ _track_record->CellID0[i] = 0 ;
+ _track_record->rejected[i] = 0 ;
+
+ _track_record->site_x[i] = 0 ;
+ _track_record->site_y[i] = 0 ;
+ _track_record->site_z[i] = 0 ;
+
+ _track_record->ref_point_x[i] = 0 ;
+ _track_record->ref_point_y[i] = 0 ;
+ _track_record->ref_point_z[i] = 0 ;
+
+ _track_record->d0_mc[i] = 0 ;
+ _track_record->phi0_mc[i] = 0 ;
+ _track_record->omega_mc[i] = 0 ;
+ _track_record->z0_mc[i] = 0 ;
+ _track_record->tanL_mc[i] = 0 ;
+
+ _track_record->d0_predicted[i] = 0 ;
+ _track_record->phi0_predicted[i] = 0 ;
+ _track_record->omega_predicted[i] = 0 ;
+ _track_record->z0_predicted[i] = 0 ;
+ _track_record->tanL_predicted[i] = 0 ;
+
+ _track_record->d0_filtered[i] = 0 ;
+ _track_record->phi0_filtered[i] = 0 ;
+ _track_record->omega_filtered[i] = 0 ;
+ _track_record->z0_filtered[i] = 0 ;
+ _track_record->tanL_filtered[i] = 0 ;
+
+ _track_record->d0_smoothed[i] = 0 ;
+ _track_record->phi0_smoothed[i] = 0 ;
+ _track_record->omega_smoothed[i] = 0 ;
+ _track_record->z0_smoothed[i] = 0 ;
+ _track_record->tanL_smoothed[i] = 0 ;
+
+
+ _track_record->chi2_inc_filtered[i] = 0 ;
+ _track_record->chi2_inc_smoothed[i] = 0 ;
+ _track_record->dim[i] = 0 ;
+
+ _track_record->cov_predicted_d0d0[i] = 0 ;
+ _track_record->cov_predicted_phi0d0[i] = 0 ;
+ _track_record->cov_predicted_phi0phi0[i] = 0 ;
+ _track_record->cov_predicted_omegad0[i] = 0 ;
+ _track_record->cov_predicted_omegaphi0[i] = 0 ;
+ _track_record->cov_predicted_omegaomega[i] = 0 ;
+ _track_record->cov_predicted_z0d0[i] = 0 ;
+ _track_record->cov_predicted_z0phi0[i] = 0 ;
+ _track_record->cov_predicted_z0omega[i] = 0 ;
+ _track_record->cov_predicted_z0z0[i] = 0 ;
+ _track_record->cov_predicted_tanLd0[i] = 0 ;
+ _track_record->cov_predicted_tanLphi0[i] = 0 ;
+ _track_record->cov_predicted_tanLomega[i] = 0 ;
+ _track_record->cov_predicted_tanLz0[i] = 0 ;
+ _track_record->cov_predicted_tanLtanL[i] = 0 ;
+
+ _track_record->cov_filtered_d0d0[i] = 0 ;
+ _track_record->cov_filtered_phi0d0[i] = 0 ;
+ _track_record->cov_filtered_phi0phi0[i] = 0 ;
+ _track_record->cov_filtered_omegad0[i] = 0 ;
+ _track_record->cov_filtered_omegaphi0[i] = 0 ;
+ _track_record->cov_filtered_omegaomega[i] = 0 ;
+ _track_record->cov_filtered_z0d0[i] = 0 ;
+ _track_record->cov_filtered_z0phi0[i] = 0 ;
+ _track_record->cov_filtered_z0omega[i] = 0 ;
+ _track_record->cov_filtered_z0z0[i] = 0 ;
+ _track_record->cov_filtered_tanLd0[i] = 0 ;
+ _track_record->cov_filtered_tanLphi0[i] = 0 ;
+ _track_record->cov_filtered_tanLomega[i] = 0 ;
+ _track_record->cov_filtered_tanLz0[i] = 0 ;
+ _track_record->cov_filtered_tanLtanL[i] = 0 ;
+
+ _track_record->cov_smoothed_d0d0[i] = 0 ;
+ _track_record->cov_smoothed_phi0d0[i] = 0 ;
+ _track_record->cov_smoothed_phi0phi0[i] = 0 ;
+ _track_record->cov_smoothed_omegad0[i] = 0 ;
+ _track_record->cov_smoothed_omegaphi0[i] = 0 ;
+ _track_record->cov_smoothed_omegaomega[i] = 0 ;
+ _track_record->cov_smoothed_z0d0[i] = 0 ;
+ _track_record->cov_smoothed_z0phi0[i] = 0 ;
+ _track_record->cov_smoothed_z0omega[i] = 0 ;
+ _track_record->cov_smoothed_z0z0[i] = 0 ;
+ _track_record->cov_smoothed_tanLd0[i] = 0 ;
+ _track_record->cov_smoothed_tanLphi0[i] = 0 ;
+ _track_record->cov_smoothed_tanLomega[i] = 0 ;
+ _track_record->cov_smoothed_tanLz0[i] = 0 ;
+ _track_record->cov_smoothed_tanLtanL[i] = 0 ;
+
+
+
+ }
+
+ }
+
+ void DiagnosticsController::new_track(MarlinKalTestTrack* trk){
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::new_track: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+ streamlog_out(DEBUG3) << " DiagnosticsController::new_track called " << std::endl;
+
+ this->clear_track_record();
+
+ _current_track = trk;
+
+ _skip_track = false;
+ _currentMCP = 0;
+ _mcpInfoStored=false;
+
+ }
+
+
+
+ void DiagnosticsController::set_intial_track_parameters(double d0, double phi0, double omega, double z0, double tanL, double pivot_x, double pivot_y, double pivot_z, TKalMatrix& cov){
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::set_intial_track_parameters: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+ streamlog_out(DEBUG3) << " DiagnosticsController::set_intial_track_parameters called " << std::endl;
+
+ _track_record->d0_seed= d0;
+ _track_record->phi0_seed= phi0;
+ _track_record->omega_seed= omega;
+ _track_record->z0_seed= z0;
+ _track_record->tanL_seed= tanL;
+
+ _track_record->seed_ref_point_x = pivot_x ;
+ _track_record->seed_ref_point_y = pivot_y ;
+ _track_record->seed_ref_point_z = pivot_z ;
+
+
+ _track_record->cov_seed_d0d0 = cov( 0 , 0 ) ;
+ _track_record->cov_seed_phi0d0 = cov( 1 , 0 ) ;
+ _track_record->cov_seed_phi0phi0 = cov( 1 , 1 ) ;
+ _track_record->cov_seed_kappad0 = cov( 2 , 0 ) ;
+ _track_record->cov_seed_kappaphi0 = cov( 2 , 1 ) ;
+ _track_record->cov_seed_kappakappa = cov( 2 , 2 ) ;
+ _track_record->cov_seed_z0d0 = cov( 3 , 0 ) ;
+ _track_record->cov_seed_z0phi0 = cov( 3 , 1 ) ;
+ _track_record->cov_seed_z0kappa = cov( 3 , 2 ) ;
+ _track_record->cov_seed_z0z0 = cov( 3 , 3 ) ;
+ _track_record->cov_seed_tanLd0 = cov( 4 , 0 ) ;
+ _track_record->cov_seed_tanLphi0 = cov( 4 , 1 ) ;
+ _track_record->cov_seed_tanLkappa = cov( 4 , 2 ) ;
+ _track_record->cov_seed_tanLz0 = cov( 4 , 3 ) ;
+ _track_record->cov_seed_tanLtanL = cov( 4 , 4 ) ;
+
+ // cov.Print();
+
+
+ streamlog_out(DEBUG3) << " $#$#$# Initial Track Parameters: "
+ << "d0 = " << d0 << " " << "[+/-" << sqrt( _track_record->cov_seed_d0d0 ) << "] "
+ << "phi0 = " << phi0 << " " << "[+/-" << sqrt( _track_record->cov_seed_phi0phi0 ) << "] "
+ << "omega = " << omega << " " << "[+/-" << sqrt( _track_record->cov_seed_kappakappa ) << "] "
+ << "z0 = " << z0 << " " << "[+/-" << sqrt( _track_record->cov_seed_z0z0 ) << "] "
+ << "tanL = " << tanL << " " << "[+/-" << sqrt( _track_record->cov_seed_tanLtanL ) << "] "
+ << "ref = " << pivot_x << " " << pivot_y << " " << pivot_z << " "
+ << std::endl;
+
+ }
+
+
+ void DiagnosticsController::record_rejected_site(ILDVTrackHit* hit, TKalTrackSite* site){
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::record_rejected_site: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+
+ if(_skip_track) return;
+
+ _track_record->rejected[_track_record->nsites] = 1;
+ _track_record->CellID0[_track_record->nsites] = hit->getLCIOTrackerHit()->getCellID0();
+
+
+ EVENT::MCParticle* mcp = hit->getLCIOTrackerHit()->ext<MarlinTrk::MCTruth4HitExt>()->simhit->getMCParticle();
+
+ ++_track_record->nsites;
+ streamlog_out(DEBUG2) << "record_rejected_site _track_record->nsites = " << _track_record->nsites << " MCParticle of simhit = " << mcp << " pdg " << mcp->getPDG() << " energy = " << mcp->getEnergy() << " id = " << mcp->id() << std::endl;
+
+ }
+
+
+ void DiagnosticsController::record_site(ILDVTrackHit* hit, TKalTrackSite* site){
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::record_site: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+ streamlog_out(DEBUG2) << "DiagnosticsController::record_site called " << std::endl;
+
+ if(_skip_track) return;
+
+ EVENT::TrackerHit* trkhit = hit->getLCIOTrackerHit();
+
+ MarlinTrk::MCTruth4HitExtStruct* ext = trkhit->ext<MarlinTrk::MCTruth4HitExt>();
+
+ if ( ! ext ) {
+
+ streamlog_out(ERROR) << "MCTruth4HitExt not attached to TrackerHit use processor SetTrackerHitExtensions to set them: exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+
+ }
+
+ EVENT::SimTrackerHit* simhit = trkhit->ext<MarlinTrk::MCTruth4HitExt>()->simhit;
+
+ if ( ! simhit ) {
+
+ streamlog_out(ERROR) << "SimTrackerHit not attached to TrackerHit using MCTruth4HitExt use processor SetTrackerHitExtensions to set them: exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+
+ }
+
+ EVENT::MCParticle* mcp = simhit->getMCParticle() ;
+
+
+ if( _track_record->nsites >= 0 ){
+
+ if ( _mcpInfoStored == false ) {
+
+ streamlog_out(DEBUG2) << "DiagnosticsController::record_site store MCParticle parameters for " << mcp << " pdg " << mcp->getPDG() << " energy = " << mcp->getEnergy() << " id = " << mcp->id() << std::endl;
+
+ _currentMCP = mcp;
+
+ _track_record->x_mcp = mcp->getVertex()[0];
+ _track_record->y_mcp = mcp->getVertex()[1];
+ _track_record->z_mcp = mcp->getVertex()[2];
+
+ _track_record->px_mcp = mcp->getMomentum()[0];
+ _track_record->py_mcp = mcp->getMomentum()[1];
+ _track_record->pz_mcp = mcp->getMomentum()[2];
+
+ double pt = sqrt(_track_record->px_mcp*_track_record->px_mcp +
+ _track_record->py_mcp*_track_record->py_mcp ) ;
+
+ _track_record->p_mcp = sqrt( pt*pt + _track_record->pz_mcp*_track_record->pz_mcp );
+
+
+ _track_record->theta_mcp = atan2( pt, _track_record->pz_mcp );
+ _track_record->phi_mcp = atan2( _track_record->py_mcp, _track_record->px_mcp );
+
+ _track_record->pdg_mcp = mcp->getPDG();
+
+ // HelixTrack helixMC( sim_pos, sim_mom, mcp->getCharge(), ml.GetBz() ) ;
+ HelixTrack helixMC( mcp->getVertex(), mcp->getMomentum(), mcp->getCharge(), site->GetBfield() ) ;
+
+ helixMC.moveRefPoint(0.0, 0.0, 0.0);
+
+ _track_record->d0_mcp= helixMC.getD0();
+ _track_record->phi0_mcp = helixMC.getPhi0();
+ _track_record->omega_mcp = helixMC.getOmega();
+ _track_record->z0_mcp = helixMC.getZ0();
+ _track_record->tanL_mcp = helixMC.getTanLambda();
+
+ _mcpInfoStored = true;
+
+ }
+ else if( _currentMCP != mcp ) {
+ _skip_track = true ; // do not store tracks formed from more than one MCParticle
+ streamlog_out(WARNING) << "DiagnosticsController::record_site: Track skipped. Not storing tracks formed from more than one MCParticle " << std::endl;
+ return ;
+ }
+
+ double sim_pos[3];
+ sim_pos[0] = simhit->getPosition()[0];
+ sim_pos[1] = simhit->getPosition()[1];
+ sim_pos[2] = simhit->getPosition()[2];
+
+ double sim_mom[3];
+ sim_mom[0] = simhit->getMomentum()[0];
+ sim_mom[1] = simhit->getMomentum()[1];
+ sim_mom[2] = simhit->getMomentum()[2];
+
+ if ( fabs(sim_mom[0]) < 1.e-09 && fabs(sim_mom[1]) < 1.e-09 && fabs(sim_mom[2]) < 1.e-09 ) {
+ // then the momentum is sub eV and therefore the momentum was certainly not set
+ streamlog_out(ERROR) << "Momentum not set in SimTrackerHit exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+
+ }
+
+
+ _track_record->CellID0[_track_record->nsites] = trkhit->getCellID0() ;
+
+ UTIL::BitField64 encoder(lcio::ILDCellID0::encoder_string);
+ encoder.setValue( trkhit->getCellID0() );
+
+ if (encoder[lcio::ILDCellID0::subdet] == lcio::ILDDetID::VXD) {
+ ++_track_record->nsites_vxd;
+ }
+ else if (encoder[lcio::ILDCellID0::subdet] == lcio::ILDDetID::SIT) {
+ ++_track_record->nsites_sit;
+ }
+ else if (encoder[lcio::ILDCellID0::subdet] == lcio::ILDDetID::FTD) {
+ ++_track_record->nsites_ftd;
+ }
+ else if (encoder[lcio::ILDCellID0::subdet] == lcio::ILDDetID::TPC) {
+ ++_track_record->nsites_tpc;
+ }
+ else if (encoder[lcio::ILDCellID0::subdet] == lcio::ILDDetID::SET) {
+ ++_track_record->nsites_set;
+ }
+
+ _track_record->chi2_inc_filtered[_track_record->nsites] = site->GetDeltaChi2();
+ _track_record->dim[_track_record->nsites] = site->GetDimension();
+
+ // create helix from MCTruth at sim hit
+ // HelixTrack helixMC( sim_pos, sim_mom, mcp->getCharge(), ml.GetBz() ) ;
+ HelixTrack helixMC( sim_pos, sim_mom, mcp->getCharge(), site->GetBfield() ) ;
+
+ // here perhaps we should move the helix to the hit to calculate the deltas or though this could still be done in the analysis code, as both sim and rec hit positions are stored ?
+ // helixMC.moveRefPoint(0.0, 0.0, 0.0);
+
+ streamlog_out(DEBUG2) << " $#$#$# SimHit Track Parameters: "
+ << "x = " << sim_pos[0] << " "
+ << "y = " << sim_pos[1] << " "
+ << "z = " << sim_pos[2] << " "
+ << "px = " << sim_mom[0] << " "
+ << "py = " << sim_mom[1] << " "
+ << "pz = " << sim_mom[2] << " "
+ << "p = " << sqrt(sim_mom[0]*sim_mom[0] + sim_mom[1]*sim_mom[1] + sim_mom[2]*sim_mom[2]) << " "
+ << "d0 = " << helixMC.getD0() << " "
+ << "phi0 = " << helixMC.getPhi0() << " "
+ << "omega = " << helixMC.getOmega() << " "
+ << "z0 = " << helixMC.getZ0() << " "
+ << "tanL = " << helixMC.getTanLambda() << " "
+ << " mcp ID = " << simhit->getMCParticle()->id()
+ << std::endl;
+
+ _track_record->d0_mc[_track_record->nsites] = helixMC.getD0();
+ _track_record->phi0_mc[_track_record->nsites] = helixMC.getPhi0();
+ _track_record->omega_mc[_track_record->nsites] = helixMC.getOmega();
+ _track_record->z0_mc[_track_record->nsites] = helixMC.getZ0();
+ _track_record->tanL_mc[_track_record->nsites] = helixMC.getTanLambda();
+
+ double rec_x = trkhit->getPosition()[0];
+ double rec_y = trkhit->getPosition()[1];
+ double rec_z = trkhit->getPosition()[2];
+
+ _track_record->site_x[_track_record->nsites] = rec_x;
+ _track_record->site_y[_track_record->nsites] = rec_y;
+ _track_record->site_z[_track_record->nsites] = rec_z;
+
+
+// // move track state to the sim hit for comparison
+ const TVector3 tpoint( sim_pos[0], sim_pos[1], sim_pos[2] ) ;
+
+ // move track state to the IP to make all comparisons straight forward
+ // const TVector3 tpoint( 0.0, 0.0, 0.0 ) ;
+
+
+ IMPL::TrackStateImpl ts;
+ int ndf;
+ double chi2;
+ double dPhi ;
+
+
+ TKalTrackState& trkState_predicted = (TKalTrackState&) site->GetState(TVKalSite::kPredicted); // this segfaults if no hits are present
+
+ THelicalTrack helix_predicted = trkState_predicted.GetHelix() ;
+ TMatrixD c0_predicted(trkState_predicted.GetCovMat());
+
+ Int_t sdim = trkState_predicted.GetDimension(); // dimensions of the track state, it will be 5 or 6
+
+ // now move to the point
+ TKalMatrix DF(sdim,sdim);
+ DF.UnitMatrix();
+ helix_predicted.MoveTo( tpoint , dPhi , &DF , &c0_predicted) ; // move helix to desired point, and get propagator matrix
+
+ streamlog_out(DEBUG2) << "DiagnosticsController::record_site get PREDICTED trackstate " << std::endl;
+
+ _current_track->ToLCIOTrackState( helix_predicted, c0_predicted, ts, chi2, ndf );
+
+ _track_record->d0_predicted[_track_record->nsites] = ts.getD0() ;
+ _track_record->phi0_predicted[_track_record->nsites] = ts.getPhi() ;
+ _track_record->omega_predicted[_track_record->nsites] = ts.getOmega() ;
+ _track_record->z0_predicted[_track_record->nsites] = ts.getZ0() ;
+ _track_record->tanL_predicted[_track_record->nsites] = ts.getTanLambda() ;
+
+
+ _track_record->cov_predicted_d0d0[_track_record->nsites] = ts.getCovMatrix()[0];
+ _track_record->cov_predicted_phi0d0[_track_record->nsites] = ts.getCovMatrix()[1];
+ _track_record->cov_predicted_phi0phi0[_track_record->nsites] = ts.getCovMatrix()[2];
+ _track_record->cov_predicted_omegad0[_track_record->nsites] = ts.getCovMatrix()[3];
+ _track_record->cov_predicted_omegaphi0[_track_record->nsites] = ts.getCovMatrix()[4];
+ _track_record->cov_predicted_omegaomega[_track_record->nsites] = ts.getCovMatrix()[5];
+ _track_record->cov_predicted_z0d0[_track_record->nsites] = ts.getCovMatrix()[6];
+ _track_record->cov_predicted_z0phi0[_track_record->nsites] = ts.getCovMatrix()[7];
+ _track_record->cov_predicted_z0omega[_track_record->nsites] = ts.getCovMatrix()[8];
+ _track_record->cov_predicted_z0z0[_track_record->nsites] = ts.getCovMatrix()[9];
+ _track_record->cov_predicted_tanLd0[_track_record->nsites] = ts.getCovMatrix()[10];
+ _track_record->cov_predicted_tanLphi0[_track_record->nsites] = ts.getCovMatrix()[11];
+ _track_record->cov_predicted_tanLomega[_track_record->nsites] = ts.getCovMatrix()[12];
+ _track_record->cov_predicted_tanLz0[_track_record->nsites] = ts.getCovMatrix()[13];
+ _track_record->cov_predicted_tanLtanL[_track_record->nsites] = ts.getCovMatrix()[14];
+
+
+ streamlog_out(DEBUG2) << "DiagnosticsController::record_site get FILTERED trackstate " << std::endl;
+
+ TKalTrackState& trkState_filtered = (TKalTrackState&) site->GetState(TVKalSite::kFiltered); // this segfaults if no hits are present
+
+ THelicalTrack helix_filtered = trkState_filtered.GetHelix() ;
+ TMatrixD c0_filtered(trkState_filtered.GetCovMat());
+
+ DF.UnitMatrix();
+ helix_filtered.MoveTo( tpoint , dPhi , &DF , &c0_filtered) ; // move helix to desired point, and get propagator matrix
+
+ IMPL::TrackStateImpl ts_f;
+
+ _current_track->ToLCIOTrackState( helix_filtered, c0_filtered, ts_f, chi2, ndf );
+
+ _track_record->d0_filtered[_track_record->nsites] = ts_f.getD0() ;
+ _track_record->phi0_filtered[_track_record->nsites] = ts_f.getPhi() ;
+ _track_record->omega_filtered[_track_record->nsites] = ts_f.getOmega() ;
+ _track_record->z0_filtered[_track_record->nsites] = ts_f.getZ0() ;
+ _track_record->tanL_filtered[_track_record->nsites] = ts_f.getTanLambda() ;
+
+
+ _track_record->cov_filtered_d0d0[_track_record->nsites] = ts_f.getCovMatrix()[0];
+ _track_record->cov_filtered_phi0d0[_track_record->nsites] = ts_f.getCovMatrix()[1];
+ _track_record->cov_filtered_phi0phi0[_track_record->nsites] = ts_f.getCovMatrix()[2];
+ _track_record->cov_filtered_omegad0[_track_record->nsites] = ts_f.getCovMatrix()[3];
+ _track_record->cov_filtered_omegaphi0[_track_record->nsites] = ts_f.getCovMatrix()[4];
+ _track_record->cov_filtered_omegaomega[_track_record->nsites] = ts_f.getCovMatrix()[5];
+ _track_record->cov_filtered_z0d0[_track_record->nsites] = ts_f.getCovMatrix()[6];
+ _track_record->cov_filtered_z0phi0[_track_record->nsites] = ts_f.getCovMatrix()[7];
+ _track_record->cov_filtered_z0omega[_track_record->nsites] = ts_f.getCovMatrix()[8];
+ _track_record->cov_filtered_z0z0[_track_record->nsites] = ts_f.getCovMatrix()[9];
+ _track_record->cov_filtered_tanLd0[_track_record->nsites] = ts_f.getCovMatrix()[10];
+ _track_record->cov_filtered_tanLphi0[_track_record->nsites] = ts_f.getCovMatrix()[11];
+ _track_record->cov_filtered_tanLomega[_track_record->nsites] = ts_f.getCovMatrix()[12];
+ _track_record->cov_filtered_tanLz0[_track_record->nsites] = ts_f.getCovMatrix()[13];
+ _track_record->cov_filtered_tanLtanL[_track_record->nsites] = ts_f.getCovMatrix()[14];
+
+
+
+ _track_record->ref_point_x[_track_record->nsites] = tpoint.X();
+ _track_record->ref_point_y[_track_record->nsites] = tpoint.Y();
+ _track_record->ref_point_z[_track_record->nsites] = tpoint.Z();
+
+
+ }
+
+ ++_track_record->nsites;
+
+ if (_track_record->nsites > MAX_SITES) {
+ _skip_track = true ;
+ this->clear_track_record();
+ streamlog_out(DEBUG4) << " DiagnosticsController::end_track: Number of site too large. Track Skipped. nsites = " << _track_record->nsites << " : maximum number of site allowed = " << MAX_SITES << std::endl;
+ return;
+ }
+
+
+ streamlog_out(DEBUG2) << "_track_record->nsites = " << _track_record->nsites << std::endl;
+ }
+
+ void DiagnosticsController::end_track(){
+
+
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::end_track: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+ streamlog_out(DEBUG3) << " DiagnosticsController::end_track called " << std::endl;
+
+ if ( _skip_track ) { // just clear the track buffers and return.
+ ++_ntracks_skipped;
+ this->clear_track_record();
+ return;
+ } else {
+
+ _track_record->chi2 = _current_track->_kaltrack->GetChi2();
+ _track_record->ndf = _current_track->_kaltrack->GetNDF();
+ _track_record->prob = TMath::Prob(_track_record->chi2, _track_record->ndf);
+
+ TIter it(_current_track->_kaltrack,kIterForward);
+
+ Int_t nsites = _current_track->_kaltrack->GetEntries();
+
+ if (nsites > MAX_SITES) {
+ _skip_track = true ;
+ ++_ntracks_skipped;
+ this->clear_track_record();
+ streamlog_out(DEBUG4) << " DiagnosticsController::end_track: Number of site too large. Track Skipped. nsites = " << nsites << " : maximum number of site allowed = " << MAX_SITES << std::endl;
+ return;
+ }
+
+
+ if( _current_track->_smoothed ){
+
+ for (Int_t isite=1; isite<nsites; isite++) {
+
+ UTIL::BitField64 encoder(lcio::ILDCellID0::encoder_string);
+ encoder.setValue( _track_record->CellID0[isite] );
+
+
+ TVKalSite* site = static_cast<TVKalSite *>( _current_track->_kaltrack->At(isite));
+
+ if ( _track_record->rejected[isite] == 0 && encoder[lcio::ILDCellID0::subdet] != 0 ) {
+
+
+ _track_record->chi2_inc_smoothed[isite] = site->GetDeltaChi2();
+
+ TKalTrackState& trkState_smoothed = (TKalTrackState&) site->GetState(TVKalSite::kSmoothed); // this segfaults if no hits are present
+
+ THelicalTrack helix_smoothed = trkState_smoothed.GetHelix() ;
+ TMatrixD c0_smoothed(trkState_smoothed.GetCovMat());
+
+ Int_t sdim = trkState_smoothed.GetDimension(); // dimensions of the track state, it will be 5 or 6
+
+ // move track state to the sim hit for comparison
+ const TVector3 tpoint( _track_record->ref_point_x[isite], _track_record->ref_point_y[isite], _track_record->ref_point_z[isite] ) ;
+
+ // now move to the point
+ TKalMatrix DF(sdim,sdim);
+ DF.UnitMatrix();
+
+ double dPhi=0;
+
+ helix_smoothed.MoveTo( tpoint , dPhi , &DF , &c0_smoothed) ; // move helix to desired point, and get propagator matrix
+
+ IMPL::TrackStateImpl ts;
+ int ndf;
+ double chi2;
+
+ _current_track->ToLCIOTrackState( helix_smoothed, c0_smoothed, ts, chi2, ndf );
+
+
+ _track_record->d0_smoothed[isite] = ts.getD0() ;
+ _track_record->phi0_smoothed[isite] = ts.getPhi() ;
+ _track_record->omega_smoothed[isite] = ts.getOmega() ;
+ _track_record->z0_smoothed[isite] = ts.getZ0() ;
+ _track_record->tanL_smoothed[isite] = ts.getTanLambda() ;
+
+
+ _track_record->cov_smoothed_d0d0[isite] = ts.getCovMatrix()[0];
+ _track_record->cov_smoothed_phi0d0[isite] = ts.getCovMatrix()[1];
+ _track_record->cov_smoothed_phi0phi0[isite] = ts.getCovMatrix()[2];
+ _track_record->cov_smoothed_omegad0[isite] = ts.getCovMatrix()[3];
+ _track_record->cov_smoothed_omegaphi0[isite] = ts.getCovMatrix()[4];
+ _track_record->cov_smoothed_omegaomega[isite] = ts.getCovMatrix()[5];
+ _track_record->cov_smoothed_z0d0[isite] = ts.getCovMatrix()[6];
+ _track_record->cov_smoothed_z0phi0[isite] = ts.getCovMatrix()[7];
+ _track_record->cov_smoothed_z0omega[isite] = ts.getCovMatrix()[8];
+ _track_record->cov_smoothed_z0z0[isite] = ts.getCovMatrix()[9];
+ _track_record->cov_smoothed_tanLd0[isite] = ts.getCovMatrix()[10];
+ _track_record->cov_smoothed_tanLphi0[isite] = ts.getCovMatrix()[11];
+ _track_record->cov_smoothed_tanLomega[isite] = ts.getCovMatrix()[12];
+ _track_record->cov_smoothed_tanLz0[isite] = ts.getCovMatrix()[13];
+ _track_record->cov_smoothed_tanLtanL[isite] = ts.getCovMatrix()[14];
+
+ }
+
+ }
+ }
+
+ IMPL::TrackStateImpl ts_at_ip;
+ int ndf;
+ double chi2;
+
+ gear::Vector3D point(0.0,0.0,0.0);
+ _current_track->propagate( point, ts_at_ip, chi2, ndf );
+
+ _track_record->d0_ip = ts_at_ip.getD0() ;
+ _track_record->phi0_ip = ts_at_ip.getPhi() ;
+ _track_record->omega_ip = ts_at_ip.getOmega() ;
+ _track_record->z0_ip = ts_at_ip.getZ0() ;
+ _track_record->tanL_ip = ts_at_ip.getTanLambda() ;
+
+
+ _track_record->cov_ip_d0d0 = ts_at_ip.getCovMatrix()[0];
+ _track_record->cov_ip_phi0d0 = ts_at_ip.getCovMatrix()[1];
+ _track_record->cov_ip_phi0phi0 = ts_at_ip.getCovMatrix()[2];
+ _track_record->cov_ip_omegad0 = ts_at_ip.getCovMatrix()[3];
+ _track_record->cov_ip_omegaphi0 = ts_at_ip.getCovMatrix()[4];
+ _track_record->cov_ip_omegaomega = ts_at_ip.getCovMatrix()[5];
+ _track_record->cov_ip_z0d0 = ts_at_ip.getCovMatrix()[6];
+ _track_record->cov_ip_z0phi0 = ts_at_ip.getCovMatrix()[7];
+ _track_record->cov_ip_z0omega = ts_at_ip.getCovMatrix()[8];
+ _track_record->cov_ip_z0z0 = ts_at_ip.getCovMatrix()[9];
+ _track_record->cov_ip_tanLd0 = ts_at_ip.getCovMatrix()[10];
+ _track_record->cov_ip_tanLphi0 = ts_at_ip.getCovMatrix()[11];
+ _track_record->cov_ip_tanLomega = ts_at_ip.getCovMatrix()[12];
+ _track_record->cov_ip_tanLz0 = ts_at_ip.getCovMatrix()[13];
+ _track_record->cov_ip_tanLtanL = ts_at_ip.getCovMatrix()[14];
+
+ _tree->Fill();
+
+ ++_ntracks_written;
+
+ }
+
+ }
+
+ void DiagnosticsController::end(){
+
+ if ( _recording_on == false ) {
+ return;
+ }
+
+ if ( _initialised == false ){
+ streamlog_out(ERROR) << "DiagnosticsController::end: Diagnostics not initialised call init(std::string root_file_name, std::string root_tree_name, bool recording_off) first : exit(1) called from file "
+ << __FILE__
+ << " line "
+ << __LINE__
+ << std::endl;
+
+ exit(1);
+ }
+
+ streamlog_out(DEBUG4) << " DiagnosticsController::end() called \n"
+ << "\t number of tracks written = " << _ntracks_written
+ << "\t number of tracks skipped = " << _ntracks_skipped
+ << std::endl;
+
+
+
+ // _tree->Print();
+ _root_file->Write();
+ _root_file->Close();
+ delete _root_file; _root_file = 0;
+
+ }
+
+
+}
+
+
+
+#endif
diff --git a/Service/TrackSystemSvc/src/DiagnosticsController.h.remove b/Service/TrackSystemSvc/src/DiagnosticsController.h.remove
new file mode 100644
index 00000000..2e42d0c3
--- /dev/null
+++ b/Service/TrackSystemSvc/src/DiagnosticsController.h.remove
@@ -0,0 +1,90 @@
+#ifndef DiagnosticsController_h
+#define DiagnosticsController_h
+
+#include "MarlinTrk/MarlinTrkDiagnostics.h"
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+class TFile;
+class TH1F;
+class TTree;
+class TKalMatrix;
+
+namespace EVENT {
+ class MCParticle;
+}
+
+class ILDVTrackHit;
+class TKalTrackSite;
+class MarlinTrkNtuple;
+
+namespace MarlinTrk{
+
+ class MarlinKalTestTrack;
+ class IMarlinTrack;
+
+
+ class DiagnosticsController {
+
+ public:
+
+ /** constructor */
+ DiagnosticsController();
+
+ /** Destructor */
+ virtual ~DiagnosticsController();
+
+
+ void init(std::string root_file_name, std::string root_Tree_name, bool _recording_on=true ) ;
+
+ void new_track(MarlinKalTestTrack* trk) ;
+
+ void set_intial_track_parameters(double d0, double phi0, double omega, double z0, double tanL, double pivot_x, double pivot_y, double pivot_z, TKalMatrix& cov);
+
+ void record_site(ILDVTrackHit* hit, TKalTrackSite* site);
+
+ void record_rejected_site(ILDVTrackHit* hit, TKalTrackSite* site);
+
+ void skip_current_track();
+
+ void end_track() ;
+
+ void end();
+
+
+ private:
+
+ DiagnosticsController(const DiagnosticsController&) ; // Prevent copy-construction
+ DiagnosticsController& operator=(const DiagnosticsController&) ; // Prevent assignment
+
+ void clear_track_record();
+
+ bool _initialised;
+ bool _recording_on;
+
+ int _ntracks_written;
+ int _ntracks_skipped;
+
+ std::string _root_file_name;
+ std::string _root_tree_name;
+
+ TFile* _root_file;
+ TTree* _tree;
+ MarlinTrkNtuple* _track_record;
+
+ MarlinKalTestTrack* _current_track;
+
+ EVENT::MCParticle* _currentMCP;
+
+ bool _mcpInfoStored;
+ bool _skip_track;
+
+
+ };
+
+
+}
+
+#endif
+
+#endif
diff --git a/Service/TrackSystemSvc/src/HelixFit.cc b/Service/TrackSystemSvc/src/HelixFit.cc
new file mode 100644
index 00000000..070bbf6c
--- /dev/null
+++ b/Service/TrackSystemSvc/src/HelixFit.cc
@@ -0,0 +1,577 @@
+//
+// HelixFit.cc
+// MarlinTrk
+//
+// Created by Steve Aplin on 9/16/11.
+//
+
+#include "TrackSystemSvc/IMarlinTrack.h"
+
+#include "TrackSystemSvc/HelixFit.h"
+#include "TrackSystemSvc/HelixTrack.h"
+//#include "streamlog/streamlog.h"
+
+#include "CLHEP/Matrix/SymMatrix.h"
+
+namespace MarlinTrk{
+
+
+ int HelixFit::fastHelixFit(int npt, double* xf, double* yf, float* rf, float* pf, double* wf, float* zf , float* wzf,int iopt,
+ float* vv0, float* ee0, float& ch2ph, float& ch2z){
+
+
+
+ if (npt < 3) {
+ //streamlog_out(ERROR) << "Cannot fit less than 3 points return 1" << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ double eps = 1.0e-16;
+#define ITMAX 15
+#define MPT 600
+#define MAX_CHI2 5000.0
+
+
+ float sp2[MPT],
+ del[MPT],deln[MPT],delzn[MPT],sxy[MPT],ss0[MPT],eee[MPT],
+ delz[MPT],grad[5],cov[15],vv1[5],dv[5];
+
+ // double xf[MPT],yf[MPT],wf[MPT],zf[MPT],wzf[MPT];
+
+ double alf,a0,a1,a2,a22,bet,cur,
+ dd,den,det,dy,d2,f,fact,fg,f1,g,gam,gam0,g1,
+ h,h2,p2,q2,rm,rn,
+ xa,xb,xd,xi,xm,xx,xy,x1,x2,den2,
+ ya,yb,yd,yi,ym,yy,y1,y2,wn,sa2b2,dd0,phic,aaa;
+
+
+ xm = 0.;
+ ym = 0.;
+
+ for(int i=1; i < 15; ++i){
+ ee0[i]=0.0;
+ }
+
+ for( int i=1; i < 5; ++i){
+ grad[i]= 0.0;
+ vv0[i] = 0.0;
+ }
+
+ float chi2=0.0;
+ ch2ph = 0.0;
+ ch2z = 0.0;
+
+ for (int i = 0; i<npt; ++i) {
+ sp2[i] = wf[i]*(rf[i]*rf[i]);
+ }
+
+
+ //std::cout << "npt = " << npt << std::endl;
+
+ // for(int i = 0; i<n; ++i){
+ //std::cout << "xf = " << xf[i] << " " << i << std::endl;
+ //std::cout << "yf = " << yf[i] << " " << i << std::endl;
+ //std::cout << "zf = " << zf[i] << " " << i << std::endl;
+ //std::cout << "rf = " << rf[i] << " " << i << std::endl;
+ //std::cout << "pf = " << pf[i] << " " << i << std::endl;
+ //std::cout << "wf = " << wf[i] << " " << i << std::endl;
+ //std::cout << "wzf = " << wzf[i] << " " << i << std::endl;
+ //}
+
+
+ wn=0.0;
+
+ for (int i =0; i<npt; ++i) {
+ xm = xm + xf[i]*wf[i];
+ ym = ym + yf[i]*wf[i];
+ wn = wn + wf[i];
+ }
+
+ rn = 1.0/wn;
+
+ xm = xm * rn;
+ ym = ym * rn;
+ x2 = 0.;
+ y2 = 0.;
+ xy = 0.;
+ xd = 0.;
+ yd = 0.;
+ d2 = 0.;
+
+ //std::cout << "xm = " << xm << std::endl;
+ //std::cout << "ym = " << ym << std::endl;
+ //std::cout << "rn = " << rn << std::endl;
+
+ for (int i =0; i<npt; ++i) {
+ xi = xf[i] - xm;
+ yi = yf[i] - ym;
+ xx = xi*xi;
+ yy = yi*yi;
+ x2 = x2 + xx*wf[i];
+ y2 = y2 + yy*wf[i];
+ xy = xy + xi*yi*wf[i];
+ dd = xx + yy;
+ xd = xd + xi*dd*wf[i];
+ yd = yd + yi*dd*wf[i];
+ d2 = d2 + dd*dd*wf[i];
+ }
+
+
+ //std::cout << "xd = " << xd << std::endl;
+ //std::cout << "yd = " << yd << std::endl;
+ //std::cout << "d2 = " << d2 << std::endl;
+
+
+ x2 = x2*rn;
+ y2 = y2*rn;
+ xy = xy*rn;
+ d2 = d2*rn;
+ xd = xd*rn;
+ yd = yd*rn;
+ f = 3.0* x2 + y2;
+ g = 3.0* y2 + x2;
+ fg = f*g;
+ h = xy + xy;
+ h2 = h*h;
+ p2 = xd*xd;
+ q2 = yd*yd;
+ gam0 = x2 + y2;
+ fact = gam0*gam0;
+ a2 = (fg-h2-d2)/fact;
+ fact = fact*gam0;
+ a1 = (d2*(f+g) - 2.0*(p2+q2))/fact;
+ fact = fact*gam0;
+ a0 = (d2*(h2-fg) + 2.0*(p2*g + q2*f) - 4.0*xd*yd*h)/fact;
+ a22 = a2 + a2;
+ yb = 1.0e30;
+ xa = 1.0;
+
+ //std::cout << "a0 = " << a0 << std::endl;
+ //std::cout << "a22 = " << a22 << std::endl;
+
+
+ // ** main iteration
+
+ for (int i = 0 ; i < ITMAX; ++i) {
+
+ ya = a0 + xa*(a1 + xa*(a2 + xa*(xa-4.0)));
+ dy = a1 + xa*(a22 + xa*(4.0*xa - 12.0));
+ xb = xa - ya/dy;
+
+ if (fabs(ya) > fabs(yb)) {
+ xb = 0.5 * (xb+xa) ;
+ }
+
+ if (fabs(xa-xb) < eps) break ;
+ xa = xb;
+ yb = ya;
+
+
+ }
+
+ // **
+
+ gam = gam0*xb;
+ f1 = f - gam;
+ g1 = g - gam;
+ x1 = xd*g1 - yd*h;
+ y1 = yd*f1 - xd*h;
+ det = f1*g1 - h2;
+ den2= 1.0/(x1*x1 + y1*y1 + gam*det*det);
+
+ if(den2 <= 0.0) {
+ // streamlog_out(ERROR) << "den2 less than or equal to zero"
+ // << " x1 = " << x1
+ // << " y1 = " << y1
+ // << " gam = " << gam
+ // << " det = " << det
+ // << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ den = sqrt(den2);
+ cur = det*den + 0.0000000001 ;
+ alf = -(xm*det + x1)*den ;
+ bet = -(ym*det + y1)*den ;
+ rm = xm*xm + ym*ym ;
+ gam = ((rm-gam)*det + 2.0*(xm*x1 + ym*y1))*den*0.5;
+
+
+ //
+ // --------> calculation of standard circle parameters
+ // nb: cur is always positive
+
+ double rr0=cur;
+ double asym = bet*xm-alf*ym;
+ double sst = 1.0;
+
+ if(asym < 0.0) {
+ sst = -1.0 ;
+ }
+
+ rr0 = sst*cur;
+
+ if( (alf*alf+bet*bet) <= 0.0 ){
+ // streamlog_out(ERROR) << "(alf*alf+bet*bet) less than or equal to zero" << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ sa2b2 = 1.0/sqrt(alf*alf+bet*bet);
+ dd0 = (1.0-1.0/sa2b2)/cur;
+ aaa = alf*sa2b2;
+
+ if( aaa > 1.0 ) aaa = 1.0;
+ if( aaa < -1.0 ) aaa =-1.0;
+
+ // std::cout << std::setprecision(10) << "aaa = " << aaa << std::endl;
+
+ phic = asin(aaa)+ M_PI_2;
+
+ if( bet > 0 ) phic = 2*M_PI - phic;
+
+ double ph0 = phic + M_PI_2;
+
+ if(rr0 <= 0.0) ph0=ph0-M_PI;
+ if(ph0 > 2*M_PI) ph0=ph0-2*M_PI;
+ if(ph0 < 0.0) ph0=ph0+2*M_PI;
+
+ vv0[0] = rr0;
+ vv0[2] = ph0;
+ vv0[3] = dd0;
+
+ // std::cout << std::setprecision(10) << "rr0 = " << rr0 << std::endl;
+ // std::cout << "ph0 = " << ph0 << std::endl;
+ // std::cout << "dd0 = " << dd0 << std::endl;
+
+ double check=sst*rr0*dd0;
+ // std::cout << "check = " << check << std::endl;
+
+ if(check > 1.0-eps && check < 1.0+eps) {
+ dd0 = dd0 - 0.007;
+ vv0[3]=dd0;
+ }
+
+ //
+ // -----> calculate phi distances to measured points
+ //
+
+ double aa0 = sst;
+ double ome = rr0;
+ double gg0 = ome*dd0-aa0;
+
+ double hh0 = 1.0/gg0;
+
+ // std::cout << "dd0 = " << dd0 << std::endl;
+ // std::cout << "ome = " << ome << std::endl;
+ // std::cout << "aa0 = " << aa0 << std::endl;
+ // std::cout << "hh0 = " << hh0 << std::endl;
+ // std::cout << "gg0 = " << gg0 << std::endl;
+
+ for (int i = 0 ; i < npt ; ++i) {
+
+ asym = bet*xf[i]-alf*yf[i] ;
+ ss0[i] = 1.0 ;
+
+ if(asym < 0.0) ss0[i] = -1.0 ;
+
+ double ff0 = ome*(rf[i]*rf[i]-dd0*dd0)/(2.0*rf[i]*gg0) + dd0/rf[i];
+
+ if(ff0 < -1.0) ff0 = -1.0;
+ if(ff0 > 1.0) ff0 = 1.0;
+
+ del[i]= ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) - pf[i];
+
+ // std::cout << "asin(ff0) = " << asin(ff0) << " i = " << i << std::endl;
+ // std::cout << "aa0 = " << aa0 << " i = " << i << std::endl;
+ // std::cout << "M_PI_2 = " << M_PI_2 << " i = " << i << std::endl;
+ // std::cout << "pf[i] = " << pf[i] << " i = " << i << std::endl;
+ // std::cout << "ff0 = " << ff0 << " i = " << i << std::endl;
+ // std::cout << "ss0[i] = " << ss0[i] << " i = " << i << std::endl;
+ // std::cout << "ph0 + (ss0[i]-aa0)* M_PI_2 = " << ph0 + (ss0[i]-aa0)* M_PI_2 << " i = " << i << std::endl;
+ // std::cout << "ss0[i]*asin(ff0) = " << ss0[i]*asin(ff0) << " i = " << i << std::endl;
+ // std::cout << "ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) = " << ph0 + (ss0[i]-aa0)* M_PI_2 + ss0[i]*asin(ff0) << std::endl;
+ // std::cout << "del[i] = " << del[i] << " i = " << i << std::endl;
+
+ if(del[i] > M_PI) del[i] = del[i] - 2*M_PI;
+ if(del[i] < -M_PI) del[i] = del[i] + 2*M_PI;
+
+
+
+ }
+
+
+
+ //
+ // -----> fit straight line in s-z
+ //
+
+
+ for (int i = 0 ; i < npt ; ++i) {
+
+ eee[i] = 0.5*vv0[0] * sqrt( fabs( (rf[i] * rf[i]-vv0[3]*vv0[3]) / (1.0-aa0*vv0[0]*vv0[3]) ) );
+
+ if(eee[i] > 0.99990) eee[i]= 0.99990;
+ if(eee[i] < -0.99990) eee[i]= -0.99990;
+
+ sxy[i]=2.0*asin(eee[i])/ome;
+
+ }
+
+
+ double sums = 0.0;
+ double sumss = 0.0;
+ double sumz = 0.0;
+ double sumzz = 0.0;
+ double sumsz = 0.0;
+ double sumw = 0.0;
+
+ for (int i = 0; i<npt; ++i) {
+ sumw = sumw + wzf[i];
+ sums = sums + sxy[i] * wzf[i];
+ sumss = sumss + sxy[i]*sxy[i] * wzf[i];
+ sumz = sumz + zf[i] * wzf[i];
+ sumzz = sumzz + zf[i]*zf[i] * wzf[i];
+ sumsz = sumsz + zf[i]*sxy[i] * wzf[i];
+ }
+
+ double denom = sumw*sumss - sums*sums;
+
+ if (fabs(denom) < eps){
+ // streamlog_out(ERROR) << "fabs(denom) less than or equal to zero" << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ double dzds = (sumw*sumsz-sums*sumz) /denom;
+ double zz0 = (sumss*sumz-sums*sumsz)/denom;
+
+ vv0[1]= dzds;
+ vv0[4]= zz0;
+
+ //
+ // -----> calculation chi**2
+ //
+ for (int i = 0 ; i<npt; ++i) {
+
+ delz[i]= zz0+dzds*sxy[i]-zf[i];
+ ch2ph = ch2ph + sp2[i]*del[i]*del[i];
+ ch2z = ch2z + wzf[i]*delz[i]*delz[i];
+ chi2 = ch2ph + ch2z;
+
+ }
+
+ if(chi2 > MAX_CHI2) {
+ // streamlog_out(ERROR) << "Chi2 greater than " << MAX_CHI2 << "return 1 " << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ for (int i = 0 ; i<npt; ++i) {
+
+ double ff0 = ome*(rf[i]*rf[i]-dd0*dd0)/(2.0*rf[i]*gg0) + dd0/rf[i];
+
+ if (ff0 > 0.99990) ff0 = 0.99990;
+
+ if (ff0 < -0.99990) ff0 = -0.99990;
+
+ double eta = ss0[i]/sqrt(fabs((1.0+ff0)*(1.0-ff0)));
+ double dfd = (1.0+hh0*hh0*(1.0-ome*ome*rf[i]*rf[i]))/(2.0*rf[i]);
+ double dfo = -aa0*(rf[i]*rf[i]-dd0*dd0)*hh0*hh0/(2.0*rf[i]);
+ double dpd = eta*dfd;
+ double dpo = eta*dfo;
+
+ // -----> derivatives of z component
+ double ggg = eee[i]/sqrt(fabs( (1.0+eee[i])*(1.0-eee[i])));
+ double dza = sxy[i];
+ check = rf[i]*rf[i]-vv0[3]*vv0[3];
+
+ if(fabs(check) > 1.0-eps) check=2.*0.007;
+
+ double dzd = 2.0*( vv0[1]/vv0[0] ) * fabs( ggg ) * ( 0.5*aa0*vv0[0]/( 1.0-aa0*vv0[3]*vv0[0] )-vv0[3]/check );
+
+ double dzo = -vv0[1]*sxy[i]/vv0[0] + vv0[1] * ggg/( vv0[0]*vv0[0]) * ( 2.0+ aa0*vv0[0]*vv0[3]/(1.0-aa0*vv0[0]*vv0[3]) );
+
+ // -----> error martix
+
+ ee0[0] = ee0[0] + sp2[i]* dpo*dpo + wzf[i] * dzo*dzo;
+ ee0[1] = ee0[1] + wzf[i] * dza*dzo;
+ ee0[2] = ee0[2] + wzf[i] * dza*dza;
+ ee0[3] = ee0[3] + sp2[i]* dpo;
+ ee0[4] = ee0[4];
+ ee0[5] = ee0[5] + sp2[i];
+ ee0[6] = ee0[6] + sp2[i]* dpo*dpd + wzf[i] * dzo*dzd;
+ ee0[7] = ee0[7] + wzf[i] * dza*dzd;
+ ee0[8] = ee0[8] + sp2[i]* dpd;
+ ee0[9] = ee0[9] + sp2[i]* dpd*dpd + wzf[i] * dzd*dzd;
+ ee0[10]= ee0[10] + wzf[i] * dzo;
+ ee0[11]= ee0[11] + wzf[i] * dza;
+ ee0[12]= ee0[12];
+ ee0[13]= ee0[13] + wzf[i] * dzd;
+ ee0[14]= ee0[14] + wzf[i];
+
+ // -----> gradient vector
+ grad[0]=grad[0] - del[i] *sp2[i]*dpo - delz[i]*wzf[i]*dzo;
+ grad[1]=grad[1] - delz[i]*wzf[i]*dza;
+ grad[2]=grad[2] - del[i] *sp2[i];
+ grad[3]=grad[3] - del[i] *sp2[i]*dpd - delz[i]*wzf[i]*dzd;
+ grad[4]=grad[4] - delz[i]*wzf[i];
+
+
+ }
+
+ if (iopt < 3) {
+ /*
+ streamlog_out(DEBUG1) << "HelixFit: " <<
+ " d0 = " << vv0[3] <<
+ " phi0 = " << vv0[2] <<
+ " omega = " << vv0[0] <<
+ " z0 = " << vv0[4] <<
+ " tanL = " << vv0[1] <<
+ std::endl;
+ */
+ return 0 ;
+ }
+
+ // ------> NEWTONS NEXT GUESS
+
+ for (int i =0; i<15; ++i) {
+ cov[i]=ee0[i];
+ }
+
+ // Convert Covariance Matrix
+ CLHEP::HepSymMatrix cov0(5) ;
+
+ int icov = 0 ;
+
+ for(int irow=0; irow<5; ++irow ){
+ for(int jcol=0; jcol<irow+1; ++jcol){
+ // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
+ // std::cout << "cov["<< icov << "] = " << _cov[icov] << std::endl ;
+ cov0[irow][jcol] = ee0[icov] ;
+ ++icov ;
+ }
+ }
+
+ int error = 0 ;
+ cov0.invert(error);
+
+ if( error != 0 ) {
+ //streamlog_out(ERROR) << "CLHEP Matrix inversion failed" << "return 1 " << std::endl;
+ ch2ph = 1.0e30;
+ ch2z = 1.0e30;
+ return 1;
+ }
+
+ icov = 0 ;
+
+ for(int irow=0; irow<5; ++irow ){
+ for(int jcol=0; jcol<irow+1; ++jcol){
+ // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
+ // std::cout << "cov["<< icov << "] = " << _cov[icov] << std::endl ;
+ cov[icov] = cov0[irow][jcol];
+ ++icov ;
+ }
+ }
+
+ // here we need to invert the matrix cov[15]
+
+
+
+ for (int i = 0; i<5; ++i) {
+ dv[i] = 0.0 ;
+ for (int j = 0; j<5; ++j) {
+ int index = 0;
+ if ( i >= j ) {
+ index = (i*i-i)/2+j ;
+ }
+ else{
+ index = (j*j-j)/2+i;
+ }
+ dv[i] = dv[i] + cov[index] * grad[j] ;
+ }
+ }
+
+
+ for (int i = 0; i<5; ++i) {
+ vv1[i] = vv0[i]+dv[i];
+ }
+
+ gg0 = vv1[0]*vv1[3]-aa0;
+
+ for (int i=0; i<npt; ++i) {
+ double ff0 = vv1[0]*(rf[i]*rf[i]-vv1[3]*vv1[3]) / (2.0*rf[i]*gg0) + vv1[3]/rf[i];
+
+ if (ff0 > 1) ff0 = 1.0;
+ if (ff0 < -1) ff0 = -1.0;
+
+ deln[i] = vv1[2] + (ss0[i]-aa0)*M_PI_2+ss0[i]*asin(ff0)-pf[i];
+
+ if(deln[i] > M_PI) deln[i] = deln[i] - 2*M_PI;
+ if(deln[i] < -M_PI) deln[i] = deln[i] + 2*M_PI;
+
+ eee[i] = 0.5*vv1[0]*sqrt( fabs( (rf[i]*rf[i]-vv1[3]*vv1[3]) / (1.0-aa0*vv1[0]*vv1[3]) ) );
+
+ if(eee[i] > 0.99990) eee[i]= 0.99990;
+ if(eee[i] < -0.99990) eee[i]= -0.99990;
+
+ sxy[i] = 2.0*asin(eee[i])/vv1[0];
+ delzn[i]= vv1[4]+vv1[1]*sxy[i]-zf[i];
+
+ }
+
+ double chi1 = 0.0;
+ ch2ph = 0.0;
+ ch2z = 0.0;
+
+ for (int i =0; i<5; ++i) {
+ chi1 = chi1 + sp2[i]*deln[i]*deln[i] + wzf[i]*delzn[i]*delzn[i];
+ ch2ph = ch2ph + sp2[i]*deln[i]*deln[i];
+ ch2z = ch2z + wzf[i]*delzn[i]*delzn[i];
+ }
+
+ if (chi1<chi2) {
+ for (int i =0; i<5; ++i) {
+ vv0[i] = vv1[i];
+ }
+ chi2 = chi1;
+ }
+ std::cout << "HelixFit: " <<
+ //streamlog_out(DEBUG1) << "HelixFit: " <<
+ " d0 = " << vv0[3] <<
+ " phi0 = " << vv0[2] <<
+ " omega = " << vv0[0] <<
+ " z0 = " << vv0[4] <<
+ " tanL = " << vv0[1] <<
+ std::endl;
+
+ return 0;
+
+ }
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Service/TrackSystemSvc/src/HelixTrack.cc b/Service/TrackSystemSvc/src/HelixTrack.cc
new file mode 100644
index 00000000..5934082d
--- /dev/null
+++ b/Service/TrackSystemSvc/src/HelixTrack.cc
@@ -0,0 +1,117 @@
+
+#include "TrackSystemSvc/IMarlinTrack.h"
+#include "TrackSystemSvc/HelixTrack.h"
+#include <cmath>
+#include <TVector3.h>
+#include <kaltest/THelicalTrack.h>
+#include <edm4hep/Vector3d.h> //plcio/DoubleThree.h>
+//#include "streamlog/streamlog.h"
+
+// defines if s of the helix increases in the direction of x2 to x3
+bool HelixTrack::forwards = true;
+
+HelixTrack::HelixTrack( const edm4hep::Vector3d& x1, const edm4hep::Vector3d& x2, const edm4hep::Vector3d& x3, double Bz, bool direction ){
+
+ // Make a KalTest THelicalTrack
+ TVector3 p1( x1[0], x1[1], x1[2] );
+ TVector3 p2( x2[0], x2[1], x2[2] );
+ TVector3 p3( x3[0], x3[1], x3[2] );
+ /*
+ streamlog_out(DEBUG2) << "HelixTrack::HelixTrack Create from hits: \n "
+ << "P1 x = " << p1.x() << " y = " << p1.y() << " z = " << p1.z() << " r = " << p1.Perp() << "\n "
+ << "P2 x = " << p2.x() << " y = " << p2.y() << " z = " << p2.z() << " r = " << p2.Perp() << "\n "
+ << "P3 x = " << p3.x() << " y = " << p3.y() << " z = " << p3.z() << " r = " << p3.Perp() << "\n "
+ << "Bz = " << Bz << " direction = " << direction
+ << std::endl;
+ */
+
+
+
+ THelicalTrack* helicalTrack;
+
+ helicalTrack = new THelicalTrack( p1, p2, p3, Bz, direction );
+
+ // Set the track parameters and convert from the KalTest system to the lcio system
+
+ _phi0 = toBaseRange( helicalTrack->GetPhi0() + M_PI/2. ) ;
+ _omega = 1. / helicalTrack->GetRho();
+ _z0 = helicalTrack->GetDz();
+ _d0 = - helicalTrack->GetDrho();
+ _tanLambda = helicalTrack->GetTanLambda();
+
+ _ref_point_x = helicalTrack->GetPivot().X() ;
+ _ref_point_y = helicalTrack->GetPivot().Y() ;
+ _ref_point_z = helicalTrack->GetPivot().Z() ;
+
+ delete helicalTrack;
+
+}
+
+
+HelixTrack::HelixTrack( const edm4hep::Vector3d& position, const edm4hep::Vector3d& p, double charge, double Bz ){
+
+ _ref_point_x = position[0] ;
+ _ref_point_y = position[1] ;
+ _ref_point_z = position[2] ;
+
+ _d0 = 0.0 ;
+ _z0 = 0.0 ;
+
+ const double pt = sqrt(p[0]*p[0]+p[1]*p[1]) ;
+
+ double radius = pt / (2.99792458E-4*Bz) ; // for r in mm, p in GeV and Bz in Tesla
+
+ _omega = charge/radius ;
+ _tanLambda = p[2]/pt ;
+
+ _phi0 = atan2(p[1],p[0]);
+
+ _phi0 = toBaseRange(_phi0);
+
+}
+
+double HelixTrack::moveRefPoint( double x, double y, double z){
+
+ const double radius = 1.0/_omega ;
+
+ const double sinPhi0 = sin(_phi0) ;
+ const double cosPhi0 = cos(_phi0) ;
+
+ const double deltaX = x - _ref_point_x ;
+ const double deltaY = y - _ref_point_y ;
+
+ double phi0Prime = atan2( sinPhi0 - (deltaX/(radius-_d0)) , cosPhi0 + (deltaY/(radius-_d0)) ) ;
+
+ while ( phi0Prime < 0 ) phi0Prime += 2.0*M_PI ;
+ while ( phi0Prime >= 2.0*M_PI ) phi0Prime -= 2.0*M_PI ;
+
+ const double d0Prime = _d0 + deltaX*sinPhi0 - deltaY*cosPhi0 + ( ( deltaX*cosPhi0 + deltaY*sinPhi0 ) * tan( (phi0Prime-_phi0) / 2.0) ) ;
+
+ // In order to have terms which behave well as Omega->0 we make use of deltaX and deltaY to replace sin( phi0Prime - phi0 ) and cos( phi0Prime - phi0 )
+
+ const double sinDeltaPhi = ( -_omega / ( 1.0 - ( _omega * d0Prime ) ) ) * ( deltaX * cosPhi0 + deltaY * sinPhi0 ) ;
+
+ const double cosDeltaPhi = 1.0 + ( _omega*_omega / ( 2.0 * ( 1.0 - _omega * d0Prime ) ) ) * ( d0Prime*d0Prime - ( deltaX + _d0 * sinPhi0 )*( deltaX + _d0 * sinPhi0 ) - ( deltaY - _d0 * cosPhi0 )*( deltaY - _d0 * cosPhi0 ) ) ;
+
+ const double s = atan2(-sinDeltaPhi,cosDeltaPhi) / _omega ;
+
+ const double z0Prime = _ref_point_z - z + _z0 + _tanLambda * s ;
+
+ phi0Prime = toBaseRange(phi0Prime);
+
+ _d0 = d0Prime ;
+ _phi0 = phi0Prime ;
+ _z0 = z0Prime ;
+
+ _ref_point_x = x;
+ _ref_point_y = y;
+ _ref_point_z = z;
+
+ return (s/radius);
+
+}
+
+
+
+
+
diff --git a/Service/TrackSystemSvc/src/IMarlinTrack.cc b/Service/TrackSystemSvc/src/IMarlinTrack.cc
new file mode 100644
index 00000000..494c1716
--- /dev/null
+++ b/Service/TrackSystemSvc/src/IMarlinTrack.cc
@@ -0,0 +1,38 @@
+
+#include "TrackSystemSvc/IMarlinTrack.h"
+
+namespace MarlinTrk{
+
+ const bool IMarlinTrack::backward = false ;
+ const bool IMarlinTrack::forward = ! IMarlinTrack::backward;
+
+ const int IMarlinTrack::modeBackward = - 1 ;
+ const int IMarlinTrack::modeClosest = 0 ;
+ const int IMarlinTrack::modeForward = + 1 ;
+
+
+ const int IMarlinTrack::success = 0 ; // no error
+ const int IMarlinTrack::error = 1 ;
+ const int IMarlinTrack::bad_intputs = 3 ;
+ const int IMarlinTrack::no_intersection = 4 ; // no intersection found
+ const int IMarlinTrack::site_discarded = 5 ; // measurement discarded by the fitter
+ const int IMarlinTrack::site_fails_chi2_cut = 6 ; // measurement discarded by the fitter due to chi2 cut
+ const int IMarlinTrack::all_sites_fail_fit = 7 ; // no single measurement added to the fit
+
+
+ /** Helper function to convert error return code to string */
+ std::string errorCode( int error ){
+
+ switch ( error ){
+ case IMarlinTrack::success : return "IMarlinTrack::success"; break;
+ case IMarlinTrack::error : return "IMarlinTrack::error"; break;
+ case IMarlinTrack::bad_intputs : return "IMarlinTrack::bad_intputs"; break;
+ case IMarlinTrack::no_intersection : return "IMarlinTrack::no_intersection"; break;
+ case IMarlinTrack::site_discarded : return "IMarlinTrack::site_discarded"; break;
+ case IMarlinTrack::site_fails_chi2_cut : return "IMarlinTrack::site_fails_chi2_cut"; break;
+ case IMarlinTrack::all_sites_fail_fit : return "IMarlinTrack::all_sites_fail_fit"; break;
+ default: return "UNKNOWN" ;
+ }
+ }
+
+}
diff --git a/Service/TrackSystemSvc/src/IMarlinTrkSystem.cc b/Service/TrackSystemSvc/src/IMarlinTrkSystem.cc
new file mode 100644
index 00000000..9ed3a16b
--- /dev/null
+++ b/Service/TrackSystemSvc/src/IMarlinTrkSystem.cc
@@ -0,0 +1,33 @@
+#include "TrackSystemSvc/IMarlinTrkSystem.h"
+#include <sstream>
+
+namespace MarlinTrk{
+
+ void IMarlinTrkSystem::setOption(unsigned CFGOption, bool val) {
+ _cfg.setOption( CFGOption, val ) ;
+ }
+
+
+ bool IMarlinTrkSystem::getOption( unsigned CFGOption) {
+ return _cfg[ CFGOption] ;
+ }
+
+
+ std::string IMarlinTrkSystem::getOptions() {
+
+ std::stringstream ss ;
+ ss << _cfg ;
+ return ss.str() ;
+ }
+
+ void IMarlinTrkSystem::registerOptions() {
+
+ _cfg.registerOption( IMarlinTrkSystem::CFG::useQMS, "useMultipleScattering", true) ;
+ _cfg.registerOption( IMarlinTrkSystem::CFG::usedEdx, "useEnergyLoss", true) ;
+ _cfg.registerOption( IMarlinTrkSystem::CFG::useSmoothing, "useSmoothingInFit", false) ;
+
+
+ }
+
+
+}
diff --git a/Service/TrackSystemSvc/src/LCIOTrackPropagators.cc.remove b/Service/TrackSystemSvc/src/LCIOTrackPropagators.cc.remove
new file mode 100644
index 00000000..19a4f650
--- /dev/null
+++ b/Service/TrackSystemSvc/src/LCIOTrackPropagators.cc.remove
@@ -0,0 +1,489 @@
+
+#include "LCIOTrackPropagators.h"
+
+#include <cmath>
+#include <iostream>
+
+#include "IMPL/TrackStateImpl.h"
+
+#include "CLHEP/Matrix/Matrix.h"
+#include "CLHEP/Matrix/SymMatrix.h"
+#include "CLHEP/Vector/TwoVector.h"
+
+//#include "streamlog/streamlog.h"
+
+
+namespace LCIOTrackPropagators{
+
+ int PropagateLCIOToNewRef( IMPL::TrackStateImpl& ts, double xref, double yref, double zref ) {
+
+ // std::cout << "PropagateLCIOToNewRef: x:y:z = " << xref << " : " << yref << " : " << zref << std::endl ;
+
+ // Convert Parameters
+
+ const double d0 = ts.getD0() ;
+ const double phi0 = ts.getPhi() ;
+ const double omega = ts.getOmega() ;
+ const double z0 = ts.getZ0() ;
+ const double tanL = ts.getTanLambda() ;
+
+ // const double charge = omega/fabs(omega) ;
+ const float* ref = ts.getReferencePoint() ;
+
+ const double radius = 1.0/omega ;
+
+ const double sinPhi0 = sin(phi0) ;
+ const double cosPhi0 = cos(phi0) ;
+
+ const double deltaX = xref - ref[0] ;
+ const double deltaY = yref - ref[1] ;
+
+ double phi0Prime = atan2( sinPhi0 - (deltaX/(radius-d0)) , cosPhi0 + (deltaY/(radius-d0)) ) ;
+
+ while ( phi0Prime < 0 ) phi0Prime += 2.0*M_PI ;
+ while ( phi0Prime >= 2.0*M_PI ) phi0Prime -= 2.0*M_PI ;
+
+ const double d0Prime = d0 + deltaX*sinPhi0 - deltaY*cosPhi0 + ( ( deltaX*cosPhi0 + deltaY*sinPhi0 ) * tan( (phi0Prime-phi0) / 2.0) ) ;
+
+ // In order to have terms which behave well as Omega->0 we make use of deltaX and deltaY to replace sin( phi0Prime - phi0 ) and cos( phi0Prime - phi0 )
+
+ const double sinDeltaPhi = ( -omega / ( 1.0 - ( omega * d0Prime ) ) ) * ( deltaX * cosPhi0 + deltaY * sinPhi0 ) ;
+
+ const double cosDeltaPhi = 1.0 + ( omega*omega / ( 2.0 * ( 1.0 - omega * d0Prime ) ) ) * ( d0Prime*d0Prime - ( deltaX + d0 * sinPhi0 )*( deltaX + d0 * sinPhi0 ) - ( deltaY - d0 * cosPhi0 )*( deltaY - d0 * cosPhi0 ) ) ;
+
+ const double s = atan2(-sinDeltaPhi,cosDeltaPhi) / omega ;
+
+ const double z0Prime = ref[2] - zref + z0 + tanL * s ;
+
+
+ // Convert Covariance Matrix
+ CLHEP::HepSymMatrix cov0(5) ;
+
+ int icov = 0 ;
+
+ for(int irow=0; irow<5; ++irow ){
+ for(int jcol=0; jcol<irow+1; ++jcol){
+ // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
+ // std::cout << "cov["<< icov << "] = " << _cov[icov] << std::endl ;
+ cov0[irow][jcol] = ts.getCovMatrix()[icov] ;
+ ++icov ;
+ }
+ }
+
+ CLHEP::HepMatrix propagatorMatrix(5, 5, 0) ;
+
+ // LC_0 = { d0, phi0, omega, z0, tanLambda }
+
+ // d d0' / d LC_0
+ propagatorMatrix(1,1) = cosDeltaPhi ;
+ propagatorMatrix(1,2) = -( radius - d0 ) * sinDeltaPhi ;
+ propagatorMatrix(1,3) = radius*radius * ( cosDeltaPhi -1 ) ;
+
+ // d phi0' / d LC_0
+ propagatorMatrix(2,1) = sinDeltaPhi / ( radius - d0Prime ) ;
+ propagatorMatrix(2,2) = ( ( radius - d0 ) * cosDeltaPhi ) / ( radius - d0Prime ) ;
+ propagatorMatrix(2,3) = radius*radius * sinDeltaPhi / ( radius - d0Prime ) ;
+
+ // d omega' / d LC_0
+ propagatorMatrix(3,3) = 1.0 ;
+
+ // d z0' / d LC_0
+ propagatorMatrix(4,1) = radius * tanL * sinDeltaPhi / ( d0Prime + radius ) ;
+ propagatorMatrix(4,2) = radius * tanL * ( 1.0 - ( ( d0 + radius ) * cosDeltaPhi / ( d0Prime + radius ) ) ) ;
+ propagatorMatrix(4,3) = radius*radius * tanL * ( (phi0Prime - phi0) - radius * sinDeltaPhi / ( d0Prime + radius ) ) ;
+ propagatorMatrix(4,4) = 1.0 ;
+ propagatorMatrix(4,5) = s ;
+
+ // d tanLambda' / d LC_0
+ propagatorMatrix(5,5) = 1.0 ;
+
+
+ CLHEP::HepSymMatrix covPrime = cov0.similarity(propagatorMatrix);
+
+ EVENT::FloatVec cov( 15 ) ;
+
+ icov = 0 ;
+
+ for(int irow=0; irow<5; ++irow ){
+ for(int jcol=0; jcol<irow+1; ++jcol){
+ // std::cout << "row = " << irow << " col = " << jcol << std::endl ;
+ cov[icov] = covPrime[irow][jcol] ;
+ // std::cout << "lcCov["<< icov << "] = " << lcCov[icov] << std::endl ;
+ ++icov ;
+ }
+ }
+
+ while ( phi0Prime < -M_PI ) phi0Prime += 2.0*M_PI ;
+ while ( phi0Prime >= M_PI ) phi0Prime -= 2.0*M_PI ;
+
+ ts.setD0( d0Prime ) ;
+ ts.setPhi( phi0Prime ) ;
+ ts.setOmega( omega ) ;
+ ts.setZ0( z0Prime ) ;
+ ts.setTanLambda( tanL ) ;
+
+
+ float refPointPrime[3] ;
+ refPointPrime[0] = xref ;
+ refPointPrime[1] = yref ;
+ refPointPrime[2] = zref ;
+
+ ts.setReferencePoint(refPointPrime) ;
+
+ ts.setCovMatrix( cov ) ;
+
+ return 0 ;
+
+
+ }
+
+ // Propagate track to a new reference point taken as its crossing point with a cylinder of infinite length centered at x0,y0, parallel to the z axis.
+ // For direction== 0 the closest crossing point will be taken
+ // For direction== 1 the first crossing traversing in positive s will be taken
+ // For direction==-1 the first crossing traversing in negative s will be taken
+
+ int PropagateLCIOToCylinder( IMPL::TrackStateImpl& ts, float r0, float x0, float y0, int direction, double epsilon){
+
+ // taken from http://paulbourke.net/geometry/2circle/tvoght.c
+
+ // std::cout << "PropagateLCIOToCylinder: r = " << r0 << " x0:y0 = " << x0 << " : " << y0 << " direction = " << direction << std::endl ;
+
+
+ const double x_ref = ts.getReferencePoint()[0] ;
+ const double y_ref = ts.getReferencePoint()[1] ;
+ const double z_ref = ts.getReferencePoint()[2] ;
+
+ const double d0 = ts.getD0() ;
+ const double z0 = ts.getZ0() ;
+ const double phi0 = ts.getPhi() ;
+ const double tanl = ts.getTanLambda() ;
+ const double omega = ts.getOmega() ;
+
+ const double rho = 1.0 / omega ;
+ const double x_pca = x_ref - d0 * sin(phi0) ;
+ const double y_pca = y_ref + d0 * cos(phi0) ;
+ const double z_pca = z_ref + z0 ;
+
+ const double sin_phi0 = sin(phi0);
+ const double cos_phi0 = cos(phi0);
+
+ const double x_c = x_ref + ( rho - d0) * sin_phi0 ;
+ const double y_c = y_ref - ( rho - d0) * cos_phi0 ;
+
+ const double r1 = fabs(rho) ;
+
+ /* dx and dy are the vertical and horizontal distances between
+ * the circle centers.
+ */
+ const double dx = x_c - x0;
+ const double dy = y_c - y0;
+
+ /* Determine the straight-line distance between the centers. */
+ const double d = hypot(dx,dy);
+
+ /* Check for solvability. */
+ if (d > (r0 + r1))
+ {
+ /* no solution. circles do not intersect. */
+ return 1;
+ }
+ if (d < fabs(r0 - r1))
+ {
+ /* no solution. one circle is contained in the other */
+ return 2;
+ }
+ if (d < epsilon)
+ {
+ /* no solution. circles have common centre */
+ return 3;
+ }
+
+ /* 'point 2' is the point where the line through the circle
+ * intersection points crosses the line between the circle
+ * centers.
+ */
+
+ /* Determine the distance from point 0 to point 2. */
+ const double a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
+
+ /* Determine the coordinates of point 2. */
+ const double x2 = x0 + (dx * a/d);
+ const double y2 = y0 + (dy * a/d);
+
+ /* Determine the distance from point 2 to either of the
+ * intersection points.
+ */
+ const double h = sqrt((r0*r0) - (a*a));
+
+ /* Now determine the offsets of the intersection points from
+ * point 2.
+ */
+ const double rx = -dy * (h/d);
+ const double ry = dx * (h/d);
+
+ /* Determine the absolute intersection points. */
+ const double x_ins1 = x2 + rx;
+ const double y_ins1 = y2 + ry;
+
+ const double x_ins2 = x2 - rx;
+ const double y_ins2 = y2 - ry;
+
+ // std::cout << "PropagateLCIOToCylinder: 1st solution x:y = " << x_ins1 << " : " << y_ins1 << std::endl ;
+ // std::cout << "PropagateLCIOToCylinder: 2nd solution x:y = " << x_ins2 << " : " << y_ins2 << std::endl ;
+
+ // now calculate the path lengths
+ double s_1 = 0.0 ;
+ double s_2 = 0.0 ;
+
+ const double delta_x1 = x_ins1 - x_pca ;
+ const double delta_y1 = y_ins1 - y_pca ;
+
+ const double sin_delta_phi1 = - omega*delta_x1*cos_phi0 - omega*delta_y1*sin_phi0 ;
+ const double cos_delta_phi1 = 1.0 - omega*delta_x1*sin_phi0 + omega*delta_y1*cos_phi0 ;
+
+ s_1 = atan2(-sin_delta_phi1,cos_delta_phi1) / omega ;
+
+
+ const double delta_x2 = x_ins2 - x_pca ;
+ const double delta_y2 = y_ins2 - y_pca ;
+
+ const double sin_delta_phi2 = - omega*delta_x2*cos_phi0 - omega*delta_y2*sin_phi0 ;
+ const double cos_delta_phi2 = 1.0 - omega*delta_x2*sin_phi0 + omega*delta_y2*cos_phi0 ;
+
+ s_2 = atan2(-sin_delta_phi2,cos_delta_phi2) / omega ;
+
+ double x=0, y=0, z=0 ;
+
+ if( direction == 0 ) { // take closest intersection
+ if( fabs(s_1) < fabs(s_2) ) {
+ // std::cout << "PropagateLCIOToCylinder: use 1st solution" << std::endl ;
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else {
+ // std::cout << "PropagateLCIOToCylinder: use 2nd solution" << std::endl ;
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+
+ else {
+
+ if ( s_1 < 0.0 ) s_1 += 2.0*M_PI * r1 ;
+ if ( s_2 < 0.0 ) s_2 += 2.0*M_PI * r1 ;
+
+ if( direction == 1 ){ // take the intersection with smallest s
+ if( s_1 < s_2 ) {
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else {
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+ else if(direction == -1) { // else take the intersection with largest s
+ if( s_1 > s_2 ){
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else{
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+ }
+
+
+ return PropagateLCIOToNewRef(ts,x,y,z);
+
+ }
+
+ int PropagateLCIOToZPlane( IMPL::TrackStateImpl& ts, float z) {
+
+
+ const double x_ref = ts.getReferencePoint()[0] ;
+ const double y_ref = ts.getReferencePoint()[1] ;
+ const double z_ref = ts.getReferencePoint()[2] ;
+
+ const double d0 = ts.getD0() ;
+ const double z0 = ts.getZ0() ;
+ const double phi0 = ts.getPhi() ;
+ const double tanl = ts.getTanLambda() ;
+ const double omega = ts.getOmega() ;
+
+ const double x_pca = x_ref - d0 * sin(phi0) ;
+ const double y_pca = y_ref + d0 * cos(phi0) ;
+ const double z_pca = z_ref + z0 ;
+
+ // get path length to crossing point
+ const double s = ( z - z_pca ) / tanl;
+
+ const double delta_phi_half = (omega*s)/2.0 ;
+
+ const double x = x_pca + s * ( sin(delta_phi_half) / delta_phi_half ) * cos( phi0 - delta_phi_half ) ;
+ const double y = y_pca + s * ( sin(delta_phi_half) / delta_phi_half ) * sin( phi0 - delta_phi_half ) ;
+
+ return PropagateLCIOToNewRef(ts,x,y,z);
+
+ }
+
+
+
+ // Propagate track to a new reference point taken as its crossing point with a plane parallel to the z axis, containing points x1,x2 and y1,y2. Tolerance for intersection determined by epsilon.
+ // For direction == 0 the closest crossing point will be taken
+ // For direction == 1 the first crossing traversing in positive s will be taken
+ // For direction == -1 the first crossing traversing in negative s will be taken
+ int PropagateLCIOToPlaneParralelToZ( IMPL::TrackStateImpl& ts, float x1, float y1, float x2, float y2, int direction, double epsilon) {
+
+ // check that direction has one of the correct values
+ if( !( direction == 0 || direction == 1 || direction == -1) ) return -1 ;
+
+ // taken from http://paulbourke.net/geometry/sphereline/raysphere.c
+
+ const double x_ref = ts.getReferencePoint()[0] ;
+ const double y_ref = ts.getReferencePoint()[1] ;
+ const double z_ref = ts.getReferencePoint()[2] ;
+
+ const double d0 = ts.getD0() ;
+ const double z0 = ts.getZ0() ;
+ const double phi0 = ts.getPhi() ;
+ const double tanl = ts.getTanLambda() ;
+ const double omega = ts.getOmega() ;
+
+ const double rho = 1.0 / omega ;
+ const double x_pca = x_ref - d0 * sin(phi0) ;
+ const double y_pca = y_ref + d0 * cos(phi0) ;
+ const double z_pca = z_ref + z0 ;
+
+ const double sin_phi0 = sin(phi0);
+ const double cos_phi0 = cos(phi0);
+
+ const double x_c = x_ref + ( rho - d0) * sin_phi0 ;
+ const double y_c = y_ref - ( rho - d0) * cos_phi0 ;
+
+ const double dx = x2 - x1 ;
+ const double dy = y2 - y1 ;
+
+ const double a = dx * dx + dy * dy ;
+
+ const double b = 2.0 * ( dx * (x1 - x_c) + dy * (y1 - y_c) ) ;
+
+ double c = x_c * x_c + y_c * y_c;
+ c += x1 * x1 + y1 * y1 ;
+
+ c -= 2.0 * ( x_c * x1 + y_c * y1 );
+ c -= rho * rho;
+
+ const double bb4ac = b * b - 4.0 * a * c;
+
+ double u1 ; // first solution
+ double u2 ; // second solution
+
+ /* Check for solvability. */
+ if (bb4ac + epsilon < 0.0 ) { // no intersection
+ return(1);
+ }
+ else if(bb4ac - epsilon < 0.0) { // circle intersects at one point, tangential
+ return(2);
+ }
+ else{
+ u1 = (-b + sqrt(bb4ac)) / (2.0 * a);
+ u2 = (-b - sqrt(bb4ac)) / (2.0 * a);
+ }
+
+ const double x_ins1 = x1 + u1 * (dx) ;
+ const double y_ins1 = y1 + u1 * (dy) ;
+
+ const double x_ins2 = x1 + u2 * (dx) ;
+ const double y_ins2 = y1 + u2 * (dy) ;
+
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: 1st solution u = " << u1 << " : " << u1 * dx << " " << u1 * dy << std::endl ;
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: 2nd solution u = " << u2 << " : " << u2 * dx << " " << u2 * dy << std::endl ;
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: 1st solution x:y = " << x_ins1 << " : " << y_ins1 << std::endl ;
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: 2nd solution x:y = " << x_ins2 << " : " << y_ins2 << std::endl ;
+
+ // now calculate the path lengths
+ double s_1 = 0.0 ;
+ double s_2 = 0.0 ;
+
+ const double delta_x1 = x_ins1 - x_pca ;
+ const double delta_y1 = y_ins1 - y_pca ;
+
+ const double sin_delta_phi1 = - omega*delta_x1*cos_phi0 - omega*delta_y1*sin_phi0 ;
+ const double cos_delta_phi1 = 1.0 - omega*delta_x1*sin_phi0 + omega*delta_y1*cos_phi0 ;
+
+ s_1 = atan2(-sin_delta_phi1,cos_delta_phi1) / omega ;
+
+ const double delta_x2 = x_ins2 - x_pca ;
+ const double delta_y2 = y_ins2 - y_pca ;
+
+ const double sin_delta_phi2 = - omega*delta_x2*cos_phi0 - omega*delta_y2*sin_phi0 ;
+ const double cos_delta_phi2 = 1.0 - omega*delta_x2*sin_phi0 + omega*delta_y2*cos_phi0 ;
+
+ s_2 = atan2(-sin_delta_phi2,cos_delta_phi2) / omega ;
+
+ double x(0.0), y(0.0), z(0.0) ;
+
+ if( direction == 0 ) { // take closest intersection
+ if( fabs(s_1) < fabs(s_2) ) {
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 1st solution " << std::endl;
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else {
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 2nd solution " << std::endl;
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+
+ else{
+
+ if ( s_1 < 0.0 ) s_1 += 2.0*M_PI * fabs(rho) ;
+ if ( s_2 < 0.0 ) s_2 += 2.0*M_PI * fabs(rho) ;
+
+ if( direction == 1 ){ // take the intersection with smallest s
+ if( s_1 < s_2 ) {
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 1st solution " << std::endl;
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else {
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 2nd solution " << std::endl;
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+ else if( direction == -1 ) { // else take the intersection with largest s
+ if( s_1 > s_2 ){
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 1st solution " << std::endl;
+ x = x_ins1 ;
+ y = y_ins1 ;
+ z = z_pca + s_1 * tanl ;
+ }
+ else{
+ // std::cout << "PropagateLCIOToPlaneParralelToZ: take 2nd solution " << std::endl;
+ x = x_ins2 ;
+ y = y_ins2 ;
+ z = z_pca + s_2 * tanl ;
+ }
+ }
+ }
+
+ return PropagateLCIOToNewRef(ts,x,y,z);
+
+ }
+
+
+} // end of TrackPropagators
diff --git a/Service/TrackSystemSvc/src/MarlinKalTest.cc b/Service/TrackSystemSvc/src/MarlinKalTest.cc
new file mode 100644
index 00000000..ee0cd734
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinKalTest.cc
@@ -0,0 +1,520 @@
+#include "MarlinKalTest.h"
+#include "MarlinKalTestTrack.h"
+
+#include "kaltest/TKalDetCradle.h"
+#include "kaltest/TVKalDetector.h"
+#include "kaltest/THelicalTrack.h"
+
+#include "kaldet/ILDVMeasLayer.h"
+
+#include "kaldet/ILDSupportKalDetector.h"
+#include "kaldet/ILDVXDKalDetector.h"
+#include "kaldet/ILDSITKalDetector.h"
+#include "kaldet/ILDSITCylinderKalDetector.h"
+#include "kaldet/ILDSETKalDetector.h"
+#include "kaldet/ILDFTDKalDetector.h"
+#include "kaldet/ILDFTDDiscBasedKalDetector.h"
+#include "kaldet/ILDTPCKalDetector.h"
+
+#include "kaldet/LCTPCKalDetector.h"
+
+//SJA:FIXME: only needed for storing the modules in the layers map
+#include <UTIL/BitField64.h>
+#include "UTIL/ILDConf.h"
+
+#include "gear/GEAR.h"
+#include "gear/BField.h"
+#include "gear/TPCParameters.h"
+#include "gear/PadRowLayout2D.h"
+#include "DetInterface/IGeoSvc.h"
+
+#include <math.h>
+#include <cmath>
+
+#include <utility>
+
+//#include "streamlog/streamlog.h"
+
+#include "kaldet/ILDMeasurementSurfaceStoreFiller.h"
+
+namespace MarlinTrk{
+
+ MarlinKalTest::MarlinKalTest( const gear::GearMgr& gearMgr, IGeoSvc* geoSvc) :
+ _ipLayer(NULL) ,
+ _gearMgr( &gearMgr ),
+ _geoSvc(geoSvc){
+
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - initializing the detector ..." << std::endl ;
+
+ _det = new TKalDetCradle ; // from kaltest. TKalDetCradle inherits from TObjArray ...
+ _det->SetOwner( true ) ; // takes care of deleting subdetector in the end ...
+
+ is_initialised = false;
+
+ this->registerOptions() ;
+
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - established " << std::endl ;
+
+
+ }
+
+ MarlinKalTest::~MarlinKalTest(){
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+ _diagnostics.end();
+#endif
+
+ delete _det ;
+ }
+
+
+ void MarlinKalTest::init() {
+
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - call this init " << std::endl ;
+
+
+ MeasurementSurfaceStore& surfstore = _gearMgr->getMeasurementSurfaceStore();
+
+ // Check if the store is filled if not fill it. NOTE: In the case it is filled we just take what we are given and in debug print a message
+ if( surfstore.isFilled() == false ) {
+
+ ILDMeasurementSurfaceStoreFiller filler( *_gearMgr );
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - set up gear surface store using " << filler.getName() << std::endl ;
+ surfstore.FillStore(&filler);
+
+ }
+ else {
+
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - MeasurementSurfaceStore is already full. Using store as filled by MeasurementSurfaceStoreFiller " << surfstore.getFillerName() << std::endl ;
+
+ }
+
+ if (_gearMgr -> getDetectorName() == "LPTPC") {
+ try{
+ kaldet::LCTPCKalDetector* tpcdet = new kaldet::LCTPCKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(tpcdet);
+ _det->Install( *tpcdet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - TPC missing in gear file: TPC Not Built " << std::endl ;
+ }
+ }
+ else {
+
+ try{
+ ILDSupportKalDetector* supportdet = new ILDSupportKalDetector( *_gearMgr, _geoSvc ) ;
+ // get the dedicated ip layer
+ _ipLayer = supportdet->getIPLayer() ;
+ // store the measurement layer id's for the active layers, Calo is defined as active
+ this->storeActiveMeasurementModuleIDs(supportdet);
+ _det->Install( *supportdet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+
+ //streamlog_out( ERROR ) << "MarlinKalTest - Support Material missing in gear file: Cannot proceed as propagations and extrapolations for cannonical track states are impossible: exit(1) called" << std::endl ;
+ exit(1);
+
+ }
+
+ try{
+ ILDVXDKalDetector* vxddet = new ILDVXDKalDetector( *_gearMgr, _geoSvc ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(vxddet);
+ _det->Install( *vxddet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - VXD missing in gear file: VXD Material Not Built " << std::endl ;
+ }
+
+
+ bool SIT_found = false ;
+ try{
+ ILDSITKalDetector* sitdet = new ILDSITKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(sitdet);
+ _det->Install( *sitdet ) ;
+ SIT_found = true ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - SIT missing in gear file: SIT Not Built " << std::endl ;
+ }
+
+ if( ! SIT_found ){
+ try{
+ ILDSITCylinderKalDetector* sitdet = new ILDSITCylinderKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(sitdet);
+ _det->Install( *sitdet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - Simple Cylinder Based SIT missing in gear file: Simple Cylinder Based SIT Not Built " << std::endl ;
+ }
+ }
+
+ try{
+ ILDSETKalDetector* setdet = new ILDSETKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(setdet);
+ _det->Install( *setdet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - SET missing in gear file: SET Not Built " << std::endl ;
+ }
+
+
+ bool FTD_found = false ;
+ try{
+ ILDFTDKalDetector* ftddet = new ILDFTDKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(ftddet);
+ _det->Install( *ftddet ) ;
+ FTD_found = true ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - Petal Based FTD missing in gear file: Petal Based FTD Not Built " << std::endl ;
+ }
+
+ if( ! FTD_found ){
+ try{
+ ILDFTDDiscBasedKalDetector* ftddet = new ILDFTDDiscBasedKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(ftddet);
+ _det->Install( *ftddet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - Simple Disc Based FTD missing in gear file: Simple Disc Based FTD Not Built " << std::endl ;
+ }
+ }
+
+ try{
+ ILDTPCKalDetector* tpcdet = new ILDTPCKalDetector( *_gearMgr ) ;
+ // store the measurement layer id's for the active layers
+ this->storeActiveMeasurementModuleIDs(tpcdet);
+ _det->Install( *tpcdet ) ;
+ }
+ catch( gear::UnknownParameterException& e){
+ //streamlog_out( MESSAGE ) << " MarlinKalTest - TPC missing in gear file: TPC Not Built " << std::endl ;
+ }
+ }
+
+ _det->Close() ; // close the cradle
+ _det->Sort() ; // sort meas. layers from inside to outside
+
+ //streamlog_out( DEBUG4 ) << " MarlinKalTest - number of layers = " << _det->GetEntriesFast() << std::endl ;
+
+ //streamlog_out( DEBUG4 ) << "Options: " << std::endl << this->getOptions() << std::endl ;
+
+ this->includeMultipleScattering( getOption(IMarlinTrkSystem::CFG::useQMS) ) ;
+ this->includeEnergyLoss( getOption(IMarlinTrkSystem::CFG::usedEdx) ) ;
+
+
+ is_initialised = true;
+
+ }
+
+ MarlinTrk::IMarlinTrack* MarlinKalTest::createTrack() {
+ //std::cout << "fucd " << "creatTrack" << std::endl;
+ if ( ! is_initialised ) {
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::createTrack: Fitter not initialised. MarlinKalTest::init() must be called before MarlinKalTest::createTrack()" << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+
+ }
+ return new MarlinTrk::MarlinKalTestTrack(this) ;
+ }
+
+ void MarlinKalTest::includeMultipleScattering( bool msOn ) {
+
+ if( msOn == true ) {
+ _det->SwitchOnMS();
+ }
+ else{
+ _det->SwitchOffMS();
+ }
+
+ }
+
+ void MarlinKalTest::includeEnergyLoss( bool energyLossOn ) {
+
+ if( energyLossOn == true ) {
+ _det->SwitchOnDEDX();
+ }
+ else{
+ _det->SwitchOffDEDX();
+ }
+
+ }
+
+ void MarlinKalTest::getSensitiveMeasurementModulesForLayer( int layerID, std::vector< const ILDVMeasLayer *>& measmodules) const {
+
+ if( ! measmodules.empty() ) {
+
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::getSensitiveMeasurementModulesForLayer vector passed as second argument is not empty " << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+
+ }
+
+ //streamlog_out( DEBUG0 ) << "MarlinKalTest::getSensitiveMeasurementModulesForLayer: layerID = " << layerID << std::endl;
+
+ std::multimap<Int_t, const ILDVMeasLayer *>::const_iterator it; //Iterator to be used along with ii
+
+
+
+ // for(it = _active_measurement_modules_by_layer.begin(); it != _active_measurement_modules_by_layer.end(); ++it) {
+ // streamlog_out( DEBUG0 ) << "Key = "<< ttdecodeILD(it->first) <<" Value = "<<it->second << std::endl ;
+ // }
+
+
+ std::pair<std::multimap<Int_t, const ILDVMeasLayer *>::const_iterator, std::multimap<Int_t, const ILDVMeasLayer *>::const_iterator> ii;
+
+ // set the module and sensor bit ranges to zero as these are not used in the map
+ lcio::BitField64 bf( UTIL::ILDCellID0::encoder_string ) ;
+ bf.setValue( layerID ) ;
+ bf[lcio::ILDCellID0::module] = 0 ;
+ bf[lcio::ILDCellID0::sensor] = 0 ;
+ layerID = bf.lowWord();
+
+ ii = this->_active_measurement_modules_by_layer.equal_range(layerID); // set the first and last entry in ii;
+
+ for(it = ii.first; it != ii.second; ++it) {
+ // streamlog_out( DEBUG0 ) <<"Key = "<< it->first <<" Value = "<<it->second << std::endl ;
+ measmodules.push_back( it->second ) ;
+ }
+
+ }
+
+ void MarlinKalTest::getSensitiveMeasurementModules( int moduleID , std::vector< const ILDVMeasLayer *>& measmodules ) const {
+
+ if( ! measmodules.empty() ) {
+
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::getSensitiveMeasurementLayer vector passed as second argument is not empty " << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+
+ }
+
+ std::pair<std::multimap<int, const ILDVMeasLayer *>::const_iterator, std::multimap<Int_t, const ILDVMeasLayer *>::const_iterator> ii;
+ ii = this->_active_measurement_modules.equal_range(moduleID); // set the first and last entry in ii;
+
+ std::multimap<int,const ILDVMeasLayer *>::const_iterator it; //Iterator to be used along with ii
+
+
+ for(it = ii.first; it != ii.second; ++it) {
+ // std::cout<<"Key = "<<it->first<<" Value = "<<it->second << std::endl ;
+ measmodules.push_back( it->second ) ;
+ }
+ }
+
+
+ void MarlinKalTest::storeActiveMeasurementModuleIDs(TVKalDetector* detector) {
+
+ Int_t nLayers = detector->GetEntriesFast() ;
+
+ for( int i=0; i < nLayers; ++i ) {
+
+ const ILDVMeasLayer* ml = dynamic_cast<const ILDVMeasLayer*>( detector->At( i ) );
+
+ if( ! ml ) {
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::storeActiveMeasurementLayerIDs dynamic_cast to ILDVMeasLayer* failed " << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+ }
+
+ if( ml->IsActive() ) {
+
+ // then get all the sensitive element id's assosiated with this ILDVMeasLayer and store them in the map
+ std::vector<int>::const_iterator it = ml->getCellIDs().begin();
+
+ while ( it!=ml->getCellIDs().end() ) {
+
+ int sensitive_element_id = *it;
+ this->_active_measurement_modules.insert(std::pair<int,const ILDVMeasLayer*>( sensitive_element_id, ml ));
+ ++it;
+
+ }
+
+ int subdet_layer_id = ml->getLayerID() ;
+
+ this->_active_measurement_modules_by_layer.insert(std::pair<int ,const ILDVMeasLayer*>(subdet_layer_id,ml));
+
+ //streamlog_out(DEBUG0) << "MarlinKalTest::storeActiveMeasurementLayerIDs added active layer with "
+ //<< " LayerID = " << subdet_layer_id << " and DetElementIDs " ;
+
+ for (it = ml->getCellIDs().begin(); it!=ml->getCellIDs().end(); ++it) {
+
+ //streamlog_out(DEBUG0) << " : " << *it ;
+
+ }
+
+ //streamlog_out(DEBUG0) << std::endl;
+
+
+
+
+ }
+
+ }
+
+ }
+
+ const ILDVMeasLayer* MarlinKalTest::getLastMeasLayer(THelicalTrack const& hel, TVector3 const& point) const {
+
+ THelicalTrack helix = hel;
+
+ double deflection_to_point = 0 ;
+ helix.MoveTo( point, deflection_to_point , 0 , 0) ;
+
+ bool isfwd = ((helix.GetKappa() > 0 && deflection_to_point < 0) || (helix.GetKappa() <= 0 && deflection_to_point > 0)) ? true : false;
+
+ int mode = isfwd ? -1 : +1 ;
+
+ // streamlog_out( DEBUG4 ) << " MarlinKalTest - getLastMeasLayer deflection to point = " << deflection_to_point << " kappa = " << helix.GetKappa() << " mode = " << mode << std::endl ;
+ // streamlog_out( DEBUG4 ) << " Point to move to:" << std::endl;
+ // point.Print();
+
+ int nsufaces = _det->GetEntriesFast();
+
+ const ILDVMeasLayer* ml_retval = 0;
+ double min_deflection = DBL_MAX;
+
+ for(int i=0; i<nsufaces; ++i) {
+
+ const ILDVMeasLayer &ml = *dynamic_cast< const ILDVMeasLayer *>(_det->At(i));
+
+ double defection_angle = 0 ;
+ TVector3 crossing_point ;
+
+ const TVSurface *sfp = dynamic_cast<const TVSurface *>(&ml); // surface at destination
+
+
+ int does_cross = sfp->CalcXingPointWith(helix, crossing_point, defection_angle, mode) ;
+
+ if( does_cross ) {
+
+ const double deflection = fabs( deflection_to_point - defection_angle ) ;
+
+ if( deflection < min_deflection ) {
+
+ // streamlog_out( DEBUG4 ) << " MarlinKalTest - crossing found for suface = " << ml.GetMLName()
+ // << std::endl
+ // << " min_deflection = " << min_deflection
+ // << " deflection = " << deflection
+ // << " deflection angle = " << defection_angle
+ // << std::endl
+ // << " x = " << crossing_point.X()
+ // << " y = " << crossing_point.Y()
+ // << " z = " << crossing_point.Z()
+ // << " r = " << crossing_point.Perp()
+ // << std::endl ;
+
+ min_deflection = deflection ;
+ ml_retval = &ml ;
+ }
+
+ }
+
+ }
+
+ return ml_retval;
+ }
+
+ const ILDVMeasLayer* MarlinKalTest::findMeasLayer( edm4hep::TrackerHit* trkhit) const {
+
+ const TVector3 hit_pos( trkhit->getPosition()[0], trkhit->getPosition()[1], trkhit->getPosition()[2]) ;
+
+ return this->findMeasLayer( trkhit->getCellID(), hit_pos ) ;
+
+ }
+
+ const ILDVMeasLayer* MarlinKalTest::findMeasLayer( int detElementID, const TVector3& point) const {
+
+ const ILDVMeasLayer* ml = 0; // return value
+
+ std::vector<const ILDVMeasLayer*> meas_modules ;
+
+ // search for the list of measurement layers associated with this CellID
+ this->getSensitiveMeasurementModules( detElementID, meas_modules ) ;
+
+ if( meas_modules.size() == 0 ) { // no measurement layers found
+
+ UTIL::BitField64 encoder( UTIL::ILDCellID0::encoder_string ) ;
+ encoder.setValue(detElementID) ;
+
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::findMeasLayer module id unkown: moduleID = " << detElementID
+ << " subdet = " << encoder[UTIL::ILDCellID0::subdet]
+ << " side = " << encoder[UTIL::ILDCellID0::side]
+ << " layer = " << encoder[UTIL::ILDCellID0::layer]
+ << " module = " << encoder[UTIL::ILDCellID0::module]
+ << " sensor = " << encoder[UTIL::ILDCellID0::sensor]
+ << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+
+ }
+ else if (meas_modules.size() == 1) { // one to one mapping
+
+ ml = meas_modules[0] ;
+
+ }
+ else { // layer has been split
+
+ bool surf_found(false);
+
+ // loop over the measurement layers associated with this CellID and find the correct one using the position of the hit
+ for( unsigned int i=0; i < meas_modules.size(); ++i) {
+
+
+
+ const TVSurface* surf = 0;
+
+ if( ! (surf = dynamic_cast<const TVSurface*> ( meas_modules[i] )) ) {
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::findMeasLayer dynamic_cast failed for surface type: moduleID = " << detElementID << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+ }
+
+ bool hit_on_surface = surf->IsOnSurface(point);
+
+ if( (!surf_found) && hit_on_surface ){
+
+ ml = meas_modules[i] ;
+ surf_found = true ;
+
+ }
+ else if( surf_found && hit_on_surface ) { // only one surface should be found, if not throw
+
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTest::findMeasLayer point found to be on two surfaces: moduleID = " << detElementID << std::endl ;
+ throw MarlinTrk::Exception(errorMsg.str());
+ }
+
+ }
+ if( ! surf_found ){ // print out debug info
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTest::findMeasLayer point not found to be on any surface matching moduleID = "
+ << detElementID
+ << ": x = " << point.x()
+ << " y = " << point.y()
+ << " z = " << point.z()
+ << std::endl ;
+ */
+ }
+ else{
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTest::findMeasLayer point found to be on surface matching moduleID = "
+ << detElementID
+ << ": x = " << point.x()
+ << " y = " << point.y()
+ << " z = " << point.z()
+ << std::endl ;
+ */
+ }
+ }
+
+ return ml ;
+
+ }
+
+} // end of namespace MarlinTrk
diff --git a/Service/TrackSystemSvc/src/MarlinKalTest.h b/Service/TrackSystemSvc/src/MarlinKalTest.h
new file mode 100644
index 00000000..ac9ed152
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinKalTest.h
@@ -0,0 +1,104 @@
+#ifndef MarlinKalTest_h
+#define MarlinKalTest_h
+
+#include "TrackSystemSvc/IMarlinTrkSystem.h"
+
+#include "gear/GearMgr.h"
+
+//LCIO:
+//#include "lcio.h"
+#include "UTIL/BitField64.h"
+//#include "UTIL/LCTOOLS.h"
+//#include <LCRTRelations.h>
+
+//#include "streamlog/streamlog.h"
+
+#include "TObjArray.h"
+#include "TVector3.h"
+
+#include <cmath>
+#include <vector>
+#include "DetInterface/IGeoSvc.h"
+
+class TKalDetCradle ;
+class TVKalDetector ;
+class ILDVMeasLayer ;
+class THelicalTrack ;
+//class IGeoSvc;
+class ILDCylinderMeasLayer;
+
+namespace edm4hep{
+ class TrackerHit ;
+}
+namespace MarlinTrk{
+ /** Interface to KaltTest Kalman fitter - instantiates and holds the detector geometry.
+ */
+ class MarlinKalTest : public IMarlinTrkSystem {
+
+ public:
+
+ friend class MarlinKalTestTrack;
+
+ // define some configuration constants
+ static const bool FitBackward = kIterBackward ;
+ static const bool FitForward = kIterForward ;
+ static const bool OrderOutgoing = true ;
+ static const bool OrderIncoming = false ;
+
+
+ /** Default c'tor, initializes the geometry from GEAR. */
+ MarlinKalTest( const gear::GearMgr& gearMgr, IGeoSvc* geoSvc) ;
+
+ /** d'tor */
+ ~MarlinKalTest() ;
+
+ /** initialise track fitter system */
+ void init() ;
+
+ /** instantiate its implementation of the IMarlinTrack */
+ IMarlinTrack* createTrack();
+
+ protected:
+
+ /** take multiple scattering into account during the fit */
+ void includeMultipleScattering(bool on);
+
+ /** take energy loss into account during the fit */
+ void includeEnergyLoss(bool on);
+
+ /** Store active measurement module IDs for a given TVKalDetector needed for navigation */
+ void storeActiveMeasurementModuleIDs(TVKalDetector* detector);
+
+ /** Store active measurement module IDs needed for navigation */
+ void getSensitiveMeasurementModules(int detElementID, std::vector< const ILDVMeasLayer *>& measmodules) const;
+
+ /** Store active measurement module IDs needed for navigation */
+ void getSensitiveMeasurementModulesForLayer(int layerID, std::vector<const ILDVMeasLayer *>& measmodules) const;
+
+ // void init(bool MSOn, bool EnergyLossOn) ;
+ bool is_initialised ;
+
+ //** find the measurment layer for a given hit
+ const ILDVMeasLayer* findMeasLayer(edm4hep::TrackerHit* trkhit) const ;
+ //** find the measurment layer for a given det element ID and point in space
+ const ILDVMeasLayer* findMeasLayer(int detElementID, const TVector3& point) const ;
+
+ // get the last layer crossed by the helix when extrapolating from the present position to the pca to point
+ const ILDVMeasLayer* getLastMeasLayer(THelicalTrack const& helix, TVector3 const& point) const ;
+
+ const ILDCylinderMeasLayer* getIPLayer() const { return _ipLayer; }
+
+ const ILDCylinderMeasLayer* _ipLayer ;
+
+ const gear::GearMgr* _gearMgr ;
+ IGeoSvc* _geoSvc;
+
+ TKalDetCradle* _det ; // the detector cradle
+
+ std::multimap< int,const ILDVMeasLayer *> _active_measurement_modules;
+
+ std::multimap< int,const ILDVMeasLayer *> _active_measurement_modules_by_layer;
+
+ } ;
+}
+#endif
diff --git a/Service/TrackSystemSvc/src/MarlinKalTestTrack.cc b/Service/TrackSystemSvc/src/MarlinKalTestTrack.cc
new file mode 100644
index 00000000..473f5ac6
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinKalTestTrack.cc
@@ -0,0 +1,1711 @@
+#include "MarlinKalTestTrack.h"
+
+#include "MarlinKalTest.h"
+#include "TrackSystemSvc/IMarlinTrkSystem.h"
+
+#include <kaltest/TKalDetCradle.h>
+#include <kaltest/TKalTrack.h>
+#include <kaltest/TKalTrackState.h>
+#include "kaltest/TKalTrackSite.h"
+#include "kaltest/TKalFilterCond.h"
+
+//#include <lcio.h>
+#include <edm4hep/TrackerHit.h>
+//#include <plcio/TrackerHitPlane.h>
+
+#include <UTIL/BitField64.h>
+#include <UTIL/Operators.h>
+#include <UTIL/ILDConf.h>
+
+#include "kaldet/ILDCylinderMeasLayer.h" // needed for dedicated IP Layer
+#include "kaldet/ILDCylinderHit.h"
+
+#include "kaldet/ILDPlanarHit.h"
+#include "kaldet/ILDPlanarStripHit.h"
+
+#include "gear/GEAR.h"
+#include "gear/BField.h"
+
+//#include "streamlog/streamlog.h"
+
+
+/** Helper class for defining a filter condition based on the delta chi2 in the AddAndFilter step.
+ */
+class KalTrackFilter : public TKalFilterCond{
+
+public:
+
+ /** C'tor - takes as optional argument the maximum allowed delta chi2 for adding the hit (in IsAccepted() )
+ */
+ KalTrackFilter(double maxDeltaChi2 = DBL_MAX) : _maxDeltaChi2( maxDeltaChi2 ), _passed_last_filter_step(true) {
+ }
+ virtual ~KalTrackFilter() {}
+
+ virtual Bool_t IsAccepted(const TKalTrackSite &site) {
+
+ double deltaChi2 = site.GetDeltaChi2();
+
+ //streamlog_out( DEBUG1 ) << " KalTrackFilter::IsAccepted called ! deltaChi2 = " << std::scientific << deltaChi2 << " _maxDeltaChi2 = " << _maxDeltaChi2 << std::endl;
+
+ _passed_last_filter_step = deltaChi2 < _maxDeltaChi2;
+
+ return ( _passed_last_filter_step ) ;
+ }
+
+ void resetFilterStatus() { _passed_last_filter_step = true; }
+ bool passedLastFilterStep() const { return _passed_last_filter_step; }
+
+protected:
+
+ double _maxDeltaChi2 ;
+ bool _passed_last_filter_step;
+
+} ;
+
+namespace MarlinTrk {
+
+ //---------------------------------------------------------------------------------------------------------------
+
+ std::string decodeILD(int detElementID) {
+ lcio::BitField64 bf(UTIL::ILDCellID0::encoder_string) ;
+ bf.setValue(detElementID) ;
+ return bf.valueString() ;
+ }
+
+ //---------------------------------------------------------------------------------------------------------------
+
+
+ MarlinKalTestTrack::MarlinKalTestTrack(MarlinKalTest* ktest)
+ : _ktest(ktest) {
+
+ _kaltrack = new TKalTrack() ;
+ _kaltrack->SetOwner() ;
+
+ _kalhits = new TObjArray() ;
+ _kalhits->SetOwner() ;
+
+ _initialised = false ;
+ _fitDirection = false ;
+ _smoothed = false ;
+
+ _trackHitAtPositiveNDF = 0;
+ _hitIndexAtPositiveNDF = 0;
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+ _ktest->_diagnostics.new_track(this) ;
+#endif
+ }
+
+
+ MarlinKalTestTrack::~MarlinKalTestTrack(){
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+ _ktest->_diagnostics.end_track() ;
+#endif
+
+ delete _kaltrack ;
+ delete _kalhits ;
+ }
+
+
+
+ int MarlinKalTestTrack::addHit( edm4hep::TrackerHit* trkhit) {
+
+ return this->addHit( trkhit, _ktest->findMeasLayer( trkhit )) ;
+
+ }
+
+ int MarlinKalTestTrack::addHit( edm4hep::TrackerHit* trkhit, const ILDVMeasLayer* ml) {
+
+ std::cout << "MarlinKalTestTrack::addHit: trkhit = " << trkhit->id() << " addr: " << trkhit << " ml = " << ml << std::endl ;
+
+ if( trkhit && ml ) {
+ //if(ml){
+ return this->addHit( trkhit, ml->ConvertLCIOTrkHit(trkhit), ml) ;
+ }
+ else {
+ //streamlog_out( ERROR ) << " MarlinKalTestTrack::addHit - bad inputs " << trkhit << " ml : " << ml << std::endl ;
+ return bad_intputs ;
+ }
+ return bad_intputs ;
+ }
+
+ int MarlinKalTestTrack::addHit( edm4hep::TrackerHit* trkhit, ILDVTrackHit* kalhit, const ILDVMeasLayer* ml) {
+ std::cout << "MarlinKalTestTrack::addHit: trkhit = " << trkhit->id() << " ILDVTrackHit: " << kalhit << " ml = " << ml << std::endl ;
+ if( kalhit && ml ) {
+ //if(ml){
+ _kalhits->Add(kalhit ) ; // Add hit and set surface found
+ _lcio_hits_to_kaltest_hits[trkhit] = kalhit ; // add hit to map relating lcio and kaltest hits
+ // _kaltest_hits_to_lcio_hits[kalhit] = trkhit ; // add hit to map relating kaltest and lcio hits
+ }
+ else {
+ delete kalhit;
+ return bad_intputs ;
+ }
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTestTrack::addHit: hit added "
+ << "number of hits for track = " << _kalhits->GetEntries()
+ << std::endl ;
+ */
+ return success ;
+
+ }
+
+
+ int MarlinKalTestTrack::initialise( bool fitDirection ) {;
+
+
+ //SJA:FIXME: check here if the track is already initialised, and for now don't allow it to be re-initialised
+ // if the track is going to be re-initialised then we would need to do it directly on the first site
+ if ( _initialised ) {
+
+ throw MarlinTrk::Exception("Track fit already initialised");
+
+ }
+
+ if (_kalhits->GetEntries() < 3) {
+
+ //streamlog_out( ERROR) << "<<<<<< MarlinKalTestTrack::initialise: Shortage of Hits! nhits = "
+ //<< _kalhits->GetEntries() << " >>>>>>>" << std::endl;
+ return error ;
+
+ }
+
+ _fitDirection = fitDirection ;
+
+ // establish the hit order
+ Int_t i1, i2, i3; // (i1,i2,i3) = (1st,mid,last) hit to filter
+ if (_fitDirection == kIterBackward) {
+ i3 = 0 ; // fg: first index is 0 and not 1
+ i1 = _kalhits->GetEntries() - 1;
+ i2 = i1 / 2;
+ } else {
+ i1 = 0 ;
+ i3 = _kalhits->GetEntries() - 1;
+ i2 = i3 / 2;
+ }
+
+
+
+ TVTrackHit *startingHit = dynamic_cast<TVTrackHit *>(_kalhits->At(i1));
+
+ // ---------------------------
+ // Create an initial start site for the track using the first hit
+ // ---------------------------
+ // set up a dummy hit needed to create initial site
+
+ TVTrackHit* pDummyHit = 0;
+
+ if ( (pDummyHit = dynamic_cast<ILDCylinderHit *>( startingHit )) ) {
+ pDummyHit = (new ILDCylinderHit(*static_cast<ILDCylinderHit*>( startingHit )));
+ }
+ else if ( (pDummyHit = dynamic_cast<ILDPlanarHit *>( startingHit )) ) {
+ pDummyHit = (new ILDPlanarHit(*static_cast<ILDPlanarHit*>( startingHit )));
+ }
+ else if ( ILDPlanarStripHit_DIM == 2 && (pDummyHit = dynamic_cast<ILDPlanarStripHit *>( startingHit )) ) {
+ pDummyHit = (new ILDPlanarStripHit(*static_cast<ILDPlanarStripHit*>( startingHit )));
+ }
+ else {
+ //streamlog_out( ERROR) << "<<<<<<<<< MarlinKalTestTrack::initialise: dynamic_cast failed for hit type >>>>>>>" << std::endl;
+ return error ;
+ }
+
+ TVTrackHit& dummyHit = *pDummyHit;
+
+ //SJA:FIXME: this constants should go in a header file
+ // give the dummy hit huge errors so that it does not contribute to the fit
+ dummyHit(0,1) = 1.e16; // give a huge error to d
+ dummyHit(1,1) = 1.e16; // give a huge error to z
+
+ // use dummy hit to create initial site
+ TKalTrackSite& initialSite = *new TKalTrackSite(dummyHit);
+
+ initialSite.SetHitOwner();// site owns hit
+ initialSite.SetOwner(); // site owns states
+
+ // ---------------------------
+ // Create initial helix
+ // ---------------------------
+
+ TVTrackHit &h1 = *dynamic_cast<TVTrackHit *>(_kalhits->At(i1)); // first hit
+ TVTrackHit &h2 = *dynamic_cast<TVTrackHit *>(_kalhits->At(i2)); // middle hit
+ TVTrackHit &h3 = *dynamic_cast<TVTrackHit *>(_kalhits->At(i3)); // last hit
+ TVector3 x1 = h1.GetMeasLayer().HitToXv(h1);
+ TVector3 x2 = h2.GetMeasLayer().HitToXv(h2);
+ TVector3 x3 = h3.GetMeasLayer().HitToXv(h3);
+
+ if ( h1.GetDimension() == 1 || h2.GetDimension() == 1 || h3.GetDimension() == 1 ) {
+
+ throw MarlinTrk::Exception("Track fit cannot be initialised from 1 Dimentional hits. Use method MarlinKalTestTrack::initialise( const edm4hep::TrackState& ts, double bfield_z, bool fitDirection )");
+
+ }
+
+ /*
+ streamlog_out(DEBUG2) << "MarlinKalTestTrack::initialise Create initial helix from hits: \n "
+ << "P1 x = " << x1.x() << " y = " << x1.y() << " z = " << x1.z() << " r = " << x1.Perp() << "\n "
+ << "P2 x = " << x2.x() << " y = " << x2.y() << " z = " << x2.z() << " r = " << x2.Perp() << "\n "
+ << "P3 x = " << x3.x() << " y = " << x3.y() << " z = " << x3.z() << " r = " << x3.Perp() << "\n "
+ << "Bz = " << h1.GetBfield() << " direction = " << _fitDirection
+ << std::endl;
+ */
+ // create helix using 3 global space points
+ THelicalTrack helstart(x1, x2, x3, h1.GetBfield(), _fitDirection); // initial helix
+
+ // ---------------------------
+ // Set up initial track state ... could try to use lcio track parameters ...
+ // ---------------------------
+
+ static TKalMatrix initialState(kSdim,1) ;
+ initialState(0,0) = 0.0 ; // dr
+ initialState(1,0) = helstart.GetPhi0() ; // phi0
+ initialState(2,0) = helstart.GetKappa() ; // kappa
+ initialState(3,0) = 0.0 ; // dz
+ initialState(4,0) = helstart.GetTanLambda() ; // tan(lambda)
+ if (kSdim == 6) initialState(5,0) = 0.; // t0
+
+
+ // ---------------------------
+ // Set up initial Covariance Matrix with very large errors
+ // ---------------------------
+
+ TKalMatrix Cov(kSdim,kSdim);
+
+ // make sure everything is initialised to zero
+ for (int i=0; i<kSdim*kSdim; ++i) {
+ Cov.GetMatrixArray()[i] = 0.0;
+ }
+
+// for (Int_t i=0; i<kSdim; i++) {
+// // fg: if the error is too large the initial helix parameters might be changed extremely by the first three (or so) hits,
+// // such that the fit will not work because the helix curls away and does not hit the next layer !!!
+// Cov(i,i) = 1.e2 ; // initialise diagonal elements of dummy error matrix
+// }
+
+ // prefer translation over rotation of the trackstate early in the fit
+
+ Cov(0,0) = 1.e6 ; // d0
+ Cov(1,1) = 1.e2 ; // dphi0
+ Cov(2,2) = 1.e2 ; // dkappa
+ Cov(3,3) = 1.e6 ; // dz
+ Cov(4,4) = 1.e2 ; // dtanL
+ if (kSdim == 6) Cov(5,5) = 1.e2; // t0
+
+ // Add initial states to the site
+ initialSite.Add(new TKalTrackState(initialState,Cov,initialSite,TVKalSite::kPredicted));
+ initialSite.Add(new TKalTrackState(initialState,Cov,initialSite,TVKalSite::kFiltered));
+
+ // add the initial site to the track: that is, give the track initial parameters and covariance
+ // matrix at the starting measurement layer
+ _kaltrack->Add(&initialSite);
+
+ _initialised = true ;
+ /*
+ streamlog_out( DEBUG2 ) << " track parameters used for init : " << std::scientific << std::setprecision(6)
+ << "\t D0 " << 0.0
+ << "\t Phi :" << toBaseRange( helstart.GetPhi0() + M_PI/2. )
+ << "\t Omega " << 1. /helstart.GetRho()
+ << "\t Z0 " << 0.0
+ << "\t tan(Lambda) " << helstart.GetTanLambda()
+
+ << "\t pivot : [" << helstart.GetPivot().X() << ", " << helstart.GetPivot().Y() << ", " << helstart.GetPivot().Z()
+ << " - r: " << std::sqrt( helstart.GetPivot().X()*helstart.GetPivot().X()+helstart.GetPivot().Y()*helstart.GetPivot().Y() ) << "]"
+ << std::endl ;
+ */
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+ // convert to LICO parameters first
+
+ double d0 = 0.0 ;
+ double phi = toBaseRange( helstart.GetPhi0() + M_PI/2. );
+ double omega = 1. /helstart.GetRho() ;
+ double z0 = 0.0 ;
+ double tanLambda = helstart.GetTanLambda() ;
+
+// Cov.Print();
+
+ _ktest->_diagnostics.set_intial_track_parameters(d0,
+ phi,
+ omega,
+ z0,
+ tanLambda,
+ helstart.GetPivot().X(),
+ helstart.GetPivot().Y(),
+ helstart.GetPivot().Z(),
+ Cov);
+
+
+
+#endif
+
+
+ return success ;
+
+ }
+
+ int MarlinKalTestTrack::initialise( const edm4hep::TrackState& ts, double bfield_z, bool fitDirection ) {
+
+ if (_kalhits->GetEntries() == 0) {
+
+ //streamlog_out( ERROR) << "<<<<<< MarlinKalTestTrack::Initialise: Number of Hits is Zero. Cannot Initialise >>>>>>>" << std::endl;
+ return error ;
+
+ }
+
+ //SJA:FIXME: check here if the track is already initialised, and for now don't allow it to be re-initialised
+ // if the track is going to be re-initialised then we would need to do it directly on the first site
+ if ( _initialised ) {
+
+ throw MarlinTrk::Exception("Track fit already initialised");
+
+ }
+ /*
+ streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::initialise using TrackState: track parameters used for init : "
+ << "\t D0 " << ts.getD0()
+ << "\t Phi :" << ts.getPhi()
+ << "\t Omega " << ts.getOmega()
+ << "\t Z0 " << ts.getZ0()
+ << "\t tan(Lambda) " << ts.getTanLambda()
+
+ << "\t pivot : [" << ts.getReferencePoint()[0] << ", " << ts.getReferencePoint()[1] << ", " << ts.getReferencePoint()[2]
+ << " - r: " << std::sqrt( ts.getReferencePoint()[0]*ts.getReferencePoint()[0]+ts.getReferencePoint()[1]*ts.getReferencePoint()[1] ) << "]"
+ << std::endl ;
+ */
+
+ _fitDirection = fitDirection ;
+
+ // for GeV, Tesla, R in mm
+ double alpha = bfield_z * 2.99792458E-4 ;
+ double kappa;
+ if ( bfield_z == 0.0 )
+ kappa = DBL_MAX;
+ else kappa = ts.omega / alpha ;
+
+ THelicalTrack helix( -ts.D0,
+ toBaseRange( ts.phi - M_PI/2. ) ,
+ kappa,
+ ts.Z0,
+ ts.tanLambda,
+ ts.referencePoint[0],
+ ts.referencePoint[1],
+ ts.referencePoint[2],
+ bfield_z );
+
+ TMatrixD cov(5,5) ;
+
+ std::array<float, 15> covLCIO = ts.covMatrix;
+
+ cov( 0 , 0 ) = covLCIO[ 0] ; // d0, d0
+ cov( 0 , 1 ) = - covLCIO[ 1] ; // d0, phi
+ cov( 0 , 2 ) = - covLCIO[ 3] / alpha ; // d0, kappa
+ cov( 0 , 3 ) = - covLCIO[ 6] ; // d0, z0
+ cov( 0 , 4 ) = - covLCIO[10] ; // d0, tanl
+
+ cov( 1 , 0 ) = - covLCIO[ 1] ; // phi, d0
+ cov( 1 , 1 ) = covLCIO[ 2] ; // phi, phi
+ cov( 1 , 2 ) = covLCIO[ 4] / alpha ; // phi, kappa
+ cov( 1 , 3 ) = covLCIO[ 7] ; // phi, z0
+ cov( 1 , 4 ) = covLCIO[11] ; // tanl, phi
+
+ cov( 2 , 0 ) = - covLCIO[ 3] / alpha ; // kappa, d0
+ cov( 2 , 1 ) = covLCIO[ 4] / alpha ; // kappa, phi
+ cov( 2 , 2 ) = covLCIO[ 5] / (alpha * alpha) ; // kappa, kappa
+ cov( 2 , 3 ) = covLCIO[ 8] / alpha ; // kappa, z0
+ cov( 2 , 4 ) = covLCIO[12] / alpha ; // kappa, tanl
+
+ cov( 3 , 0 ) = - covLCIO[ 6] ; // z0, d0
+ cov( 3 , 1 ) = covLCIO[ 7] ; // z0, phi
+ cov( 3 , 2 ) = covLCIO[ 8] / alpha ; // z0, kappa
+ cov( 3 , 3 ) = covLCIO[ 9] ; // z0, z0
+ cov( 3 , 4 ) = covLCIO[13] ; // z0, tanl
+
+ cov( 4 , 0 ) = - covLCIO[10] ; // tanl, d0
+ cov( 4 , 1 ) = covLCIO[11] ; // tanl, phi
+ cov( 4 , 2 ) = covLCIO[12] / alpha ; // tanl, kappa
+ cov( 4 , 3 ) = covLCIO[13] ; // tanl, z0
+ cov( 4 , 4 ) = covLCIO[14] ; // tanl, tanl
+
+// cov.Print();
+
+ // move the helix to either the position of the last hit or the first depending on initalise_at_end
+
+ // default case initalise_at_end
+ int index = _kalhits->GetEntries() - 1 ;
+ // or initialise at start
+ if( _fitDirection == IMarlinTrack::forward ){
+ index = 0 ;
+ }
+
+ TVTrackHit* kalhit = dynamic_cast<TVTrackHit *>(_kalhits->At(index));
+
+ double dphi;
+
+ TVector3 initial_pivot ;
+
+ // Leave the pivot at the origin for a 1-dim hit
+ if (kalhit->GetDimension() > 1) {
+
+ initial_pivot = kalhit->GetMeasLayer().HitToXv(*kalhit) ;
+ }
+ else{
+ initial_pivot = TVector3(0.0,0.0,0.0);
+ }
+
+
+ // ---------------------------
+ // Create an initial start site for the track using the hit
+ // ---------------------------
+ // set up a dummy hit needed to create initial site
+
+ TVTrackHit* pDummyHit = 0;
+
+ if ( (pDummyHit = dynamic_cast<ILDCylinderHit *>( kalhit )) ) {
+ pDummyHit = (new ILDCylinderHit(*static_cast<ILDCylinderHit*>( kalhit )));
+
+ }
+ else if ( (pDummyHit = dynamic_cast<ILDPlanarHit *>( kalhit )) ) {
+ pDummyHit = (new ILDPlanarHit(*static_cast<ILDPlanarHit*>( kalhit )));
+ }
+ else if ( (pDummyHit = dynamic_cast<ILDPlanarStripHit *>( kalhit )) ) {
+
+ pDummyHit = (new ILDPlanarStripHit(*static_cast<ILDPlanarStripHit*>( kalhit )));
+
+ const TVMeasLayer *ml = &pDummyHit->GetMeasLayer();
+
+ const TVSurface* surf = dynamic_cast<const TVSurface*>(ml);
+
+ if (surf) {
+ double phi;
+
+ surf->CalcXingPointWith(helix, initial_pivot, phi);
+
+ } else {
+ //streamlog_out( ERROR) << "<<<<<<<<< MarlinKalTestTrack::initialise: dynamic_cast failed for TVSurface >>>>>>>" << std::endl;
+ return error ;
+ }
+
+
+ }
+ else {
+ //streamlog_out( ERROR) << "<<<<<<<<< MarlinKalTestTrack::initialise: dynamic_cast failed for hit type >>>>>>>" << std::endl;
+ return error ;
+ }
+
+ TVTrackHit& dummyHit = *pDummyHit;
+
+ //SJA:FIXME: this constants should go in a header file
+ // give the dummy hit huge errors so that it does not contribute to the fit
+ dummyHit(0,1) = 1.e16; // give a huge error to d
+
+ if(dummyHit.GetDimension()>1) dummyHit(1,1) = 1.e16; // give a huge error to z
+
+ // use dummy hit to create initial site
+ TKalTrackSite& initialSite = *new TKalTrackSite(dummyHit);
+
+ initialSite.SetHitOwner();// site owns hit
+ initialSite.SetOwner(); // site owns states
+
+ // ---------------------------
+ // Set up initial track state
+ // ---------------------------
+
+ helix.MoveTo( initial_pivot, dphi, 0, &cov );
+
+ static TKalMatrix initialState(kSdim,1) ;
+ initialState(0,0) = helix.GetDrho() ; // d0
+ initialState(1,0) = helix.GetPhi0() ; // phi0
+ initialState(2,0) = helix.GetKappa() ; // kappa
+ initialState(3,0) = helix.GetDz(); // dz
+ initialState(4,0) = helix.GetTanLambda() ; // tan(lambda)
+ if (kSdim == 6) initialState(5,0) = 0.; // t0
+
+ // make sure that the pivot is in the right place
+ initialSite.SetPivot(initial_pivot);
+
+ // ---------------------------
+ // Set up initial Covariance Matrix
+ // ---------------------------
+
+ TKalMatrix covK(kSdim,kSdim) ;
+ for(int i=0;i<5;++i) {
+ for(int j=0;j<5;++j) {
+ covK[i][j] = cov[i][j] ;
+ }
+ }
+ if (kSdim == 6) covK(5,5) = 1.e6; // t0
+
+// covK.Print();
+
+ // Add initial states to the site
+ initialSite.Add(new TKalTrackState(initialState,covK,initialSite,TVKalSite::kPredicted));
+ initialSite.Add(new TKalTrackState(initialState,covK,initialSite,TVKalSite::kFiltered));
+
+ // add the initial site to the track: that is, give the track initial parameters and covariance
+ // matrix at the starting measurement layer
+ _kaltrack->Add(&initialSite);
+
+ _initialised = true ;
+
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+ // convert to LICO parameters first
+
+ double d0 = - helix.GetDrho() ;
+ double phi = toBaseRange( helix.GetPhi0() + M_PI/2. );
+ double omega = 1. /helix.GetRho() ;
+ double z0 = helix.GetDz() ;
+ double tanLambda = helix.GetTanLambda() ;
+
+ _ktest->_diagnostics.set_intial_track_parameters(d0,
+ phi,
+ omega,
+ z0,
+ tanLambda,
+ helix.GetPivot().X(),
+ helix.GetPivot().Y(),
+ helix.GetPivot().Z(),
+ covK);
+
+
+
+#endif
+
+ return success ;
+
+ }
+
+ int MarlinKalTestTrack::addAndFit( ILDVTrackHit* kalhit, double& chi2increment, TKalTrackSite*& site, double maxChi2Increment) {
+
+ //streamlog_out(DEBUG1) << "MarlinKalTestTrack::addAndFit called : maxChi2Increment = " << std::scientific << maxChi2Increment << std::endl ;
+
+ if ( ! _initialised ) {
+
+ throw MarlinTrk::Exception("Track fit not initialised");
+
+ }
+
+ // here do dynamic cast repeatedly in DEBUG statement as this will be stripped out any way for production code
+ // otherwise we have to do the cast outside of the DEBUG statement and it won't be stripped out
+ /*streamlog_out( DEBUG1 ) << "Kaltrack::addAndFit : add site to track at index : "
+ << (dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) ))->GetIndex()
+ << " for type "
+ << dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) )->GetName() ;
+ streamlog_out( DEBUG0 ) << " with CellIDs:";
+
+ for (unsigned int i = 0; i < (dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) )->getNCellIDs());++i) {
+ streamlog_out( DEBUG0 ) << " : "
+ << dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) )->getCellIDs()[i] ;
+
+ }
+
+ streamlog_out( DEBUG1 ) << std::endl ;
+ */
+ TKalTrackSite* temp_site = new TKalTrackSite(*kalhit); // create new site for this hit
+
+ KalTrackFilter filter( maxChi2Increment );
+ filter.resetFilterStatus();
+
+ temp_site->SetFilterCond( &filter ) ;
+
+
+ // this is the only point at which a hit is actually filtered
+ // and it is here that we can get the GetDeltaChi2 vs the maxChi2Increment
+ // it will always be possible to get the delta chi2 so long as we have a link to the sites ...
+ // although calling smooth will natrually update delta chi2.
+
+
+ if (!_kaltrack->AddAndFilter(*temp_site)) {
+
+ chi2increment = temp_site->GetDeltaChi2() ;
+ // get the measurement layer of the current hit
+ const ILDVMeasLayer* ml = dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) ) ;
+ TVector3 pos = ml->HitToXv(*kalhit);
+ std::cout << "debug: Kaltrack::addAndFit : site discarded! at index : " << ml->GetIndex()
+ << " for type " << ml->GetName()
+ << " chi2increment = " << chi2increment
+ << " maxChi2Increment = " << maxChi2Increment
+ << " x = " << pos.x()
+ << " y = " << pos.y()
+ << " z = " << pos.z()
+ << " with CellIDs: " << std::endl;
+
+ for (unsigned int i = 0; i < (dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) )->getNCellIDs());++i) {
+ std::cout << "debug: CellID = "
+ << dynamic_cast<const ILDVMeasLayer*>( &(kalhit->GetMeasLayer() ) )->getCellIDs()[i]
+ << std::endl ;
+ }
+
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+ _ktest->_diagnostics.record_rejected_site(kalhit, temp_site);
+#endif
+
+ delete temp_site; // delete site if filter step failed
+
+
+ // compiling the code below with the cmake option CMAKE_BUILD_TYPE=Debug
+ // and with LDFLAGS=-Wl,--no-undefined
+ // causes an undefined reference error
+ // the problem gets fixed using the if/else statement below
+
+ // this fails
+ //return filter.usedForLastFilterStep() ? site_fails_chi2_cut : site_discarded ;
+
+ // this also fails..
+ //bool rc = filter.usedForLastFilterStep() ;
+ //return (rc ? site_fails_chi2_cut : site_discarded);
+
+ // but this works ?!!
+ //return ( true ? site_fails_chi2_cut : site_discarded);
+
+ // and this also works..
+ //streamlog_out(DEBUG2) << " addAndFit : Site Fails Chi2 cut ? " << filter.passedLastFilterStep() << std::endl;
+
+ if( filter.passedLastFilterStep() == false ) {
+ return site_fails_chi2_cut ;
+ } else {
+ return site_discarded ;
+ }
+
+ }
+
+ site = temp_site;
+ chi2increment = site->GetDeltaChi2() ;
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+ _ktest->_diagnostics.record_site(kalhit, site);
+#endif
+
+ return success ;
+
+ }
+
+ int MarlinKalTestTrack::addAndFit( edm4hep::TrackerHit* trkhit, double& chi2increment, double maxChi2Increment) {
+
+ if( ! trkhit ) {
+ std::cout << "Error: MarlinKalTestTrack::addAndFit(edm4hep::TrackerHit trkhit, double& chi2increment, double maxChi2Increment): trkhit == 0" << std::endl;
+ return bad_intputs ;
+ }
+
+ const ILDVMeasLayer* ml = _ktest->findMeasLayer( trkhit ) ;
+
+ if( ml == 0 ){
+ // fg: not sure if ml should ever be 0 - but it seems to happen,
+ // if point is not on surface and more than one surface exists ...
+
+ std::cout << "Error>>>>>>>>>>> no measurment layer found for trkhit cellid0 : "
+ << decodeILD( trkhit->getCellID() ) << " at "
+ << trkhit->getPosition() << std::endl ;
+
+ return IMarlinTrack::bad_intputs ;
+ }
+
+ ILDVTrackHit* kalhit = ml->ConvertLCIOTrkHit(trkhit) ;
+
+ if( kalhit == 0 ){ //fg: ml->ConvertLCIOTrkHit returns 0 if hit not on surface !!!
+ return IMarlinTrack::bad_intputs ;
+ }
+
+ TKalTrackSite* site = 0 ;
+ int error_code = this->addAndFit( kalhit, chi2increment, site, maxChi2Increment);
+
+ if( error_code != success ){
+
+ delete kalhit;
+
+ // if the hit fails for any reason other than the Chi2 cut record the Chi2 contibution as DBL_MAX
+ if( error_code != site_fails_chi2_cut ) {
+ chi2increment = DBL_MAX;
+ }
+
+ _outlier_chi2_values.push_back(std::make_pair(trkhit, chi2increment));
+
+ //streamlog_out( DEBUG2 ) << ">>>>>>>>>>> addAndFit Number of Outliers : "
+ //<< _outlier_chi2_values.size() << std::endl;
+
+ return error_code ;
+ }
+ else {
+ this->addHit(trkhit, kalhit, ml ) ;
+ _hit_used_for_sites[trkhit] = site ;
+ _hit_chi2_values.push_back(std::make_pair(trkhit, chi2increment));
+ }
+
+ // set the values for the point at which the fit becomes constained
+ if( _trackHitAtPositiveNDF == 0 && _kaltrack->GetNDF() >= 0){
+
+ _trackHitAtPositiveNDF = trkhit;
+ _hitIndexAtPositiveNDF = _kaltrack->IndexOf( site );
+ /*
+ streamlog_out( DEBUG2 ) << ">>>>>>>>>>> Fit is now constrained at : "
+ << decodeILD( trkhit->getCellID() )
+ << " pos " << trkhit->getPosition()
+ << " trkhit = " << _trackHitAtPositiveNDF
+ << " index of kalhit = " << _hitIndexAtPositiveNDF
+ << " NDF = " << _kaltrack->GetNDF()
+ << std::endl;
+ */
+ }
+
+ return success ;
+
+ }
+
+
+
+ int MarlinKalTestTrack::testChi2Increment( edm4hep::TrackerHit* trkhit, double& chi2increment ) {
+
+ //if( ! trkhit ) {
+ // streamlog_out( ERROR) << "MarlinKalTestTrack::addAndFit(edm4hep::TrackerHit trkhit, double& chi2increment, double maxChi2Increment): trkhit == 0" << std::endl;
+ // return IMarlinTrack::bad_intputs ;
+ //}
+
+ const ILDVMeasLayer* ml = _ktest->findMeasLayer( trkhit ) ;
+
+ if( ml == 0 ){
+ // fg: not sure if ml should ever be 0 - but it seems to happen,
+ // if point is not on surface and more than one surface exists ...
+
+ std::cout << "Error>>>>>>>>>>> no measurment layer found for trkhit cellid0 : "
+ << decodeILD( trkhit->getCellID() ) << " at "
+ << trkhit->getPosition() << std::endl ;
+
+ return IMarlinTrack::bad_intputs ;
+
+ }
+
+ ILDVTrackHit* kalhit = ml->ConvertLCIOTrkHit(trkhit) ;
+
+ if( kalhit == 0 ){ //fg: ml->ConvertLCIOTrkHit returns 0 if hit not on surface !!!
+ return IMarlinTrack::bad_intputs ;
+ }
+
+
+ TKalTrackSite* site = 0 ;
+ int error_code = this->addAndFit( kalhit, chi2increment, site, -DBL_MAX); // using -DBL_MAX here ensures the hit will never be added to the fit
+
+ delete kalhit;
+
+ return error_code;
+
+ }
+
+
+
+ int MarlinKalTestTrack::fit( double maxChi2Increment ) {
+
+ // SJA:FIXME: what do we do about calling fit after we have already added hits and filtered
+ // I guess this would created new sites when addAndFit is called
+ // one option would be to remove the sites
+ // need to check where the sites are stored ... probably in the KalTrackSystem
+ //
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::fit() called " << std::endl ;
+
+ if ( ! _initialised ) {
+
+ throw MarlinTrk::Exception("Track fit not initialised");
+
+ }
+
+ // ---------------------------
+ // Prepare hit iterrator for adding hits to kaltrack
+ // ---------------------------
+
+ TIter next(_kalhits, _fitDirection);
+
+ // ---------------------------
+ // Start Kalman Filter
+ // ---------------------------
+
+ ILDVTrackHit *kalhit = 0;
+
+ while ( (kalhit = dynamic_cast<ILDVTrackHit *>( next() ) ) ) {
+
+ double chi2increment;
+ TKalTrackSite* site;
+ int error_code = this->addAndFit( kalhit, chi2increment, site, maxChi2Increment );
+
+
+ edm4hep::TrackerHit* trkhit = kalhit->getLCIOTrackerHit();
+
+ if( error_code == 0 ){ // add trkhit to map associating trkhits and sites
+ _hit_used_for_sites[trkhit] = site;
+ _hit_chi2_values.push_back(std::make_pair(trkhit, chi2increment));
+
+ // set the values for the point at which the fit becomes constained
+ if( _trackHitAtPositiveNDF == 0 && _kaltrack->GetNDF() >= 0){
+
+ _trackHitAtPositiveNDF = trkhit;
+ _hitIndexAtPositiveNDF = _kaltrack->IndexOf( site );
+ /*
+ streamlog_out( DEBUG2 ) << ">>>>>>>>>>> Fit is now constrained at : "
+ << decodeILD( trkhit->getCellID() )
+ << " pos " << trkhit->getPosition()
+ << " trkhit = " << _trackHitAtPositiveNDF
+ << " index of kalhit = " << _hitIndexAtPositiveNDF
+ << " NDF = " << _kaltrack->GetNDF()
+ << std::endl;
+ */
+ }
+
+ }
+ else { // hit rejected by the filter, so store in the list of rejected hits
+
+ // if the hit fails for any reason other than the Chi2 cut record the Chi2 contibution as DBL_MAX
+ if( error_code != site_fails_chi2_cut ) {
+ chi2increment = DBL_MAX;
+ }
+
+ _outlier_chi2_values.push_back(std::make_pair(trkhit, chi2increment));
+ //streamlog_out( DEBUG2 ) << ">>>>>>>>>>> fit(): Number of Outliers : "
+ //<< _outlier_chi2_values.size() << std::endl;
+
+ _hit_not_used_for_sites.push_back(trkhit) ;
+
+ }
+
+ } // end of Kalman filter
+
+ if( _ktest->getOption( MarlinTrk::IMarlinTrkSystem::CFG::useSmoothing ) ){
+ //streamlog_out( DEBUG2 ) << "Perform Smoothing for All Previous Measurement Sites " << std::endl ;
+ int error = this->smooth() ;
+
+ if( error != success ) return error ;
+
+ }
+
+ //return _hit_used_for_sites.empty() == false ? success : all_sites_fail_fit ;
+ if( _hit_used_for_sites.empty() == false )
+ {
+ return success ;
+ }
+ else{
+ return all_sites_fail_fit ;
+ }
+
+ }
+
+
+ /** smooth all track states
+ */
+ int MarlinKalTestTrack::smooth(){
+
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::smooth() " << std::endl ;
+
+ //fg: we should actually smooth all sites - it is then up to the user which smoothed tracks state to take
+ // for any furthter extrapolation/propagation ...
+
+ if( !_smoothed )
+ _kaltrack->SmoothAll() ;
+
+ //SJA:FIXME: in the current implementation it is only possible to smooth back to the 4th site.
+ // This is due to the fact that the covariance matrix is not well defined at the first 3 measurement sites filtered.
+
+ // _kaltrack->SmoothBackTo( _hitIndexAtPositiveNDF + 1 ) ;
+
+ _smoothed = true ;
+
+ return success ;
+
+ }
+
+
+ /** smooth track states from the last filtered hit back to the measurement site associated with the given hit
+ */
+ int MarlinKalTestTrack::smooth( edm4hep::TrackerHit* trkhit ) {
+
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::smooth( edm4hep::TrackerHit " << trkhit << " ) " << std::endl ;
+
+ if ( !trkhit ) {
+ return bad_intputs ;
+ }
+
+ std::map<edm4hep::TrackerHit*, TKalTrackSite*>::const_iterator it;
+
+ TKalTrackSite* site = 0 ;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ int index = _kaltrack->IndexOf( site );
+
+ _kaltrack->SmoothBackTo( index ) ;
+
+ _smoothed = true ;
+
+ return success ;
+
+ }
+
+
+ int MarlinKalTestTrack::getTrackState( edm4hep::TrackState& ts, double& chi2, int& ndf ) {
+
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::getTrackState( edm4hep::TrackState& ts ) " << std::endl ;
+
+ // use the last filtered track state
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ this->ToLCIOTrackState( site, ts, chi2, ndf );
+
+ return success ;
+
+
+ }
+
+
+ int MarlinKalTestTrack::getTrackState( edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf ) {
+
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::getTrackState(edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts ) using hit: " << trkhit << " with cellID0 = " << trkhit->getCellID() << std::endl ;
+
+ TKalTrackSite* site = 0 ;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ //streamlog_out( DEBUG1 ) << "MarlinKalTestTrack::getTrackState: site " << site << std::endl;
+
+ this->ToLCIOTrackState( *site, ts, chi2, ndf );
+
+ return success ;
+ }
+
+
+ int MarlinKalTestTrack::getHitsInFit( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) {
+
+ std::copy( _hit_chi2_values.begin() , _hit_chi2_values.end() , std::back_inserter( hits ) ) ;
+
+ // this needs more thought. What about when the hits are added using addAndFit?
+
+ // need to check the order so that we can return the list ordered in time
+ // as they will be added to _hit_chi2_values in the order of fitting
+ // not in the order of time
+//
+// if( _fitDirection == IMarlinTrack::backward ){
+// std::reverse_copy( _hit_chi2_values.begin() , _hit_chi2_values.end() , std::back_inserter( hits ) ) ;
+// } else {
+// std::copy( _hit_chi2_values.begin() , _hit_chi2_values.end() , std::back_inserter( hits ) ) ;
+// }
+
+ return success ;
+
+ }
+
+ int MarlinKalTestTrack::getOutliers( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) {
+
+ std::copy( _outlier_chi2_values.begin() , _outlier_chi2_values.end() , std::back_inserter( hits ) ) ;
+
+
+ // this needs more thought. What about when the hits are added using addAndFit?
+// // need to check the order so that we can return the list ordered in time
+// // as they will be added to _hit_chi2_values in the order of fitting
+// // not in the order of time
+//
+// if( _fitDirection == IMarlinTrack::backward ){
+// std::reverse_copy( _outlier_chi2_values.begin() , _outlier_chi2_values.end() , std::back_inserter( hits ) ) ;
+// } else {
+// std::copy( _outlier_chi2_values.begin() , _outlier_chi2_values.end() , std::back_inserter( hits ) ) ;
+// }
+
+
+ return success ;
+
+ }
+
+
+ int MarlinKalTestTrack::getNDF( int& ndf ){
+
+ if( _initialised == false ) {
+ return error;
+ } else {
+
+ ndf = _kaltrack->GetNDF();
+ return success;
+
+ }
+
+ }
+
+ int MarlinKalTestTrack::getTrackerHitAtPositiveNDF( edm4hep::TrackerHit*& trkhit ) {
+
+ trkhit = _trackHitAtPositiveNDF;
+ return success;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ){
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ return this->extrapolate( point, site, ts, chi2, ndf ) ;
+
+ }
+
+ int MarlinKalTestTrack::extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf ) {
+
+ TKalTrackSite* site = 0 ;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code;
+
+ return this->extrapolate( point, *site, ts, chi2, ndf ) ;
+
+ }
+
+ int MarlinKalTestTrack::extrapolate( const edm4hep::Vector3d& point, const TKalTrackSite& site ,edm4hep::TrackState& ts, double& chi2, int& ndf ){
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ) called " << std::endl ;
+
+ TKalTrackState& trkState = (TKalTrackState&) site.GetCurState(); // this segfaults if no hits are present
+
+ THelicalTrack helix = trkState.GetHelix() ;
+ double dPhi ;
+
+ // convert the gear point supplied to TVector3
+ const TVector3 tpoint( point.x, point.y, point.z ) ;
+
+ Int_t sdim = trkState.GetDimension(); // dimensions of the track state, it will be 5 or 6
+ TKalMatrix sv(sdim,1);
+
+ // now move to the point
+ TKalMatrix DF(sdim,sdim);
+ DF.UnitMatrix();
+ helix.MoveTo( tpoint , dPhi , &DF , 0) ; // move helix to desired point, and get propagator matrix
+
+ TMatrixD c0(trkState.GetCovMat());
+
+ TKalMatrix DFt = TKalMatrix(TMatrixD::kTransposed, DF);
+ c0 = DF * c0 * DFt ; // update the covariance matrix
+
+ this->ToLCIOTrackState( helix, c0, ts, chi2, ndf );
+
+ return success;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ return this->extrapolateToLayer( layerID, site, ts, chi2, ndf, detElementID, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToLayer( int layerID, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ return this->extrapolateToLayer( layerID, *site, ts, chi2, ndf, detElementID, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToLayer( int layerID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::extrapolateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID ) called " << std::endl ;
+
+ edm4hep::Vector3d crossing_point ;
+ const ILDVMeasLayer* ml = 0;
+
+ int error_code = this->intersectionWithLayer( layerID, site, crossing_point, detElementID, ml, mode ) ;
+
+ if( error_code != 0 ) return error_code ;
+
+ return this->extrapolate( crossing_point, site, ts, chi2, ndf ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ return this->extrapolateToDetElement( detElementID, site, ts, chi2, ndf, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToDetElement( int detElementID, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ return this->extrapolateToDetElement( detElementID, *site, ts, chi2, ndf, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::extrapolateToDetElement( int detElementID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::extrapolateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) called " << std::endl ;
+
+ edm4hep::Vector3d crossing_point ;
+
+ const ILDVMeasLayer* ml = 0;
+ int error_code = this->intersectionWithDetElement( detElementID, site, crossing_point, ml, mode ) ;
+
+ if( error_code != 0 ) return error_code ;
+
+ return this->extrapolate( crossing_point, site, ts, chi2, ndf ) ;
+
+ }
+
+
+
+ int MarlinKalTestTrack::propagate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ){
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ // check if the point is inside the beampipe
+ // SJA:FIXME: really we should also check if the PCA to the point is also less than R
+ const ILDVMeasLayer* ml = (sqrt(point.x*point.x+point.y*point.y) < _ktest->getIPLayer()->GetR()) ? _ktest->getIPLayer() : 0;
+
+ return this->propagate( point, site, ts, chi2, ndf, ml ) ;
+
+ }
+
+ int MarlinKalTestTrack::propagate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf ){
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ // check if the point is inside the beampipe
+ // SJA:FIXME: really we should also check if the PCA to the point is also less than R
+
+ const ILDVMeasLayer* ml = _ktest->getIPLayer();
+
+ if ( ml )
+ if (sqrt(point.x*point.x+point.y*point.y) > _ktest->getIPLayer()->GetR()) ml = NULL;
+
+// const ILDVMeasLayer* ml = (point.r() < _ktest->getIPLayer()->GetR()) ? _ktest->getIPLayer() : 0;
+
+ return this->propagate( point, *site, ts, chi2, ndf, ml ) ;
+
+ }
+
+ int MarlinKalTestTrack::propagate( const edm4hep::Vector3d& point, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, const ILDVMeasLayer* ml ){
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::propagate( const edm4hep::Vector3d& point, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf ) called " << std::endl ;
+
+ // convert the gear point supplied to TVector3
+ const TVector3 tpoint( point.x, point.y, point.z ) ;
+
+ TKalTrackState& trkState = (TKalTrackState&) site.GetCurState(); // this segfaults if no hits are present
+
+ THelicalTrack helix = trkState.GetHelix() ;
+ double dPhi = 0.0;
+
+
+ Int_t sdim = trkState.GetDimension(); // dimensions of the track state, it will be 5 or 6
+ TKalMatrix sv(sdim,1);
+
+ TKalMatrix F(sdim,sdim); // propagator matrix to be returned by transport function
+ F.UnitMatrix(); // set the propagator matrix to the unit matrix
+
+ TKalMatrix Q(sdim,sdim); // noise matrix to be returned by transport function
+ Q.Zero();
+ TVector3 x0; // intersection point to be returned by transport
+
+ TMatrixD c0(trkState.GetCovMat());
+
+ // the last layer crossed by the track before point
+ if( ! ml ){
+ ml = _ktest->getLastMeasLayer(helix, tpoint);
+ }
+
+ if ( ml ) {
+
+ _ktest->_det->Transport(site, *ml, x0, sv, F, Q ) ; // transport to last layer cross before point
+
+ // given that we are sure to have intersected the layer ml as this was provided via getLastMeasLayer, x0 will lie on the layer
+ // this could be checked with the method isOnSurface
+ // so F will be the propagation matrix from the current location to the last surface and Q will be the noise matrix up to this point
+
+
+ TKalMatrix Ft = TKalMatrix(TMatrixD::kTransposed, F);
+ c0 = F * c0 * Ft + Q; // update covaraince matrix and add the MS assosiated with moving to tvml
+
+ helix.MoveTo( x0 , dPhi , 0 , 0 ) ; // move the helix to tvml
+
+
+ }
+ else { // the current site is at the last surface before the point to propagate to
+
+ ml = dynamic_cast<const ILDVMeasLayer*>(&(site.GetHit().GetMeasLayer())) ;
+
+ }
+
+ // get whether the track is incomming or outgoing at the last surface
+ const TVSurface *sfp = dynamic_cast<const TVSurface *>(ml); // last surface
+
+ TMatrixD dxdphi = helix.CalcDxDphi(0); // tangent vector at last surface
+ TVector3 dxdphiv(dxdphi(0,0),dxdphi(1,0),dxdphi(2,0)); // convert matirix diagonal to vector
+// Double_t cpa = helix.GetKappa(); // get pt
+
+ Bool_t isout = -dPhi*dxdphiv.Dot(sfp->GetOutwardNormal(x0)) < 0 ? kTRUE : kFALSE; // out-going or in-coming at the destination surface
+
+ // now move to the point
+ TKalMatrix DF(sdim,sdim);
+ DF.UnitMatrix();
+ helix.MoveTo( tpoint , dPhi , &DF , 0) ; // move helix to desired point, and get propagator matrix
+
+ TKalMatrix Qms(sdim, sdim);
+ ml->CalcQms(isout, helix, dPhi, Qms); // calculate MS for the final step through the present material
+
+ TKalMatrix DFt = TKalMatrix(TMatrixD::kTransposed, DF);
+ c0 = DF * c0 * DFt + Qms ; // update the covariance matrix
+
+
+ this->ToLCIOTrackState( helix, c0, ts, chi2, ndf );
+
+ return success;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ return this->propagateToLayer( layerID, site, ts, chi2, ndf, detElementID, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToLayer( int layerID, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ return this->propagateToLayer( layerID, *site, ts, chi2, ndf, detElementID, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToLayer( int layerID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::propagateToLayer( int layerID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID ) called " << std::endl;
+
+ edm4hep::Vector3d crossing_point ;
+
+ const ILDVMeasLayer* ml = 0;
+
+ int error_code = this->intersectionWithLayer( layerID, site, crossing_point, detElementID, ml, mode) ;
+
+ if( error_code != success ) return error_code ;
+
+ return this->propagate( crossing_point, site, ts, chi2, ndf , ml) ;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ return this->propagateToDetElement( detElementID, site, ts, chi2, ndf, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToDetElement( int detElementID, edm4hep::TrackerHit* trkhit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ return this->propagateToDetElement( detElementID, *site, ts, chi2, ndf, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::propagateToDetElement( int detElementID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::propagateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode ) called " << std::endl ;
+
+ edm4hep::Vector3d crossing_point ;
+
+ const ILDVMeasLayer* ml = 0;
+ int error_code = this->intersectionWithDetElement( detElementID, site, crossing_point, ml, mode ) ;
+
+ if( error_code != 0 ) return error_code ;
+
+ return this->propagate( crossing_point, site, ts, chi2, ndf, ml ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::intersectionWithDetElement( int detElementID, edm4hep::Vector3d& point, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+
+ const ILDVMeasLayer* ml = 0;
+ return this->intersectionWithDetElement( detElementID, site, point, ml, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::intersectionWithDetElement( int detElementID, edm4hep::TrackerHit* trkhit, edm4hep::Vector3d& point, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ const ILDVMeasLayer* ml = 0;
+ return this->intersectionWithDetElement( detElementID, *site, point, ml, mode ) ;
+
+ }
+
+ int MarlinKalTestTrack::intersectionWithDetElement( int detElementID, const TKalTrackSite& site, edm4hep::Vector3d& point, const ILDVMeasLayer*& ml, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::intersectionWithDetElement( int detElementID, const TKalTrackSite& site, edm4hep::Vector3d& point, const ILDVMeasLayer*& ml, int mode) called " << std::endl;
+
+ std::vector<const ILDVMeasLayer*> meas_modules ;
+ _ktest->getSensitiveMeasurementModules( detElementID, meas_modules ) ;
+
+ if( meas_modules.size() == 0 ) {
+
+ std::stringstream errorMsg;
+ errorMsg << "MarlinKalTestTrack::intersectionWithDetElement detector element id unkown: detElementID = "
+ << decodeILD( detElementID ) << std::endl ;
+
+ throw MarlinTrk::Exception(errorMsg.str());
+
+ }
+
+ int dummy_detElementID; // not returned here as this is a single element as far as the outside world is concerned. Could check they are equal if we wanted ...
+ int error_code = this->findIntersection( meas_modules, site, point, dummy_detElementID, ml, mode ) ;
+
+ if( error_code == success ){
+
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTestTrack::intersectionWithDetElement intersection with detElementID = "
+ << decodeILD( detElementID )
+ << ": at x = " << point.x
+ << " y = " << point.y
+ << " z = " << point.z
+ << std::endl ;
+ */
+ }
+
+ else if( error_code == no_intersection ) {
+
+ ml = 0;
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTestTrack::intersectionWithDetElement No intersection with detElementID = "
+ << decodeILD( detElementID )
+ << std::endl ;
+ */
+ }
+
+ return error_code ;
+
+ }
+
+ int MarlinKalTestTrack::intersectionWithLayer( int layerID, edm4hep::Vector3d& point, int& detElementID, int mode ) {
+
+ const TKalTrackSite& site = *(dynamic_cast<const TKalTrackSite*>(_kaltrack->Last())) ;
+ const ILDVMeasLayer* ml = 0;
+ return this->intersectionWithLayer( layerID, site, point, detElementID, ml, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::intersectionWithLayer( int layerID, edm4hep::TrackerHit* trkhit, edm4hep::Vector3d& point, int& detElementID, int mode ) {
+
+ TKalTrackSite* site = 0;
+ int error_code = getSiteFromLCIOHit(trkhit, site);
+
+ if( error_code != success ) return error_code ;
+
+ const ILDVMeasLayer* ml = 0;
+ return this->intersectionWithLayer( layerID, *site, point, detElementID, ml, mode ) ;
+
+ }
+
+
+ int MarlinKalTestTrack::intersectionWithLayer( int layerID, const TKalTrackSite& site, edm4hep::Vector3d& point, int& detElementID, const ILDVMeasLayer*& ml, int mode ) {
+
+ //streamlog_out(DEBUG2) << "MarlinKalTestTrack::intersectionWithLayer( int layerID, const TKalTrackSite& site, edm4hep::Vector3d& point, int& detElementID, int mode) called layerID = " << layerID << std::endl;
+
+ std::vector<ILDVMeasLayer const*> meas_modules ;
+ _ktest->getSensitiveMeasurementModulesForLayer( layerID, meas_modules ) ;
+
+ if( meas_modules.size() == 0 ) {
+
+ //streamlog_out(DEBUG5)<< "MarlinKalTestTrack::intersectionWithLayer layer id unknown: layerID = " << decodeILD( layerID ) << std::endl ;
+ return no_intersection;
+
+ }
+
+ // int index_of_intersected;
+ int error_code = this->findIntersection( meas_modules, site, point, detElementID, ml, mode ) ;
+
+ if( error_code == success ){
+
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTestTrack::intersectionWithLayer intersection with layerID = "
+ << layerID
+ << ": at x = " << point.x
+ << " y = " << point.y
+ << " z = " << point.z
+ << " r = " << sqrt(point.x*point.x+point.y*point.y)
+ << " detElementID = " << detElementID
+ << " " << decodeILD( detElementID )
+ << std::endl ;
+ */
+ }
+ else if( error_code == no_intersection ) {
+
+ ml = 0;
+ /*
+ streamlog_out(DEBUG1) << "MarlinKalTestTrack::intersectionWithLayer No intersection with layerID = "
+ << layerID
+ << " " << decodeILD( layerID )
+ << std::endl ;
+ */
+ }
+
+ return error_code ;
+
+
+ }
+
+
+ int MarlinKalTestTrack::findIntersection( const ILDVMeasLayer& meas_module, const TKalTrackSite& site, edm4hep::Vector3d& point, double& dphi, int& detElementID, int mode ) {
+
+
+ TKalTrackState& trkState = (TKalTrackState&) site.GetCurState();
+
+
+ //--------- DEBUG --------------
+ // TKalTrackState* tsSmoothed = ( &((TVKalSite&)site).GetState(TVKalSite::kSmoothed) != 0 ?
+ // &(TKalTrackState&) ((TVKalSite&)site).GetState( TVKalSite::kSmoothed ) : 0 ) ;
+ // if( tsSmoothed == &trkState )
+ // streamlog_out(DEBUG2) << "************ MarlinKalTestTrack::intersectionWithLayer : using smoothed TrackState !!!!! " << std::endl ;
+
+ // TKalTrackState* tsFiltered = ( &((TVKalSite&)site).GetState(TVKalSite::kFiltered) != 0 ?
+ // &(TKalTrackState&) ((TVKalSite&)site).GetState( TVKalSite::kFiltered ) : 0 ) ;
+ // if( tsFiltered == &trkState )
+ // streamlog_out(DEBUG2) << "************ MarlinKalTestTrack::intersectionWithLayer : using filtered TrackState !!!!! " << std::endl ;
+ // //------------------------------
+
+
+ THelicalTrack helix = trkState.GetHelix() ;
+
+ TVector3 xto; // reference point at destination to be returned by CalcXinPointWith
+
+ int crossing_exist = meas_module.getIntersectionAndCellID(helix, xto, dphi, detElementID, mode);
+ // int crossing_exist = surf->CalcXingPointWith(helix, xto, dphi, mode) ;
+
+ //streamlog_out(DEBUG1) << "MarlinKalTestTrack::intersectionWithLayer crossing_exist = " << crossing_exist << " dphi " << dphi << " with detElementIDs: " << detElementID ;
+
+ //streamlog_out(DEBUG1) << std::endl ;
+
+
+ if( crossing_exist == 0 ) {
+ return no_intersection ;
+ }
+ else {
+
+ point.x = xto.X();
+ point.y = xto.Y();
+ point.z = xto.Z();
+
+ }
+
+ return success ;
+
+ }
+
+
+
+ int MarlinKalTestTrack::findIntersection( std::vector<ILDVMeasLayer const*>& meas_modules, const TKalTrackSite& site, edm4hep::Vector3d& point, int& detElementID, const ILDVMeasLayer*& ml, int mode ) {
+
+ unsigned int n_modules = meas_modules.size() ;
+
+ double dphi_min = DBL_MAX; // use to store the min deflection angle found so that can avoid the crossing on the far side of the layer
+ bool surf_found(false);
+
+ for( unsigned int i = 0 ; i < n_modules ; ++i ){
+
+ double dphi = 0;
+ // need to send a temporary point as we may get the crossing point with the layer on the oposite side of the layer
+ edm4hep::Vector3d point_temp ;
+
+ int temp_detElementID;
+
+ int error_code = findIntersection( *meas_modules[i], site, point_temp, dphi, temp_detElementID, mode ) ;
+
+ if( error_code == success ) {
+
+ // make sure we get the next crossing
+ if( fabs(dphi) < dphi_min ) {
+
+ dphi_min = fabs(dphi) ;
+ surf_found = true ;
+ ml = meas_modules[i];
+ detElementID = temp_detElementID;
+ point = point_temp ;
+ }
+
+ }
+ else if( error_code != no_intersection ) { // in which case error_code is an error rather than simply a lack of intersection, so return
+
+ return error_code ;
+
+ }
+
+ }
+
+ // check if the surface was found and return accordingly
+ if ( surf_found ) {
+ return success ;
+ }
+ else {
+ return no_intersection ;
+ }
+
+
+ }
+
+
+
+ void MarlinKalTestTrack::ToLCIOTrackState( const THelicalTrack& helix, const TMatrixD& cov, edm4hep::TrackState& ts, double& chi2, int& ndf) const {
+
+ chi2 = _kaltrack->GetChi2();
+ ndf = _kaltrack->GetNDF();
+
+ //============== convert parameters to LCIO convention ====
+
+ // fill 5x5 covariance matrix from the 6x6 covariance matrix above
+ TMatrixD covK(5,5) ; for(int i=0;i<5;++i) for(int j=0;j<5;++j) covK[i][j] = cov[i][j] ;
+
+ // this is for incomming tracks ...
+ double phi = toBaseRange( helix.GetPhi0() + M_PI/2. ) ;
+ double omega = 1. /helix.GetRho() ;
+ double d0 = - helix.GetDrho() ;
+ double z0 = helix.GetDz() ;
+ double tanLambda = helix.GetTanLambda() ;
+
+ ts.D0 = d0;
+ ts.phi = phi; // fi0 - M_PI/2. ) ;
+ ts.omega = omega;
+ ts.Z0 = z0;
+ ts.tanLambda = tanLambda;
+
+ Double_t cpa = helix.GetKappa();
+ double alpha = omega / cpa ; // conversion factor for omega (1/R) to kappa (1/Pt)
+
+ std::array<float, 15> covLCIO;
+ covLCIO[ 0] = covK( 0 , 0 ) ; // d0, d0
+
+ covLCIO[ 1] = - covK( 1 , 0 ) ; // phi0, d0
+ covLCIO[ 2] = covK( 1 , 1 ) ; // phi0, phi
+
+ covLCIO[ 3] = - covK( 2 , 0 ) * alpha ; // omega, d0
+ covLCIO[ 4] = covK( 2 , 1 ) * alpha ; // omega, phi
+ covLCIO[ 5] = covK( 2 , 2 ) * alpha * alpha ; // omega, omega
+
+ covLCIO[ 6] = - covK( 3 , 0 ) ; // z0 , d0
+ covLCIO[ 7] = covK( 3 , 1 ) ; // z0 , phi
+ covLCIO[ 8] = covK( 3 , 2 ) * alpha ; // z0 , omega
+ covLCIO[ 9] = covK( 3 , 3 ) ; // z0 , z0
+
+ covLCIO[10] = - covK( 4 , 0 ) ; // tanl, d0
+ covLCIO[11] = covK( 4 , 1 ) ; // tanl, phi
+ covLCIO[12] = covK( 4 , 2 ) * alpha ; // tanl, omega
+ covLCIO[13] = covK( 4 , 3 ) ; // tanl, z0
+ covLCIO[14] = covK( 4 , 4 ) ; // tanl, tanl
+
+ ts.covMatrix = covLCIO;
+
+ float pivot[3] ;
+ pivot[0] = helix.GetPivot().X() ;
+ pivot[1] = helix.GetPivot().Y() ;
+ pivot[2] = helix.GetPivot().Z() ;
+ ts.referencePoint = pivot;
+ /*
+ streamlog_out( DEBUG2 ) << " kaltest track parameters: "
+ << " chi2/ndf " << chi2 / ndf
+ << " chi2 " << chi2
+ << " ndf " << ndf
+ << " prob " << TMath::Prob(chi2, ndf)
+ << std::endl
+
+ << "\t D0 " << d0 << "[+/-" << sqrt( covLCIO[0] ) << "] "
+ << "\t Phi :" << phi << "[+/-" << sqrt( covLCIO[2] ) << "] "
+ << "\t Omega " << omega << "[+/-" << sqrt( covLCIO[5] ) << "] "
+ << "\t Z0 " << z0 << "[+/-" << sqrt( covLCIO[9] ) << "] "
+ << "\t tan(Lambda) " << tanLambda << "[+/-" << sqrt( covLCIO[14]) << "] "
+
+ << "\t pivot : [" << pivot[0] << ", " << pivot[1] << ", " << pivot[2]
+ << " - r: " << std::sqrt( pivot[0]*pivot[0]+pivot[1]*pivot[1] ) << "]"
+ << std::endl ;
+ */
+
+ }
+
+
+ void MarlinKalTestTrack::ToLCIOTrackState( const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf ) const {
+
+ TKalTrackState& trkState = (TKalTrackState&) site.GetCurState(); // GetCutState will return the last added state to this site
+ // Assuming everything has proceeded as expected
+ // this will be Predicted -> Filtered -> Smoothed
+
+ THelicalTrack helix = trkState.GetHelix() ;
+
+ TMatrixD c0(trkState.GetCovMat());
+
+ this->ToLCIOTrackState( helix, c0, ts, chi2, ndf );
+
+ }
+
+
+ int MarlinKalTestTrack::getSiteFromLCIOHit( edm4hep::TrackerHit* trkhit, TKalTrackSite*& site ) const {
+
+ std::map<edm4hep::TrackerHit*,TKalTrackSite*>::const_iterator it;
+
+ it = _hit_used_for_sites.find(trkhit) ;
+
+ if( it == _hit_used_for_sites.end() ) { // hit not associated with any site
+
+ bool found = false;
+
+ for( unsigned int i = 0; i < _hit_not_used_for_sites.size(); ++i) {
+ if( trkhit == _hit_not_used_for_sites[i] ) found = true ;
+ }
+
+ if( found ) {
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::getSiteFromLCIOHit: hit was rejected during filtering" << std::endl ;
+ return site_discarded ;
+ }
+ else {
+ //streamlog_out( DEBUG2 ) << "MarlinKalTestTrack::getSiteFromLCIOHit: hit " << trkhit << " not in list of supplied hits" << std::endl ;
+ return bad_intputs ;
+ }
+ }
+
+ site = it->second;
+
+
+ //streamlog_out( DEBUG1 ) << "MarlinKalTestTrack::getSiteFromLCIOHit: site " << site << " found for hit " << trkhit << std::endl ;
+ return success ;
+
+ }
+
+} // end of namespace MarlinTrk
diff --git a/Service/TrackSystemSvc/src/MarlinKalTestTrack.h b/Service/TrackSystemSvc/src/MarlinKalTestTrack.h
new file mode 100644
index 00000000..50551db8
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinKalTestTrack.h
@@ -0,0 +1,360 @@
+#ifndef MarlinKalTestTrack_h
+#define MarlinKalTestTrack_h
+
+#include "TrackSystemSvc/IMarlinTrack.h"
+#include "TrackSystemSvc/IMarlinTrkSystem.h"
+
+#include <TObjArray.h>
+
+#include <cmath>
+
+#include "TMatrixD.h"
+
+
+class TKalTrack ;
+class THelicalTrack ;
+class TKalTrackSite ;
+class ILDVTrackHit ;
+class ILDVMeasLayer ;
+
+namespace edm4hep{
+ class TrackerHit ;
+}
+
+namespace MarlinTrk{
+ class MarlinKalTest;
+
+/** Implementation of the IMarlinTrack interface, using KalTest and KalDet to provide
+ * the needed functionality for a Kalman Filter.
+ *
+ * @version $Id: MarlinKalTestTrack.h 3641 2012-06-13 13:04:36Z aplin $
+ * @author S.Aplin, F. Gaede DESY
+ */
+
+ class MarlinKalTestTrack : public MarlinTrk::IMarlinTrack {
+ public:
+ MarlinKalTestTrack(MarlinKalTest* ktest) ;
+
+ ~MarlinKalTestTrack() ;
+
+ protected:
+
+ private:
+
+ MarlinKalTestTrack(const MarlinKalTestTrack&) ; // Prevent copy-construction
+ MarlinKalTestTrack& operator=(const MarlinKalTestTrack&) ; // Prevent assignment
+
+ // make member functions private to force use through interface
+
+ /** add hit to track - the hits have to be added ordered in time ( i.e. typically outgoing )
+ * this order will define the direction of the energy loss used in the fit
+ */
+ int addHit(edm4hep::TrackerHit* hit) ;
+
+ /** add hit to track - the hits have to be added ordered in time ( i.e. typically outgoing )
+ * this order will define the direction of the energy loss used in the fit
+ */
+ int addHit(edm4hep::TrackerHit* trkhit, const ILDVMeasLayer* ml) ;
+
+ /** add hit to track - the hits have to be added ordered in time ( i.e. typically outgoing )
+ * this order will define the direction of the energy loss used in the fit
+ */
+ int addHit( edm4hep::TrackerHit* trkhit, ILDVTrackHit* kalhit, const ILDVMeasLayer* ml) ;
+
+ /** initialise the fit using the hits added up to this point -
+ * the fit direction has to be specified using IMarlinTrack::backward or IMarlinTrack::forward.
+ * this is the order wrt the order used in addHit() that will be used in the fit()
+ */
+ int initialise( bool fitDirection );
+
+ /** initialise the fit with a track state, and z component of the B field in Tesla.
+ * the fit direction has to be specified using IMarlinTrack::backward or IMarlinTrack::forward.
+ * this is the order that will be used in the fit().
+ * it is the users responsibility that the track state is consistent with the order
+ * of the hits used in addHit() ( i.e. the direction of energy loss )
+ */
+ int initialise( const edm4hep::TrackState& ts, double bfield_z, bool fitDirection ) ;
+
+
+ /** perform the fit of all current hits, returns error code ( IMarlinTrack::success if no error ) .
+ * the fit will be performed in the order specified at initialise() wrt the order used in addHit(), i.e.
+ * IMarlinTrack::backward implies fitting from the outside to the inside for tracks comming from the IP.
+ */
+ int fit( double maxChi2Increment=DBL_MAX ) ;
+
+
+ /** smooth all track states
+ */
+ int smooth() ;
+
+
+ /** smooth track states from the last filtered hit back to the measurement site associated with the given hit
+ */
+ int smooth( edm4hep::TrackerHit* hit ) ;
+
+
+ /** update the current fit using the supplied hit, return code via int. Provides the Chi2 increment to the fit from adding the hit via reference.
+ * the given hit will not be added if chi2increment > maxChi2Increment.
+ */
+ int addAndFit( edm4hep::TrackerHit* hit, double& chi2increment, double maxChi2Increment=DBL_MAX ) ;
+
+ /** update the current fit using the supplied hit, return code via int. Provides the Chi2 increment to the fit from adding the hit via reference.
+ * the given hit will not be added if chi2increment > maxChi2Increment.
+ */
+ int addAndFit( ILDVTrackHit* kalhit, double& chi2increment, TKalTrackSite*& site, double maxChi2Increment=DBL_MAX ) ;
+
+
+ /** obtain the chi2 increment which would result in adding the hit to the fit. This method will not alter the current fit, and the hit will not be stored in the list of hits or outliers
+ */
+ int testChi2Increment( edm4hep::TrackerHit* hit, double& chi2increment ) ;
+
+
+ // Track State Accessesors
+
+ /** get track state, returning TrackState, chi2 and ndf via reference
+ */
+ int getTrackState( edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+
+ /** get track state at measurement associated with the given hit, returning TrackState, chi2 and ndf via reference
+ */
+ int getTrackState( edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+
+ /** get the list of hits included in the fit, together with the chi2 contributions of the hits.
+ * Pointers to the hits together with their chi2 contribution will be filled into a vector of
+ * pairs consitining of the pointer as the first part of the pair and the chi2 contribution as
+ * the second.
+ */
+ int getHitsInFit( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) ;
+
+ /** get the list of hits which have been rejected by from the fit due to the a chi2 increment greater than threshold,
+ * Pointers to the hits together with their chi2 contribution will be filled into a vector of
+ * pairs consitining of the pointer as the first part of the pair and the chi2 contribution as
+ * the second.
+ */
+ int getOutliers( std::vector<std::pair<edm4hep::TrackerHit*, double> >& hits ) ;
+
+
+ /** get the current number of degrees of freedom for the fit.
+ */
+ int getNDF( int& ndf ) ;
+
+ /** get TrackeHit at which fit became constrained, i.e. ndf >= 0
+ */
+ int getTrackerHitAtPositiveNDF( edm4hep::TrackerHit*& trkhit ) ;
+
+ // PROPAGATORS
+
+ /** propagate the fit to the point of closest approach to the given point, returning TrackState, chi2 and ndf via reference
+ */
+ int propagate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+
+ /** propagate the fit at the measurement site associated with the given hit, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int propagate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+
+ /** propagate the fit at the provided measurement site, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int propagate( const edm4hep::Vector3d& point, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, const ILDVMeasLayer* ml = 0 ) ;
+
+
+ /** propagate the fit to the numbered sensitive layer, returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int propagateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** propagate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int propagateToLayer( int layerID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** propagate the fit at the measurement site, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int propagateToLayer( int layerID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** propagate the fit to sensitive detector element, returning TrackState, chi2 and ndf via reference
+ */
+ int propagateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+ /** propagate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int propagateToDetElement( int detEementID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+ /** propagate the fit at the measurement site, to sensitive detector element,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int propagateToDetElement( int detEementID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+
+
+ // EXTRAPOLATORS
+
+ /** extrapolate the fit to the point of closest approach to the given point, returning TrackState, chi2 and ndf via reference
+ */
+ int extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int extrapolate( const edm4hep::Vector3d& point, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+ /** extrapolate the fit at the measurement site, to the point of closest approach to the given point,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int extrapolate( const edm4hep::Vector3d& point, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf ) ;
+
+ /** extrapolate the fit to numbered sensitive layer, returning TrackState via provided reference
+ */
+ int extrapolateToLayer( int layerID, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int extrapolateToLayer( int layerID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to numbered sensitive layer,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int extrapolateToLayer( int layerID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int& detElementID, int mode=modeClosest ) ;
+
+ /** extrapolate the fit to sensitive detector element, returning TrackState, chi2 and ndf via reference
+ */
+ int extrapolateToDetElement( int detElementID, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning TrackState, chi2 and ndf via reference
+ */
+ int extrapolateToDetElement( int detEementID, edm4hep::TrackerHit* hit, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to sensitive detector element,
+ * returning TrackState, chi2, ndf and integer ID of sensitive detector element via reference
+ */
+ int extrapolateToDetElement( int detEementID, const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf, int mode=modeClosest ) ;
+
+
+
+ // INTERSECTORS
+
+
+ /** extrapolate the fit to numbered sensitive layer, returning intersection point in global coordinates and integer ID of the
+ * intersected sensitive detector element via reference
+ */
+ int intersectionWithLayer( int layerID, edm4hep::Vector3d& point, int& detElementID, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to numbered sensitive layer,
+ * returning intersection point in global coordinates and integer ID of the intersected sensitive detector element via reference
+ */
+ int intersectionWithLayer( int layerID, edm4hep::TrackerHit* hit, edm4hep::Vector3d& point, int& detElementID, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to numbered sensitive layer,
+ * returning intersection point in global coordinates and integer ID of the intersected sensitive detector element via reference
+ */
+ int intersectionWithLayer( int layerID, const TKalTrackSite& site, edm4hep::Vector3d& point, int& detElementID, const ILDVMeasLayer*& ml, int mode=modeClosest ) ;
+
+
+ /** extrapolate the fit to numbered sensitive detector element, returning intersection point in global coordinates via reference
+ */
+ int intersectionWithDetElement( int detElementID, edm4hep::Vector3d& point, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site associated with the given hit, to sensitive detector element,
+ * returning intersection point in global coordinates via reference
+ */
+ int intersectionWithDetElement( int detElementID, edm4hep::TrackerHit* hit, edm4hep::Vector3d& point, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to sensitive detector element,
+ * returning intersection point in global coordinates via reference
+ */
+ int intersectionWithDetElement( int detElementID, const TKalTrackSite& site, edm4hep::Vector3d& point, const ILDVMeasLayer*& ml, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to sensitive detector elements contained in the std::vector,
+ * and return intersection point in global coordinates via reference
+ */
+ int findIntersection( std::vector<ILDVMeasLayer const*>& meas_modules, const TKalTrackSite& site, edm4hep::Vector3d& point, int& detElementID, const ILDVMeasLayer*& ml, int mode=modeClosest ) ;
+
+ /** extrapolate the fit at the measurement site, to the ILDVMeasLayer,
+ * and return intersection point in global coordinates via reference
+ */
+ int findIntersection( const ILDVMeasLayer& meas_module, const TKalTrackSite& site, edm4hep::Vector3d& point, double& dphi, int& detElementIDconst, int mode=modeClosest ) ;
+
+
+
+
+ //** end of memeber functions from IMarlinTrack interface
+
+ /** fill LCIO Track State with parameters from helix and cov matrix
+ */
+ void ToLCIOTrackState( const TKalTrackSite& site, edm4hep::TrackState& ts, double& chi2, int& ndf ) const ;
+
+ /** fill LCIO Track State with parameters from helix and cov matrix
+ */
+ void ToLCIOTrackState( const THelicalTrack& helix, const TMatrixD& cov, edm4hep::TrackState& ts, double& chi2, int& ndf ) const ;
+
+ /** get the measurement site associated with the given lcio TrackerHit trkhit
+ */
+ int getSiteFromLCIOHit( edm4hep::TrackerHit* trkhit, TKalTrackSite*& site ) const ;
+
+
+
+ /** helper function to restrict the range of the azimuthal angle to ]-pi,pi]*/
+ inline double toBaseRange( double phi) const {
+ while( phi <= -M_PI ){ phi += 2. * M_PI ; }
+ while( phi > M_PI ){ phi -= 2. * M_PI ; }
+ return phi ;
+ }
+
+
+ // memeber variables
+
+ TKalTrack* _kaltrack;
+
+ std::vector<edm4hep::TrackerHit*> _lcioHits ;
+
+ TObjArray* _kalhits;
+
+ MarlinKalTest* _ktest;
+
+ edm4hep::TrackerHit* _trackHitAtPositiveNDF;
+ int _hitIndexAtPositiveNDF;
+
+ /** used to store whether initial track state has been supplied or created
+ */
+ bool _initialised ;
+
+ /** used to store the fit direction supplied to intialise
+ */
+ bool _fitDirection ;
+
+
+ /** used to store whether smoothing has been performed
+ */
+ bool _smoothed ;
+
+ /** map to store relation between lcio hits and measurement sites
+ */
+ std::map<edm4hep::TrackerHit*, TKalTrackSite*> _hit_used_for_sites ;
+
+ /** map to store relation between lcio hits kaltest hits
+ */
+ std::map<edm4hep::TrackerHit*, ILDVTrackHit*> _lcio_hits_to_kaltest_hits ;
+
+ /** vector to store lcio hits rejected for measurement sites
+ */
+ std::vector<edm4hep::TrackerHit*> _hit_not_used_for_sites ;
+
+ /** vector to store the chi-sqaure increment for measurement sites
+ */
+ std::vector< std::pair<edm4hep::TrackerHit*, double> > _hit_chi2_values ;
+
+ /** vector to store the chi-sqaure increment for measurement sites
+ */
+ std::vector< std::pair<edm4hep::TrackerHit*, double> > _outlier_chi2_values ;
+
+ } ;
+}
+#endif
diff --git a/Service/TrackSystemSvc/src/MarlinTrkDiagnostics.cc.remove b/Service/TrackSystemSvc/src/MarlinTrkDiagnostics.cc.remove
new file mode 100644
index 00000000..46aebfe6
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinTrkDiagnostics.cc.remove
@@ -0,0 +1,63 @@
+
+#include "MarlinTrkDiagnostics.h"
+
+#include "EVENT/LCObject.h"
+#include "UTIL/BitSet32.h"
+#include "UTIL/ILDConf.h"
+
+#ifdef MARLINTRK_DIAGNOSTICS_ON
+
+namespace MarlinTrk{
+
+
+ void getMCParticlesForTrackerHit(EVENT::TrackerHit* trkhit, std::vector<EVENT::MCParticle*>& mcps){
+
+ if ( !trkhit ) {
+ return;
+ }
+
+ // make sure there is nothing in the vector we wish to return
+ mcps.clear();
+
+ // first check if this is a composite space point
+ if(UTIL::BitSet32( trkhit->getType() )[ UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ]){
+
+ const EVENT::LCObjectVec rawObjects = trkhit->getRawHits();
+
+ for (unsigned iraw = 0; iraw < rawObjects.size(); ++iraw) {
+
+ EVENT::TrackerHit* rawHit = dynamic_cast< EVENT::TrackerHit* >( rawObjects[iraw] );
+
+ if( rawHit && rawHit->ext<MarlinTrk::MCTruth4HitExt>()){
+
+ EVENT::MCParticle* mcp = rawHit->ext<MarlinTrk::MCTruth4HitExt>()->simhit->getMCParticle();
+ bool found = false;
+ // check that it is not already in the vector
+ for (unsigned imcp=0; imcp<mcps.size(); ++imcp) {
+ if (mcp == mcps[imcp]) {
+ found = true;
+ break;
+ }
+ }
+
+ if( found == false ) mcps.push_back(mcp);
+
+ }
+
+
+ } // end of loop over rawObjects
+
+ // end if COMPOSITE_SPACEPOINT
+ } else {
+
+
+ if( trkhit->ext<MarlinTrk::MCTruth4HitExt>()){
+ mcps.push_back(trkhit->ext<MarlinTrk::MCTruth4HitExt>()->simhit->getMCParticle());
+ }
+
+
+ }
+ }
+}
+
+#endif
diff --git a/Service/TrackSystemSvc/src/MarlinTrkUtils.cc b/Service/TrackSystemSvc/src/MarlinTrkUtils.cc
new file mode 100644
index 00000000..f77df79e
--- /dev/null
+++ b/Service/TrackSystemSvc/src/MarlinTrkUtils.cc
@@ -0,0 +1,784 @@
+
+#include "TrackSystemSvc/MarlinTrkUtils.h"
+
+#include <vector>
+#include <algorithm>
+
+#include "TrackSystemSvc/IMarlinTrack.h"
+#include "TrackSystemSvc/HelixTrack.h"
+
+#include "lcio.h"
+//#include <IMPL/TrackImpl.h>
+//#include <IMPL/TrackStateImpl.h>
+//#include <EVENT/TrackerHit.h>
+#include "edm4hep/TrackerHit.h"
+#include "edm4hep/TrackState.h"
+#include "edm4hep/Track.h"
+
+#include <UTIL/BitField64.h>
+#include <UTIL/ILDConf.h>
+#include <UTIL/BitSet32.h>
+
+//#include "streamlog/streamlog.h"
+
+#include "TMatrixD.h"
+
+#define MIN_NDF 6
+
+namespace MarlinTrk {
+
+
+// // Check if a square matrix is Positive Definite
+// bool Matrix_Is_Positive_Definite(const EVENT::FloatVec& matrix){
+//
+// std::cout << "\n MarlinTrk::Matrix_Is_Positive_Definite(EVENT::FloatVec& matrix): " << std::endl;
+//
+// int icol,irow;
+//
+// int nrows = 5;
+//
+// TMatrixD cov(nrows,nrows) ;
+//
+// bool matrix_is_positive_definite = true;
+//
+// int icov = 0;
+// for(int irow=0; irow<nrows; ++irow ){
+// for(int jcol=0; jcol<irow+1; ++jcol){
+// cov(irow,jcol) = matrix[icov];
+// cov(jcol,irow) = matrix[icov];
+//// std::cout << " " << matrix[icov] ;
+// ++icov ;
+// }
+//// std::cout << std::endl;
+// }
+//
+//// cov.Print();
+//
+// double *pU = cov.GetMatrixArray();
+//
+// for (icol = 0; icol < nrows; icol++) {
+// const int rowOff = icol * nrows;
+//
+// //Compute fU(j,j) and test for non-positive-definiteness.
+// double ujj = pU[rowOff+icol];
+// double diagonal = ujj;
+//// std::cout << "ERROR: diagonal = " << diagonal << std::endl;
+//
+// for (irow = 0; irow < icol; irow++) {
+// const int pos_ij = irow*nrows+icol;
+// std::cout << " " << pU[pos_ij] ;
+// ujj -= pU[pos_ij]*pU[pos_ij];
+// }
+// std::cout << " " << diagonal << std::endl;
+//
+//
+// if (ujj <= 0) {
+// matrix_is_positive_definite = false;
+// }
+// }
+//
+// std::cout << std::endl;
+//
+// if ( matrix_is_positive_definite == false ) {
+// std::cout << "******************************************************" << std::endl;
+// std::cout << "** ERROR: matrix shown not to be positive definite **" << std::endl;
+// std::cout << "******************************************************" << std::endl;
+// }
+//
+// return matrix_is_positive_definite;
+//
+// }
+
+
+
+ int createTrackStateAtCaloFace( IMarlinTrack* marlinTrk, edm4hep::TrackState* track, edm4hep::TrackerHit* trkhit, bool tanL_is_positive );
+
+ int createFinalisedLCIOTrack( IMarlinTrack* marlinTrk, std::vector<edm4hep::TrackerHit*>& hit_list, edm4hep::Track* track, bool fit_backwards, const std::array<float,15>& initial_cov_for_prefit, float bfield_z, double maxChi2Increment){
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if ( hit_list.empty() ) return IMarlinTrack::bad_intputs ;
+
+ if( track == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: TrackImpl == NULL ") ) ;
+ }
+
+ int return_error = 0;
+
+
+ ///////////////////////////////////////////////////////
+ // produce prefit parameters
+ ///////////////////////////////////////////////////////
+
+ edm4hep::TrackState pre_fit ;
+
+ std::cout << "fucd:=====================" << std::endl;
+ return_error = createPrefit(hit_list, &pre_fit, bfield_z, fit_backwards);
+ std::cout << "fucd:=====================" << return_error << std::endl;
+ pre_fit.covMatrix = initial_cov_for_prefit;
+
+ ///////////////////////////////////////////////////////
+ // use prefit parameters to produce Finalised track
+ ///////////////////////////////////////////////////////
+
+ if( return_error == 0 ) {
+
+ return_error = createFinalisedLCIOTrack( marlinTrk, hit_list, track, fit_backwards, &pre_fit, bfield_z, maxChi2Increment);
+
+ } else {
+ std::cout << "MarlinTrk::createFinalisedLCIOTrack : Prefit failed error = " << return_error << std::endl;
+ }
+
+
+ return return_error;
+
+ }
+
+ int createFinalisedLCIOTrack( IMarlinTrack* marlinTrk, std::vector<edm4hep::TrackerHit*>& hit_list, edm4hep::Track* track, bool fit_backwards, edm4hep::TrackState* pre_fit, float bfield_z, double maxChi2Increment){
+
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if ( hit_list.empty() ) return IMarlinTrack::bad_intputs ;
+
+ if( track == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: TrackImpl == NULL ") ) ;
+ }
+
+ if( pre_fit == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: TrackStateImpl == NULL ") ) ;
+ }
+
+
+ int fit_status = createFit(hit_list, marlinTrk, pre_fit, bfield_z, fit_backwards, maxChi2Increment);
+
+ if( fit_status != IMarlinTrack::success ){
+
+ std::cout << "MarlinTrk::createFinalisedLCIOTrack fit failed: fit_status = " << fit_status << std::endl;
+
+ return fit_status;
+
+ }
+
+ int error = finaliseLCIOTrack(marlinTrk, track, hit_list);
+
+ return error;
+ }
+
+
+
+
+ int createFit( std::vector<edm4hep::TrackerHit*>& hit_list, IMarlinTrack* marlinTrk, edm4hep::TrackState* pre_fit, float bfield_z, bool fit_backwards, double maxChi2Increment){
+
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if ( hit_list.empty() ) return IMarlinTrack::bad_intputs;
+
+ if( marlinTrk == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::createFit: IMarlinTrack == NULL ") ) ;
+ }
+
+ if( pre_fit == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::createFit: TrackStateImpl == NULL ") ) ;
+ }
+
+ int return_error = 0;
+
+
+// ///////////////////////////////////////////////////////
+// // check that the prefit has the reference point at the correct location
+// ///////////////////////////////////////////////////////
+//
+// if (( fit_backwards == IMarlinTrack::backward && pre_fit->getLocation() != lcio::TrackState::AtLastHit )
+// ||
+// ( fit_backwards == IMarlinTrack::forward && pre_fit->getLocation() != lcio::TrackState::AtFirstHit )) {
+// std::stringstream ss ;
+//
+// ss << "MarlinTrk::createFinalisedLCIOTrack track state must be set at either first or last hit. Location = ";
+// ss << pre_fit->getLocation();
+//
+// throw EVENT::Exception( ss.str() );
+//
+// }
+
+ ///////////////////////////////////////////////////////
+ // add hits to IMarlinTrk
+ ///////////////////////////////////////////////////////
+
+ std::vector<edm4hep::TrackerHit*>::iterator it = hit_list.begin();
+
+ // start by trying to add the hits to the track we want to finally use.
+ std::cout << "MarlinTrk::createFit Start Fit: AddHits: number of hits to fit " << hit_list.size() << std::endl;
+
+ std::vector<edm4hep::TrackerHit*> added_hits;
+ unsigned int ndof_added = 0;
+
+ for( it = hit_list.begin() ; it != hit_list.end() ; ++it ) {
+
+ edm4hep::TrackerHit* trkHit = *it;
+ bool isSuccessful = false;
+ std::cout << "debug TrackerHit pointer " << trkHit << std::endl;
+ if( UTIL::BitSet32( trkHit->getType() )[ UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ] ){ //it is a composite spacepoint
+
+ //Split it up and add both hits to the MarlinTrk
+ //const EVENT::LCObjectVec rawObjects = trkHit->getRawHits();
+ std::cout << "space point is not still valid! pelease wait updating..." <<std::endl;
+ exit(1);
+ /*
+ int nRawHit = trkHit->rawHits_size();
+
+ for( unsigned k=0; k< nRawHit; k++ ){
+
+ edm4hep::TrackerHit* rawHit = dynamic_cast< edm4hep::TrackerHit* >(trkHit->getRawHits(k));
+
+ if( marlinTrk->addHit( rawHit ) == IMarlinTrack::success ){
+
+ isSuccessful = true; //if at least one hit from the spacepoint gets added
+ ++ndof_added;
+// streamlog_out(DEBUG4) << "MarlinTrk::createFit ndof_added = " << ndof_added << std::endl;
+ }
+ }
+ */
+ }
+ else { // normal non composite hit
+
+ if (marlinTrk->addHit( trkHit ) == IMarlinTrack::success ) {
+ isSuccessful = true;
+ ndof_added += 2;
+// streamlog_out(DEBUG4) << "MarlinTrk::createFit ndof_added = " << ndof_added << std::endl;
+ }
+ }
+
+ if (isSuccessful) {
+ added_hits.push_back(trkHit);
+ }
+ else{
+ std::cout << "Hit " << it - hit_list.begin() << " Dropped " << std::endl;
+ }
+
+ }
+
+ if( ndof_added < MIN_NDF ) {
+ //streamlog_out(DEBUG2) << "MarlinTrk::createFit : Cannot fit less with less than " << MIN_NDF << " degrees of freedom. Number of hits = " << added_hits.size() << " ndof = " << ndof_added << std::endl;
+ return IMarlinTrack::bad_intputs;
+ }
+
+
+
+ ///////////////////////////////////////////////////////
+ // set the initial track parameters
+ ///////////////////////////////////////////////////////
+
+ return_error = marlinTrk->initialise( *pre_fit, bfield_z, IMarlinTrack::backward ) ;
+ if (return_error != IMarlinTrack::success) {
+
+ //streamlog_out(DEBUG5) << "MarlinTrk::createFit Initialisation of track fit failed with error : " << return_error << std::endl;
+
+ return return_error;
+
+ }
+
+ ///////////////////////////////////////////////////////
+ // try fit and return error
+ ///////////////////////////////////////////////////////
+ int status = marlinTrk->fit(maxChi2Increment);
+ std::cout << "fucd===================" << status << std::endl;
+
+ return status;
+
+ }
+
+
+
+ int createPrefit( std::vector<edm4hep::TrackerHit*>& hit_list, edm4hep::TrackState* pre_fit, float bfield_z, bool fit_backwards){
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if ( hit_list.empty() ) return IMarlinTrack::bad_intputs ;
+
+ if( pre_fit == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: TrackStateImpl == NULL ") ) ;
+ }
+
+ ///////////////////////////////////////////////////////
+ // loop over all the hits and create a list consisting only 2D hits
+ ///////////////////////////////////////////////////////
+
+ std::vector<edm4hep::TrackerHit*> twoD_hits;
+
+ for (unsigned ihit=0; ihit < hit_list.size(); ++ihit) {
+
+ // check if this a space point or 2D hit
+ if(UTIL::BitSet32( hit_list[ihit]->getType() )[ UTIL::ILDTrkHitTypeBit::ONE_DIMENSIONAL ] == false ){
+ // then add to the list
+ twoD_hits.push_back(hit_list[ihit]);
+
+ }
+ }
+
+ ///////////////////////////////////////////////////////
+ // check that there are enough 2-D hits to create a helix
+ ///////////////////////////////////////////////////////
+
+ if (twoD_hits.size() < 3) { // no chance to initialise print warning and return
+ //streamlog_out(WARNING) << "MarlinTrk::createFinalisedLCIOTrack Cannot create helix from less than 3 2-D hits" << std::endl;
+ return IMarlinTrack::bad_intputs;
+ }
+
+ ///////////////////////////////////////////////////////
+ // make a helix from 3 hits to get a trackstate
+ ///////////////////////////////////////////////////////
+
+ // SJA:FIXME: this may not be the optimal 3 hits to take in certain cases where the 3 hits are not well spread over the track length
+ const edm4hep::Vector3d& x1 = twoD_hits[0]->getPosition();
+ const edm4hep::Vector3d& x2 = twoD_hits[ twoD_hits.size()/2 ]->getPosition();
+ const edm4hep::Vector3d& x3 = twoD_hits.back()->getPosition();
+
+ HelixTrack helixTrack( x1, x2, x3, bfield_z, HelixTrack::forwards );
+
+ if ( fit_backwards == IMarlinTrack::backward ) {
+ pre_fit->location = MarlinTrk::Location::AtLastHit;
+ helixTrack.moveRefPoint(hit_list.back()->getPosition()[0], hit_list.back()->getPosition()[1], hit_list.back()->getPosition()[2]);
+ } else {
+ pre_fit->location = MarlinTrk::Location::AtFirstHit;
+ helixTrack.moveRefPoint(hit_list.front()->getPosition()[0], hit_list.front()->getPosition()[1], hit_list.front()->getPosition()[2]);
+ }
+
+
+ const float referencePoint[3] = { helixTrack.getRefPointX() , helixTrack.getRefPointY() , helixTrack.getRefPointZ() };
+
+ pre_fit->D0 = helixTrack.getD0();
+ pre_fit->phi = helixTrack.getPhi0();
+ pre_fit->omega = helixTrack.getOmega();
+ pre_fit->Z0 = helixTrack.getZ0();
+ pre_fit->tanLambda = helixTrack.getTanLambda();
+
+ pre_fit->referencePoint = referencePoint;
+
+ return IMarlinTrack::success;
+
+ }
+
+ int finaliseLCIOTrack( IMarlinTrack* marlintrk, edm4hep::Track* track, std::vector<edm4hep::TrackerHit*>& hit_list, edm4hep::TrackState* atLastHit, edm4hep::TrackState* atCaloFace){
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if( marlintrk == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: IMarlinTrack == NULL ") ) ;
+ }
+
+ if( track == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: TrackImpl == NULL ") ) ;
+ }
+
+ if( atCaloFace && atLastHit == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: atLastHit == NULL ") ) ;
+ }
+
+ if( atLastHit && atCaloFace == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::finaliseLCIOTrack: atCaloFace == NULL ") ) ;
+ }
+
+
+
+ ///////////////////////////////////////////////////////
+ // error to return if any
+ ///////////////////////////////////////////////////////
+ int return_error = 0;
+
+ int ndf = 0;
+ double chi2 = -DBL_MAX;
+
+ /////////////////////////////////////////////////////////////
+ // First check NDF to see if it make any sense to continue.
+ // The track will be dropped if the NDF is less than 0
+ /////////////////////////////////////////////////////////////
+
+ return_error = marlintrk->getNDF(ndf);
+
+ if ( return_error != IMarlinTrack::success) {
+ //streamlog_out(DEBUG3) << "MarlinTrk::finaliseLCIOTrack: getNDF returns " << return_error << std::endl;
+ return return_error;
+ } else if( ndf < 0 ) {
+ //streamlog_out(DEBUG2) << "MarlinTrk::finaliseLCIOTrack: number of degrees of freedom less than 0 track dropped : NDF = " << ndf << std::endl;
+ return IMarlinTrack::error;
+ } else {
+ //streamlog_out(DEBUG1) << "MarlinTrk::finaliseLCIOTrack: NDF = " << ndf << std::endl;
+ }
+
+
+
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // get the list of hits used in the fit
+ // add these to the track, add spacepoints as long as at least on strip hit is used.
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ std::vector<std::pair<edm4hep::TrackerHit*, double> > hits_in_fit;
+ std::vector<std::pair<edm4hep::TrackerHit*, double> > outliers;
+ std::vector<edm4hep::TrackerHit*> used_hits;
+
+ hits_in_fit.reserve(300);
+ outliers.reserve(300);
+
+ marlintrk->getHitsInFit(hits_in_fit);
+ marlintrk->getOutliers(outliers);
+
+ ///////////////////////////////////////////////
+ // now loop over the hits provided for fitting
+ // we do this so that the hits are added in the
+ // order in which they have been fitted
+ ///////////////////////////////////////////////
+
+ for ( unsigned ihit = 0; ihit < hit_list.size(); ++ihit) {
+
+ edm4hep::TrackerHit* trkHit = hit_list[ihit];
+
+ if( UTIL::BitSet32( trkHit->getType() )[ UTIL::ILDTrkHitTypeBit::COMPOSITE_SPACEPOINT ] ){ //it is a composite spacepoint
+ std::cout << "Error: space point is not still valid! pelease wait updating..." <<std::endl;
+ exit(1);
+ /*
+ // get strip hits
+ int nRawHit = trkHit->rawHits_size();
+
+ for( unsigned k=0; k< nRawHit; k++ ){
+
+ edm4hep::TrackerHit* rawHit = dynamic_cast< edm4hep::TrackerHit* >(trkHit->getRawHits(k));
+
+ bool is_outlier = false;
+
+ // here we loop over outliers as this will be faster than looping over the used hits
+ for ( unsigned ohit = 0; ohit < outliers.size(); ++ohit) {
+
+ if ( rawHit == outliers[ohit].first ) {
+ is_outlier = true;
+ break; // break out of loop over outliers
+ }
+ }
+
+ if (is_outlier == false) {
+ used_hits.push_back(hit_list[ihit]);
+ track->addTrackerHit(*used_hits.back());
+ break; // break out of loop over rawObjects
+ }
+ }
+ */
+ } else {
+
+ bool is_outlier = false;
+
+ // here we loop over outliers as this will be faster than looping over the used hits
+ for ( unsigned ohit = 0; ohit < outliers.size(); ++ohit) {
+
+ if ( trkHit == outliers[ohit].first ) {
+ is_outlier = true;
+ break; // break out of loop over outliers
+ }
+ }
+
+ if (is_outlier == false) {
+ used_hits.push_back(hit_list[ihit]);
+ track->addToTrackerHits(*used_hits.back());
+ }
+ }
+ }
+
+
+// ///////////////////////////////////////////////////////////////////////////
+// // We now need to find out at which point the fit is constrained
+// // and therefore be able to provide well formed (pos. def.) cov. matrices
+// ///////////////////////////////////////////////////////////////////////////
+//
+
+
+ ///////////////////////////////////////////////////////
+ // first hit
+ ///////////////////////////////////////////////////////
+
+ edm4hep::TrackState* trkStateAtFirstHit = new edm4hep::TrackState() ;
+ edm4hep::TrackerHit* firstHit = hits_in_fit.back().first;
+
+ ///////////////////////////////////////////////////////
+ // last hit
+ ///////////////////////////////////////////////////////
+
+ edm4hep::TrackState* trkStateAtLastHit = new edm4hep::TrackState() ;
+ edm4hep::TrackerHit* lastHit = hits_in_fit.front().first;
+
+ edm4hep::TrackerHit* last_constrained_hit = 0 ;
+ marlintrk->getTrackerHitAtPositiveNDF(last_constrained_hit);
+
+ return_error = marlintrk->smooth(lastHit);
+
+ if ( return_error != IMarlinTrack::success ) {
+ //streamlog_out(DEBUG4) << "MarlinTrk::finaliseLCIOTrack: return_code for smoothing to " << lastHit << " = " << return_error << " NDF = " << ndf << std::endl;
+ delete trkStateAtFirstHit;
+ delete trkStateAtLastHit;
+ return return_error ;
+ }
+
+
+ ///////////////////////////////////////////////////////
+ // first create trackstate at IP
+ ///////////////////////////////////////////////////////
+ const edm4hep::Vector3d point; // nominal IP
+
+ edm4hep::TrackState* trkStateIP = new edm4hep::TrackState() ;
+
+
+ ///////////////////////////////////////////////////////
+ // make sure that the track state can be propagated to the IP
+ ///////////////////////////////////////////////////////
+
+ return_error = marlintrk->propagate(point, firstHit, *trkStateIP, chi2, ndf ) ;
+
+ if ( return_error != IMarlinTrack::success ) {
+ //streamlog_out(DEBUG4) << "MarlinTrk::finaliseLCIOTrack: return_code for propagation = " << return_error << " NDF = " << ndf << std::endl;
+ delete trkStateIP;
+ delete trkStateAtFirstHit;
+ delete trkStateAtLastHit;
+
+ return return_error ;
+ }
+
+ trkStateIP->location = MarlinTrk::Location::AtIP;
+ track->addToTrackStates(*trkStateIP);
+ track->setChi2(chi2);
+ track->setNdf(ndf);
+
+
+ ///////////////////////////////////////////////////////
+ // set the track states at the first and last hits
+ ///////////////////////////////////////////////////////
+
+ ///////////////////////////////////////////////////////
+ // @ first hit
+ ///////////////////////////////////////////////////////
+
+ //streamlog_out( DEBUG5 ) << " >>>>>>>>>>> create TrackState AtFirstHit" << std::endl ;
+
+
+ return_error = marlintrk->getTrackState(firstHit, *trkStateAtFirstHit, chi2, ndf ) ;
+
+ if ( return_error == 0 ) {
+ trkStateAtFirstHit->location = MarlinTrk::Location::AtFirstHit;
+ track->addToTrackStates(*trkStateAtFirstHit);
+ } else {
+ //streamlog_out( WARNING ) << " >>>>>>>>>>> MarlinTrk::finaliseLCIOTrack: could not get TrackState at First Hit " << firstHit << std::endl ;
+ delete trkStateAtFirstHit;
+ }
+
+ double r_first = firstHit->getPosition()[0]*firstHit->getPosition()[0] + firstHit->getPosition()[1]*firstHit->getPosition()[1];
+
+ track->setRadiusOfInnermostHit(sqrt(r_first));
+
+ if ( atLastHit == 0 && atCaloFace == 0 ) {
+
+ ///////////////////////////////////////////////////////
+ // @ last hit
+ ///////////////////////////////////////////////////////
+
+ //streamlog_out( DEBUG5 ) << " >>>>>>>>>>> create TrackState AtLastHit : using trkhit " << last_constrained_hit << std::endl ;
+
+ edm4hep::Vector3d last_hit_pos(lastHit->getPosition());
+
+ return_error = marlintrk->propagate(last_hit_pos, last_constrained_hit, *trkStateAtLastHit, chi2, ndf);
+
+// return_error = marlintrk->getTrackState(lastHit, *trkStateAtLastHit, chi2, ndf ) ;
+
+ if ( return_error == 0 ) {
+ trkStateAtLastHit->location = MarlinTrk::Location::AtLastHit;
+ track->addToTrackStates(*trkStateAtLastHit);
+ } else {
+ //streamlog_out( DEBUG5 ) << " >>>>>>>>>>> MarlinTrk::finaliseLCIOTrack: could not get TrackState at Last Hit " << last_constrained_hit << std::endl ;
+ delete trkStateAtLastHit;
+ }
+
+// const EVENT::FloatVec& ma = trkStateAtLastHit->getCovMatrix();
+//
+// Matrix_Is_Positive_Definite( ma );
+
+ ///////////////////////////////////////////////////////
+ // set the track state at Calo Face
+ ///////////////////////////////////////////////////////
+
+ edm4hep::TrackState trkStateCalo;
+ bool tanL_is_positive = trkStateIP->tanLambda >0 ;
+
+ return_error = createTrackStateAtCaloFace(marlintrk, &trkStateCalo, last_constrained_hit, tanL_is_positive);
+// return_error = createTrackStateAtCaloFace(marlintrk, trkStateCalo, lastHit, tanL_is_positive);
+
+ if ( return_error == 0 ) {
+ trkStateCalo.location = MarlinTrk::Location::AtCalorimeter;
+ track->addToTrackStates(trkStateCalo);
+ } else {
+ //streamlog_out( WARNING ) << " >>>>>>>>>>> MarlinTrk::finaliseLCIOTrack: could not get TrackState at Calo Face " << std::endl ;
+ //delete trkStateCalo;
+ }
+ } else {
+ track->addToTrackStates(*atLastHit);
+ track->addToTrackStates(*atCaloFace);
+ }
+
+ ///////////////////////////////////////////////////////
+ // done
+ ///////////////////////////////////////////////////////
+ return return_error;
+ }
+
+
+ int createTrackStateAtCaloFace( IMarlinTrack* marlintrk, edm4hep::TrackState* trkStateCalo, edm4hep::TrackerHit* trkhit, bool tanL_is_positive ){
+
+ //streamlog_out( DEBUG5 ) << " >>>>>>>>>>> createTrackStateAtCaloFace : using trkhit " << trkhit << " tanL_is_positive = " << tanL_is_positive << std::endl ;
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if( marlintrk == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::createTrackStateAtCaloFace: IMarlinTrack == NULL ") ) ;
+ }
+
+ if( trkStateCalo == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::createTrackStateAtCaloFace: TrackImpl == NULL ") ) ;
+ }
+
+ if( trkhit == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::createTrackStateAtCaloFace: TrackHit == NULL ") ) ;
+ }
+
+ int return_error = 0;
+
+ double chi2 = -DBL_MAX;
+ int ndf = 0;
+
+ UTIL::BitField64 encoder( lcio::ILDCellID0::encoder_string ) ;
+ encoder.reset() ; // reset to 0
+
+ encoder[lcio::ILDCellID0::subdet] = lcio::ILDDetID::ECAL ;
+ encoder[lcio::ILDCellID0::side] = lcio::ILDDetID::barrel;
+ encoder[lcio::ILDCellID0::layer] = 0 ;
+
+ int detElementID = 0;
+
+ return_error = marlintrk->propagateToLayer(encoder.lowWord(), trkhit, *trkStateCalo, chi2, ndf, detElementID, IMarlinTrack::modeForward ) ;
+
+ if (return_error == IMarlinTrack::no_intersection ) { // try forward or backward
+ if (tanL_is_positive) {
+ encoder[lcio::ILDCellID0::side] = lcio::ILDDetID::fwd;
+ }
+ else{
+ encoder[lcio::ILDCellID0::side] = lcio::ILDDetID::bwd;
+ }
+ return_error = marlintrk->propagateToLayer(encoder.lowWord(), trkhit, *trkStateCalo, chi2, ndf, detElementID, IMarlinTrack::modeForward ) ;
+ }
+
+ if (return_error !=IMarlinTrack::success ) {
+ //streamlog_out( DEBUG5 ) << " >>>>>>>>>>> createTrackStateAtCaloFace : could not get TrackState at Calo Face: return_error = " << return_error << std::endl ;
+ }
+
+
+ return return_error;
+
+
+ }
+
+ void addHitNumbersToTrack(edm4hep::Track* track, std::vector<edm4hep::TrackerHit*>& hit_list, bool hits_in_fit, UTIL::BitField64& cellID_encoder){
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if( track == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::addHitsToTrack: TrackImpl == NULL ") ) ;
+ }
+
+ std::map<int, int> hitNumbers;
+
+ hitNumbers[lcio::ILDDetID::VXD] = 0;
+ hitNumbers[lcio::ILDDetID::SIT] = 0;
+ hitNumbers[lcio::ILDDetID::FTD] = 0;
+ hitNumbers[lcio::ILDDetID::TPC] = 0;
+ hitNumbers[lcio::ILDDetID::SET] = 0;
+ hitNumbers[lcio::ILDDetID::ETD] = 0;
+
+ for(unsigned int j=0; j<hit_list.size(); ++j) {
+
+ cellID_encoder.setValue(hit_list.at(j)->getCellID()) ;
+ int detID = cellID_encoder[UTIL::ILDCellID0::subdet];
+ ++hitNumbers[detID];
+ // streamlog_out( DEBUG1 ) << "Hit from Detector " << detID << std::endl;
+ }
+
+ int offset = 2 ;
+ if ( hits_in_fit == false ) { // all hit atributed by patrec
+ offset = 1 ;
+ }
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::VXD]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::FTD]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::SIT]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::TPC]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::SET]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::ETD]);
+ //track->subdetectorHitNumbers().resize(2 * lcio::ILDDetID::ETD);
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::VXD - offset ] = hitNumbers[lcio::ILDDetID::VXD];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::FTD - offset ] = hitNumbers[lcio::ILDDetID::FTD];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SIT - offset ] = hitNumbers[lcio::ILDDetID::SIT];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::TPC - offset ] = hitNumbers[lcio::ILDDetID::TPC];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SET - offset ] = hitNumbers[lcio::ILDDetID::SET];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::ETD - offset ] = hitNumbers[lcio::ILDDetID::ETD];
+
+
+
+ }
+
+ void addHitNumbersToTrack(edm4hep::Track* track, std::vector<std::pair<edm4hep::TrackerHit* , double> >& hit_list, bool hits_in_fit, UTIL::BitField64& cellID_encoder){
+
+ ///////////////////////////////////////////////////////
+ // check inputs
+ ///////////////////////////////////////////////////////
+ if( track == 0 ){
+ throw EVENT::Exception( std::string("MarlinTrk::addHitsToTrack: TrackImpl == NULL ") ) ;
+ }
+
+ std::map<int, int> hitNumbers;
+
+ hitNumbers[lcio::ILDDetID::VXD] = 0;
+ hitNumbers[lcio::ILDDetID::SIT] = 0;
+ hitNumbers[lcio::ILDDetID::FTD] = 0;
+ hitNumbers[lcio::ILDDetID::TPC] = 0;
+ hitNumbers[lcio::ILDDetID::SET] = 0;
+ hitNumbers[lcio::ILDDetID::ETD] = 0;
+
+ for(unsigned int j=0; j<hit_list.size(); ++j) {
+
+ cellID_encoder.setValue(hit_list.at(j).first->getCellID()) ;
+ int detID = cellID_encoder[UTIL::ILDCellID0::subdet];
+ ++hitNumbers[detID];
+ // streamlog_out( DEBUG1 ) << "Hit from Detector " << detID << std::endl;
+ }
+
+ int offset = 2 ;
+ if ( hits_in_fit == false ) { // all hit atributed by patrec
+ offset = 1 ;
+ }
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::VXD]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::FTD]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::SIT]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::TPC]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::SET]);
+ track->addToSubDetectorHitNumbers(hitNumbers[lcio::ILDDetID::ETD]);
+ //track->subdetectorHitNumbers().resize(2 * lcio::ILDDetID::ETD);
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::VXD - offset ] = hitNumbers[lcio::ILDDetID::VXD];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::FTD - offset ] = hitNumbers[lcio::ILDDetID::FTD];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SIT - offset ] = hitNumbers[lcio::ILDDetID::SIT];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::TPC - offset ] = hitNumbers[lcio::ILDDetID::TPC];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::SET - offset ] = hitNumbers[lcio::ILDDetID::SET];
+ //track->subdetectorHitNumbers()[ 2 * lcio::ILDDetID::ETD - offset ] = hitNumbers[lcio::ILDDetID::ETD];
+
+ }
+
+}
diff --git a/Service/TrackSystemSvc/src/TrackSystemSvc.cpp b/Service/TrackSystemSvc/src/TrackSystemSvc.cpp
new file mode 100644
index 00000000..78bb2779
--- /dev/null
+++ b/Service/TrackSystemSvc/src/TrackSystemSvc.cpp
@@ -0,0 +1,72 @@
+#include "GearSvc/IGearSvc.h"
+#include "gear/GearMgr.h"
+
+#include "MarlinKalTest.h"
+
+#include "TrackSystemSvc.h"
+
+DECLARE_COMPONENT(TrackSystemSvc)
+
+TrackSystemSvc::TrackSystemSvc(const std::string& name, ISvcLocator* svc)
+ : base_class(name, svc),
+ m_trackSystem(nullptr){
+}
+
+TrackSystemSvc::~TrackSystemSvc(){
+}
+
+MarlinTrk::IMarlinTrkSystem* TrackSystemSvc::getTrackSystem(){
+ if(!m_trackSystem){
+ auto _gear = service<IGearSvc>("GearSvc");
+ if ( !_gear ) {
+ error() << "Failed to find GearSvc ..." << endmsg;
+ return 0;
+ }
+ gear::GearMgr* mgr = _gear->getGearMgr();
+
+ auto _geoSvc = service<IGeoSvc>("GeoSvc");
+ if ( !_geoSvc ) {
+ error() << "Failed to find GeoSvc ..." << endmsg;
+ return 0;
+ }
+ m_trackSystem = new MarlinTrk::MarlinKalTest( *mgr, _geoSvc ) ;
+ }
+ return m_trackSystem;
+}
+
+StatusCode TrackSystemSvc::initialize(){
+
+ auto _gear = service<IGearSvc>("GearSvc");
+ if ( !_gear ) {
+ error() << "Failed to find GearSvc ..." << endmsg;
+ return StatusCode::FAILURE;
+ }
+ gear::GearMgr* mgr = _gear->getGearMgr();
+
+ auto _geoSvc = service<IGeoSvc>("GeoSvc");
+ if ( !_geoSvc ) {
+ error() << "Failed to find GeoSvc ..." << endmsg;
+ return StatusCode::FAILURE;
+ }
+ m_trackSystem = new MarlinTrk::MarlinKalTest( *mgr, _geoSvc ) ;
+
+ return StatusCode::SUCCESS;
+}
+
+void TrackSystemSvc::removeTrackSystem(){
+ if ( m_trackSystem ) {
+ delete m_trackSystem;
+ m_trackSystem = nullptr;
+ }
+ return;
+}
+
+StatusCode TrackSystemSvc::finalize(){
+
+ // if ( m_trackSystem ) {
+ // delete m_trackSystem;
+ // m_trackSystem = nullptr;
+ // }
+
+ return StatusCode::SUCCESS;
+}
diff --git a/Service/TrackSystemSvc/src/TrackSystemSvc.h b/Service/TrackSystemSvc/src/TrackSystemSvc.h
new file mode 100644
index 00000000..45830a4f
--- /dev/null
+++ b/Service/TrackSystemSvc/src/TrackSystemSvc.h
@@ -0,0 +1,22 @@
+#ifndef TrackSystemSvc_h
+#define TrackSystemSvc_h
+
+#include "TrackSystemSvc/ITrackSystemSvc.h"
+#include <GaudiKernel/Service.h>
+
+class TrackSystemSvc : public extends<Service, ITrackSystemSvc>{
+ public:
+ TrackSystemSvc(const std::string& name, ISvcLocator* svc);
+ ~TrackSystemSvc();
+
+ MarlinTrk::IMarlinTrkSystem* getTrackSystem() override;
+ void removeTrackSystem() override;
+
+ StatusCode initialize() override;
+ StatusCode finalize() override;
+
+ private:
+ MarlinTrk::IMarlinTrkSystem* m_trackSystem;
+};
+
+#endif
--
GitLab