diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..906b53eff120aa483f60fbf1dd2c4501831ff9bb
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h
@@ -0,0 +1,38 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __ITRIG_L2_FAST_EXTRAPOLATION_TOOL_H__
+#define __ITRIG_L2_FAST_EXTRAPOLATION_TOOL_H__
+
+#include "GaudiKernel/IAlgTool.h"
+#include <vector>
+
+namespace Trk {
+ class TrkPlanarSurface;
+ class TrkTrackState;
+}
+
+static const InterfaceID IID_ITrigL2FastExtrapolationTool("ITrigL2FastExtrapolationTool",1,0);
+
+
+ /** @class ITrigL2FastExtrapolationTool
+
+ provides the abstract interface for a track extrapolation tool used by TrigInDetTrackFitter
+
+ @author D.Emeliyanov <http://consult.cern.ch/xwho>
+ */
+
+ class ITrigL2FastExtrapolationTool : virtual public IAlgTool {
+
+ public:
+ /** other standard AlgTool methods */
+
+ static const InterfaceID& interfaceID () //!< the Tool's interface
+ { return IID_ITrigL2FastExtrapolationTool; }
+ virtual Trk::TrkTrackState* extrapolate(Trk::TrkTrackState*,
+ Trk::TrkPlanarSurface*,
+ Trk::TrkPlanarSurface*, bool) = 0;
+ };
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2TrackFittingTool.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2TrackFittingTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..01f66862c08149ee01ee666aa4280ae7506cebeb
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/ITrigL2TrackFittingTool.h
@@ -0,0 +1,37 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __ITRIG_L2_TRACK_FITTING_TOOL_H__
+#define __ITRIG_L2_TRACK_FITTING_TOOL_H__
+
+#include "GaudiKernel/IAlgTool.h"
+#include <vector>
+
+namespace Trk {
+ class TrkBaseNode;
+ class TrkTrackState;
+}
+
+static const InterfaceID IID_ITrigL2TrackFittingTool("ITrigL2TrackFittingTool",1,0);
+
+
+ /** @class ITrigL2TrackFittingTool
+
+ provides the abstract interface for track fitting tools used by TrigInDetTrackFitter
+
+ @author D.Emeliyanov <http://consult.cern.ch/xwho>
+ */
+
+ class ITrigL2TrackFittingTool : virtual public IAlgTool {
+
+ public:
+ /** other standard AlgTool methods */
+
+ static const InterfaceID& interfaceID () //!< the Tool's interface
+ { return IID_ITrigL2TrackFittingTool; }
+
+ virtual Trk::TrkTrackState* fit(Trk::TrkTrackState*, std::vector<Trk::TrkBaseNode*>&, bool) = 0;
+ };
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/PerigeeFilteringNodes.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/PerigeeFilteringNodes.h
new file mode 100755
index 0000000000000000000000000000000000000000..6a591f6c9031a18c2bf58398f02ab2b11a05083a
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/PerigeeFilteringNodes.h
@@ -0,0 +1,99 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __PERIGEEFILTERINGNODES_H__
+#define __PERIGEEFILTERINGNODES_H__
+
+class TrigSiSpacePoint;
+
+class BasePerigeeFilteringNode
+{
+ public:
+ BasePerigeeFilteringNode();
+ virtual ~BasePerigeeFilteringNode(){};
+ virtual double m_getChi2(double*,double*) = 0;
+ virtual void m_runFilter(double*,double*);
+ virtual void m_applyMultScatt(double*,double*);
+ virtual void m_applyEnergyLoss(double*,double*,int);
+ virtual int m_getNdof() = 0;
+ virtual double m_getRho() = 0;
+ void m_setType(char type)
+ {
+ m_nType=type;
+ }
+ char m_getType()
+ {
+ return m_nType;
+ }
+ void m_setLayer(long lay)
+ {
+ m_nLayer=lay;
+ }
+ long m_getLayer()
+ {
+ return m_nLayer;
+ }
+ const TrigSiSpacePoint* m_getTrigSp()
+ {
+ return m_pTrigSp;
+ }
+ bool m_isAccepted();
+ void m_acceptIt();
+ void m_rejectIt();
+ protected:
+ char m_nType,m_nStatus;
+ long m_nLayer;
+ double m_dChi2;
+ double m_D[2][2];
+ double m_B[2][5];
+ double m_resid[2];
+ double m_EffSigmaMS;
+ double m_EffRadLength;
+ double m_Rho;
+ const TrigSiSpacePoint* m_pTrigSp;
+};
+
+class BarrelPerigeeFilteringNode: public BasePerigeeFilteringNode
+{
+ private:
+ double m_Ri;
+ double m_yPhi,m_sigmaPhi;
+ double m_yZ,m_sigmaZ;
+ public:
+ BarrelPerigeeFilteringNode(const TrigSiSpacePoint*);
+ ~BarrelPerigeeFilteringNode(){};
+ double m_getChi2(double*,double*);
+ void m_runFilter(double*,double*);
+ void m_applyMultScatt(double*,double*);
+ void m_applyEnergyLoss(double*,double*,int);
+ double m_getEffRadLength(double*);
+ int m_getNdof();
+ double m_getRho()
+ {
+ return m_Ri;
+ }
+};
+
+class DiscPerigeeFilteringNode: public BasePerigeeFilteringNode
+{
+ private:
+ double m_Zi;
+ double m_yPhi,m_sigmaPhi;
+ double m_yR,m_sigmaR;
+ public:
+ DiscPerigeeFilteringNode(const TrigSiSpacePoint*);
+ ~DiscPerigeeFilteringNode(){};
+ double m_getChi2(double*,double*);
+ void m_applyMultScatt(double*,double*);
+ void m_applyEnergyLoss(double*,double*,int);
+ double m_getEffRadLength(double*);
+ void m_runFilter(double*,double*);
+ int m_getNdof();
+ double m_getRho()
+ {
+ return m_Rho;
+ }
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigDkfTrackMakerTool.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigDkfTrackMakerTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..f28e389f24e7a8cf5a91a0196118510dd1ef8719
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigDkfTrackMakerTool.h
@@ -0,0 +1,52 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGDKFTRACKMAKERTOOL_H__
+#define __TRIGDKFTRACKMAKERTOOL_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+
+#include "TrigInDetToolInterfaces/ITrigDkfTrackMakerTool.h"
+#include <vector>
+
+#include "InDetIdentifier/SCT_ID.h"
+#include "InDetIdentifier/PixelID.h"
+#include "InDetReadoutGeometry/PixelDetectorManager.h"
+#include "InDetReadoutGeometry/SCT_DetectorManager.h"
+#include "TrkTrack/Track.h"
+
+
+class AtlasDetectorID;
+class PixelID;
+class SCT_ID;
+
+class TrigDkfTrackMakerTool : virtual public ITrigDkfTrackMakerTool, public AthAlgTool {
+ public:
+
+ // standard AlgTool methods
+ TrigDkfTrackMakerTool(const std::string&,const std::string&,const IInterface*);
+ virtual ~TrigDkfTrackMakerTool();
+
+ // standard Athena methods
+ StatusCode initialize();
+ StatusCode finalize();
+
+ bool createDkfTrack(std::vector<const TrigSiSpacePoint*>&, std::vector<Trk::TrkBaseNode*>&, double);
+ bool createDkfTrack(const Trk::Track& track, std::vector<Trk::TrkBaseNode*>& vpTrkNodes, double DChi2);
+
+
+ private:
+ const PixelID* m_pixelId;
+ const SCT_ID* m_sctId;
+ const AtlasDetectorID* m_idHelper;
+
+ const InDetDD::PixelDetectorManager* m_pixelManager;
+ const InDetDD::SCT_DetectorManager* m_SCT_Manager;
+
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetBremDetectionTool.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetBremDetectionTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..1020510c59a3de60aa9856a9b78c1c0f7baaf90b
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetBremDetectionTool.h
@@ -0,0 +1,98 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDETBREMDETECTIONTOOL_H__
+#define __TRIGINDETBREMDETECTIONTOOL_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/MsgStream.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetBremDetectionTool.h"
+#include <vector>
+
+#define MAX_RES_SIZE 100
+#define MAX_INP_SIZE 20
+
+
+class LSMSolution
+{
+public:
+ void m_report();
+ void m_fixVariable(int);
+ bool m_isOnConstraint(int);
+ double& m_Significance();
+ double& m_Cov(int,int);
+ LSMSolution(int);
+ LSMSolution();
+ virtual ~LSMSolution();
+ double& operator[] (int);
+
+private:
+ int m_fixedVariables[MAX_INP_SIZE];
+ double m_Chi2;
+ double m_C[MAX_INP_SIZE][MAX_INP_SIZE];
+ int m_size;
+ double m_u[MAX_INP_SIZE];
+};
+
+class TrigInDetBremDetectionTool : virtual public ITrigInDetBremDetectionTool, public AthAlgTool {
+ public:
+
+ // standard AlgTool methods
+ TrigInDetBremDetectionTool(const std::string&,const std::string&,const IInterface*);
+ virtual ~TrigInDetBremDetectionTool();
+
+ // standard Athena methods
+ StatusCode initialize();
+ StatusCode finalize();
+ virtual void reset();
+ virtual bool addNewPoint(Trk::TrkTrackState*,Trk::TrkBaseNode*, Trk::TrkPlanarSurface*, double a[5][5],double);
+ virtual bool solve(int);
+ virtual void modifySurfaces(int);
+ virtual void report(int);
+
+ private:
+
+ void m_mixSolutions(LSMSolution*, LSMSolution*);
+ bool m_checkFeasibility(LSMSolution*);
+ int m_findBestDirection(double*);
+ bool m_goodGradient(double*);
+ bool m_isZempty();
+ double m_getCriterionValue(LSMSolution*);
+ LSMSolution* m_solveLSM();
+ void m_fixVariables(const int *);
+ void m_precomputeGain();
+ void m_report();
+ void m_getGradient(LSMSolution*,double[]);
+ bool CholeskyDecompositionNxN(double*,int);
+ bool invertMatrixNxN(double*, int);
+ double m_K[MAX_INP_SIZE][MAX_RES_SIZE];
+ double m_W[MAX_INP_SIZE][MAX_INP_SIZE];
+ int m_size;
+
+ double m_A[MAX_RES_SIZE][MAX_INP_SIZE];
+ double m_MG[5][MAX_INP_SIZE];
+ double m_jX[MAX_INP_SIZE];
+ double m_jY[MAX_INP_SIZE];
+ double m_jZ[MAX_INP_SIZE];
+ double m_S[MAX_RES_SIZE][2];
+ mutable MsgStream m_log; //!< msgstream as private member (-> speed)
+ int m_outputLevel; //!< private member to control debug messages
+ double m_totalPath;
+ double m_minDistance;
+ double m_P0;
+ double m_SignificanceCut;
+ int m_lsmSize,m_resSize;
+ std::vector<int> m_resSizes;
+ double m_R[MAX_INP_SIZE];
+
+ int m_Parray[MAX_INP_SIZE];
+ int m_Zarray[MAX_INP_SIZE];
+ int m_sign;
+ double m_Threshold;
+ LSMSolution* m_pLS;
+ Trk::TrkPlanarSurface* m_surfArray[MAX_INP_SIZE];
+ };
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetCombinedTrackFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetCombinedTrackFitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e37a0709f3ad10771cd740ce1cc487493cfda9f
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetCombinedTrackFitter.h
@@ -0,0 +1,36 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDET_COMBINED_TRACKFITTER_H__
+#define __TRIGINDET_COMBINED_TRACKFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetToolInterfaces/ITrigDkfTrackMakerTool.h"
+#include "TrigInDetTrackFitter/ITrigL2TrackFittingTool.h"
+
+class TrigInDetCombinedTrackFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter
+{
+ public:
+ TrigInDetCombinedTrackFitter( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigInDetCombinedTrackFitter(){};
+ virtual StatusCode initialize();
+ virtual StatusCode finalize();
+ void fit(TrigInDetTrackCollection*);
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis&) {return nullptr;}
+
+private:
+ bool m_doMultScatt,m_doBremm;
+ double m_momentumThreshold,m_DChi2;
+ ToolHandle<ITrigL2TrackFittingTool> m_highPtFitter;
+ ToolHandle<ITrigL2TrackFittingTool> m_lowPtFitter;
+ //ToolHandle<ITrigL2TrackFittingTool> m_robustFitter;
+
+ ToolHandle<ITrigDkfTrackMakerTool> m_trackMaker;
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetKarimakiFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetKarimakiFitter.h
new file mode 100755
index 0000000000000000000000000000000000000000..9892c0a0a3a2a883f35339a0a26f5c018ffae4d0
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetKarimakiFitter.h
@@ -0,0 +1,82 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDETKARIMAKIFITTER_H__
+#define __TRIGINDETKARIMAKIFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+
+#include "InDetIdentifier/SCT_ID.h"
+#include "InDetIdentifier/PixelID.h"
+
+
+class PixelID;
+class SCT_ID;
+
+
+class TrigInDetKarimakiFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter {
+
+ // Ancillary track structure
+ struct AncillaryTrack {
+
+ // Prefit info
+ double xm1;
+ double ym1;
+ double cx1;
+ double cy1;
+ double eta;
+ double phi;
+
+ // Fit RZ info
+ double cotantheta;
+ double ang;
+ double z0;
+
+ // Fit RPhi info
+ double q;
+ double xc;
+ double yc;
+ double rc;
+ double phic;
+ double k;
+ double delta;
+ double pt;
+
+ // Fit chi2 info
+ double chi2;
+ double chi2rz;
+ double chi2rp;
+ int nSP;
+ };
+
+
+ public:
+
+ TrigInDetKarimakiFitter(const std::string&, const std::string&, const IInterface*);
+ virtual ~TrigInDetKarimakiFitter();
+
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+ void fit(TrigInDetTrackCollection*);
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis&) {return nullptr;}
+
+ private:
+
+ void circlefitRPhi(TrigInDetTrack*, TrigInDetKarimakiFitter::AncillaryTrack&);
+ void fitRZ(TrigInDetTrack*, TrigInDetKarimakiFitter::AncillaryTrack&);
+ void fitRPhi(TrigInDetTrack*, TrigInDetKarimakiFitter::AncillaryTrack&);
+
+ private:
+
+ MsgStream* m_log;
+ const PixelID* m_pixelId;
+ const SCT_ID* m_sctId;
+
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetOfflineTrackFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetOfflineTrackFitter.h
new file mode 100755
index 0000000000000000000000000000000000000000..bd2322ec33a549ce4ffe254d29331da5b929621d
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetOfflineTrackFitter.h
@@ -0,0 +1,42 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDET_OFFLINETRACKFITTER_H__
+#define __TRIGINDET_OFFLINETRACKFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+#include "TrkFitterInterfaces/ITrackFitter.h"
+#include "TrkFitterUtils/FitterTypes.h"
+#include "InDetIdentifier/SCT_ID.h"
+#include "InDetIdentifier/PixelID.h"
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+
+class TrigTimer;
+
+
+class TrigInDetOfflineTrackFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter
+{
+ public:
+ TrigInDetOfflineTrackFitter( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigInDetOfflineTrackFitter(){};
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+ void fit(TrigInDetTrackCollection*);
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis&) {return nullptr;}
+private:
+ const PixelID* m_pixelId;
+ const SCT_ID* m_sctId;
+ double m_DChi2;
+ bool m_doMultScatt,m_doBremm,m_offlineClusters;
+ ToolHandle<Trk::ITrackFitter> m_trackFitter;
+
+#define TRIGOFFLINETRACKFIT_NTIMERS 10
+ TrigTimer* m_timer[TRIGOFFLINETRACKFIT_NTIMERS];
+ bool m_timers;
+
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetPerigeeFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetPerigeeFitter.h
new file mode 100755
index 0000000000000000000000000000000000000000..7826e4d4bfbe09d3a810bd56c7bc96b6eb06199b
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetPerigeeFitter.h
@@ -0,0 +1,52 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDETPERIGEEFITTER_H__
+#define __TRIGINDETPERIGEEFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+
+class TrigTimer;
+namespace MagField {
+ class IMagFieldSvc;
+}
+
+class TrigInDetPerigeeFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter
+{
+ public:
+ TrigInDetPerigeeFitter( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigInDetPerigeeFitter();
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+ void fit(TrigInDetTrackCollection*);
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis&) {return nullptr;}
+private:
+
+ void m_clear();
+
+ double m_DChi2;
+ double m_consb;
+ bool m_straightLineMode;
+ bool m_doMultScatt;
+ bool m_doBremmCorr;
+ std::string m_bfieldToolName;
+
+ ServiceHandle<MagField::IMagFieldSvc> m_MagFieldSvc;
+
+#define PERIGEEFIT_NTIMERS 8
+ TrigTimer* m_timer[PERIGEEFIT_NTIMERS];
+ bool m_timers;
+
+};
+
+
+
+
+
+
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetSctKFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetSctKFitter.h
new file mode 100755
index 0000000000000000000000000000000000000000..3a64f8a6e2f177a9db851f1eef11cace561de6fa
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetSctKFitter.h
@@ -0,0 +1,108 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+//
+// filename: TrigInDetSctKFitter.h
+//
+// authors: original SctKFitting code
+// Patrice LEBRUN <lebrun@in2p3.fr>
+// Sijin QIAN <sijin.qian@cern.ch>
+// Thorsten HUEHN <huehn@scri.fsu.edu>
+//
+// idScan version
+// Nikos Konstantinidis
+// nk@hep.ucl.ac.uk
+//
+// migration to Athena
+// Malte Muller
+// mm@hep.ucl.ac.uk
+//
+// AlgTool version and migration from IdScan
+// Dmitry Emeliyanov
+// D.Emeliyanov@rl.ac.uk
+//
+// Description: simple Kalman fitting engine
+//
+// date: 19/11/2002
+//
+// -------------------------------
+// ATLAS Collaboration
+////////////////////////////////////////////////////////////////////////////////
+
+
+#ifndef __TRIGINDETSCTKFITTER_H__
+#define __TRIGINDETSCTKFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+
+class TrigInDetSctKFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter
+{
+ public:
+ void fit(TrigInDetTrackCollection* recoTracks );
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis&) {return nullptr;}
+ TrigInDetSctKFitter( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigInDetSctKFitter();
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+
+ private:
+
+ bool m_doBremmCorr;
+ bool m_doMultScatt;
+ double m_DChi2;
+
+ /////////////////////////////////
+ // mostly FORTRAN migrated code
+ /////////////////////////////////
+
+ // parameters
+ long paramset;
+ double minTrackRadius[4];
+ double maxDChi2[4];
+
+ void cyl_prg_convertor( double *pari, double *covi, double *r__,
+ double *xb, double *yb,
+ double *par, double *cov, long *ierr );
+
+ void matrix_inverter( double *wgt, double *cov, long *ierr );
+ void tkffit( long *np, double *rcoord, double *phcoord, double *zcoord,
+ double *errxy, double *errz, double *errr, long *ihtype, double *rf,
+ double *paraf, double *em1, double *chi2, long *ihdrop, long *ierr,
+ bool electron );
+ void tfit3p(double *rco, double *fco, double *zco, double *sigxy,
+ double *sigz, double *sigr, double *r3, double *para,
+ double *wgc, double *chi2c, long *ierr);
+ void tvtoxx(double *pari, double *wctr, double *par, double *wf);
+ void txprcw(double *wgi, double *der, double *wgf);
+ void txprpw(double *wgi, double *der, double *wgf);
+ void txxdcy(double *p2, double *p1, double *der);
+ void txxdpl(double *p2, double *p1, double *der);
+ void tswtch(long *iz, double *p1, double *wm1);
+
+ void txtrpa(double *parami, long *idir, double *radf,
+ double *zmin, double *zmax, double *sinbmx,
+ long *iopt, double *paramf, double *der,
+ double *alrphi, long *ierr);
+
+ void txxpla(double *parami, long *idir, double *zf, double *rmin,
+ double *rmax, long *iopt, double *paramf,
+ double *der, double *alrphi, long *ierr);
+
+ void tmscat(double *sinth, double *pinv, double *xrl, double *wg,
+ double *dir);
+
+ void add_E_loss(double *x0, double *dir, double *par6, double *wg);
+
+ void trf2xy(long *idir, double *r, double *phi, double *wm) const;
+
+ void taddpt(double *df, double *dz, double *wff, double *wzz,
+ double *dp, double *wotr, double *achi2, long *ierr);
+
+ void tsolve(double *w, double *vy, double *vz, double *d, long *ier);
+};
+
+#endif
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetTrackFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetTrackFitter.h
new file mode 100755
index 0000000000000000000000000000000000000000..0051067bf9d52def43803a119b4d42f61f1f8838
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigInDetTrackFitter.h
@@ -0,0 +1,75 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGINDETTRACKFITTER_H__
+#define __TRIGINDETTRACKFITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+
+#include "MagFieldInterfaces/IMagFieldSvc.h"
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetToolInterfaces/ITrigDkfTrackMakerTool.h"
+
+class TrigTimer;
+
+namespace Trk {
+ class TrkBaseNode;
+ class TrkTrackState;
+ class TrkPlanarSurface;
+}
+
+namespace MagField {
+ class IMagFieldSvc;
+}
+
+struct FitStatStruct {
+ FitStatStruct(int id) : m_algorithmId(id) {
+ m_nTracksTotal = 0;
+ for(int i=0;i<5;i++) m_fitErrors[i]=0;
+ }
+ int m_algorithmId;
+ long int m_nTracksTotal;
+ long int m_fitErrors[5];
+};
+
+class TrigInDetTrackFitter: public AthAlgTool, virtual public ITrigInDetTrackFitter
+{
+ public:
+ TrigInDetTrackFitter( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigInDetTrackFitter(){};
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+ void fitTrack(TrigInDetTrack&);
+ void fit(TrigInDetTrackCollection*);
+ Trk::Track* fitTrack(const Trk::Track&, const Trk::ParticleHypothesis& matEffects = Trk::pion);
+ TrackCollection* fit(const TrackCollection&, const Trk::ParticleHypothesis& matEffects = Trk::pion);
+private:
+
+ Trk::TrkTrackState* m_extrapolate(Trk::TrkTrackState*,
+ Trk::TrkPlanarSurface*,
+ Trk::TrkPlanarSurface*);
+ void m_matrixInversion5x5(double a[5][5]);
+ void m_getMagneticField(double[3],double*);
+
+ void correctScale(Trk::TrkTrackState*);
+
+ double m_DChi2;
+ bool m_doMultScatt;
+ bool m_doBremm;
+ bool m_offlineClusters;
+ ServiceHandle<MagField::IMagFieldSvc> m_MagFieldSvc;
+ ToolHandle<ITrigDkfTrackMakerTool> m_trackMaker;
+
+ std::vector<FitStatStruct> m_fitStats;
+ int m_algorithmId;
+
+#define TRIGTRACKFIT_NTIMERS 10
+ TrigTimer* m_timer[TRIGTRACKFIT_NTIMERS];
+ bool m_timers;
+
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2FastExtrapolationTool.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2FastExtrapolationTool.h
new file mode 100644
index 0000000000000000000000000000000000000000..01d82d686e6ac3773d3097faebb60dd5e72c87be
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2FastExtrapolationTool.h
@@ -0,0 +1,38 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIG_L2_FAST_EXTRAPOLATION_TOOL_H__
+#define __TRIG_L2_FAST_EXTRAPOLATION_TOOL_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+
+#include "TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h"
+
+namespace Trk {
+ class TrkTrackState;
+ class TrkPlanarSurface;
+}
+namespace MagField {
+ class IMagFieldSvc;
+}
+
+class TrigL2FastExtrapolationTool: public AthAlgTool, virtual public ITrigL2FastExtrapolationTool
+{
+ public:
+ TrigL2FastExtrapolationTool( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigL2FastExtrapolationTool(){};
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+ Trk::TrkTrackState* extrapolate(Trk::TrkTrackState*,
+ Trk::TrkPlanarSurface*,
+ Trk::TrkPlanarSurface*, bool smooth=false);
+private:
+ void m_matrixInversion5x5(double a[5][5]);
+ void m_getMagneticField(double[3],double*);
+ ServiceHandle<MagField::IMagFieldSvc> m_MagFieldSvc;
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2HighPtTrackFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2HighPtTrackFitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ed6b5525c1fbcdba3690b26bc1b17398705de6a
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2HighPtTrackFitter.h
@@ -0,0 +1,45 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIG_L2_HIGH_PT_TRACK_FITTER_H__
+#define __TRIG_L2_HIGH_PT_TRACK_FITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/MsgStream.h"
+#include "GaudiKernel/ToolHandle.h"
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h"
+#include "TrigInDetTrackFitter/ITrigL2TrackFittingTool.h"
+#include "TrkToolInterfaces/IRIO_OnTrackCreator.h"
+#include <vector>
+
+namespace Trk {
+ class IRIO_OnTrackCreator;
+}
+
+class TrigL2HighPtTrackFitter : virtual public ITrigL2TrackFittingTool, public AthAlgTool {
+ public:
+
+ // standard AlgTool methods
+ TrigL2HighPtTrackFitter(const std::string&,const std::string&,const IInterface*);
+ virtual ~TrigL2HighPtTrackFitter(){};
+
+ // standard Athena methods
+ StatusCode initialize();
+ StatusCode finalize();
+ Trk::TrkTrackState* fit(Trk::TrkTrackState*, std::vector<Trk::TrkBaseNode*>&, bool runSmoother=true);
+
+ private:
+
+ void m_recalibrateFilteringNode(Trk::TrkBaseNode*, Trk::TrkTrackState*);
+
+ bool m_recalibrate;
+
+ ToolHandle<ITrigL2FastExtrapolationTool> m_fastExtrapolator;
+ ToolHandle<Trk::IRIO_OnTrackCreator> m_ROTcreator;
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2LowPtTrackFitter.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2LowPtTrackFitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..c00d4edd97c7a3eb4380d355a900cd594e6052f5
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2LowPtTrackFitter.h
@@ -0,0 +1,54 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIG_L2_LOW_PT_TRACK_FITTER_H__
+#define __TRIG_L2_LOW_PT_TRACK_FITTER_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/MsgStream.h"
+#include "GaudiKernel/ToolHandle.h"
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h"
+#include "TrigInDetTrackFitter/ITrigL2TrackFittingTool.h"
+#include "TrkToolInterfaces/IRIO_OnTrackCreator.h"
+#include "TrkExInterfaces/IExtrapolator.h"
+#include <vector>
+
+namespace Trk {
+ class TrkPlanarSurface;
+ class IRIO_OnTrackCreator;
+ class IExtrapolator;
+}
+
+
+class TrigL2LowPtTrackFitter : virtual public ITrigL2TrackFittingTool, public AthAlgTool {
+ public:
+
+ // standard AlgTool methods
+ TrigL2LowPtTrackFitter(const std::string&,const std::string&,const IInterface*);
+ virtual ~TrigL2LowPtTrackFitter(){};
+
+ // standard Athena methods
+ StatusCode initialize();
+ StatusCode finalize();
+ Trk::TrkTrackState* fit(Trk::TrkTrackState*, std::vector<Trk::TrkBaseNode*>&, bool runSmoother=true);
+
+ private:
+
+ Trk::TrkTrackState* m_extrapolateOffline(Trk::TrkTrackState*,
+ Trk::TrkPlanarSurface*,
+ Trk::TrkPlanarSurface*,int);
+
+ void m_recalibrateFilteringNode(Trk::TrkBaseNode*, Trk::TrkTrackState*);
+
+ bool m_recalibrate;
+
+ ToolHandle<ITrigL2FastExtrapolationTool> m_fastExtrapolator;
+ ToolHandle<Trk::IRIO_OnTrackCreator> m_ROTcreator;
+ ToolHandle<Trk::IExtrapolator> m_extrapolator;
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2ResidualCalculator.h b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2ResidualCalculator.h
new file mode 100644
index 0000000000000000000000000000000000000000..64954ba1498b8ddf7e305286b80cea957bba2326
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/TrigInDetTrackFitter/TrigL2ResidualCalculator.h
@@ -0,0 +1,61 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#ifndef __TRIGL2RESIDUALCALCULATOR_H__
+#define __TRIGL2RESIDUALCALCULATOR_H__
+
+#include "AthenaBaseComps/AthAlgTool.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "GaudiKernel/ServiceHandle.h"
+
+#include "TrigInDetToolInterfaces/ITrigL2ResidualCalculator.h"
+#include "TrigInDetToolInterfaces/ITrigDkfTrackMakerTool.h"
+
+class AtlasDetectorID;
+class PixelID;
+class SCT_ID;
+class TrigL2HitResidual;
+
+namespace Trk {
+ class TrkBaseNode;
+ class TrkTrackState;
+ class TrkPlanarSurface;
+}
+
+namespace MagField {
+ class IMagFieldSvc;
+}
+
+class TrigL2ResidualCalculator: public AthAlgTool, virtual public ITrigL2ResidualCalculator
+{
+ public:
+
+ TrigL2ResidualCalculator( const std::string&, const std::string&, const IInterface* );
+ virtual ~TrigL2ResidualCalculator(){};
+ virtual StatusCode initialize();
+ virtual StatusCode finalize ();
+
+ StatusCode getResiduals(const TrigInDetTrack*, std::vector<TrigL2HitResidual>&);
+ StatusCode getUnbiassedResiduals(const TrigInDetTrack*, std::vector<TrigL2HitResidual>&);
+
+private:
+
+ Trk::TrkTrackState* m_extrapolate(Trk::TrkTrackState*,
+ Trk::TrkPlanarSurface*,
+ Trk::TrkPlanarSurface*);
+ void m_matrixInversion5x5(double a[5][5]);
+ void m_getMagneticField(double[3],double*);
+
+ double m_DChi2;
+ bool m_doMultScatt;
+ bool m_doBremm;
+ bool m_offlineClusters;
+ ServiceHandle<MagField::IMagFieldSvc> m_MagFieldSvc;
+ ToolHandle<ITrigDkfTrackMakerTool> m_trackMaker;
+ const PixelID* m_pixelId;
+ const SCT_ID* m_sctId;
+ const AtlasDetectorID* m_idHelper;
+};
+
+#endif
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/cmt/requirements b/Trigger/TrigTools/TrigInDetTrackFitter/cmt/requirements
new file mode 100755
index 0000000000000000000000000000000000000000..769e8a679587c3b300d1148ea187dc22aa9471a3
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/cmt/requirements
@@ -0,0 +1,44 @@
+package TrigInDetTrackFitter
+
+author Dmitry Emeliyanov <D.Emeliyanov@rl.ac.uk>
+
+public
+
+# General
+use TrkTrack TrkTrack-* Tracking/TrkEvent
+use AtlasPolicy AtlasPolicy-*
+use GaudiInterface GaudiInterface-* External
+use InDetIdentifier InDetIdentifier-* InnerDetector/InDetDetDescr
+use InDetReadoutGeometry InDetReadoutGeometry-* InnerDetector/InDetDetDescr
+use TrigInDetEvent TrigInDetEvent-* Trigger/TrigEvent
+use TrkDistributedKalmanFilter TrkDistributedKalmanFilter-* Tracking/TrkFitter
+use TrigInDetToolInterfaces TrigInDetToolInterfaces-* Trigger/TrigTools
+use TrkFitterUtils TrkFitterUtils-* Tracking/TrkFitter
+use TrkFitterInterfaces TrkFitterInterfaces-* Tracking/TrkFitter
+use TrkToolInterfaces TrkToolInterfaces-* Tracking/TrkTools
+use TrkExInterfaces TrkExInterfaces-* Tracking/TrkExtrapolation
+use MagFieldInterfaces MagFieldInterfaces-* MagneticField
+use AthenaBaseComps AthenaBaseComps-* Control
+
+library TrigInDetTrackFitter "*.cxx components/*.cxx"
+
+apply_pattern component_library
+apply_pattern declare_joboptions files="*.py"
+apply_pattern declare_python_modules files="*.py"
+#apply_pattern declare_non_standard_include name=doc
+
+private
+
+use StoreGate StoreGate-* Control
+use AtlasDetDescr AtlasDetDescr-* DetectorDescription
+
+use InDetPrepRawData InDetPrepRawData-* InnerDetector/InDetRecEvent
+use InDetRIO_OnTrack InDetRIO_OnTrack-* InnerDetector/InDetRecEvent
+use TrigTimeAlgs TrigTimeAlgs-* Trigger/TrigTools
+
+use TrkSurfaces TrkSurfaces-* Tracking/TrkDetDescr
+use TrkParameters TrkParameters-* Tracking/TrkEvent
+use TrkPrepRawData TrkPrepRawData-* Tracking/TrkEvent
+use TrkTrack TrkTrack-* Tracking/TrkEvent
+use TrkEventPrimitives TrkEventPrimitives-* Tracking/TrkEvent
+use TrkRIO_OnTrack TrkRIO_OnTrack-* Tracking/TrkEvent
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/doc/mainpage.h b/Trigger/TrigTools/TrigInDetTrackFitter/doc/mainpage.h
new file mode 100755
index 0000000000000000000000000000000000000000..5d57eb7fb57178cce10d857e70e84b6e50f7b7c6
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/doc/mainpage.h
@@ -0,0 +1,38 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+/**
+
+@mainpage TrigInDetTrackFitter Package
+
+This package contains implementations of various track fitting tools for the LVL ID
+tracking. All these tools are based on the same abstract interface: ITrigInDetTrackFitter.
+
+@author Dmitry.Emeliyanov@cern.ch
+
+@section TrigInDetTrackFitIntro Introduction
+
+The list of track fitting tools is below:
+
+ - TrigInDetTrackFitter - a default track fitter for TrigIDSCAN
+ - TrigInDetPerigeeFitter - a fast fitter which estimates only track perigee parameters and
+uses TrigSiSpacePoints directly, i.e. without dissolving them into clusters.
+ - TrigInDetKarimakiFitter - a track fitter for TrigSiTrack
+ - TrigInDetSctKFitter - an old fitter based on Sijin QIAN's code
+
+@section TrigInDetTrackFitOverview Overview
+
+The fitting tools make use of the following abstract interface
+
+ - ITrigInDetTrackFitter
+
+This interface contains method
+
+void fit(TrigInDetTrackCollection* recoTracks )
+
+where recoTracks is a collection (vector) of TrigInDetTrack tracks. A track fitter updates parameters
+of the input tracks so that no new tracks are created.
+
+*/
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/python/TrigInDetTrackFitter_Config.py b/Trigger/TrigTools/TrigInDetTrackFitter/python/TrigInDetTrackFitter_Config.py
new file mode 100644
index 0000000000000000000000000000000000000000..dde499bd11adbcb1b306248fe83869c25585775e
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/python/TrigInDetTrackFitter_Config.py
@@ -0,0 +1,133 @@
+# Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+
+from TrigInDetTrackFitter.TrigInDetTrackFitterConf import TrigInDetCombinedTrackFitter
+from TrigInDetTrackFitter.TrigInDetTrackFitterConf import TrigL2HighPtTrackFitter
+from TrigInDetTrackFitter.TrigInDetTrackFitterConf import TrigL2LowPtTrackFitter
+from InDetTrigRecExample.InDetTrigFlags import InDetTrigFlags
+
+
+from TrkExTools.TrkExToolsConf import Trk__Extrapolator
+
+class ConfiguredTrigL2_Extrapolator(Trk__Extrapolator) :
+ __slots__ = []
+ def __init__(self, name = 'ConfiguredTrigL2_Extrapolator') :
+ Trk__Extrapolator.__init__(self, name)
+ from AthenaCommon.AppMgr import ToolSvc
+ from TrkDetDescrSvc.AtlasTrackingGeometrySvc import AtlasTrackingGeometrySvc
+
+ from IOVDbSvc.CondDB import conddb
+ if not (conddb.folderRequested( "/Indet/TrkErrorScaling" ) or conddb.folderRequested( "/Indet/Onl/TrkErrorScaling" )):
+ conddb.addFolderSplitOnline("INDET", "/Indet/Onl/TrkErrorScaling", "/Indet/TrkErrorScaling" )
+
+ from TrkExSTEP_Propagator.TrkExSTEP_PropagatorConf import Trk__STEP_Propagator
+ TrigL2_StepPropagator = Trk__STEP_Propagator(name = 'TrigL2_StepPropagator')
+ ToolSvc += TrigL2_StepPropagator
+ from TrkExRungeKuttaPropagator.TrkExRungeKuttaPropagatorConf import Trk__RungeKuttaPropagator
+ TrigL2_RKPropagator = Trk__RungeKuttaPropagator(name = 'TrigL2_RKPropagator')
+ ToolSvc += TrigL2_RKPropagator
+ from InDetTrigRecExample.InDetTrigFlags import InDetTrigFlags
+ if InDetTrigFlags.propagatorType() is "STEP":
+ TrigL2_Propagator = TrigL2_StepPropagator
+ else:
+ TrigL2_Propagator = TrigL2_RKPropagator
+
+ from AthenaCommon.AppMgr import ServiceMgr as svcMgr
+ AtlasTrackingGeometrySvc = svcMgr.AtlasTrackingGeometrySvc
+ from TrkExTools.TrkExToolsConf import Trk__Navigator
+ TrigL2_Navigator = Trk__Navigator(name = 'TrigL2_Navigator',TrackingGeometrySvc = AtlasTrackingGeometrySvc)
+ ToolSvc += TrigL2_Navigator
+ from TrkExTools.TrkExToolsConf import Trk__MaterialEffectsUpdator
+ TrigL2_MaterialUpdator = Trk__MaterialEffectsUpdator(name = "TrigL2_MaterialEffectsUpdator")
+ ToolSvc += TrigL2_MaterialUpdator
+ TrigL2_SubPropagators = []
+ TrigL2_SubUpdators = []
+ TrigL2_SubPropagators += [ TrigL2_Propagator.name() ]
+ TrigL2_SubUpdators += [ TrigL2_MaterialUpdator.name() ]
+ TrigL2_SubPropagators += [ TrigL2_Propagator.name() ]
+ TrigL2_SubUpdators += [ TrigL2_MaterialUpdator.name() ]
+ TrigL2_SubPropagators += [ TrigL2_StepPropagator.name() ]
+ TrigL2_SubUpdators += [ TrigL2_MaterialUpdator.name() ]
+ self.Propagators = [ TrigL2_RKPropagator, TrigL2_StepPropagator]
+ self.MaterialEffectsUpdators = [ TrigL2_MaterialUpdator ]
+ self.Navigator = TrigL2_Navigator
+ self.SubPropagators = TrigL2_SubPropagators
+ self.SubMEUpdators = TrigL2_SubUpdators
+ #self.DoCaloDynamic = False #Obsolete
+
+
+
+
+from TrkRIO_OnTrackCreator.TrkRIO_OnTrackCreatorConf import Trk__RIO_OnTrackCreator
+
+class ConfiguredTrigL2_InDetRotCreator(Trk__RIO_OnTrackCreator) :
+ __slots__ = []
+ def __init__(self, name = 'ConfiguredTrigL2_InDetRotCreator') :
+ Trk__RIO_OnTrackCreator.__init__(self,name)
+ from SiClusterOnTrackTool.SiClusterOnTrackToolConf import InDet__SCT_ClusterOnTrackTool
+ from SiClusterOnTrackTool.SiClusterOnTrackToolConf import InDet__PixelClusterOnTrackTool
+ from InDetTrigRecExample.InDetTrigConditionsAccess import PixelConditionsSetup
+ from InDetTrigRecExample.InDetTrigFlags import InDetTrigFlags
+
+ if InDetTrigFlags.doCommissioning() :
+ myL2_SCT_ClusterOnTrackTool = InDet__SCT_ClusterOnTrackTool("TrigL2_SCT_ClusterOnTrackTool",
+ CorrectionStrategy = 0,
+ ErrorStrategy = 0)
+ myL2_PixelClusterOnTrackTool = InDet__PixelClusterOnTrackTool("TrigL2_PixelClusterOnTrackTool",
+ PixelOfflineCalibSvc=PixelConditionsSetup.instanceName('PixelOfflineCalibSvc'),
+ ErrorStrategy = 0)
+ else:
+ myL2_SCT_ClusterOnTrackTool = InDet__SCT_ClusterOnTrackTool("TrigL2_SCT_ClusterOnTrackTool",CorrectionStrategy = 0,ErrorStrategy = 2)
+ myL2_PixelClusterOnTrackTool = InDet__PixelClusterOnTrackTool("TrigL2_PixelClusterOnTrackTool",PixelOfflineCalibSvc=PixelConditionsSetup.instanceName('PixelOfflineCalibSvc'),ErrorStrategy = 1)
+
+ from AthenaCommon.AppMgr import ToolSvc
+ ToolSvc += myL2_PixelClusterOnTrackTool
+ ToolSvc += myL2_SCT_ClusterOnTrackTool
+ self.Mode='indet'
+ self.ToolPixelCluster = myL2_PixelClusterOnTrackTool
+ self.ToolSCT_Cluster = myL2_SCT_ClusterOnTrackTool
+
+
+
+
+class ConfiguredTrigL2LowPtTrackFitter(TrigL2LowPtTrackFitter) :
+ __slots__ = []
+ def __init__(self, name = 'ConfiguredTrigL2LowPtTrackFitter') :
+ TrigL2LowPtTrackFitter.__init__(self,name)
+ from AthenaCommon.AppMgr import ToolSvc
+ offlineExtrapolator = ConfiguredTrigL2_Extrapolator()
+ ToolSvc += offlineExtrapolator
+ offlineRotCreator = ConfiguredTrigL2_InDetRotCreator()
+ ToolSvc += offlineRotCreator
+ self.useROTs=False
+ self.ROTcreator=offlineRotCreator
+ self.TrackExtrapolatorTool=offlineExtrapolator
+
+
+
+
+class ConfiguredTrigL2_TrackFitter(TrigInDetCombinedTrackFitter) :
+ __slots__ = []
+ def __init__(self, name = "ConfiguredTrigL2_TrackFitter") :
+ TrigInDetCombinedTrackFitter.__init__(self, name)
+ from AthenaCommon.AppMgr import ToolSvc
+
+ offlineRotCreator = ConfiguredTrigL2_InDetRotCreator()
+ ToolSvc += offlineRotCreator
+
+ trigL2HighPtTrackFitter = TrigL2HighPtTrackFitter(name='TrigL2HighPtTrackFitter',
+ useROTs=False,
+ ROTcreator=offlineRotCreator)
+ trigL2LowPtTrackFitter = ConfiguredTrigL2LowPtTrackFitter()
+
+ ToolSvc += trigL2HighPtTrackFitter
+ ToolSvc += trigL2LowPtTrackFitter
+
+ self.HighPtTrackFitter = trigL2HighPtTrackFitter
+ self.LowPtTrackFitter = trigL2LowPtTrackFitter
+
+
+
+
+
+
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/share/jobOfragment_TrigInDetTrackFitter.py b/Trigger/TrigTools/TrigInDetTrackFitter/share/jobOfragment_TrigInDetTrackFitter.py
new file mode 100755
index 0000000000000000000000000000000000000000..fd799421b07ee0b7504312c9ef15fe985bcdba8b
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/share/jobOfragment_TrigInDetTrackFitter.py
@@ -0,0 +1,16 @@
+theApp.Dlls += ['TrkDistributedKalmanFilter']
+theApp.Dlls += [ "TrigInDetTrackFitter" ]
+ToolSvc = Service( "ToolSvc" )
+TrigInDetFitter = Algorithm( 'ToolSvc.TrigInDetTrackFitter' )
+TrigInDetFitter.UseAthenaFieldService=TRUE
+TrigInDetFitter.doMultScattering=TRUE
+TrigInDetFitter.Chi2Cut=100.0
+TrigInDetFitter.OfflineClusters=TriggerFlags.useOfflineSpacePoints
+TrigInDetPerigeeFitter = Algorithm( 'ToolSvc.TrigInDetPerigeeFitter' )
+TrigInDetPerigeeFitter.UseAthenaFieldService=TRUE
+TrigInDetPerigeeFitter.doMultScattering=TRUE
+TrigInDetPerigeeFitter.doBremmCorrection=FALSE
+TrigInDetPerigeeFitter.StraightLineMode=FALSE
+SctKFitter = Algorithm( 'ToolSvc.TrigInDetSctKFitter' )
+SctKFitter.doMultScattering=TRUE
+SctKFitter.doBremmCorrection=FALSE
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/PerigeeFilteringNodes.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/PerigeeFilteringNodes.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..7524c5e62a84ef5675af8f7fce2fe25b41d59d2e
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/PerigeeFilteringNodes.cxx
@@ -0,0 +1,323 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetTrackFitter/PerigeeFilteringNodes.h"
+
+BarrelPerigeeFilteringNode::BarrelPerigeeFilteringNode(const TrigSiSpacePoint* psp)
+{
+ m_Ri=(*psp).r();
+ m_yPhi=(*psp).phi();
+ m_yZ=(*psp).z();
+ m_sigmaPhi=(*psp).dphi();
+ m_sigmaZ=(*psp).dz();
+ m_setType(0);
+ m_pTrigSp=psp;
+ m_acceptIt();
+}
+
+DiscPerigeeFilteringNode::DiscPerigeeFilteringNode(const TrigSiSpacePoint* psp)
+{
+ m_Zi=(*psp).z();
+ m_yPhi=(*psp).phi();
+ m_yR=(*psp).r();
+ m_sigmaPhi=(*psp).dphi();
+ m_sigmaR=(*psp).dr();
+ m_setType(1);
+ m_pTrigSp=psp;
+ m_acceptIt();
+}
+
+BasePerigeeFilteringNode::BasePerigeeFilteringNode()
+{
+ m_nStatus=1; m_pTrigSp=NULL;
+}
+
+void BasePerigeeFilteringNode::m_rejectIt()
+{
+ m_nStatus=0;
+}
+
+void BasePerigeeFilteringNode::m_acceptIt()
+{
+ m_nStatus=1;
+}
+
+bool BasePerigeeFilteringNode::m_isAccepted()
+{
+ return (m_nStatus==1);
+}
+
+void BasePerigeeFilteringNode::m_runFilter(double* Rk, double* Gk)
+{
+ double Kk[5][2];
+ int i,j,m,ind;
+
+ for(i=0;i<5;i++)
+ {
+ for(j=0;j<2;j++)
+ {
+ Kk[i][j]=0.0;
+ for(m=0;m<2;m++) Kk[i][j]+=m_B[m][i]*m_D[m][j];
+ }
+ }
+ for(i=0;i<5;i++) Rk[i]+=Kk[i][0]*m_resid[0]+Kk[i][1]*m_resid[1];
+ ind=0;
+ for(j=0;j<5;j++)
+ for(i=0;i<=j;i++)
+ {
+ Gk[ind]-=Kk[i][0]*m_B[0][j]+Kk[i][1]*m_B[1][j];
+ ind++;
+ }
+}
+
+void BasePerigeeFilteringNode::m_applyMultScatt(double* Rk,double* Gk)
+{
+ double s2,pt,sint,l1;
+ double F[5][2];
+ double rk;
+
+ pt=-0.3/Rk[0];
+ sint=1+Rk[4]*Rk[4];
+ pt*=(pt*sint);
+ s2=m_EffSigmaMS*m_EffSigmaMS/pt;
+ rk=Rk[0]*Rk[0]*m_Rho*m_Rho;
+ l1=1.0/(1.0-4*rk);
+ F[0][0]=0.0;
+ F[0][1]=-Rk[0]*Rk[4];
+ F[1][0]=m_Rho*(1-0.5*rk);
+ F[1][1]=rk*Rk[4]/Rk[0];
+ F[2][0]=(1-2*rk)*l1;
+ F[2][1]=-F[0][1]*2*m_Rho*(1-0.5*rk)*l1;
+ F[3][0]=-F[0][1]*m_Rho*l1;
+ F[3][1]=m_Rho*sint;
+ F[4][0]=0.0;
+ F[4][1]=-sint;
+ int i;
+ sint=sqrt(sint);
+ for(i=0;i<5;i++) F[i][0]*=sint;
+ Gk[0]+=s2*F[0][1]*F[0][1];
+ Gk[1]+=s2*F[0][1]*F[1][1];
+ Gk[2]+=s2*(F[1][0]*F[1][0]+F[1][1]*F[1][1]);
+ Gk[3]+=s2*F[0][1]*F[2][1];
+ Gk[4]+=s2*(F[1][0]*F[2][0]+F[1][1]*F[2][1]);
+ Gk[5]+=s2*(F[2][0]*F[2][0]+F[2][1]*F[2][1]);
+ Gk[6]+=s2*F[0][1]*F[3][1];
+ Gk[7]+=s2*(F[1][0]*F[3][0]+F[1][1]*F[3][1]);
+ Gk[8]+=s2*(F[2][0]*F[3][0]+F[2][1]*F[3][1]);
+ Gk[9]+=s2*(F[3][0]*F[3][0]+F[3][1]*F[3][1]);
+ Gk[10]+=s2*F[0][1]*F[4][1];
+ Gk[11]+=s2*F[1][1]*F[4][1];
+ Gk[12]+=s2*F[2][1]*F[4][1];
+ Gk[13]+=s2*F[3][1]*F[4][1];
+ Gk[14]+=s2*F[4][1]*F[4][1];
+}
+
+void BasePerigeeFilteringNode::m_applyEnergyLoss(double* Rk, double* Gk, int dir)
+{
+ double alpha,k0,rho2,rho3,rho4,sigma2;
+
+ alpha=m_EffRadLength*(1-0.5*m_EffRadLength);
+ sigma2=Rk[0]*Rk[0]*m_EffRadLength*(0.415-0.744*m_EffRadLength);
+ rho2=m_Rho*m_Rho;
+ rho3=m_Rho*rho2;
+ rho4=m_Rho*rho3;
+ k0=Rk[0]*dir*alpha;
+ Rk[0]+=k0;
+ Rk[1]+=-k0*rho2;
+ Rk[2]+=-k0*2*m_Rho;
+ Gk[0]+=sigma2;
+ Gk[1]+=-rho2*sigma2;
+ Gk[2]+=rho4*sigma2;
+ Gk[3]+=-2*m_Rho*sigma2;
+ Gk[4]+=2*rho3*sigma2;
+ Gk[5]+=4*rho2*sigma2;
+}
+
+
+double BarrelPerigeeFilteringNode::m_getChi2(double* Rk, double* Gk)
+{
+ double chi2=0.0,zres,detr;
+ double H[2][5];
+ double V[2][2];
+
+ double h1,h2,h3,sinz,cosz;
+
+ zres=m_yZ-Rk[3];
+ h3=zres*2*Rk[0]/Rk[4];
+ sinz=sin(h3);cosz=cos(h3);
+ h1=1.0/(1.0-2*Rk[1]*Rk[0]);
+ h2=(Rk[1]-m_Ri)*(Rk[1]+m_Ri);
+ H[0][0]=h1*h1*h2;
+ H[0][1]=h1+2*Rk[0]*Rk[0]*H[0][0];
+ H[0][2]=-m_Ri*cos(m_yPhi-Rk[2]);
+ H[1][0]=0.5*(sinz-h3*cosz)/(Rk[0]*Rk[0]);
+ H[1][2]=m_Ri*sin(m_yPhi-Rk[2]);
+ H[1][3]=cosz/Rk[4];
+ H[1][4]=zres*cosz/(Rk[4]*Rk[4]);
+
+ m_B[0][0]=H[0][0]*Gk[0]+H[0][1]*Gk[1]+H[0][2]*Gk[3];
+ m_B[0][1]=H[0][0]*Gk[1]+H[0][1]*Gk[2]+H[0][2]*Gk[4];
+ m_B[0][2]=H[0][0]*Gk[3]+H[0][1]*Gk[4]+H[0][2]*Gk[5];
+ m_B[0][3]=H[0][0]*Gk[6]+H[0][1]*Gk[7]+H[0][2]*Gk[8];
+ m_B[0][4]=H[0][0]*Gk[10]+H[0][1]*Gk[11]+H[0][2]*Gk[12];
+
+ m_B[1][0]=H[1][0]*Gk[0]+H[1][2]*Gk[3]+H[1][3]*Gk[6]+H[1][4]*Gk[10];
+ m_B[1][1]=H[1][0]*Gk[1]+H[1][2]*Gk[4]+H[1][3]*Gk[7]+H[1][4]*Gk[11];
+ m_B[1][2]=H[1][0]*Gk[3]+H[1][2]*Gk[5]+H[1][3]*Gk[8]+H[1][4]*Gk[12];
+ m_B[1][3]=H[1][0]*Gk[6]+H[1][2]*Gk[8]+H[1][3]*Gk[9]+H[1][4]*Gk[13];
+ m_B[1][4]=H[1][0]*Gk[10]+H[1][2]*Gk[12]+H[1][3]*Gk[13]+H[1][4]*Gk[14];
+
+ V[0][0]=m_sigmaPhi*H[0][2];
+ V[0][0]=V[0][0]*V[0][0];
+ V[0][1]=H[0][2]*H[1][2]*m_sigmaPhi*m_sigmaPhi;V[1][0]=V[0][1];
+ V[1][1]=m_sigmaZ*m_sigmaZ*H[1][3]*H[1][3]+m_sigmaPhi*m_sigmaPhi*
+ H[1][2]*H[1][2];
+
+ V[0][0]+=m_B[0][0]*H[0][0]+m_B[0][1]*H[0][1]+m_B[0][2]*H[0][2];
+ V[1][1]+=m_B[1][0]*H[1][0]+m_B[1][2]*H[1][2]+m_B[1][3]*H[1][3]+
+ m_B[1][4]*H[1][4];
+ V[0][1]+=m_B[0][0]*H[1][0]+m_B[0][2]*H[1][2]+m_B[0][3]*H[1][3]+
+ m_B[0][4]*H[1][4];
+ V[1][0]+=m_B[1][0]*H[0][0]+m_B[1][1]*H[0][1]+m_B[1][2]*H[0][2];
+ detr=V[0][0]*V[1][1]-V[0][1]*V[0][1];
+ detr=1.0/detr;
+ m_D[0][0]=V[1][1]*detr;m_D[1][1]=V[0][0]*detr;m_D[0][1]=-V[0][1]*detr;
+ m_D[1][0]=m_D[0][1];
+ m_resid[0]=-H[1][2]-h1*(Rk[1]-Rk[0]*(Rk[1]*Rk[1]+m_Ri*m_Ri));
+ m_resid[1]=H[0][2]+0.5*sinz/Rk[0];
+ chi2=m_resid[0]*m_resid[0]*m_D[0][0]+2.0*m_resid[0]*m_resid[1]*m_D[0][1]+
+ m_resid[1]*m_resid[1]*m_D[1][1];
+ return chi2;
+}
+
+void BarrelPerigeeFilteringNode::m_applyMultScatt(double* Rk, double* Gk)
+{
+ m_EffRadLength=m_getEffRadLength(Rk);
+ m_EffSigmaMS=0.0136*sqrt(m_EffRadLength)*
+ (1.0+0.038*log(m_EffRadLength));
+ m_Rho=m_Ri;
+ BasePerigeeFilteringNode::m_applyMultScatt(Rk,Gk);
+}
+
+void BarrelPerigeeFilteringNode::m_applyEnergyLoss(double* Rk, double* Gk, int dir)
+{
+ m_EffRadLength=m_getEffRadLength(Rk);
+ m_Rho=m_Ri;
+ BasePerigeeFilteringNode::m_applyEnergyLoss(Rk,Gk,dir);
+}
+
+void BarrelPerigeeFilteringNode::m_runFilter(double* Rk, double* Gk)
+{
+ BasePerigeeFilteringNode::m_runFilter(Rk,Gk);
+}
+
+int BarrelPerigeeFilteringNode::m_getNdof()
+{
+ return 2;
+}
+
+double BarrelPerigeeFilteringNode::m_getEffRadLength(double* Rk)
+{
+ const double x0=0.022;
+
+ double x0eff=x0*sqrt(1+Rk[4]*Rk[4]);
+ return x0eff;
+}
+
+double DiscPerigeeFilteringNode::m_getChi2(double* Rk, double* Gk)
+{
+ double chi2=0.0,zres,detr;
+ double H[2][5];
+ double V[2][2];
+ double h1,h2,h3,sinz,cosz,dhdR1,dhdR2;
+
+ zres=m_Zi-Rk[3];
+ h3=zres*2*Rk[0]/Rk[4];
+ sinz=sin(h3);cosz=cos(h3);
+ h1=1.0/(1.0-2*Rk[1]*Rk[0]);
+ h2=(Rk[1]-m_yR)*(Rk[1]+m_yR);
+ dhdR1=sin(m_yPhi-Rk[2]);
+ dhdR2=cos(m_yPhi-Rk[2]);
+ H[0][0]=h1*h1*h2;
+ H[0][1]=h1+2*Rk[0]*Rk[0]*H[0][0];
+ H[0][2]=-m_yR*dhdR2;
+ H[1][0]=0.5*(sinz-h3*cosz)/(Rk[0]*Rk[0]);
+ H[1][2]=m_yR*dhdR1;
+ H[1][3]=cosz/Rk[4];
+ H[1][4]=zres*cosz/(Rk[4]*Rk[4]);
+ dhdR1=sin(m_yPhi-Rk[2]);
+ dhdR2=cos(m_yPhi-Rk[2]);
+
+ m_B[0][0]=H[0][0]*Gk[0]+H[0][1]*Gk[1]+H[0][2]*Gk[3];
+ m_B[0][1]=H[0][0]*Gk[1]+H[0][1]*Gk[2]+H[0][2]*Gk[4];
+ m_B[0][2]=H[0][0]*Gk[3]+H[0][1]*Gk[4]+H[0][2]*Gk[5];
+ m_B[0][3]=H[0][0]*Gk[6]+H[0][1]*Gk[7]+H[0][2]*Gk[8];
+ m_B[0][4]=H[0][0]*Gk[10]+H[0][1]*Gk[11]+H[0][2]*Gk[12];
+
+ m_B[1][0]=H[1][0]*Gk[0]+H[1][2]*Gk[3]+H[1][3]*Gk[6]+H[1][4]*Gk[10];
+ m_B[1][1]=H[1][0]*Gk[1]+H[1][2]*Gk[4]+H[1][3]*Gk[7]+H[1][4]*Gk[11];
+ m_B[1][2]=H[1][0]*Gk[3]+H[1][2]*Gk[5]+H[1][3]*Gk[8]+H[1][4]*Gk[12];
+ m_B[1][3]=H[1][0]*Gk[6]+H[1][2]*Gk[8]+H[1][3]*Gk[9]+H[1][4]*Gk[13];
+ m_B[1][4]=H[1][0]*Gk[10]+H[1][2]*Gk[12]+H[1][3]*Gk[13]+H[1][4]*Gk[14];
+
+ V[0][0]=m_sigmaPhi*m_sigmaPhi*H[0][2]*H[0][2]+
+ m_sigmaR*m_sigmaR*dhdR1*dhdR1;
+ V[0][1]=H[0][2]*H[1][2]*m_sigmaPhi*m_sigmaPhi+
+ dhdR1*dhdR2*m_sigmaR*m_sigmaR;
+ V[1][0]=V[0][1];
+ V[1][1]=m_sigmaR*m_sigmaR*dhdR2*dhdR2+m_sigmaPhi*m_sigmaPhi*
+ H[1][2]*H[1][2];
+
+ V[0][0]+=m_B[0][0]*H[0][0]+m_B[0][1]*H[0][1]+m_B[0][2]*H[0][2];
+ V[1][1]+=m_B[1][0]*H[1][0]+m_B[1][2]*H[1][2]+m_B[1][3]*H[1][3]+
+ m_B[1][4]*H[1][4];
+ V[0][1]+=m_B[0][0]*H[1][0]+m_B[0][2]*H[1][2]+m_B[0][3]*H[1][3]+
+ m_B[0][4]*H[1][4];
+ V[1][0]+=m_B[1][0]*H[0][0]+m_B[1][1]*H[0][1]+m_B[1][2]*H[0][2];
+ detr=V[0][0]*V[1][1]-V[0][1]*V[0][1];
+ detr=1.0/detr;
+ m_D[0][0]=V[1][1]*detr;m_D[1][1]=V[0][0]*detr;m_D[0][1]=-V[0][1]*detr;
+ m_D[1][0]=m_D[0][1];
+ m_resid[0]=-H[1][2]-h1*(Rk[1]-Rk[0]*(Rk[1]*Rk[1]+m_yR*m_yR));
+ m_resid[1]=H[0][2]+0.5*sinz/Rk[0];
+ chi2=m_resid[0]*m_resid[0]*m_D[0][0]+2.0*m_resid[0]*m_resid[1]*m_D[0][1]+
+ m_resid[1]*m_resid[1]*m_D[1][1];
+
+ return chi2;
+}
+
+void DiscPerigeeFilteringNode::m_applyMultScatt(double* Rk, double* Gk)
+{
+ m_EffRadLength=m_getEffRadLength(Rk);
+ m_EffSigmaMS=0.0136*sqrt(m_EffRadLength)*
+ (1.0+0.038*log(m_EffRadLength));
+ m_Rho=m_yR;
+ BasePerigeeFilteringNode::m_applyMultScatt(Rk,Gk);
+}
+
+void DiscPerigeeFilteringNode::m_applyEnergyLoss(double* Rk, double* Gk, int dir)
+{
+ m_EffRadLength=m_getEffRadLength(Rk);
+ m_Rho=m_yR;
+ BasePerigeeFilteringNode::m_applyEnergyLoss(Rk,Gk,dir);
+}
+
+void DiscPerigeeFilteringNode::m_runFilter(double* Rk, double* Gk)
+{
+ BasePerigeeFilteringNode::m_runFilter(Rk,Gk);
+}
+
+int DiscPerigeeFilteringNode::m_getNdof()
+{
+ return 2;
+}
+
+double DiscPerigeeFilteringNode::m_getEffRadLength(double* Rk)
+{
+ const double x0=0.022;
+
+ double x0eff=x0/(fabs(Rk[4])*sqrt(1+Rk[4]*Rk[4]));
+ return x0eff;
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigDkfTrackMakerTool.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigDkfTrackMakerTool.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..f54d680f9684be1be37879aa2f6507b7141759cf
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigDkfTrackMakerTool.cxx
@@ -0,0 +1,355 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigDkfTrackMakerTool tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 17.03.2009 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+
+#include "TrigInDetEvent/TrigSiSpacePoint.h"
+
+#include "AtlasDetDescr/AtlasDetectorID.h"
+#include "InDetPrepRawData/SCT_Cluster.h"
+#include "InDetPrepRawData/PixelCluster.h"
+#include "InDetRIO_OnTrack/SCT_ClusterOnTrack.h"
+#include "InDetRIO_OnTrack/PixelClusterOnTrack.h"
+
+#include "TrigInDetToolInterfaces/ITrigDkfTrackMakerTool.h"
+#include "TrigInDetTrackFitter/TrigDkfTrackMakerTool.h"
+#include "TrkSurfaces/Surface.h"
+#include "TrkSurfaces/TrapezoidBounds.h"
+
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+
+
+TrigDkfTrackMakerTool::TrigDkfTrackMakerTool(const std::string& t,
+ const std::string& n,
+ const IInterface* p ): AthAlgTool(t,n,p)
+{
+ declareInterface< ITrigDkfTrackMakerTool >( this );
+ m_idHelper=NULL;
+}
+
+StatusCode TrigDkfTrackMakerTool::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+ MsgStream athenaLog(msgSvc(), name());
+
+ athenaLog << MSG::INFO <<"In initialize..."<<endreq;
+
+ StoreGateSvc* detStore;
+ sc = service("DetectorStore", detStore);
+ if ( sc.isFailure() ) {
+ athenaLog << MSG::FATAL << "DetStore service not found" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ if (detStore->retrieve(m_idHelper, "AtlasID").isFailure()) {
+ athenaLog << MSG::FATAL << "Could not get AtlasDetectorID helper AtlasID" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ // Get SCT & pixel Identifier helpers
+
+ if (detStore->retrieve(m_pixelId, "PixelID").isFailure()) {
+ athenaLog << MSG::FATAL << "Could not get Pixel ID helper" << endreq;
+ return StatusCode::FAILURE;
+ }
+ if (detStore->retrieve(m_sctId, "SCT_ID").isFailure()) {
+ athenaLog << MSG::FATAL << "Could not get SCT ID helper" << endreq;
+ return StatusCode::FAILURE;
+ }
+ sc = detStore->retrieve(m_pixelManager);
+ if( sc.isFailure() )
+ {
+ athenaLog << MSG::ERROR << "Could not retrieve Pixel DetectorManager from detStore."<<endreq;
+ return sc;
+ }
+ sc = detStore->retrieve(m_SCT_Manager);
+ if( sc.isFailure() )
+ {
+ athenaLog << MSG::ERROR << "Could not retrieve SCT DetectorManager from detStore." << endreq;
+ return sc;
+ }
+
+ athenaLog << MSG::INFO << "TrigDkfTrackMakerTool constructed "<< endreq;
+ return sc;
+}
+
+StatusCode TrigDkfTrackMakerTool::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+TrigDkfTrackMakerTool::~TrigDkfTrackMakerTool()
+{
+
+}
+
+bool TrigDkfTrackMakerTool::createDkfTrack(std::vector<const TrigSiSpacePoint*>& siSpacePoints,
+ std::vector<Trk::TrkBaseNode*>& vpTrkNodes,
+ double DChi2)
+{
+ const double radLength=0.022;
+
+ std::vector<const TrigSiSpacePoint*>::iterator pSPIt,lastSPIt;
+
+ double C[3],N[3],M[3][3];int i;
+ Amg::Vector3D mx,my,mz;
+
+ MsgStream athenaLog(msgSvc(), name());
+ int outputLevel = msgSvc()->outputLevel( name() );
+
+ vpTrkNodes.clear();
+
+ if(siSpacePoints.size()==0)
+ {
+ if (outputLevel <= MSG::WARNING)
+ athenaLog << MSG::ERROR << "Cannot create a DKF track -- TrigInDetTrack has no hits"
+ << endreq;
+ return false;
+ }
+ pSPIt=siSpacePoints.begin();lastSPIt=siSpacePoints.end();
+ for(; pSPIt != lastSPIt; pSPIt++)
+ {
+ const TrigSiSpacePoint* pSP=(*pSPIt);
+
+ // std::cout<<"SP layer="<<pSP->layer()<<" r="<<pSP->r()<<" phi="<<pSP->phi()<<" z="<<pSP->z()<<std::endl;
+
+ Identifier ID=pSP->identify();
+ if(m_idHelper->is_sct(ID))
+ {
+ const InDet::SiCluster *pCL[2];
+ pCL[0] = pSP->clusters().first;
+ pCL[1] = pSP->clusters().second;
+ if((pCL[0]==NULL)||(pCL[1]==NULL)) continue;
+
+ IdentifierHash idHash[2];
+ InDetDD::SiDetectorElement* pEL[2];
+ double RadVec[2];
+ int index[2];
+ for(i=0;i<2;i++)
+ {
+ idHash[i]=m_sctId->wafer_hash(m_sctId->wafer_id(pCL[i]->identify()));
+ pEL[i]=m_SCT_Manager->getDetectorElement(idHash[i]);
+ const Trk::Surface& rSurf=pEL[i]->surface();
+
+ // const Trk::Surface& rSurf=pCL[i]->detectorElement()->surface();
+ RadVec[i]=rSurf.center().mag();
+ index[i]=i;
+ }
+ if(RadVec[0]>RadVec[1])
+ {
+ index[0]=1;index[1]=0;
+ }
+ for (int iClusInSP=0; iClusInSP<2; iClusInSP++)
+ {
+ const Trk::Surface& rSurf=pEL[index[iClusInSP]]->surface();
+ //const Trk::Surface& rSurf=pCL[i]->detectorElement()->surface();
+ N[0]=rSurf.normal().x();
+ N[1]=rSurf.normal().y();
+ N[2]=rSurf.normal().z();
+ C[0]=rSurf.center().x();
+ C[1]=rSurf.center().y();
+ C[2]=rSurf.center().z();
+
+ mx=rSurf.transform().rotation().block(0,0,3,1);
+ my=rSurf.transform().rotation().block(0,1,3,1);
+ mz=rSurf.transform().rotation().block(0,2,3,1);
+ for(i=0;i<3;i++)
+ {
+ M[i][0]=mx[i];M[i][1]=my[i];M[i][2]=mz[i];
+ }
+ Trk::TrkPlanarSurface* pS = new Trk::TrkPlanarSurface(C,N,M,radLength,
+ &(pEL[index[iClusInSP]]->surface()));
+ //std::cout<<"created SCT surface"<<std::endl;pS->m_report();
+ /*
+ double locCov;
+ try {
+ const Trk::ErrorMatrix& errMatRef=pCL[index[iClusInSP]]->localErrorMatrix();
+ locCov=errMatRef.covariance()[0][0];
+ }
+ catch(Trk::PrepRawDataUndefinedVariable) {
+ locCov=pEL[index[iClusInSP]]->phiPitch();
+ locCov=locCov*locCov/12.0;
+ }
+ // override
+
+ locCov=pEL[index[iClusInSP]]->phiPitch();
+ locCov=locCov*locCov/12.0;
+ */
+
+ if(pEL[index[iClusInSP]]->design().shape()!=InDetDD::Trapezoid)
+ {
+ //vpTrkNodes.push_back(new Trk::TrkClusterNode(pS,DChi2,pCL[index[iClusInSP]]->localPosition()[0],locCov));
+ vpTrkNodes.push_back(new Trk::TrkClusterNode(pS,DChi2,pCL[index[iClusInSP]]));
+ }
+ else
+ {
+ const Trk::SurfaceBounds& rBounds=rSurf.bounds();
+ const Trk::TrapezoidBounds& ecBounds=
+ dynamic_cast<const Trk::TrapezoidBounds&>(rBounds);
+ double R=(ecBounds.maxHalflengthX()+ecBounds.minHalflengthX())*
+ ecBounds.halflengthY()/
+ (ecBounds.maxHalflengthX()-ecBounds.minHalflengthX());
+ vpTrkNodes.push_back(new Trk::TrkEndCapClusterNode(pS,DChi2,pCL[index[iClusInSP]],R));
+
+ // vpTrkNodes.push_back(new Trk::TrkEndCapClusterNode(pS,DChi2,R,pCL[index[iClusInSP]]->localPosition()[0],locCov));
+ }
+ }
+ }
+ else if(m_idHelper->is_pixel(ID))
+ {
+ const InDet::SiCluster* pCL=pSP->clusters().first;
+
+ if(pCL)
+ {
+
+ const IdentifierHash idHash=
+ m_pixelId->wafer_hash(m_pixelId->wafer_id(pCL->identify()));
+ InDetDD::SiDetectorElement* pEL=m_pixelManager->getDetectorElement(idHash);
+ const Trk::Surface& rSurf=pEL->surface();
+
+ //const Trk::Surface& rSurf=pCL->detectorElement()->surface();
+
+ N[0]=rSurf.normal().x();
+ N[1]=rSurf.normal().y();
+ N[2]=rSurf.normal().z();
+ C[0]=rSurf.center().x();
+ C[1]=rSurf.center().y();
+ C[2]=rSurf.center().z();
+ mx=rSurf.transform().rotation().block(0,0,3,1);
+ my=rSurf.transform().rotation().block(0,1,3,1);
+ mz=rSurf.transform().rotation().block(0,2,3,1);
+ for(i=0;i<3;i++)
+ {
+ M[i][0]=mx[i];M[i][1]=my[i];M[i][2]=mz[i];
+ }
+ Trk::TrkPlanarSurface* pS = new Trk::TrkPlanarSurface(C,N,M,radLength,&(pEL->surface()));
+ /*
+ std::cout<<"created PIX surface"<<std::endl;
+ std::cout<<"local position: "<<pCL->localPosition()[0]<<" "<<pCL->localPosition()[1]<<std::endl;
+ pS->m_report();
+ */
+ // double locPos[2];
+ /*
+ double locCov[4];
+ try {
+ const Trk::ErrorMatrix& errMatRef=pCL->localErrorMatrix();
+ locCov[0]=errMatRef.covariance()[0][0];
+ locCov[1]=errMatRef.covariance()[0][1];
+ locCov[2]=errMatRef.covariance()[1][0];
+ locCov[3]=errMatRef.covariance()[1][1];
+ }
+ catch(Trk::PrepRawDataUndefinedVariable) {
+ //locCov[0]=pEL->phiPitch()*pEL->phiPitch()/12.0;
+ locCov[1]=0.0;locCov[2]=0.0;
+ //locCov[3]=pEL->etaPitch()*pEL->etaPitch()/12.0;
+ locCov[3]=0.3*0.3;locCov[0]=0.012*0.012;
+ }
+ */
+ // override
+ /*
+ locCov[0]=pEL->phiPitch()*pEL->phiPitch()/12.0;
+ locCov[1]=0.0;locCov[2]=0.0;
+ locCov[3]=pEL->etaPitch()*pEL->etaPitch()/12.0;
+ locPos[0]=pCL->localPosition()[0];
+ locPos[1]=pCL->localPosition()[1];
+ */
+ //vpTrkNodes.push_back(new Trk::TrkPixelNode(pS,DChi2,locPos,locCov));
+ vpTrkNodes.push_back(new Trk::TrkPixelNode(pS,DChi2,pCL));
+ }
+ }
+ }
+ if (outputLevel <= MSG::DEBUG)
+ athenaLog << MSG::DEBUG << vpTrkNodes.size()<<" filtering nodes created"<<endreq;
+
+ return true;
+}
+
+bool TrigDkfTrackMakerTool::createDkfTrack(const Trk::Track& track,
+ std::vector<Trk::TrkBaseNode*>& vpTrkNodes,
+ double DChi2) {
+ vpTrkNodes.clear();
+
+ if(track.measurementsOnTrack()->size()==0)
+ {
+ ATH_MSG_ERROR("Cannot create a DKF track -- Trk::Track has no hits");
+ return false;
+ }
+ for (auto tMOT = track.measurementsOnTrack()->begin(); tMOT != track.measurementsOnTrack()->end(); ++tMOT) {
+ const Trk::Surface& rSurf=(*tMOT)->associatedSurface();
+ constexpr double radLength=0.022;
+ double C[3],N[3],M[3][3];
+ Amg::Vector3D mx,my,mz;
+ N[0]=rSurf.normal().x();
+ N[1]=rSurf.normal().y();
+ N[2]=rSurf.normal().z();
+ C[0]=rSurf.center().x();
+ C[1]=rSurf.center().y();
+ C[2]=rSurf.center().z();
+ mx=rSurf.transform().rotation().block(0,0,3,1);
+ my=rSurf.transform().rotation().block(0,1,3,1);
+ mz=rSurf.transform().rotation().block(0,2,3,1);
+ for(int i=0;i<3;i++) {
+ M[i][0]=mx[i];M[i][1]=my[i];M[i][2]=mz[i];
+ }
+ Trk::TrkPlanarSurface* pS = new Trk::TrkPlanarSurface(C,N,M,radLength, &(rSurf));
+
+
+
+ const InDet::SiClusterOnTrack* siCLOT = dynamic_cast<const InDet::SiClusterOnTrack*>(*tMOT);
+ if (siCLOT==nullptr) {
+ ATH_MSG_VERBOSE("siCLOT is null");
+ continue;
+ }
+ const InDet::SiCluster* siCL = dynamic_cast<const InDet::SiCluster*>(siCLOT->prepRawData());
+ if (siCL==nullptr) {
+ ATH_MSG_VERBOSE("siCL is null");
+ continue;
+ }
+ Identifier id = (*tMOT)->associatedSurface().associatedDetectorElement()->identify();
+ ATH_MSG_DEBUG("Identifier: " << m_idHelper->print_to_string(id));
+ if(m_idHelper->is_sct(id)) {
+ const InDetDD::SiDetectorElement* sctElement = dynamic_cast<const InDetDD::SiDetectorElement*>
+ ((*tMOT)->associatedSurface().associatedDetectorElement());
+ if(sctElement->design().shape()==InDetDD::Trapezoid) //SCT Endcap
+ {
+ ATH_MSG_DEBUG("SCT endcap node");
+ const Trk::SurfaceBounds& rBounds=rSurf.bounds();
+ const Trk::TrapezoidBounds& ecBounds= dynamic_cast<const Trk::TrapezoidBounds&>(rBounds);
+ double R=(ecBounds.maxHalflengthX()+ecBounds.minHalflengthX())*
+ ecBounds.halflengthY()/(ecBounds.maxHalflengthX()-ecBounds.minHalflengthX());
+ vpTrkNodes.push_back(new Trk::TrkEndCapClusterNode(pS,DChi2,siCL,R));
+ }
+ else {//SCT Barrel
+ ATH_MSG_DEBUG("SCT barrel node");
+ vpTrkNodes.push_back(new Trk::TrkClusterNode(pS,DChi2,siCL));
+ }
+ }
+ else if (m_idHelper->is_pixel(id)) {//Pixel
+ ATH_MSG_DEBUG("Pixel node");
+ vpTrkNodes.push_back(new Trk::TrkPixelNode(pS,DChi2,siCL));
+ }
+ }
+ ATH_MSG_DEBUG(vpTrkNodes.size()<<" filtering nodes created");
+
+ return true;
+}
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetBremDetectionTool.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetBremDetectionTool.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..5b21604a997bb2458023bda84a3e98d163ec68df
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetBremDetectionTool.cxx
@@ -0,0 +1,898 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+//////////////////////////////////////////////////////////////////
+// TrigInDetBremDetectionTool.cxx
+// Source file for TrigInDetBremDetectionTool
+///////////////////////////////////////////////////////////////////
+// (c) ATLAS Detector software
+///////////////////////////////////////////////////////////////////
+// Dmitry.Emeliyanov@cern.ch
+///////////////////////////////////////////////////////////////////
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+#include "TrigInDetTrackFitter/TrigInDetBremDetectionTool.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+#include <cmath>
+#include <cstring>
+using std::memset;
+
+//#define IDE_DEBUG 1
+
+TrigInDetBremDetectionTool::TrigInDetBremDetectionTool(const std::string& t,const std::string& n,const IInterface* p) :
+ AthAlgTool (t,n,p),
+ m_log(msgSvc(), n),
+ m_outputLevel(1),
+ m_minDistance(10.0),
+ m_P0(0.6),
+ m_SignificanceCut(4.5)
+{
+ declareProperty("minDistance",m_minDistance);
+ declareProperty("noBremProbability",m_P0);
+ declareProperty("MinSignificance",m_SignificanceCut);
+ declareInterface<ITrigInDetBremDetectionTool>( this );
+ m_pLS=NULL;
+}
+
+// destructor
+TrigInDetBremDetectionTool::~TrigInDetBremDetectionTool()
+{
+
+}
+// initialize
+StatusCode TrigInDetBremDetectionTool::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ m_log.setLevel(outputLevel()); // individual outputlevel not known before initialise
+ m_outputLevel=msgSvc()->outputLevel( name() );
+ m_Threshold=2.0*log(m_P0/(1.0-m_P0));
+
+ m_log << MSG::INFO << "initialize() successful in " << name() << endreq;
+ m_log << MSG::INFO << "Input detection threshold is set to " << m_Threshold << endreq;
+
+ return sc;
+}
+
+// finalize
+StatusCode TrigInDetBremDetectionTool::finalize()
+{
+ if(m_pLS!=NULL) delete m_pLS;
+
+ m_log << MSG::INFO << "finalize() successful in " << name() << endreq;
+ return StatusCode::SUCCESS;
+}
+
+void TrigInDetBremDetectionTool::reset()
+{
+ m_totalPath=0.0;
+ m_lsmSize=0;m_resSizes.clear();m_resSize=0;
+ memset(&m_A[0][0],0,sizeof(m_A));
+ memset(&m_MG[0][0],0,sizeof(m_MG));
+ memset(&m_S[0][0],0,sizeof(m_S));
+ if(m_pLS!=NULL) delete m_pLS;m_pLS=NULL;
+}
+
+bool TrigInDetBremDetectionTool::addNewPoint(Trk::TrkTrackState* pTS,
+ Trk::TrkBaseNode* pN, Trk::TrkPlanarSurface* pS, double F[5][5],
+ double delta)
+{
+ int i,j,dim,k;
+ double res[5];
+ double K[5][2];
+ double H[2][5];
+ double S[2][2];
+ double mu[5][5];
+ double HF[2][5];
+ double M[5];
+ double lP[3],gP[3];
+
+
+ m_totalPath+=fabs(delta);
+
+ dim=pN->m_getKalmanGain(K);
+ dim=pN->m_getMeasurementMatrix(H);
+ dim=pN->m_getInverseResidualVariance(S);
+ dim=pN->m_getResiduals(res);
+
+#ifdef IDE_DEBUG
+
+ m_log<<MSG::INFO<<"new measurement, size="<<dim<< endreq;
+ m_log<< MSG::INFO << " Accumulated path = "<< m_totalPath << endreq;
+
+ printf("Jacobian:\n");
+ for(i=0;i<5;i++)
+ {
+ for(j=0;j<5;j++)
+ printf("%E ",F[i][j]);
+ printf("\n");
+ }
+
+#endif
+
+ if(dim!=0)
+ {
+#ifdef IDE_DEBUG
+ printf("Measurement matrix:\n");
+ for(i=0;i<dim;i++)
+ {
+ for(j=0;j<5;j++)
+ printf("%E ",H[i][j]);
+ printf("\n");
+ }
+#endif
+ if(m_lsmSize!=0)
+ {
+ m_resSizes.push_back(dim);
+ for(i=0;i<dim;i++)
+ {
+ m_R[m_resSize]=res[i];
+ for(j=0;j<dim;j++) m_S[m_resSize][j]=S[i][j];
+ m_resSize++;
+ }
+ }
+ for(i=0;i<dim;i++)
+ for(j=0;j<5;j++)
+ {
+ HF[i][j]=0.0;
+ for(k=0;k<5;k++) HF[i][j]+=H[i][k]*F[k][j];
+ }
+ for(i=0;i<5;i++)
+ for(j=0;j<5;j++)
+ {
+ mu[i][j]=F[i][j];
+ for(k=0;k<dim;k++) mu[i][j]-=K[i][k]*HF[k][j];
+ }
+ }
+ else
+ {
+ for(i=0;i<5;i++)
+ for(j=0;j<5;j++)
+ {
+ mu[i][j]=F[i][j];
+ }
+ }
+ for(i=1;i<=m_lsmSize;i++)
+ {
+ for(k=0;k<5;k++)
+ {
+ M[k]=0.0;
+ for(j=0;j<5;j++) M[k]+=mu[k][j]*m_MG[j][i];
+ }
+ for(k=0;k<5;k++) m_MG[k][i]=M[k];
+ }
+ for(i=0;i<dim;i++)
+ {
+ for(k=1;k<=m_lsmSize;k++)
+ {
+ m_A[i+m_resSize-dim][k-1]=0.0;
+ for(j=0;j<5;j++) m_A[i+m_resSize-dim][k-1]+=HF[i][j]*m_MG[j][k];
+ }
+ }
+#ifdef IDE_DEBUG
+ printf("Matrix A:\n");
+ for(i=0;i<m_resSize;i++)
+ {
+ for(j=0;j<m_lsmSize;j++) printf("%E ",m_A[i][j]);
+ printf("\n");
+ }
+#endif
+ if(m_totalPath<m_minDistance) return false;
+
+ m_totalPath=0.0;
+
+ m_lsmSize++;
+
+ for(i=0;i<4;i++)
+ {
+ m_MG[i][m_lsmSize]=0.0;
+ }
+ m_MG[4][m_lsmSize]=1.0;
+
+ m_surfArray[m_lsmSize-1]=pS;
+
+ lP[0]=pTS->m_getTrackState(0);lP[1]=pTS->m_getTrackState(1);
+ lP[2]=0.0;
+ pS->m_transformPointToGlobal(lP,gP);
+ m_jX[m_lsmSize-1]=gP[0];m_jY[m_lsmSize-1]=gP[1];m_jZ[m_lsmSize-1]=gP[2];
+#ifdef IDE_DEBUG
+ m_log << MSG::INFO << " Added new jump at "<<gP[0]<<" "<<gP[1]<<" "<<gP[2]<<endreq;
+#endif
+ return true;
+}
+
+bool TrigInDetBremDetectionTool::solve(int Sign)
+{
+ int i;
+ bool isSolved=false;
+ bool isFeasible=false;
+ int size=m_lsmSize-1;
+ int nIter;
+ double Chi2Opt;
+ double g[MAX_INP_SIZE];
+
+ m_sign=Sign;
+ m_size=m_lsmSize-1;
+ m_precomputeGain();
+ if(m_pLS!=NULL) delete m_pLS;m_pLS=NULL;
+
+ m_pLS=new LSMSolution(size);
+ for(i=0;i<m_size;i++)
+ {
+ m_pLS->m_fixVariable(i);
+ }
+ for(i=0;i<MAX_INP_SIZE;i++)
+ {
+ m_Zarray[i]=1;m_Parray[i]=0;
+ }
+ nIter=0;
+ while(!isSolved)
+ {
+ m_getGradient(m_pLS,g);
+#ifdef IDE_DEBUG
+ printf("++++ Gradient: ++++ Sign %d\n",Sign);
+ for(i=0;i<size;i++)
+ {
+ printf("%E ",g[i]);
+ }
+ printf("\n");
+#endif
+ Chi2Opt=m_getCriterionValue(m_pLS);
+#ifdef IDE_DEBUG
+ printf("***** Criterion = %f ******\n",Chi2Opt);
+#endif
+ if(nIter>100) break;
+ if(m_isZempty())
+ {
+ isSolved=true;break;
+ }
+ if(m_goodGradient(g))
+ {
+#ifdef IDE_DEBUG
+ printf("Good gradient - solved\n");
+#endif
+ isSolved=true;break;
+ }
+#ifdef IDE_DEBUG
+ else printf("Not good gradient - searching ...\n");
+#endif
+ int bestI=m_findBestDirection(g);
+#ifdef IDE_DEBUG
+ printf("Selected %d grad = %f\n",bestI,g[bestI]);
+#endif
+ m_Zarray[bestI]=0;m_Parray[bestI]=1;
+#ifdef IDE_DEBUG
+ for(i=0;i<m_size;i++) printf("%d ",m_Zarray[i]);
+ printf("\n");
+ for(i=0;i<m_size;i++) printf("%d ",m_Parray[i]);
+ printf("\n");
+#endif
+ isFeasible=false;
+
+ while(!isFeasible)
+ {
+ LSMSolution* pST=m_solveLSM();
+#ifdef IDE_DEBUG
+ pST->m_report();
+#endif
+ isFeasible=m_checkFeasibility(pST);
+ if(!isFeasible)
+ {
+#ifdef IDE_DEBUG
+ printf("Non-feasible solution");
+#endif
+ m_mixSolutions(pST,m_pLS);
+ delete pST;
+ }
+ else
+ {
+#ifdef IDE_DEBUG
+ printf("Feasible solution\n");
+#endif
+ delete m_pLS;
+ m_pLS=pST;
+ }
+ }
+ nIter++;
+ }
+ int nChanges=0;
+ bool nonZero=false;
+
+ for(i=0;i<m_size;i++)
+ {
+ m_Zarray[i]=1;
+ if(!m_pLS->m_isOnConstraint(i))
+ {
+ m_Zarray[i]=0;
+ double utmp=(*m_pLS)[i];
+ (*m_pLS)[i]=0.0;
+ double Chi2Zero=m_getCriterionValue(m_pLS);
+ if(m_outputLevel<=MSG::DEBUG)
+ {
+ m_log<<MSG::DEBUG<<i+1<<" Chi2* = "<<Chi2Opt<<" Chi20 = "<<Chi2Zero<<" Th="<<m_Threshold<<endreq;
+ }
+ (*m_pLS)[i]=utmp;
+ double deltaChi2=Chi2Zero-Chi2Opt;
+ if(deltaChi2<m_Threshold)
+ {
+ m_Zarray[i]=1;
+ nChanges++;
+ }
+ else nonZero=true;
+
+ }
+ }
+ if((nChanges!=0)&&nonZero)
+ {
+ LSMSolution* pUS=m_solveLSM();
+ if(m_outputLevel<=MSG::DEBUG)
+ {
+ m_log<<MSG::DEBUG<<"LSM Solution"<<endreq;
+ pUS->m_report();
+ Chi2Opt=m_getCriterionValue(pUS);
+ m_log<<MSG::DEBUG<<"Criterion = "<<Chi2Opt<<endreq;
+ }
+ delete m_pLS;
+ m_pLS=pUS;
+ }
+ if(!nonZero)
+ {
+ delete m_pLS;
+ m_pLS=new LSMSolution(size);
+ for(i=0;i<m_size;i++)
+ {
+ m_pLS->m_fixVariable(i);
+ }
+ if(m_outputLevel<=MSG::DEBUG)
+ {
+ m_log<<MSG::DEBUG<<"LSM Solution"<<endreq;
+ m_pLS->m_report();
+ Chi2Opt=m_getCriterionValue(m_pLS);
+ m_log<<MSG::DEBUG<<"Criterion = "<<Chi2Opt<<endreq;
+ }
+ }
+ return isSolved;
+}
+
+void TrigInDetBremDetectionTool::m_precomputeGain()
+{
+ int i,j,k;
+ memset(&m_K[0][0],0,sizeof(m_K));
+
+ int sJ,J0;
+
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ J0=0;
+ for(std::vector<int>::iterator iIt=m_resSizes.begin();iIt!=m_resSizes.end();++iIt)
+ {
+ for(sJ=0;sJ<(*iIt);sJ++)
+ {
+ j=J0+sJ;
+ m_K[i][j]=0.0;
+ for(k=J0;k<J0+(*iIt);k++)
+ {
+ m_K[i][j]+=m_A[k][i]*m_S[k][sJ];
+ }
+ j++;
+ }
+ J0+=(*iIt);
+ }
+ }
+
+ for(i=0;i<m_lsmSize-1;i++)
+ for(j=i;j<m_lsmSize-1;j++)
+ {
+ m_W[i][j]=0.0;
+ for(k=0;k<m_resSize;k++) m_W[i][j]+=m_K[i][k]*m_A[k][j];
+ m_W[j][i]=m_W[i][j];
+ }
+}
+
+
+void TrigInDetBremDetectionTool::m_getGradient(LSMSolution* pLS, double g[])
+{
+ int i,j;
+
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ g[i]=0.0;
+ for(j=i;j<m_resSize;j++) g[i]+=m_K[i][j]*m_R[j];
+ }
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ for(j=0;j<m_lsmSize-1;j++) g[i]-=m_W[i][j]*(*pLS)[j];
+ }
+}
+
+bool TrigInDetBremDetectionTool::m_goodGradient(double* g)
+{
+ int i,np=0;
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ if(m_Zarray[i]==0) continue;
+ if(g[i]*m_sign>0.0) np++;
+ }
+ return (np==0);
+}
+
+int TrigInDetBremDetectionTool::m_findBestDirection(double* g)
+{
+ int i,bestI=0;
+ double maxGrad=-1.0;
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ if(g[i]*m_sign>maxGrad)
+ {
+ maxGrad=g[i]*m_sign;
+ bestI=i;
+ }
+ }
+ return bestI;
+}
+
+bool TrigInDetBremDetectionTool::m_isZempty()
+{
+ int i;
+
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ if(m_Zarray[i]==1) return false;
+ }
+ return true;
+}
+
+bool TrigInDetBremDetectionTool::CholeskyDecompositionNxN(double* a,int Size)
+{
+ int i,j,k;
+ double sum;
+ double* p=new double[Size];
+
+ for(i=0;i<Size;i++)
+ {
+ for(j=i;j<Size;j++)
+ {
+ sum=a[i+j*Size];
+ for(k=i-1;k>=0;k--)
+ sum-=a[i+k*Size]*a[j+k*Size];
+ if(i==j)
+ {
+ if(sum<=0.0)
+ {
+ printf("Cholesky decomp. failed\n");delete[] p;
+ return false;
+ }
+ p[i]=sqrt(sum);
+ }
+ else a[j+i*Size]=sum/p[i];
+ }
+ }
+ for(i=0;i<Size;i++)
+ {
+ a[i+Size*i]=1.0/p[i];
+ for(j=i+1;j<Size;j++)
+ {
+ sum=0.0;for(k=i;k<j;k++) sum-=a[j+Size*k]*a[k+Size*i];
+ a[j+Size*i]=sum/p[j];
+ }
+ }
+ delete[] p;
+ return true;
+}
+
+
+bool TrigInDetBremDetectionTool::invertMatrixNxN(double* a, int size)
+{
+ double detr;
+ int i,j;
+ bool rc=false;
+
+ if(size==1)
+ {
+ a[0]=1.0/a[0];
+ rc=true;
+ }
+ else if(size==2)
+ {
+ detr=a[0]*a[3]-a[1]*a[2];
+ detr=1.0/detr;
+ double b[2][2];
+ b[0][0]=a[3]*detr;
+ b[1][0]=b[0][1]=-a[1]*detr;
+ b[1][1]=a[0]*detr;
+ for(i=0;i<2;i++) for(j=0;j<2;j++) a[i+j*size]=b[i][j];
+ rc=true;
+ }
+ else if(size==3)
+ {
+ double A[3][3],b[3][3],M[3];
+ for(i=0;i<3;i++) for(j=0;j<3;j++) A[i][j]=a[i+j*size];
+
+ M[0]=A[1][1]*A[2][2]-A[1][2]*A[1][2];
+ M[1]=A[0][1]*A[2][2]-A[1][2]*A[0][2];
+ M[2]=A[0][1]*A[1][2]-A[1][1]*A[0][2];
+ detr=A[0][0]*M[0]-A[0][1]*M[1]+A[0][2]*M[2];
+ detr=1.0/detr;
+ b[0][0]=M[0]*detr;
+ b[0][1]=b[1][0]=-M[1]*detr;
+ b[0][2]=b[2][0]=M[2]*detr;
+ b[1][1]=(A[0][0]*A[2][2]-A[0][2]*A[0][2])*detr;
+ b[1][2]=b[2][1]=-(A[0][0]*A[1][2]-A[0][1]*A[0][2])*detr;
+ b[2][2]=(A[0][0]*A[1][1]-A[0][1]*A[0][1])*detr;
+ for(i=0;i<3;i++) for(j=0;j<3;j++) a[i+j*size]=b[i][j];
+ rc=true;
+ }
+ else if(size>3)
+ {
+ if(!CholeskyDecompositionNxN(a,size)) return false;
+
+ double* L=new double[size*size];
+ double* LT=new double[size*size];
+
+ memset(L,0,sizeof(double)*size*size);
+ memset(LT,0,sizeof(double)*size*size);
+
+ for(i=0;i<size;i++)
+ for(j=0;j<=i;j++)
+ {
+ L[i+j*size]=a[i+j*size];
+ LT[j+i*size]=L[i+size*j];
+ }
+ for(i=0;i<size;i++)
+ {
+ for(j=i;j<size;j++)
+ {
+ double sum=0.0;
+ for(int k=0;k<size;k++) sum+=LT[i+k*size]*L[k+j*size];
+ a[j+i*size]=a[i+j*size]=sum;
+ }
+ }
+ delete[] L;
+ delete[] LT;
+ rc=true;
+ }
+ return rc;
+}
+
+
+LSMSolution* TrigInDetBremDetectionTool::m_solveLSM()
+{
+ LSMSolution* pLS=NULL;
+
+ int i,j,idxI,idxJ,newSize=m_size;
+ double W[MAX_INP_SIZE][MAX_INP_SIZE];
+ double P[MAX_INP_SIZE][MAX_INP_SIZE];
+ double E[MAX_INP_SIZE];
+ double chi2;
+
+ memset(&W[0][0],0,sizeof(W));
+
+ for(i=0;i<m_size;i++) if(m_Zarray[i]==1) newSize--;
+
+ // printf("newSize = %d\n",newSize);
+
+ idxI=-1;idxJ=-1;
+ for(i=0;i<m_size;i++)
+ {
+ if(m_Zarray[i]==1) continue;
+ idxI++;idxJ=idxI-1;
+ for(j=i;j<m_size;j++)
+ {
+ if(m_Zarray[j]==1) continue;
+ idxJ++;
+ W[idxI][idxJ]=0.0;
+ // printf("Computing W: I=%d J=%d\n",idxI,idxJ);
+ W[idxJ][idxI]=W[idxI][idxJ]=m_W[i][j];
+ }
+ }
+ /*
+ printf("Reduced W:\n");
+ for(i=0;i<newSize;i++)
+ {
+ for(j=0;j<newSize;j++) printf("%E ",W[i][j]);
+ printf("\n");
+ }
+ */
+ double* a=new double[newSize*newSize];
+ for(i=0;i<newSize;i++) for(j=0;j<newSize;j++) a[i+j*newSize]=W[i][j];
+ bool rc=this->invertMatrixNxN(a,newSize);
+ if(!rc)
+ {
+ delete[] a;
+ return NULL;
+ }
+ for(i=0;i<newSize;i++) for(j=0;j<newSize;j++) P[i][j]=a[i+j*newSize];
+ /*
+ printf("Inverted W:\n");
+ for(i=0;i<newSize;i++)
+ {
+ for(j=0;j<newSize;j++)
+ printf("%E ",P[i][j]);
+ printf("\n");
+ }
+ */
+ idxI=-1;
+
+ double Q[MAX_INP_SIZE];
+ memset(&Q[0],0,sizeof(Q));
+ for(i=0;i<m_size;i++)
+ {
+ if(m_Zarray[i]==1) continue;
+ idxI++;
+ Q[idxI]=0.0;
+ for(j=0;j<m_resSize;j++) Q[idxI]+=m_K[i][j]*m_R[j];
+ }
+ //printf("Solution:\n");
+ for(i=0;i<newSize;i++)
+ {
+ E[i]=0.0;
+ for(j=0;j<newSize;j++) E[i]+=P[i][j]*Q[j];
+ // printf("%E \n",E[i]);
+ }
+ chi2=0.0;
+ for(i=0;i<newSize;i++)
+ for(j=0;j<newSize;j++) chi2+=E[i]*W[i][j]*E[j];
+ // printf("Significance: %f\n",chi2);
+ pLS = new LSMSolution(m_size);
+ pLS->m_Significance()=chi2;
+
+ idxI=-1;
+ for(i=0;i<m_size;i++)
+ {
+ if(m_Zarray[i]==1)
+ {
+ (*pLS)[i]=0.0;pLS->m_fixVariable(i);
+ continue;
+ }
+ idxI++;
+ (*pLS)[i]=E[idxI];idxJ=-1;
+ for(j=0;j<m_size;j++)
+ {
+ if(m_Zarray[j]==1) continue;
+ idxJ++;
+ pLS->m_Cov(i,j)=P[idxI][idxJ];
+ }
+ }
+ delete[] a;
+ return pLS;
+}
+
+
+double TrigInDetBremDetectionTool::m_getCriterionValue(LSMSolution* pLS)
+{
+ double R[MAX_RES_SIZE];
+ int i,j,k;
+ double crit=0.0;
+
+ for(i=0;i<m_resSize;i++)
+ {
+ R[i]=m_R[i];
+ for(j=0;j<m_size;j++) R[i]-=m_A[i][j]*(*pLS)[j];
+ }
+ i=0;
+ for(std::vector<int>::iterator iIt=m_resSizes.begin();iIt!=m_resSizes.end();++iIt)
+ {
+ for(j=0;j<(*iIt);j++)
+ {
+ for(k=0;k<(*iIt);k++) crit+=R[i+j]*m_S[i+j][k]*R[i+k];
+ }
+ i+=(*iIt);
+ }
+ return crit;
+}
+
+bool TrigInDetBremDetectionTool::m_checkFeasibility(LSMSolution * pS)
+{
+ int i;
+ bool rc=true;
+ for(i=0;i<m_size;i++)
+ {
+ if(m_Parray[i]!=1) continue;
+ if((*pS)[i]*m_sign<0.0)
+ {
+ rc=false;break;
+ }
+ }
+ return rc;
+}
+
+void TrigInDetBremDetectionTool::m_mixSolutions(LSMSolution* pST, LSMSolution* pLS)
+{
+ int i,iMin=-1;
+ double aMin=1000000.0,a;
+
+ for(i=0;i<m_size;i++)
+ {
+ if(m_Parray[i]!=1) continue;
+ if(m_sign*(*pST)[i]>0.0) continue;
+ a=(*pLS)[i]/((*pLS)[i]-(*pST)[i]);
+ // printf("i=%d alpha=%f\n",i,a);
+ if(a<aMin)
+ {
+ aMin=a;iMin=i;
+ }
+ }
+ if(iMin<0) return;
+ // printf("Closest component %d with alpha=%f\n",iMin,aMin);
+ for(i=0;i<m_size;i++)
+ {
+ (*pLS)[i]=(1.0-aMin)*(*pLS)[i]+aMin*(*pST)[i];
+ }
+ m_Zarray[iMin]=1;m_Parray[iMin]=0;
+}
+
+void TrigInDetBremDetectionTool::modifySurfaces(int flag)
+{
+ int i;
+
+ if(flag!=0)
+ {
+ if(m_pLS!=NULL)
+ {
+ if(m_pLS->m_Significance()>m_SignificanceCut)
+ {
+ for(i=0;i<m_size;i++)
+ {
+ if(!m_pLS->m_isOnConstraint(i))
+ {
+ m_surfArray[i]->m_setBreakPoint((*m_pLS)[i]);
+ if(m_outputLevel<=MSG::DEBUG)
+ {
+ m_log<<MSG::DEBUG<<i<<" Breakpoint is set, u="<<(*m_pLS)[i]<<
+ " X="<<m_jX[i]<<" Y="<<m_jY[i]<<" Z="<<m_jZ[i]<<endreq;
+ }
+ }
+ else
+ if(m_outputLevel<=MSG::DEBUG)
+ {
+ m_log<<MSG::DEBUG<<i<<" Not a Breakpoint X="<<m_jX[i]<<" Y="<<m_jY[i]<<
+ " Z="<<m_jZ[i]<<endreq;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ for(i=0;i<m_size;i++)
+ {
+ m_surfArray[i]->m_unsetBreakPoint();
+ }
+ }
+ delete m_pLS;m_pLS=NULL;
+}
+
+
+void TrigInDetBremDetectionTool::report(int flag)
+{
+ int i,j,k;
+
+ if(flag==1) //Residuals
+ {
+ m_log << MSG::DEBUG << "IDE Residuals : "<< endreq;
+ i=0;
+ for(std::vector<int>::iterator iIt=m_resSizes.begin();iIt!=m_resSizes.end();++iIt)
+ {
+ for(j=0;j<(*iIt);j++)
+ {
+ m_log << MSG::DEBUG << " "<<m_R[i]<< endreq;
+ i++;
+ }
+ }
+ }
+ if(flag==2) //LSM
+ {
+ i=0;
+ m_log << MSG::DEBUG << "LSM Problem : nU="<<m_lsmSize-1<<" nR="<<m_resSize<< endreq;
+ for(std::vector<int>::iterator iIt=m_resSizes.begin();iIt!=m_resSizes.end();++iIt)
+ {
+ for(j=0;j<(*iIt);j++)
+ {
+ m_log << MSG::DEBUG <<m_R[i]<<" [";
+ for(k=0;k<(*iIt);k++) m_log << MSG::DEBUG <<m_S[i][k]<<" ";
+ m_log << MSG::DEBUG <<"] ";
+ for(k=0;k<m_lsmSize-1;k++)
+ m_log << MSG::DEBUG <<m_A[i][k]<<" ";
+ m_log << MSG::DEBUG << endreq;
+ i++;
+ }
+ }
+ m_log << MSG::DEBUG << "LSM Gain Matrix "<< endreq;
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ for(j=0;j<m_resSize;j++)
+ m_log << MSG::DEBUG <<m_K[i][j]<<" ";
+ m_log << MSG::DEBUG << endreq;
+ }
+ m_log << MSG::DEBUG << "LSM Weight Matrix "<< endreq;
+ for(i=0;i<m_lsmSize-1;i++)
+ {
+ for(j=0;j<m_lsmSize-1;j++)
+ m_log << MSG::DEBUG <<m_W[i][j]<<" ";
+ m_log << MSG::DEBUG << endreq;
+ }
+
+ m_log << MSG::DEBUG << "-----------------------"<< endreq;
+ }
+}
+
+
+LSMSolution::LSMSolution()
+{
+ m_size=0;
+ int i,j;
+ m_Chi2=0.0;
+ for(i=0;i<=m_size;i++)
+ {
+ m_u[i]=0.0;
+ m_fixedVariables[i]=0;
+ for(j=0;j<=m_size;j++) m_C[i][j]=0.0;
+ }
+}
+
+LSMSolution::~LSMSolution()
+{
+
+}
+
+LSMSolution::LSMSolution(int size) : m_size(size)
+{
+ int i,j;
+ m_Chi2=0.0;
+ for(i=0;i<=m_size;i++)
+ {
+ m_u[i]=0.0;
+ m_fixedVariables[i]=0;
+ for(j=0;j<=m_size;j++) m_C[i][j]=0.0;
+ }
+}
+
+double& LSMSolution::operator[] (int i)
+{
+ if((i<0)||(i>=m_size))
+ return m_u[0];
+ else return m_u[i];
+}
+
+double& LSMSolution::m_Cov(int i, int j)
+{
+ if((i<0)||(i>=m_size)) return m_C[0][0];
+ if((j<0)||(j>=m_size)) return m_C[0][0];
+ return m_C[i][j];
+}
+
+double& LSMSolution::m_Significance()
+{
+ return m_Chi2;
+}
+
+bool LSMSolution::m_isOnConstraint(int i)
+{
+ return (m_fixedVariables[i]==1);
+}
+
+void LSMSolution::m_fixVariable(int i)
+{
+ m_fixedVariables[i]=1;
+}
+
+void LSMSolution::m_report()
+{
+ int i,j;
+
+ printf("LSM Solution size %d\n",m_size);
+ for(i=0;i<m_size;i++)
+ {
+ printf("%E ",m_u[i]);
+ if(m_isOnConstraint(i)) printf("constr. ");
+ else printf("free ");
+ for(j=0;j<m_size;j++) printf("%E ",m_C[i][j]);
+ printf("\n");
+ }
+ printf("Significance %f\n",m_Chi2);
+
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetCombinedTrackFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetCombinedTrackFitter.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..86953b10a9515b6b30a50fb79926c20aed3ff460
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetCombinedTrackFitter.cxx
@@ -0,0 +1,252 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigInDetCombinedTrackFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 15.06.2010 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetEvent/TrigInDetTrackCollection.h"
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetCombinedTrackFitter.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+TrigInDetCombinedTrackFitter::TrigInDetCombinedTrackFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ):
+ AthAlgTool(t,n,p),
+ m_doMultScatt(true),
+ m_doBremm(false),
+ m_momentumThreshold(10000.0),
+ m_highPtFitter("TrigL2HighPtTrackFitter"),
+ m_lowPtFitter("TrigL2LowPtTrackFitter"),
+ m_trackMaker("TrigDkfTrackMakerTool")
+{
+ declareInterface< ITrigInDetTrackFitter >( this );
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremm=false);
+ declareProperty( "HighPtTrackFitter", m_highPtFitter, "TrigL2HighPtTrackFitter");
+ declareProperty( "LowPtTrackFitter", m_lowPtFitter, "TrigL2LowPtTrackFitter");
+ declareProperty( "MomentumThreshold",m_momentumThreshold=10000.0);
+ declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+}
+
+StatusCode TrigInDetCombinedTrackFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ sc=m_trackMaker.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_trackMaker);
+ return sc;
+ }
+ sc=m_highPtFitter.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_highPtFitter);
+ return sc;
+ }
+ sc=m_lowPtFitter.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_lowPtFitter);
+ return sc;
+ }
+ ATH_MSG_INFO("TrigInDetCombinedTrackFitter initialized ");
+ return sc;
+}
+
+StatusCode TrigInDetCombinedTrackFitter::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+
+void TrigInDetCombinedTrackFitter::fit(TrigInDetTrackCollection* recoTracks )
+{
+ double Rk[5], Phi0, D0, Z0, Eta, errD0, errZ0, errEta, errPhi0, errPt;
+ double CV[5][5],a,b,c,Theta;
+ std::vector<double>* pCov;
+ TrigInDetTrackCollection::iterator trIt,lastIt;
+
+ std::vector<Trk::TrkBaseNode*> vpTrkNodes;
+
+ trIt = recoTracks->begin();lastIt = recoTracks->end();
+ for(; trIt !=lastIt; trIt++)
+ {
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>((*trIt)->param());
+ if(param==NULL)
+ {
+ ATH_MSG_WARNING("Fit Failed -- TrigInDetTrack has no parameters");
+ continue;
+ }
+ if(fabs(param->pT())<100.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<param->pT()<<" - skipping fit");
+ continue;
+ }
+
+ // 1. Create filtering nodes
+
+ bool trackResult = m_trackMaker->createDkfTrack(*(*trIt)->siSpacePoints(),vpTrkNodes, m_DChi2);
+ if(!trackResult) continue;
+
+ int nHits=vpTrkNodes.size();
+
+ ATH_MSG_DEBUG(nHits<<" filtering nodes created");
+
+ // 2. Create initial track state:
+
+ double chi2tot=0.0;
+ int ndoftot=-5;
+
+ Rk[0]=param->a0();
+ Rk[1]=param->z0();
+ Rk[2]=param->phi0();
+ Theta=2.0*atan(exp(-param->eta()));
+ Rk[3]=Theta;
+ Rk[4]=sin(Theta)/param->pT();
+ double Momentum = 1/Rk[4];
+
+ Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ double Gk[5][5];
+ memset(&Gk[0][0],0,sizeof(Gk));
+ Gk[0][0]=100.0;Gk[1][1]=1000.0;Gk[2][2]=0.01;Gk[3][3]=0.01;Gk[4][4]=1e-6;
+ pTS->m_setTrackCovariance(Gk);
+ if(m_doMultScatt)
+ pTS->m_setScatteringMode(1);
+ if(m_doBremm)
+ pTS->m_setScatteringMode(2);
+
+ ATH_MSG_DEBUG("Initial params: locT="<<Rk[0]<<" locL="<<Rk[1]<<" phi="<<Rk[2]
+ <<" theta="<<Rk[3]<<" Q="<<Rk[4]<<" pT="<<sin(Rk[3])/Rk[4]);
+
+ // 3. Call appropriate fitter
+
+ if(fabs(Momentum)<m_momentumThreshold)
+ {
+ pTS = m_lowPtFitter->fit(pTS,vpTrkNodes,true);
+ }
+ else
+ {
+ pTS = m_highPtFitter->fit(pTS,vpTrkNodes,true);
+ }
+
+ //pTS = m_lowPtFitter->fit(pTS,vpTrkNodes);
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt(vpTrkNodes.begin()),
+ pnEnd(vpTrkNodes.end());
+ if(pTS!=NULL)
+ {
+
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ ATH_MSG_DEBUG("dChi2="<<(*pnIt)->m_getChi2());
+ if((*pnIt)->m_isValidated())
+ {
+ chi2tot+=(*pnIt)->m_getChi2();
+ ndoftot+=(*pnIt)->m_getNdof();
+ }
+ }
+ double Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ Phi0 = pTS->m_getTrackState(2);
+ if(Phi0>M_PI) Phi0-=2*M_PI;
+ if(Phi0<-M_PI) Phi0+=2*M_PI;
+ Eta = -log(sin(0.5*pTS->m_getTrackState(3))/cos(0.5*pTS->m_getTrackState(3)));
+ Z0 = pTS->m_getTrackState(1);
+ D0 = pTS->m_getTrackState(0);
+
+ errD0 = sqrt(pTS->m_getTrackCovariance(0,0));
+ errZ0 = sqrt(pTS->m_getTrackCovariance(1,1));
+ errPhi0 = sqrt(pTS->m_getTrackCovariance(2,2));
+ errEta = sqrt(pTS->m_getTrackCovariance(3,3))/fabs(sin(pTS->m_getTrackState(3)));
+ b=cos(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ c=-Pt/pTS->m_getTrackState(4);
+ a=-1.0/sin(pTS->m_getTrackState(3));
+ errPt = sqrt(b*b*(pTS->m_getTrackCovariance(3,3))+c*c*(pTS->m_getTrackCovariance(4,4))+
+ 2.0*b*c*(pTS->m_getTrackCovariance(3,4)));
+
+ pCov=new std::vector<double>;
+
+ CV[0][0]=pTS->m_getTrackCovariance(0,0);
+ CV[0][1]=pTS->m_getTrackCovariance(0,2);
+ CV[0][2]=pTS->m_getTrackCovariance(0,1);
+ CV[0][3]=a*(pTS->m_getTrackCovariance(0,3));
+ CV[0][4]=b*(pTS->m_getTrackCovariance(0,3))+c*(pTS->m_getTrackCovariance(0,4));
+ CV[1][1]=pTS->m_getTrackCovariance(2,2);
+
+ CV[1][2]=pTS->m_getTrackCovariance(1,2);
+ CV[1][3]=a*(pTS->m_getTrackCovariance(2,3));
+ CV[1][4]=b*(pTS->m_getTrackCovariance(2,3))+c*(pTS->m_getTrackCovariance(2,4));
+ CV[2][2]=pTS->m_getTrackCovariance(1,1);
+ CV[2][3]=a*(pTS->m_getTrackCovariance(1,3));
+ CV[2][4]=b*(pTS->m_getTrackCovariance(1,3))+c*(pTS->m_getTrackCovariance(1,4));
+ CV[3][3]=a*a*(pTS->m_getTrackCovariance(3,3));
+ CV[3][4]=a*(b*(pTS->m_getTrackCovariance(3,3))+c*(pTS->m_getTrackCovariance(3,4)));
+ CV[4][4]=b*b*(pTS->m_getTrackCovariance(3,3))+2.0*b*c*(pTS->m_getTrackCovariance(3,4))+
+ c*c*(pTS->m_getTrackCovariance(4,4));
+
+ for(int i=0;i<5;i++)
+ for(int j=i;j<5;j++) pCov->push_back(CV[i][j]);
+
+ const TrigInDetTrackFitPar* tidtfp = new TrigInDetTrackFitPar(D0,Phi0,Z0,Eta, Pt,
+ errD0,errPhi0,errZ0,
+ errEta,errPt,pCov);
+ delete pTS;
+
+ ATH_MSG_DEBUG("Total chi2 ="<<chi2tot<<" NDOF="<<ndoftot);
+ ATH_MSG_DEBUG("Fitted parameters: d0="<<D0<<" phi0="<<Phi0<<" z0="<<Z0
+ <<" eta0="<<Eta<<" pt="<<Pt);
+
+ if((ndoftot<0) || (fabs(Pt)<100.0) || (std::isnan(Pt)))
+ {
+ ATH_MSG_DEBUG("Fit failed - possibly floating point problem");
+ delete tidtfp;
+ (*trIt)->chi2(1e8);
+ }
+ else
+ {
+ delete param;
+ (*trIt)->param(tidtfp);
+ if(ndoftot>1) chi2tot/=ndoftot;
+ (*trIt)->chi2(chi2tot);
+ }
+ }
+ else
+ {
+ ATH_MSG_DEBUG("Track fitter failure ");
+ (*trIt)->chi2(1e8);
+ }
+
+ pnIt=vpTrkNodes.begin();pnEnd=vpTrkNodes.end();
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ delete((*pnIt)->m_getSurface());
+ delete (*pnIt);
+ }
+ vpTrkNodes.clear();
+ }
+}
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetKarimakiFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetKarimakiFitter.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..eaf8e04c52cb675b458c8413aa111148831a7d68
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetKarimakiFitter.cxx
@@ -0,0 +1,404 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigInDetKarimakiFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 13.02.2007 Package created
+//
+// Author: Carlo Schiavi, Genova
+// e-mail: Carlo.Schiavi@ge.infn.it
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+
+#include "StoreGate/StoreGateSvc.h"
+
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetEvent/TrigInDetTrackCollection.h"
+
+#include "AtlasDetDescr/AtlasDetectorID.h"
+#include "InDetPrepRawData/SCT_Cluster.h"
+#include "InDetPrepRawData/PixelCluster.h"
+#include "TrigTimeAlgs/TrigTimerSvc.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetKarimakiFitter.h"
+
+
+
+/*********************************************************************************/
+/** Constructor **/
+/*********************************************************************************/
+TrigInDetKarimakiFitter::TrigInDetKarimakiFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ) : AthAlgTool(t,n,p) {
+
+ // Declare interface
+ declareInterface< ITrigInDetTrackFitter >( this );
+
+ // Declare properties
+ //declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+}
+
+
+/*********************************************************************************/
+/** Destructor **/
+/*********************************************************************************/
+TrigInDetKarimakiFitter::~TrigInDetKarimakiFitter() {
+}
+
+
+/*********************************************************************************/
+/** initialize **/
+/*********************************************************************************/
+StatusCode TrigInDetKarimakiFitter::initialize() {
+
+ // Base init and get message svc
+ m_log = new MsgStream(msgSvc(), name());
+ StatusCode sc = AthAlgTool::initialize();
+ if(sc.isFailure()) {
+ (*m_log) << MSG::FATAL << "unable to initialize" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ // Retrieve Detector Description
+ StoreGateSvc* detectorStore;
+ sc = service("DetectorStore", detectorStore);
+ if(sc.isFailure()) {
+ (*m_log) << MSG::FATAL << "unable to locate Detector Store" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ // Get SCT & pixel Identifier helpers
+ if (detectorStore->retrieve(m_pixelId, "PixelID").isFailure()) {
+ (*m_log) << MSG::FATAL << "Could not get Pixel ID helper" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ if (detectorStore->retrieve(m_sctId, "SCT_ID").isFailure()) {
+ (*m_log) << MSG::FATAL << "Could not get SCT ID helper" << endreq;
+ return StatusCode::FAILURE;
+ }
+
+ (*m_log) << MSG::INFO << "TrigInDetKarimakiFitter constructed" << endreq;
+ return sc;
+}
+
+
+/*********************************************************************************/
+/** finalize **/
+/*********************************************************************************/
+StatusCode TrigInDetKarimakiFitter::finalize() {
+
+ // Destroy message svc
+ delete m_log;
+
+ // Base finalize
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+
+/*********************************************************************************/
+/** fit **/
+/*********************************************************************************/
+void TrigInDetKarimakiFitter::fit(TrigInDetTrackCollection* tracks) {
+
+ // Loop on tracks
+ TrigInDetTrackCollection::iterator t, tEnd=tracks->end();
+ for(t=tracks->begin(); t!=tEnd; t++) {
+
+ // Ancillary track
+ TrigInDetKarimakiFitter::AncillaryTrack at;
+
+ // Perform three point fit
+ if((*t)->siSpacePoints()->size()>2) {
+ circlefitRPhi(*t, at);
+ fitRZ(*t, at);
+ }
+
+ // Perform full fit
+ if((*t)->siSpacePoints()->size()>3) {
+ fitRPhi(*t, at);
+ }
+
+ // Evaluate final chi2 value
+ (*t)->chi2(sqrt(at.chi2rz*at.chi2rz + at.chi2rp*at.chi2rp));
+ }
+}
+
+
+/*********************************************************************************/
+/** circlefitRPhi **/
+/*********************************************************************************/
+void TrigInDetKarimakiFitter::circlefitRPhi(TrigInDetTrack* t, TrigInDetKarimakiFitter::AncillaryTrack& at) {
+
+ /** Fitting variables. **/
+ double phi=0, d0=0;
+ double x[3], y[3];
+ double tr, cx1, cy1, cx2, cy2, xm1, ym1, xm2, ym2;
+ double phi0, dph0p;
+
+ /** Get space point indexes. **/
+ double rsp, rval[3]={10000.0, 10000.0, 10000.0};
+ int ind, rind[3]={0, 1, 2};
+
+ std::vector<const TrigSiSpacePoint*>::iterator sp, spEnd=t->siSpacePoints()->end();
+ for(sp=t->siSpacePoints()->begin(), ind=0; sp!=spEnd; sp++, ind++) {
+ // Save indices for preliminary RPhi fit
+ rsp = (*sp)->r();
+ if(rsp<rval[0]) {rval[0]=rsp; rind[0]=ind;}
+ if(rsp>122.5 && (rsp-122.5)<rval[1]) {rval[1]=rsp-122.5; rind[1]=ind;}
+ if(rsp>350.0 && (rsp-350.0)<rval[2]) {rval[2]=rsp-350.5; rind[2]=ind;}
+ }
+ if(rind[0]==rind[1] || rind[1]==rind[2]) {rind[0]=0; rind[1]=1; rind[2]=t->siSpacePoints()->size()-1;}
+
+ /** Get space point coordinates. */
+ for(int i=0; i<3; i++) {
+ x[i] = (*(t->siSpacePoints()))[rind[i]]->x();
+ y[i] = (*(t->siSpacePoints()))[rind[i]]->y();
+ }
+
+ /** Evaluate useful quantities. */
+ xm1 = (x[1]+x[0])/2;
+ ym1 = (y[1]+y[0])/2;
+ cx1 = y[1] - y[0];
+ cy1 = x[0] - x[1];
+
+ xm2 = (x[1]+x[2])/2;
+ ym2 = (y[1]+y[2])/2;
+ cx2 = y[1] - y[2];
+ cy2 = x[2] - x[1];
+
+ /** Check if track is too straight. */
+ bool straight=false;
+ if(cy1/cx1 == cy2/cx2) {
+ straight=true;
+ }
+ else {
+ // Evaluate circle radius
+ tr = (xm2-xm1+(ym1-ym2)*cx2/cy2)/(cx1-cy1*cx2/cy2);
+ at.xc = xm1 + cx1*tr;
+ at.yc = ym1 + cy1*tr;
+ at.rc = sqrt((at.xc-x[0])*(at.xc-x[0])+(at.yc-y[0])*(at.yc-y[0]));
+ at.pt = at.rc*1.042*9.0/15.0;
+ if(at.pt>=1000000.0) straight=true;
+ }
+
+ /** Treat straight tracks. */
+ if(straight) {
+ phi = atan2(y[1]-y[0], x[1]-x[0]);
+ if(phi<-M_PI) phi+=2*M_PI;
+ if(phi>M_PI) phi-=2*M_PI;
+ at.pt = 1000000.0;
+ at.xc = at.yc = at.rc = 0;
+ double phiV = atan2(y[0], x[0]);
+ d0 = sqrt(x[0]*x[0]+y[0]*y[0])*sin(phiV-phi);
+ }
+ /** Treat curved tracks. */
+ else {
+ at.phic = atan2(at.xc, -at.yc);
+ d0 = sqrt(at.xc*at.xc+at.yc*at.yc)-at.rc;
+ /** Evaluate charge. */
+ phi0 = atan2(at.xc-x[0],-at.yc+y[0]);
+ dph0p = phi0-at.phic;
+ if (dph0p > M_PI) dph0p = dph0p - 2*M_PI;
+ if (dph0p < -M_PI) dph0p = dph0p + 2*M_PI;
+ if (dph0p > 0) {
+ at.q = -1;
+ } else {
+ at.q = 1;
+ }
+ at.pt = at.pt*at.q;
+
+ /** Evaluate phi0. */
+ phi = atan2(-at.xc, at.yc) + (1+at.q)/2*M_PI;
+ if(phi<-M_PI) phi+=2*M_PI;
+ if(phi>M_PI) phi-=2*M_PI;
+
+ /** Evaluate d0 sign. */
+ double absd0 = fabs(d0);
+ double phiV = atan2(y[0], x[0]);
+ if(phiV<-M_PI) phiV+=2*M_PI;
+ if(phiV> M_PI) phiV-=2*M_PI;
+ float newsign = sin(phiV-phi);
+ if (newsign<0){
+ d0 = -absd0;
+ } else {
+ d0 = absd0;
+ }
+ }
+
+ /** Save track parameters. */
+ TrigInDetTrackFitPar* par = const_cast<TrigInDetTrackFitPar*>(t->param());
+ par->a0(d0);
+ par->phi0(phi);
+ par->pT(at.pt);
+
+ /** Save chi square value . */
+ at.nSP = 3;
+ at.chi2rp = 0;
+}
+
+
+
+/*********************************************************************************/
+/*********************************************************************************/
+/** fitRZ **/
+/*********************************************************************************/
+/*********************************************************************************/
+void TrigInDetKarimakiFitter::fitRZ(TrigInDetTrack* t, TrigInDetKarimakiFitter::AncillaryTrack& at) {
+
+ /** Fill sum variables. */
+
+ // Sum variable
+ double n=0, rr=0, rz=0, r=0, z=0;
+ double iphi, ir, iz, ezez;
+
+ // Preliminary track parameters
+ TrigInDetTrackFitPar* par = const_cast<TrigInDetTrackFitPar*>(t->param());
+ double z1=(*(t->siSpacePoints()))[0]->z(), z2=(*(t->siSpacePoints()))[1]->z();
+ double r1=(*(t->siSpacePoints()))[0]->r(), r2=(*(t->siSpacePoints()))[1]->r();
+ at.cotantheta = (z2-z1)/(r2-r1);
+
+ // Loop on SPs
+ std::vector<const TrigSiSpacePoint*>::iterator sp, spEnd=t->siSpacePoints()->end();
+ for(sp=t->siSpacePoints()->begin(); sp!=spEnd; sp++) {
+ // Evaluate error on Z
+ ezez = ((*sp)->dz()*(*sp)->dz()) + (at.cotantheta*at.cotantheta)*((*sp)->dr()*(*sp)->dr());
+ // Evaluate sum variables
+ iphi = atan2(at.xc-(*sp)->x(), -at.yc+(*sp)->y());
+ ir = iphi-at.phic; if(ir>M_PI) ir=ir-2*M_PI; if(ir<-M_PI) ir=ir+2*M_PI;
+ iz = (*sp)->z();
+ n+=1/ezez; rr+=(ir*ir)/ezez; rz+=(ir*iz)/ezez; r+=ir/ezez; z+=iz/ezez;
+ }
+
+
+ /** Evaluate line parameters. */
+ double invdelta, z0;
+ invdelta = 1/(n*rr-r*r);
+ z0 = invdelta*(rr*z-r*rz);
+ at.ang = invdelta*(n*rz-r*z);
+
+
+ /** Evaluate track parameters. */
+ double theta = atan2(-at.q*at.rc, at.ang);
+ if(theta<0) theta = theta + (1+at.q)/2*M_PI;
+ par->eta(-log(tan(theta/2)));
+ par->z0(z0);
+
+
+ /** Evaluate chi square. */
+ at.cotantheta = 1/tan(theta);
+ double chi2 = 0;
+ for(sp=t->siSpacePoints()->begin(); sp!=spEnd; sp++) {
+ // Evaluate error on Z
+ ezez = ((*sp)->dz()*(*sp)->dz()) + (at.cotantheta*at.cotantheta)*((*sp)->dr()*(*sp)->dr());
+ // Evaluate sum variables
+ iphi = atan2(at.xc-(*sp)->x(), -at.yc+(*sp)->y());
+ ir = iphi-at.phic; if(ir>M_PI) ir=ir-2*M_PI; if(ir<-M_PI) ir=ir+2*M_PI;
+ iz = (*sp)->z();
+ double izgood = z0 + at.ang*ir;
+ chi2+=(iz-izgood)*(iz-izgood)/ezez;
+ }
+
+ /* Save chi square value. */
+ at.chi2rz = chi2;
+ at.nSP = t->siSpacePoints()->size();
+}
+
+
+
+/*********************************************************************************/
+/*********************************************************************************/
+/** fitRPhi **/
+/*********************************************************************************/
+/*********************************************************************************/
+void TrigInDetKarimakiFitter::fitRPhi(TrigInDetTrack* t, TrigInDetKarimakiFitter::AncillaryTrack& at) {
+
+ /** Fill sum variables. */
+ // Sum variable
+ double Cxx, Cxy, Cyy, Cxrr, Cyrr, Crrrr, q1, q2;
+ double Sw=0, x=0, y=0, xx=0, xy=0, yy=0, xrr=0, yrr=0, rr=0, rrrr=0;
+ double ix, iy, ir, iphi, iphic, ee;
+ double phi, rho, d0;
+
+ // Preliminary track parameters
+ TrigInDetTrackFitPar* par = const_cast<TrigInDetTrackFitPar*>(t->param());
+ phi = par->phi0();
+
+ // Loop on SPs
+ std::vector<const TrigSiSpacePoint*>::iterator sp, spEnd=t->siSpacePoints()->end();
+ for(sp=t->siSpacePoints()->begin(); sp!=spEnd; sp++) {
+ // Get coordinates
+ ix = (*sp)->x();
+ iy = (*sp)->y();
+ ir = (*sp)->r();
+ iphi = (*sp)->phi();
+ iphic = atan2(iy-at.yc, ix-at.xc);
+ // Evaluate error
+ ee = (ir*ir*(*sp)->dphi()*(*sp)->dphi()*cos(M_PI/2+iphi-iphic)*cos(M_PI/2+iphi-iphic)) +
+ ((*sp)->dr()*(*sp)->dr()*sin(M_PI/2+iphi-iphic)*sin(M_PI/2+iphi-iphic));
+ // Evaluate sum variables
+ Sw+=1/ee; x+=ix/ee; y+=iy/ee; xx+=(ix*ix)/ee; xy+=(ix*iy)/ee; yy+=(iy*iy)/ee; xrr+=(ix*ir*ir)/ee; yrr+=(iy*ir*ir)/ee; rr+=(ir*ir)/ee; rrrr+=(ir*ir*ir*ir)/ee;
+ }
+ // Divide by weight sum
+ x/=Sw; y/=Sw; xx/=Sw; xy/=Sw; yy/=Sw; xrr/=Sw; yrr/=Sw; rr/=Sw; rrrr/=Sw;
+
+
+ /** Evaluate circle parameters. */
+ Cxx=xx-x*x; Cxy=xy-x*y; Cyy=yy-y*y; Cxrr=xrr-x*rr; Cyrr=yrr-y*rr; Crrrr=rrrr-rr*rr;
+ q1=(Crrrr*Cxy)-(Cxrr*Cyrr); q2=Crrrr*(Cxx-Cyy)-(Cxrr*Cxrr)+(Cyrr*Cyrr);
+
+ double signx = cos(phi)/fabs(cos(phi)); double signy = sin(phi)/fabs(sin(phi));
+
+ phi = 0.5*atan2(signy*2*q1/q2, signx);
+ if(phi>M_PI) phi-=2*M_PI; else if(phi<-M_PI) phi+=2*M_PI;
+
+ if(signx<0 && signy>=0) {
+ if(phi<0) phi=M_PI/2-phi;
+ else phi=M_PI-phi;
+ }
+ else if(signx>=0 && signy<0) {
+ if(phi<0) phi=3*M_PI/2-phi;
+ else phi=-phi;
+ }
+ else if(signx>=0 && signy>=0) {
+ if(phi<0) phi=M_PI/2+phi;
+ else phi=phi;
+ }
+ else if(signx<0 && signy<0) {
+ if(phi<0) phi=3*M_PI/2+phi;
+ else phi=M_PI+phi;
+ }
+ if(phi>M_PI) phi-=2*M_PI; else if(phi<-M_PI) phi+=2*M_PI;
+
+ at.k = (sin(phi)*Cxrr-cos(phi)*Cyrr)/Crrrr;
+ at.delta = -at.k*rr + sin(phi)*x - cos(phi)*y;
+ rho = 2*at.k/sqrt(1-4*at.delta*at.k);
+ d0 = -2*at.delta/(1+sqrt(1-4*at.delta*at.k));
+
+ /** Evaluate track parameters. */
+ par->phi0(phi);
+ par->a0(d0);
+ par->pT(1.042*(1/rho)*9.0/15.0);
+
+
+ /** Evaluate chi square. */
+ double chi2 = Sw*(1+rho*d0)*(1+rho*d0)*(sin(phi)*sin(phi)*Cxx - 2*sin(phi)*cos(phi)*Cxy + cos(phi)*cos(phi)*Cyy - at.k*at.k*Crrrr);
+
+ /* Save chi square value. */
+ at.chi2rp = chi2;
+ at.nSP = t->siSpacePoints()->size();
+}
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetOfflineTrackFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetOfflineTrackFitter.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..155ffb931e46eaca0817b4f4e1c5325b91b374dc
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetOfflineTrackFitter.cxx
@@ -0,0 +1,325 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigInDetOfflineTrackFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 08.05.2010 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetEvent/TrigInDetTrackCollection.h"
+
+#include "TrkTrack/TrackCollection.h"
+
+#include "InDetPrepRawData/SCT_Cluster.h"
+#include "InDetPrepRawData/PixelCluster.h"
+#include "TrigTimeAlgs/TrigTimerSvc.h"
+#include "TrkParameters/TrackParameters.h"
+#include "TrkEventPrimitives/FitQuality.h"
+#include "TrkEventPrimitives/ParamDefs.h"
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetOfflineTrackFitter.h"
+
+TrigInDetOfflineTrackFitter::TrigInDetOfflineTrackFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ):
+ AthAlgTool(t,n,p),
+ m_trackFitter("Trk::GlobalChi2Fitter/InDetTrigTrackFitter")
+{
+ declareInterface< ITrigInDetTrackFitter >( this );
+
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremm=false);
+ declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+ declareProperty( "OfflineClusters", m_offlineClusters = true);
+ declareProperty("OfflineTrackFitter", m_trackFitter);
+}
+
+StatusCode TrigInDetOfflineTrackFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ StoreGateSvc* detStore;
+ sc = service("DetectorStore", detStore);
+ if ( sc.isFailure() ) {
+ ATH_MSG_FATAL("DetStore service not found");
+ return StatusCode::FAILURE;
+ }
+
+ sc=m_trackFitter.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_trackFitter);
+ return sc;
+ }
+
+ /*
+ StatusCode scFit = toolSvc->retrieveTool(m_FitterName, m_FitterInstance, m_trackFitter);
+ if (scFit.isFailure())
+ {
+ athenaLog<<MSG::ERROR<<"Could not find refit tool of type "<<m_FitterName<<". Exiting."<< endreq;
+ return scFit;
+ }
+ else
+ {
+ athenaLog << MSG::INFO << "Refit tool \""<<m_FitterName<<" "<<m_FitterInstance<<"\" booked."<< endreq;
+ }
+ */
+ if (detStore->retrieve(m_pixelId, "PixelID").isFailure()) {
+ ATH_MSG_FATAL("Could not get Pixel ID helper");
+ return StatusCode::FAILURE;
+ }
+
+ if (detStore->retrieve(m_sctId, "SCT_ID").isFailure()) {
+ ATH_MSG_FATAL("Could not get SCT ID helper");
+ return StatusCode::FAILURE;
+ }
+
+ ITrigTimerSvc* timerSvc;
+ StatusCode scTime = service( "TrigTimerSvc", timerSvc);
+ if( scTime.isFailure() ) {
+ ATH_MSG_INFO("Unable to locate Service TrigTimerSvc ");
+ m_timers = false;
+ }
+ else{
+ m_timers = true;
+ }
+// add some timers:
+//
+ if ( m_timers ) {
+ m_timer[0] = timerSvc->addItem("TrigToOffline");
+ m_timer[1] = timerSvc->addItem("OfflineFit");
+ m_timer[2] = timerSvc->addItem("OfflineToTrig");
+ }
+ ATH_MSG_INFO("TrigInDetOflineTrackFitter constructed ");
+ return sc;
+}
+
+StatusCode TrigInDetOfflineTrackFitter::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+void TrigInDetOfflineTrackFitter::fit(TrigInDetTrackCollection* recoTracks )
+{
+ double Pt, Phi0, D0, Z0, Eta, errD0, errZ0, errEta, errPhi0, errPt,a,b,c;
+ double CV[5][5],Theta;
+ std::vector<double>* pCov;
+ TrigInDetTrackCollection::iterator trIt,lastIt;
+
+ if(m_timers)
+ {
+ m_timer[0]->start();
+ m_timer[0]->pause();
+ m_timer[1]->start();
+ m_timer[1]->pause();
+ m_timer[2]->start();
+ m_timer[2]->pause();
+ }
+
+ trIt = recoTracks->begin();lastIt = recoTracks->end();
+ for(; trIt !=lastIt; trIt++)
+ {
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>((*trIt)->param());
+ if(param==NULL)
+ {
+ ATH_MSG_WARNING("Fit Failed -- TrigInDetTrack has no parameters");
+ continue;
+ }
+ Pt=param->pT();
+ if(fabs(Pt)<100.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ continue;
+ }
+ if (!(*trIt)->siSpacePoints())
+ {
+ ATH_MSG_DEBUG("Track has no spacepoints - skipping fit");
+ continue;
+ }
+
+ // 1. TrigInDetTrack -> TrkTrack
+ if ( m_timers ) m_timer[0]->resume();
+ Phi0=param->phi0();
+ Theta=2.0*atan(exp(-param->eta()));
+ AmgSymMatrix(5)* pM = new AmgSymMatrix(5);
+ pM->setZero();
+ (*pM)(0,0)=100.0;
+ (*pM)(1,1)=100.0;
+ (*pM)(2,2)=0.01;
+ (*pM)(3,3)=0.01;
+ (*pM)(4,4)=1e-6;
+ const Trk::PerigeeSurface perSurf;
+ const Trk::Perigee* pMP = new Trk::Perigee(param->a0(),param->z0(),Phi0,Theta,sin(Theta)/Pt,perSurf, pM);
+
+ Trk::PrepRawDataSet prepRDColl;
+
+ std::vector<const TrigSiSpacePoint*>::iterator pSP=(*trIt)->siSpacePoints()->begin();
+ std::vector<const TrigSiSpacePoint*>::iterator lastSP=(*trIt)->siSpacePoints()->end();
+ for(int iSP=0; pSP != lastSP; pSP++, iSP++)
+ {
+ if (m_sctId->is_sct((*pSP)->identify()))
+ {
+ for (int iClusInSP=0; iClusInSP<2; iClusInSP++)
+ {
+ const InDet::SCT_Cluster* pCL;
+ if (iClusInSP == 1)
+ pCL = dynamic_cast<const InDet::SCT_Cluster*>((*pSP)->clusters().first);
+ else
+ pCL = dynamic_cast<const InDet::SCT_Cluster*>((*pSP)->clusters().second);
+ if(!pCL) continue;
+ /*
+ const Trk::LocalParameters* loc = new Trk::LocalParameters(pCL->localPosition());
+ const Trk::GlobalPosition glo(pCL->globalPosition());
+ const Trk::ErrorMatrix* err = new Trk::ErrorMatrix(pCL->localErrorMatrix ());
+ const IdentifierHash idHash = m_sctId->wafer_hash(m_sctId->wafer_id(pCL->identify()));
+ InDet::SCT_ClusterOnTrack* pSC=new InDet::SCT_ClusterOnTrack(pCL,loc,err,idHash,glo);
+ */
+ prepRDColl.push_back(pCL);
+ }
+ }
+ else
+ { // Pixel spacepoint
+ const InDet::PixelCluster* pCL = dynamic_cast<const InDet::PixelCluster*>((*pSP)->clusters().first);
+ if(!pCL) continue;
+ /*
+ const Trk::LocalParameters* loc = new Trk::LocalParameters(pCL->localPosition());
+ const Trk::GlobalPosition glo(pCL->globalPosition());
+ const Trk::ErrorMatrix* err = new Trk::ErrorMatrix(pCL->localErrorMatrix ());
+ const IdentifierHash idHash = m_pixelId->wafer_hash(m_pixelId->wafer_id(pCL->identify()));
+ InDet::PixelClusterOnTrack* pSC=new InDet::PixelClusterOnTrack(pCL,loc,err,idHash,glo,
+ pCL->gangedPixel());
+ */
+ prepRDColl.push_back(pCL);
+ }
+ }
+ if ( m_timers ) m_timer[0]->pause();
+ if ( m_timers ) m_timer[1]->resume();
+
+ Trk::Track* newtrack = m_trackFitter->fit(prepRDColl,*pMP,false,Trk::pion);
+
+ if ( m_timers ) m_timer[1]->pause();
+ if ( m_timers ) m_timer[2]->resume();
+
+ delete pMP;
+
+ bool badFit=false;
+ if(newtrack!=NULL)
+ {
+ const Trk::Perigee *aMeasPer = newtrack->perigeeParameters();
+ if (aMeasPer==nullptr)
+ {
+ ATH_MSG_DEBUG("Could not get Trk::MeasuredPerigee");
+ badFit=true;
+ }
+ else
+ {
+ D0 = aMeasPer->parameters()[Trk::d0];
+ Z0 = aMeasPer->parameters()[Trk::z0];
+ Phi0 = aMeasPer->parameters()[Trk::phi0];
+ Theta = aMeasPer->parameters()[Trk::theta];
+ if(Phi0>M_PI) Phi0-=2*M_PI;
+ if(Phi0<-M_PI) Phi0+=2*M_PI;
+ Eta = -log(sin(0.5*Theta)/cos(0.5*Theta));
+
+ double qOverP = aMeasPer->parameters()[Trk::qOverP];
+ Pt=sin(Theta)/qOverP;
+ ATH_MSG_DEBUG("Refitted offline parameters " << D0 << " " << Z0 << " "
+ << Phi0 << " " << Eta << " " << Pt);
+
+ double Gk[5][5];
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ Gk[i][j]=(*aMeasPer->covariance())(i,j);
+
+ errD0 = sqrt(Gk[0][0]);
+ errZ0 = sqrt(Gk[1][1]);
+ errPhi0 = sqrt(Gk[2][2]);
+ errEta = sqrt(Gk[3][3])/fabs(sin(Theta));
+ b=cos(Theta)/qOverP;
+ c=-Pt/qOverP;
+ a=-1.0/sin(Theta);
+ errPt = sqrt(b*b*(Gk[3][3])+c*c*Gk[4][4]+2.0*b*c*Gk[3][4]);
+
+ pCov=new std::vector<double>;
+
+ CV[0][0]=Gk[0][0];
+ CV[0][1]=Gk[0][2];
+ CV[0][2]=Gk[0][1];
+ CV[0][3]=a*Gk[0][3];
+ CV[0][4]=b*Gk[0][3]+c*Gk[0][4];
+ CV[1][1]=Gk[2][2];
+
+ CV[1][2]=Gk[1][2];
+ CV[1][3]=a*Gk[2][3];
+ CV[1][4]=b*Gk[2][3]+c*Gk[2][4];
+ CV[2][2]=Gk[1][1];
+ CV[2][3]=a*Gk[1][3];
+ CV[2][4]=b*Gk[1][3]+c*Gk[1][4];
+ CV[3][3]=a*a*Gk[3][3];
+ CV[3][4]=a*(b*Gk[3][3]+c*Gk[3][4]);
+ CV[4][4]=b*b*Gk[3][3]+2.0*b*c*Gk[3][4]+c*c*Gk[4][4];
+
+ const Trk::FitQuality* pFQ=newtrack->fitQuality();
+ double chi2tot=pFQ->chiSquared();
+ int ndoftot=pFQ->numberDoF();
+
+ for(int i=0;i<5;i++)
+ for(int j=i;j<5;j++) pCov->push_back(CV[i][j]);
+ pCov->push_back(ndoftot+0.001);
+
+ const TrigInDetTrackFitPar* tidtfp = new TrigInDetTrackFitPar(D0,Phi0,Z0,Eta, Pt,
+ errD0,errPhi0,errZ0,
+ errEta,errPt,pCov);
+
+ ATH_MSG_DEBUG("Total chi2 ="<<chi2tot<<" NDOF="<<ndoftot);
+ ATH_MSG_DEBUG("Fitted parameters: d0="<<D0<<" phi0="<<Phi0<<" z0="<<Z0
+ <<" eta0="<<Eta<<" pt="<<Pt);
+ if((ndoftot<0) || (fabs(Pt)<100.0) || (std::isnan(Pt)))
+ {
+ ATH_MSG_DEBUG("Fit failed - possibly floating point problem");
+ delete tidtfp;
+ (*trIt)->chi2(1e8);
+ }
+ else
+ {
+ delete param;
+ if(ndoftot>1) chi2tot/=ndoftot;
+ (*trIt)->param(tidtfp);
+ (*trIt)->chi2(chi2tot);
+ }
+ }
+ }
+ else badFit=true;
+ if(badFit)
+ {
+ ATH_MSG_DEBUG("Offline fitter failed");
+ (*trIt)->chi2(1e8);
+ }
+ delete newtrack;
+ if ( m_timers ) m_timer[2]->pause();
+ }
+ if(m_timers)
+ {
+ m_timer[0]->stop();
+ m_timer[1]->stop();
+ m_timer[2]->stop();
+ }
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetPerigeeFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetPerigeeFitter.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..bae418867219609fca6bd0563832864a488ad710
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetPerigeeFitter.cxx
@@ -0,0 +1,258 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+#include "GaudiKernel/MsgStream.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetTrackFitter/TrigInDetPerigeeFitter.h"
+#include "MagFieldInterfaces/IMagFieldSvc.h"
+#include "TrigInDetTrackFitter/PerigeeFilteringNodes.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+TrigInDetPerigeeFitter::TrigInDetPerigeeFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ): AthAlgTool(t,n,p),
+ m_MagFieldSvc("AtlasFieldSvc",this->name())
+{
+ declareInterface< ITrigInDetTrackFitter >( this );
+ declareProperty( "StraightLineMode", m_straightLineMode = false );
+ declareProperty( "AthenaFieldService", m_MagFieldSvc, "AtlasFieldSvc");
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremmCorr=false);
+
+ declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+}
+
+StatusCode TrigInDetPerigeeFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ StoreGateSvc* detStore;
+ sc = service("DetectorStore", detStore);
+ if ( sc.isFailure() ) {
+ ATH_MSG_FATAL("DetStore service not found");
+ return StatusCode::FAILURE;
+ };
+
+ if(!m_straightLineMode)
+ {
+ sc = m_MagFieldSvc.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Unable to retrieve Athena MagFieldService");
+ return StatusCode::FAILURE;
+ }
+ double pos[3];
+ pos[0]=0.0;pos[1]=0.0;pos[2]=0.0;
+ double field[3];
+ m_MagFieldSvc->getField(pos,field);
+ m_consb=0.029997*field[2];
+ if(fabs(m_consb)<1e-6)
+ {
+ m_straightLineMode=true;
+ m_consb=0.6;
+ }
+ }
+ else
+ {
+ m_consb=0.6;
+ }
+ if(m_straightLineMode)
+ {
+ ATH_MSG_INFO("Straight line fitting mode ON ");
+ }
+ else
+ {
+ ATH_MSG_INFO("Field constant is set to "<<m_consb);
+ }
+ /*
+ ITrigTimerSvc* timerSvc;
+ sc = service( "TrigTimerSvc", timerSvc);
+ if( sc.isFailure() ) {
+ ATH_MSG_INFO<< "Unable to locate Service TrigTimerSvc " << endreq;
+ m_timers = false;
+ }
+ else{
+ m_timers = true;
+ }
+ if ( m_timers ) {
+ m_timer[0] = timerSvc->addItem("CreateNodes");
+ m_timer[1] = timerSvc->addItem("ForwardFilter");
+ m_timer[2] = timerSvc->addItem("Smoother");
+ m_timer[3] = timerSvc->addItem("TrigTrackPars");
+ m_timer[4] = timerSvc->addItem("CleanUp");
+ m_timer[5] = timerSvc->addItem("Validate");
+ m_timer[6] = timerSvc->addItem("Update");
+ }
+ */
+ ATH_MSG_INFO("TrigInDetPerigeeFitter constructed ");
+ return sc;
+}
+
+StatusCode TrigInDetPerigeeFitter::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+TrigInDetPerigeeFitter::~TrigInDetPerigeeFitter()
+{
+
+}
+
+
+void TrigInDetPerigeeFitter::fit(TrigInDetTrackCollection* recoTracks)
+{
+ int i,nType,nHits;
+ double chi2tot;
+ int ndoftot;
+ double Rk[5];
+ double Gk[15];
+ BarrelPerigeeFilteringNode* pBS;
+ DiscPerigeeFilteringNode* pDS;
+ std::vector<class BasePerigeeFilteringNode*> vpHits;
+
+ double Pt, Phi0, D0, Z0, Eta, errD0, errZ0, errEta, errPhi0, errPt;
+ double CV[5][5],a,b;
+ std::vector<double>* pCov;
+
+ vpHits.clear();
+ TrigInDetTrackCollection::iterator trIt = recoTracks->begin();
+ TrigInDetTrackCollection::iterator lastIt = recoTracks->end();
+ for(; trIt !=lastIt; trIt++)
+ {
+ if((*trIt)->siSpacePoints()->size()==0)
+ {
+ ATH_MSG_ERROR("Perigee Fit Failed -- TrigInDetTrack has no hits");
+ continue;
+ }
+ std::vector<const TrigSiSpacePoint*>::iterator pSPIt=(*trIt)->siSpacePoints()->begin();
+ std::vector<const TrigSiSpacePoint*>::iterator lastSPIt=(*trIt)->siSpacePoints()->end();
+ for(; pSPIt != lastSPIt; pSPIt++)
+ {
+ const TrigSiSpacePoint* pSP=(*pSPIt);
+ nType=(pSP->dr()>pSP->dz())?1:0;
+ switch(nType)
+ {
+ case 0:
+ {
+ pBS=new BarrelPerigeeFilteringNode(pSP);
+ vpHits.push_back(pBS);
+ break;
+ }
+ case 1:
+ {
+ pDS=new DiscPerigeeFilteringNode(pSP);
+ vpHits.push_back(pDS);
+ break;
+ }
+ }
+ }
+ nHits=vpHits.size();
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>((*trIt)->param());
+ if(param==NULL)
+ {
+ ATH_MSG_ERROR("Perigee Fit Failed -- TrigInDetTrack has no parameters");
+ for(std::vector<class BasePerigeeFilteringNode*>::iterator phIt=vpHits.begin();phIt!=vpHits.end();++phIt)
+ {
+ delete (*phIt);
+ }
+ vpHits.clear();
+ continue;
+ }
+ else
+ {
+ double tgHalfTheta=exp(-param->eta());
+ Rk[4]=0.5*(1-tgHalfTheta*tgHalfTheta)/tgHalfTheta;
+ if(fabs(Rk[4])<1e-6) Rk[4]=1e-6;
+ Rk[2]=param->phi0();
+ Rk[0]=0.5*m_consb/(param->pT()*1000.0);
+ Rk[1]=param->a0();
+ Rk[3]=param->z0();
+ for(i=0;i<15;i++) Gk[i]=0.0;
+ Gk[0]=0.01;Gk[2]=1.0;Gk[5]=10.0;Gk[9]=10.0;Gk[14]=0.2;
+ if(m_straightLineMode)
+ {
+ Rk[0]=1e-10;Gk[0]=1e-8;
+ }
+ chi2tot=0.0;ndoftot=0;
+ for(std::vector<class BasePerigeeFilteringNode*>::reverse_iterator pbhIt=vpHits.rbegin();
+ pbhIt!=vpHits.rend();++pbhIt)
+ {
+ double dchi2=(*pbhIt)->m_getChi2(Rk,Gk);
+ if(dchi2<m_DChi2)
+ {
+ (*pbhIt)->m_acceptIt();
+ chi2tot+=dchi2;
+ ndoftot+=(*pbhIt)->m_getNdof();
+ (*pbhIt)->m_runFilter(Rk,Gk);
+ }
+ else (*pbhIt)->m_rejectIt();
+ if(m_doMultScatt) (*pbhIt)->m_applyMultScatt(Rk,Gk);
+ if(m_doBremmCorr) (*pbhIt)->m_applyEnergyLoss(Rk,Gk,-1);
+ }
+ chi2tot/=ndoftot-5;
+ Pt = -m_consb*0.5/Rk[0]; // put the right sign on Pt
+ Phi0 = Rk[2];
+ if(Phi0>M_PI) Phi0-=2*M_PI;
+ if(Phi0<-M_PI) Phi0+=2*M_PI;
+ Eta = log(Rk[4]+sqrt(1+Rk[4]*Rk[4]));
+ Z0 = Rk[3];
+ D0 = Rk[1];
+
+ errD0 = sqrt(Gk[2]);
+ errZ0 = sqrt(Gk[9]);
+ errEta = sqrt(Gk[14]/(1+Rk[4]*Rk[4]));
+ errPhi0 = sqrt(Gk[5]);
+ errPt = 0.5*m_consb*sqrt(Gk[0])/(Rk[0]*Rk[0]);
+
+ pCov=new std::vector<double>;
+ a=1/sqrt(1+Rk[4]*Rk[4]);
+ b=m_consb*0.5/(Rk[0]*Rk[0]);
+ CV[0][0]=Gk[2];
+ CV[0][1]=Gk[4];
+ CV[0][2]=Gk[7];
+ CV[0][3]=a*Gk[11];
+ CV[0][4]=b*Gk[1];
+ CV[1][1]=Gk[5];
+ CV[1][2]=Gk[8];
+ CV[1][3]=a*Gk[12];
+ CV[1][4]=b*Gk[3];
+ CV[2][2]=Gk[9];
+ CV[2][3]=a*Gk[13];
+ CV[2][4]=b*Gk[6];
+ CV[3][3]=a*a*Gk[14];
+ CV[3][4]=a*b*Gk[10];
+ CV[4][4]=b*b*Gk[0];
+ for(int i=0;i<5;i++)
+ for(int j=i;j<5;j++) pCov->push_back(CV[i][j]);
+ const TrigInDetTrackFitPar* tidtfp = new TrigInDetTrackFitPar(D0,Phi0,Z0,Eta, Pt,
+ errD0,errPhi0,errZ0,
+ errEta,errPt,pCov);
+ ATH_MSG_DEBUG("Perigee parameters: d0="<<D0<<" phi0="<<Phi0<<" z0="<<Z0
+ <<" eta0="<<Eta<<" pt="<<Pt);
+ delete param;
+ (*trIt)->param(tidtfp);
+ (*trIt)->chi2(chi2tot);
+ (*trIt)->siSpacePoints()->clear();
+ int nRejected=0;
+ for(std::vector<class BasePerigeeFilteringNode*>::iterator phIt=vpHits.begin();
+ phIt!=vpHits.end();++phIt)
+ {
+ if((*phIt)->m_isAccepted())
+ {
+ (*trIt)->siSpacePoints()->push_back((*phIt)->m_getTrigSp());
+ }
+ else nRejected++;
+ delete (*phIt);
+ }
+ ATH_MSG_DEBUG(nHits<<" hits, "<<nRejected<<" rejected ");
+ vpHits.clear();
+ }
+ }
+}
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetSctKFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetSctKFitter.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..68312bd8d6adb1c390c4b3d7f0251ee246d39904
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetSctKFitter.cxx
@@ -0,0 +1,2358 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigInDetSctKFitter
+// ( see header-file for details )
+// -------------------------------
+// ATLAS Collaboration
+////////////////////////////////////////////////////////////////////////////////
+
+#include "GaudiKernel/ToolFactory.h"
+#include "GaudiKernel/MsgStream.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetEvent/TrigInDetTrackCollection.h"
+#include "TrigInDetTrackFitter/TrigInDetSctKFitter.h"
+
+#include <cstring>
+using std::memset;
+
+TrigInDetSctKFitter::TrigInDetSctKFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ): AthAlgTool(t,n,p)
+{
+ declareInterface< ITrigInDetTrackFitter >( this );
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremmCorr=false);
+ declareProperty( "Chi2Cut", m_DChi2 = 200.0);
+ // set parameters
+ paramset = 0;
+ memset( minTrackRadius, 0, sizeof(minTrackRadius) );
+ memset( maxDChi2, 0, sizeof(maxDChi2) );
+
+ maxDChi2[0] = m_DChi2;
+ maxDChi2[1] = m_DChi2;
+}
+
+StatusCode TrigInDetSctKFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ ATH_MSG_INFO("SctKFitter constructed ");
+ return sc;
+}
+
+StatusCode TrigInDetSctKFitter::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+TrigInDetSctKFitter::~TrigInDetSctKFitter()
+{
+
+}
+
+void TrigInDetSctKFitter::fit(TrigInDetTrackCollection* recoTracks)
+{
+
+ long ier, nHits, ihtype[73], ihdrop[73];
+ double wm1[15],cov1[15],par_kf[5],par_official[5],x0=0.,y0=0.,rp,cov2[15];
+ double rco[73],fco[73],zco[73],errr[73],errxy[73],errz[73],chi2;
+ bool electron=m_doBremmCorr;
+
+ const double consb = 0.006;
+
+// to convert GeV to MeV and cm to mm
+ const double GeVToMeVConv = 1000.;
+ const double mmTocmConv = 0.1;
+ const double cmTommConv = 10.;
+
+ double Pt, Phi0, Eta, Z0, D0, errD0, errZ0, errEta, errPhi0, errPt;
+
+
+ TrigInDetTrackCollection::iterator trIt = recoTracks->begin();
+ TrigInDetTrackCollection::iterator lastIt = recoTracks->end();
+ for(; trIt !=lastIt; trIt++)
+ {
+ if((*trIt)->siSpacePoints()->size()==0)
+ {
+ ATH_MSG_ERROR("SctKFitter Failed -- TrigInDetTrack has no hits");
+ continue;
+ }
+ std::vector<const TrigSiSpacePoint*>::iterator pSPIt=(*trIt)->siSpacePoints()->begin();
+ std::vector<const TrigSiSpacePoint*>::iterator lastSPIt=(*trIt)->siSpacePoints()->end();
+ int i=0;
+ for(; pSPIt != lastSPIt; pSPIt++)
+ {
+ const TrigSiSpacePoint* pSP=(*pSPIt);
+
+ // to convert GeV to MeV and cm to mm
+ rco[i] = pSP->r()*mmTocmConv;
+ fco[i] = pSP->phi();
+ zco[i] = pSP->z()*mmTocmConv;
+
+ errr[i] = pSP->dr()*mmTocmConv;
+ errxy[i]= pSP->dphi() * pSP->r()*mmTocmConv; //this converts dPhi into dXY
+ errz[i] = pSP->dz()*mmTocmConv;
+
+ if( errr[i] > errz[i] )
+ {
+ ihtype[i]=1; // endcap
+ }
+ else
+ {
+ ihtype[i]=0; // barrel
+ }
+ i++;
+ }
+ nHits=i;
+ tkffit(&nHits,&rco[0],&fco[0],&zco[0],&errxy[0],&errz[0],
+ &errr[0],&ihtype[0],&rp,&par_kf[0],&wm1[0],&chi2,
+ &ihdrop[0],&ier,electron);
+
+ if ( ier ) {
+ ATH_MSG_ERROR("SctKFitter: error in tkffit -> " << ier);
+ continue;
+ }
+ long dropped = 0;
+ for (long j=0; j<nHits; j++) {
+ dropped += ihdrop[j];
+ }
+
+ //convert weight matrix to cov matrix
+ matrix_inverter(&wm1[0],&cov1[0],&ier);
+ if( ier ) {
+ ATH_MSG_ERROR(" SctKFitter: error in matrix conversion -> " << ier);
+ continue;
+ }
+ cyl_prg_convertor(&par_kf[0], &cov1[0], &rp, &x0, &y0,
+ &par_official[0], &cov2[0], &ier);
+
+ // to convert GeV to MeV and cm to mm
+ Pt = -consb/par_official[4]*GeVToMeVConv; // put the right sign on Pt
+ Phi0 = par_official[3];
+ Eta = -log(tan(par_official[2]/2.));
+ Z0 = par_official[1]*cmTommConv;
+ D0 = par_official[0]*cmTommConv;
+
+ errD0 = sqrt(cov2[0])*cmTommConv;
+ errZ0 = sqrt(cov2[2])*cmTommConv;
+ errEta = sqrt(cov2[5])/fabs(sin(par_official[2]));
+ errPhi0 = sqrt(cov2[9]);
+ errPt = consb*sqrt(cov2[14])/(par_official[4]*par_official[4])*GeVToMeVConv;
+ ATH_MSG_DEBUG("Perigee parameters: d0="<<D0<<" phi0="<<Phi0<<" z0="<<Z0
+ <<" eta0="<<Eta<<" pt="<<Pt);
+
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>((*trIt)->param());
+ const TrigInDetTrackFitPar* tidtfp = new TrigInDetTrackFitPar(D0,Phi0,Z0,Eta, Pt,
+ errD0,errPhi0,errZ0,errEta,errPt,NULL);
+
+ if(param!=NULL) delete param;
+ (*trIt)->param(tidtfp);
+ (*trIt)->chi2(chi2);
+ (*trIt)->siSpacePoints()->clear();
+
+ std::vector<const TrigSiSpacePoint*> spacePointsOnTrack;
+
+ std::vector<const TrigSiSpacePoint*>::iterator hitItr = (*trIt)->siSpacePoints()->begin();
+
+ for( long i = 0; i < nHits; ++i, ++hitItr )
+ if( ! ihdrop[i] )
+ (*trIt)->siSpacePoints()->push_back((*hitItr));
+ }
+}
+
+void TrigInDetSctKFitter::cyl_prg_convertor(double *pari, double *covi, double *r__,
+ double *xb, double *yb,
+ double *par, double *cov, long *ierr)
+{
+ double der11, der14, der15, der23, der45, cosf, sinf, curv;
+ long i__, j;
+ double rdphi, rsinb, cotth;
+ double x0, y0;
+ double tem, psi;
+
+/* Function : Compute the "perigee" parameters (at the point of the closest*/
+/* approach to the beam axis or to a reference position near a*/
+/* secondary vertex) from the cylindrical-polar parameters */
+/* of reconstructed tracks, and their covariance and weight */
+/* matrices. */
+/* For error propagation, an approximation is made, i.e. assuming*/
+/* that the r coordinate of reference point is small w.r.t. the*/
+/* radius of curvature. This is always true if the tracks are */
+/* extrapolated to the positions where are close to the beam (or the*/
+/* reference position near a secondary vertex) before calling this*/
+/* routine. */
+/* No account is taken for multiple scattering (m/s) in this*/
+/* routine, so that, for the primary vertex, all tracks should be*/
+/* extrapolated into the inside of the beam pipe before calling this*/
+/* routine; i.e. the m/s should be taken care in the extrapolation.*/
+
+/* Arguments : Input : PARI(1:5) : track parameters in cylindrical-polar*/
+/* coordinates, PARI(1) -- phi (rad.)*/
+/* PARI(2) -- z (cm)*/
+/* PARI(3) -- zeta (rad.)*/
+/* PARI(4) -- beta (rad.)*/
+/* PARI(5) -- 1/Rtr(1/cm)*/
+/* COVI(1:15) : covariance matrix of PARI. */
+/* the numbering convention is 1 */
+/* 2 3 */
+/* 4 5 6 */
+/* 7 8 9 10*/
+/* 11 12 13 14 15*/
+/* R : the radius of the reference point where*/
+/* above parameters are defined (in cm).*/
+/* XB,YB : coordinates of the "beam". */
+
+/* Output : PAR(1:5) : "perigee" parameters, */
+/* 1 : epsilon (impact par. in xy projection, with sign)*/
+/* 2 : z coordinate */
+/* 3 : theta */
+/* 4 : phi angle */
+/* 5 : 1/Rtr (Rtr = radius of curvature, with sign)*/
+/* COV(1:15) : covariance matrix of PAR. */
+/* WGT(1:15) : weight matrix of PAR. */
+
+/* Errors : IERR = 0 : no error; */
+/* 1 : incorrect data in inputs PARI, COVI or R; */
+/* 2 : error in converting COV (WGT not computed). */
+
+/* Authers : P. BILLOIR, S. QIAN July 1990 */
+
+/* Modified by : S. QIAN for CMS March 1994 */
+/* (1) the output parameter "theta" is changed to "ctg(theta)";*/
+/* (2) the appoximate perigee formula is replaced by a precise one.*/
+
+/**************************************************************************/
+
+/* Internal variables */
+
+/* DATA CONSB /.0053964/ */
+
+/*If magnetic field is not 4 Tesla, this CONSB has to be modified, or to be*/
+/* calculated by reading the field map. */
+
+ /* Parameter adjustments */
+ // --wgt;
+ --cov;
+ --par;
+ --covi;
+ --pari;
+
+ /* Function Body */
+/* ________________________ */
+
+ *ierr = 0;
+ for (i__=1; i__<6; i__++) par[i__]=0.;
+
+/* exit if incorrect data */
+
+ if (*r__ == 0. || pari[3] == 0. || covi[1] < 0. ||
+ covi[3] < 0. || covi[6] < 0. || covi[10] < 0. || covi[15] < 0.) {
+ *ierr = 1;
+ return;
+ }
+
+/* computation of "perigee" parameters */
+
+ x0 = *r__ * cos(pari[1]) - *xb;
+ y0 = *r__ * sin(pari[1]) - *yb;
+ curv = pari[5];
+ psi = pari[1] + pari[4];
+ sinf = sin(psi);
+ cosf = cos(psi);
+ rsinb = x0 * sinf - y0 * cosf;
+
+/* epsilon (impact parameter in xy projection, with geometrical sign) */
+/* phi at perigee (range 0,2*PI) */
+
+ if (curv == 0.) {
+ par[1] = rsinb;
+ par[4] = psi;
+ rdphi = -x0 * cosf - y0 * sinf;
+ } else {
+ tem = (x0 * x0 + y0 * y0) * curv - rsinb * 2.;
+ par[1] = - tem / (sqrt(curv * tem + 1.) + 1.);
+ par[4] = fmod(atan2(-x0*curv + sinf, y0*curv + cosf) + 2*M_PI, 2*M_PI);
+ rdphi = (fmod(par[4] - psi + 3*M_PI, 2*M_PI) - M_PI) / curv;
+ }
+
+/* z of perigee */
+ cotth = 1. / tan(pari[3]);
+ par[2] = pari[2] + cotth * rdphi;
+
+/* theta and 1/R at perigee (unchanged) */
+ par[3] = pari[3];
+ par[5] = curv;
+
+/* Computation of covariance matrix */
+
+/* transformation from (Phi, z, theta, beta, 1/Rtr) */
+/* to (Phi, z, theta, psi, 1/Rtr) */
+ cov[1] = covi[1];
+ cov[2] = covi[2];
+ cov[3] = covi[3];
+ cov[4] = covi[4];
+ cov[5] = covi[5];
+ cov[6] = covi[6];
+ cov[7] = covi[7]+covi[1];
+ cov[8] = covi[8]+covi[2];
+ cov[9] = covi[9]+covi[4];
+ cov[10] = covi[10]+covi[1]+2.*covi[7];
+ cov[11] = covi[11];
+ cov[12] = covi[12];
+ cov[13] = covi[13];
+ cov[14] = covi[14]+covi[11];
+ cov[15] = covi[15];
+
+/* transformation from Phi to r*Phi */
+
+ for (i__ = 1; i__ <= 5; ++i__) {
+ j = i__ * (i__ - 1) / 2 + 1;
+ cov[j] *= *r__;
+ }
+ cov[1] *= *r__;
+ if (pari[5] == 0.) {
+ for(i__=11; i__<15; i__++) cov[i__]=0.;
+ cov[15] = 1.;
+ der15 = 0.;
+ der45 = 0.;
+ } else {
+
+/* transformation from (r*Phi, psi, z0, theta, 1/Rtr) */
+/* to (epsilon, zp, theta, phip, 1/Rtr) */
+
+/* approximation for derivatives d(epsilon)/d(1/Rtr), d(phip)/d(1/Rtr),*/
+/* d(epsilon)/d(r*Phi), d(epsilon)/d(psi), and d(zp)/d(theta), */
+/* the others are exactly or approximately 1 (diagonal terms) */
+/* or 0 (non-diagonal terms). */
+ der15 = -(rdphi * rdphi) / 2.;
+ der45 = rdphi;
+ }
+
+ der11 = rdphi / sqrt(x0 * x0 + y0 * y0 + 1e-20);
+ der14 = -rdphi;
+ der23 = -(cotth * cotth + 1.) * rdphi;
+
+ cov[1] = der11 * der11 * cov[1]
+ + der11 * 2. * (der14 * cov[7] + der15 * cov[11])
+ + der14 * (der14 * cov[10] + der15 * 2. * cov[14])
+ + der15 * der15 * cov[15];
+ cov[2] = der11 * (cov[2] + der23 * cov[4])
+ + der14 * (cov[8] + der23 * cov[9])
+ + der15 * (cov[12] + der23 * cov[13]);
+ cov[3] = cov[3] + der23 * 2. * cov[5] + der23 * der23 * cov[6];
+ cov[4] = der11 * cov[4] + der14 * cov[9] + der15 * cov[13];
+ cov[5] += der23 * cov[6];
+ cov[7] = der11 * (cov[7] + der45 * cov[11])
+ + der14 * (cov[10] + der45 * cov[14])
+ + der15 * (cov[14] + der45 * cov[15]);
+ cov[8] = cov[8] + der23 * cov[9]
+ + der45 * (cov[12] + der23 * cov[13]);
+ cov[9] += der45 * cov[13];
+ cov[10] = cov[10] + der45 * 2. * cov[14] + der45 * der45 * cov[15];
+ cov[11] = der11 * cov[11] + der14 * cov[14] + der15 * cov[15];
+ cov[12] += der23 * cov[13];
+ cov[14] += der45 * cov[15];
+
+/* transformation from theta to ctg(theta)
+
+ tem = -1. - cotth * cotth;
+ cov[4] *= tem;
+ cov[5] *= tem;
+ cov[6] *= tem*tem;
+ cov[9] *= tem;
+ cov[13] *= tem;
+
+ invert matrix COV to get WGT
+*/
+
+ // tmxinv_(&cov[1], &wgt[1], ierr);
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::matrix_inverter(double *wgt, double *covm, long *ierr)
+{
+ double t11, s12, s13, s14, s15, t22, s23, s24, s25, t33, s34, s35,
+ t44, s45, t55, t45, t34, t35, t23, t24, t25, t12, t13, t14, t15;
+
+/************************************************************************/
+/* Inversion of a (5x5) symmetric positive matrix (Cholesky method) */
+/* Internal variables are in double precision */
+/* Check on positivity (IERR=2 if WGT not positive) */
+/* COVM (output) may overwrite WGT (input) */
+/************************************************************************/
+
+ /* Parameter adjustments */
+ --covm;
+ --wgt;
+
+ /* Function Body */
+ *ierr = 0;
+
+ if (wgt[1] <= 0.) {
+ *ierr = 2;
+ return;
+ }
+ t11 = 1. / sqrt(wgt[1]);
+ s12 = wgt[2] * t11;
+ s13 = wgt[4] * t11;
+ s14 = wgt[7] * t11;
+ s15 = wgt[11] * t11;
+
+ t22 = wgt[3] - s12 * s12;
+ if (t22 <= 0.) {
+ *ierr = 2;
+ return;
+ }
+ t22 = 1. / sqrt(t22);
+ s23 = (wgt[5] - s12 * s13) * t22;
+ s24 = (wgt[8] - s12 * s14) * t22;
+ s25 = (wgt[12] - s12 * s15) * t22;
+
+ t33 = wgt[6] - s13 * s13 - s23 * s23;
+ if (t33 <= 0.) {
+ *ierr = 2;
+ return;
+ }
+ t33 = 1. / sqrt(t33);
+ s34 = (wgt[9] - s13 * s14 - s23 * s24) * t33;
+ s35 = (wgt[13] - s13 * s15 - s23 * s25) * t33;
+
+ t44 = wgt[10] - s14 * s14 - s24 * s24 - s34 * s34;
+ if (t44 <= 0.) {
+ *ierr = 2;
+ return;
+ }
+ t44 = 1. / sqrt(t44);
+ s45 = (wgt[14] - s14 * s15 - s24 * s25 - s34 * s35) * t44;
+
+ t55 = wgt[15] - s15 * s15 - s25 * s25 - s35 * s35 - s45 * s45;
+ if (t55 <= 0.) {
+ *ierr = 2;
+ return;
+ }
+ t55 = 1. / sqrt(t55);
+
+ t45 = -t44 * (s45 * t55);
+ t34 = -t33 * (s34 * t44);
+ t35 = -t33 * (s34 * t45 + s35 * t55);
+ t23 = -t22 * (s23 * t33);
+ t24 = -t22 * (s23 * t34 + s24 * t44);
+ t25 = -t22 * (s23 * t35 + s24 * t45 + s25 * t55);
+ t12 = -t11 * (s12 * t22);
+ t13 = -t11 * (s12 * t23 + s13 * t33);
+ t14 = -t11 * (s12 * t24 + s13 * t34 + s14 * t44);
+ t15 = -t11 * (s12 * t25 + s13 * t35 + s14 * t45 + s15 * t55);
+
+ covm[1] = t11 * t11 + t12 * t12 + t13 * t13 + t14 * t14 + t15 * t15;
+ covm[2] = t12 * t22 + t13 * t23 + t14 * t24 + t15 * t25;
+ covm[3] = t22 * t22 + t23 * t23 + t24 * t24 + t25 * t25;
+ covm[4] = t13 * t33 + t14 * t34 + t15 * t35;
+ covm[5] = t23 * t33 + t24 * t34 + t25 * t35;
+ covm[6] = t33 * t33 + t34 * t34 + t35 * t35;
+ covm[7] = t14 * t44 + t15 * t45;
+ covm[8] = t24 * t44 + t25 * t45;
+ covm[9] = t34 * t44 + t35 * t45;
+ covm[10] = t44 * t44 + t45 * t45;
+ covm[11] = t15 * t55;
+ covm[12] = t25 * t55;
+ covm[13] = t35 * t55;
+ covm[14] = t45 * t55;
+ covm[15] = t55 * t55;
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::tkffit(long *np, double *rcoord, double *phcoord, double *zcoord,
+ double *errxy, double *errz, double *errr, long *ihtype, double *rf,
+ double *paraf, double *em1, double *chi2, long *ihdrop, long *ierr,
+ bool electron)
+{
+ /* System generated locals */
+ long i__1;
+ double r__1;
+
+ /* Local variables */
+ double pari[5], sinb, cons, sigr[3], sigz[3], sint, achi2;
+ long i__;
+ double d1, d2;
+ long i3[3];
+ double sigxy[3], r3, x0;
+ long ih;
+ double dp[5], parabb[6], parabe[6], w11, w22;
+ double wm[15], alrphi;
+ double wm1[15];
+ double fco[3], der[8], dir;
+ long ier;
+ double phi, rco[3], zco[3];
+ double c_b7=-9999., c_b8=9999., c_b9=.9999, c_b20=0.;
+ long c__0=0, c__1=1, c_n1=-1;
+
+ double fac1=M_PI/180.;
+
+/* -------------------------------- */
+
+/* Main routine of the Kalman-Filtering method for track fitting (at this */
+/* moment for barrel tracker only; the forwards region and barrel/forward*/
+/* mixed region will be integrated in later). Some subroutines are */
+/* learnt from Prof. Billior. */
+
+/* In order to feed the vertex fitting package (i.e. TVSIMP and TVFULL), */
+/* after calling this routine, another 2 routines have to be called */
+/* sequentially: */
+/* (1) TXTOBP: to extrapolate the fitted track into the beam-pipe and */
+/* to take the multiple scattering of beam-pipe into account;*/
+/* and/or, to re-order the track cylindrical-polar parameters from */
+/* (phi,z,theta,beta,1/R) to (phi,psi,z,theta,1/p), if the */
+/* track is generated outside the beam pipe (i.e. it does */
+/* not intercept with the beam pipe); */
+/* (2) TCLPRG: to convert the cylindrical-polar parameters into the */
+/* perigee parameters. */
+/* -Sijin QIAN, 3/4/1994*/
+
+/*These 2 steps have been already integrated into a vertex fitting steering*/
+/* routine, i.e. TVSTEE; so now the fitted vertex and re-fitted tracks can*/
+/* be obtained by calling TVSTEE right after this TKFFIT. */
+
+/* -Sijin QIAN, 22/4/1994*/
+
+/* The merge of the barrel and forward regions is completed. */
+/* -Sijin QIAN, 7/5/1994*/
+/* ------------------------------------------- */
+
+/*Modification for the OO tracker model: ignor the multiple scattering of the*/
+/* material in the tracker temporarily.*/
+/* -Sijin QIAN, 29/6/1997*/
+/* ------------------------------------------- */
+
+/* Modification for the Atlas OO TRT tracker: */
+/* (1) increase the max. points on track to 73; */
+/* (2) activate the multiple scattering of material in the tracker, and the*/
+/* material on each hit now depends on the number of hits. */
+/* -Sijin QIAN, 30/11/1998*/
+/* ------------------------------------------- */
+/* Input: np -- Number of points on the track (max. 73, min. 3) */
+/* rcoord(1:73) -- r coordinates of points */
+/* (ordered from small to large) (cm) */
+/* phcoord(1:73) -- phi coord. of points (radians) */
+/* zcoord(1:73) -- z coord. of points (cm) */
+/* errxy(1:73) -- error on r-phi (cm) */
+/* errz(1:73) -- error on z (cm) */
+/* errr(1:73) -- error on r (cm) */
+/* ihtype(1:73) -- type of the hit (0 for barrel, 1 for forward) */
+
+/*Output: rf -- radial location of the fitted track parameters (cm)*/
+/* paraf(1:5) -- fitted track parameters (phi,z,theta,beta,1/R) */
+/* em1(1:15) -- error matrix for the parameters */
+/* (now it's a weight matrix for Atlas, Dec.1,1998)*/
+/* chi2 -- Chi2 of the fit */
+/* ihdrop(1:73) -- index for the rejected hit (1 for yes, 0 for no)*/
+/* ierr -- error message, =1 np<=3 or np>73 */
+/* =2 error in TFIT3P */
+/* =3 error in TVTOXX or TXXTOV */
+/* =4 error in TSOLVE */
+/* -------------------------------- */
+
+ /* Parameter adjustments */
+ --ihdrop;
+ --em1;
+ --paraf;
+ --ihtype;
+ --errr;
+ --errz;
+ --errxy;
+ --zcoord;
+ --phcoord;
+ --rcoord;
+
+ /* Function Body */
+ *ierr = 0;
+ if (*np < 3 || *np > 73) {
+ *ierr = 1;
+ return;
+ }
+ //cons = 83.3;
+ cons = 166.6;
+ for (i__ = 1; i__ <= 73; ++i__) ihdrop[i__] = 0;
+
+ x0 = .16 / double(*np);
+
+/* determine 3 initial points */
+
+ i3[0] = *np;
+ if(electron) {
+ i3[1] = *np - 1;
+ i3[2] = *np - 2;
+ } else {
+ i3[1] = (*np + 2) / 2;
+ i3[2] = 1;
+ }
+
+/* start the track from the outmost layer and extrapolate it from point */
+/* to point. */
+
+ for (ih = 0; ih < 3; ++ih) {
+ rco[ih] = rcoord[i3[ih]];
+ fco[ih] = fmod(phcoord[i3[ih]] + 2*M_PI, 2*M_PI) / fac1;
+ zco[ih] = zcoord[i3[ih]];
+ sigxy[ih] = errxy[i3[ih]];
+ sigz[ih] = errz[i3[ih]];
+ sigr[ih] = errr[i3[ih]];
+ }
+
+ // tfit3p_(rco, fco, zco, sigxy, sigz, sigr, &r3, pari, wm, chi2, &ier);
+
+ tfit3p(&rco[0], &fco[0], &zco[0], &sigxy[0], &sigz[0], &sigr[0], &r3, &pari[0], &wm[0], chi2, &ier);
+ if (ier != 0) {
+ *ierr = 2;
+ return;
+ }
+ pari[0] *= fac1;
+
+ dir = 1.;
+ // tvtoxx_(pari, wm, ¶bb[1], wm1);
+ tvtoxx(&pari[0], &wm[0], ¶bb[1], &wm1[0]);
+ for(ih=0;ih<15;ih++) wm[ih]=wm1[ih];
+
+ parabb[0] = r3;
+ // tswtch(&c__0, parabb, wm);
+ if (ihtype[*np] == 1) tswtch(&c__0, ¶bb[0], &wm[0]);
+
+ for (ih = *np; ih >= 1; --ih) {
+ if (ihtype[ih] == 0) {
+ /* txtrpa_(parabb, &c__0, &rcoord[ih], &c_b7, &c_b8, &c_b9, &c__0,
+ parabe, der, &alrphi, &ier);
+ */
+ txtrpa(¶bb[0], &c__0, &rcoord[ih], &c_b7, &c_b8, &c_b9, &c__0,
+ ¶be[0], &der[0], &alrphi, &ier);
+ if (ier != 0) {
+ goto L11;
+ }
+ txxdcy(¶be[0], ¶bb[0], &der[0]);
+ txprcw(&wm[0], &der[0], &wm[0]);
+
+/* Add m/s into error matrix, then convert it to weight matrix. (Barrel) */
+
+ } else {
+ /*
+ txxpla_(parabb, &c__0, &zcoord[ih], &c_b20, &c_b8, &c__0, parabe,
+ der, &alrphi, &ier);
+ */
+ txxpla(¶bb[0], &c__0, &zcoord[ih], &c_b20, &c_b8, &c__0, ¶be[0],
+ &der[0], &alrphi, &ier);
+ if (ier != 0) {
+ goto L11;
+ }
+ txxdpl(¶be[0], ¶bb[0], &der[0]);
+ txprpw(&wm[0], &der[0], &wm[0]);
+
+/* Add m/s into error matrix, then convert it to weight matrix. (Forward) */
+
+ }
+ sint = sin(parabe[3]);
+ r__1 = parabe[5] * cons * sint;
+ // tmscat_(&sint, &r__1, &x0, wm, &dir);
+ tmscat(&sint, &r__1, &x0, &wm[0], &dir);
+ if(electron==true) add_E_loss(&x0, &dir, ¶bb[5], &wm[0]);
+
+ if (ih == i3[0] || ih == i3[1] || ih == i3[2]) {
+ for(long ii=0; ii<6; ii++) parabb[ii]=parabe[ii];
+ goto L11;
+ }
+
+ parabb[0] = parabe[0];
+ if (ihtype[ih] == 0) {
+/* Computing 2nd power */
+ r__1 = rcoord[ih] / errxy[ih];
+ w11 = r__1 * r__1;
+ d1 = fmod(phcoord[ih] - parabe[1] + 2*M_PI + M_PI, 2*M_PI) - M_PI;
+ w22 = 1. / (errz[ih] * errz[ih]);
+ d2 = zcoord[ih] - parabe[2];
+ } else {
+ r3 = sqrt(parabe[1] * parabe[1] + parabe[2] * parabe[2]);
+ phi = fmod(atan2(parabe[2], parabe[1])+2*M_PI,2*M_PI);
+ // trf2xy_(&c_n1, &r3, &phi, wm);
+ trf2xy(&c_n1, &r3, &phi, &wm[0]);
+ w11 = 1. / (errr[ih] * errr[ih]);
+ d1 = rcoord[ih] - r3;
+/* Computing 2nd power */
+ r__1 = rcoord[ih] / errxy[ih];
+ w22 = r__1 * r__1;
+ d2 = fmod(phcoord[ih] - phi + 2*M_PI + M_PI, 2*M_PI) - M_PI;
+ }
+
+/* Add a point to the track, update the parameter and weight matrix. */
+
+ taddpt(&d1, &d2, &w11, &w22, &dp[0], &wm[0], &achi2, &ier);
+
+ if (ier != 0) {
+ ihdrop[ih] = 1;
+ for(long ii=1; ii<6; ii++) parabb[ii]=parabe[ii];
+ } else {
+ if (ihtype[ih] == 0) {
+ for(long ii=1; ii<6; ii++) parabb[ii]=parabe[ii]+dp[ii-1];
+ sinb = fabs(sin(parabb[4]));
+ } else {
+ r3 += dp[0];
+ phi += dp[1];
+ parabb[1] = r3 * cos(phi);
+ parabb[2] = r3 * sin(phi);
+ for(long ii=3; ii<6; ii++) parabb[ii]=parabe[ii]+dp[ii-1];
+ sinb = fabs(sin(parabb[4] - phi));
+ trf2xy(&c__1, &r3, &phi, &wm[0]);
+ }
+
+ if (sinb >= 1.) {
+ *ierr = 2;
+ return;
+ }
+ *chi2 += achi2;
+/* parabb(6)=1./(1./parabb(6)-SIGN(8.e-2,PARABb(6))) */
+ }
+L11:
+ if (ih==1) goto L10;
+
+ if ((i__1 = ihtype[ih] - ihtype[ih - 1]) < 0) {
+ goto L311;
+ } else if (i__1 == 0) {
+ goto L312;
+ } else {
+ goto L313;
+ }
+L311:
+ tswtch(&c__0, ¶bb[0], &wm[0]);
+L312:
+ goto L10;
+L313:
+ tswtch(&c__1, ¶bb[0], &wm[0]);
+L10:
+ ;
+ }
+
+ if (ihtype[1] == 1) {
+ tswtch(&c__1, ¶bb[0], &wm[0]);
+ }
+ for(long ii=1; ii<6; ii++) paraf[ii]=parabb[ii];
+ *rf = parabb[0];
+ for(long ij=1; ij<16; ij++) em1[ij]=wm[ij-1];
+/* CALL TMXINV(WM,EM1,IERR) */
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::tfit3p(double *rco, double *fco, double *zco, double *sigxy,
+ double *sigz, double *sigr, double *r3, double *para,
+ double *wgc, double *chi2c, long *ierr)
+{
+ double der13, der23, der24, der15, der25, cosa, d21sq, fmax, d23sq;
+ double sina, sinb, sdet, tanl, tnsb, rotf, rinv, rtrk, alph3;
+ double alph[2], sinb2, r1, r2, r__3, sinb22[2];
+
+ long i;
+ double x[3], y[3], z[3];
+
+ double z0, sumwz, d23, ds[2], xc, yc, rr[3], sumwsz, fac;
+ double dx21, dy21, dx23, dy23;
+ double wff[3], tem, rfz[9] /* was [3][3] */, wzz[3];
+ double d_r = M_PI/180.;
+
+/* ---------------------------------------------------------- */
+
+/* Subroutine for fitting 3 points to a helix */
+/*
+ error code: ierr=1 --- Track radius too small ( < 0.5 GeV, i.e. 83 cm)
+ =2 --- sin(beta) >=1
+ =3 --- two hits have the same (x,y)
+*/
+
+/* ---------------------------------------------------------- */
+
+ /* Parameter adjustments */
+ --wgc;
+ --para;
+ --sigr;
+ --sigz;
+ --sigxy;
+ --zco;
+ --fco;
+ --rco;
+
+ /* Function Body */
+ for (i = 1; i <= 3; ++i) {
+ rfz[i * 3 - 3] = rco[i];
+ rfz[i * 3 - 2] = fco[i];
+ rfz[i * 3 - 1] = zco[i];
+ }
+
+/* Rotate 3 points to 0-90 degrees region */
+
+ fmax = std::max(std::max(rfz[1], rfz[4]),rfz[7]);
+ if (fmax <= 90.) {
+ rotf = 0.;
+ goto L77;
+ }
+
+ if (fmax >= 270. && std::min(std::min(rfz[1],rfz[4]),rfz[7]) <= 90.) {
+ rotf = 10. - fmax;
+ goto L77;
+ }
+ rotf = 80. - fmax;
+L77:
+
+/* The X and Y coords. of 3 points */
+
+ for (i = 1; i <= 3; ++i) {
+ x[i - 1] = rfz[i * 3 - 3] * cos((rfz[i * 3 - 2] + rotf) * d_r);
+ y[i - 1] = rfz[i * 3 - 3] * sin((rfz[i * 3 - 2] + rotf) * d_r);
+ z[i - 1] = rfz[i * 3 - 1];
+ rr[i - 1] = rfz[i * 3 - 3];
+ }
+
+/* To calculate the parameters (in XY projection) at the point 3 */
+
+ dy21 = y[1] - y[0];
+ dx21 = x[1] - x[0];
+ d21sq = dx21 * dx21 + dy21 * dy21;
+ dy23 = y[1] - y[2];
+ dx23 = x[1] - x[2];
+ d23sq = dx23 * dx23 + dy23 * dy23;
+ if (d21sq==0 || d23sq==0) {*ierr=3; return;}
+ d23 = sqrt(d23sq);
+
+ tem = dx21 * dy23 - dx23 * dy21;
+ if (tem == 0.) {
+ rtrk = 15000.;
+ xc=yc=sdet=0.;
+ } else {
+ sdet = .5 / tem;
+ xc = x[1] + sdet * (d23sq * dy21 - d21sq * dy23);
+ yc = y[1] + sdet * (d21sq * dx23 - d23sq * dx21);
+
+ r1 = xc - x[1];
+ r2 = yc - y[1];
+ rtrk = sqrt(r1 * r1 + r2 * r2);
+ }
+
+/* If the radius of helix .LT. 10 m: exact solution */
+
+ // if(rtrk<1000.) {
+
+/* To reject the helix with radius .LT. the RMIN */
+
+/* For Pmin=1GeV */
+/* IF(RTRK.LT.72.) THEN */
+/* For Pmin=0.5GeV */
+ // if (rtrk < 17.) {
+ if (rtrk < minTrackRadius[paramset]) {
+ *ierr = 1;
+ return;
+ }
+
+/* About the sign of the curvature */
+
+ if (sdet < 0.) rtrk *= -1.;
+ rinv = 1. / rtrk;
+
+/* Angles at the point 3 and curvature distances of 3-2 and 3-1 */
+
+ if(tem!=0) {
+
+ sina = (x[2] - xc) * rinv;
+ cosa = (yc - y[2]) * rinv;
+ alph3 = atan2(sina, cosa);
+ alph[1] = atan2((x[1] - xc) * rinv, (yc - y[1]) * rinv);
+ alph[0] = atan2((x[0] - xc) * rinv, (yc - y[0]) * rinv);
+ ds[1] = rtrk * (alph[1] - alph3);
+ ds[0] = rtrk * (alph[0] - alph3);
+ for(i=0; i<2; i++) {
+ sinb = (sin(alph[i]) * x[i] - cos(alph[i]) * y[i]) / rr[i];
+ if (fabs(sinb) >= 1.) {
+ *ierr = 2;
+ return;
+ }
+ sinb22[i]=sinb*sinb;
+ }
+ }
+
+/* If the radius of helix .GT. 10 m: approximation to the 1st order of 1/r */
+
+ else {
+
+ double rrr = 2.*(y[1]-(y[2]*dx21-y[0]*dx23)/(dx21-dx23))/(dx23*dx21);
+ double tem1=dx23/d23;
+ tem1 = tem1*tem1*tem1*rrr;
+ sina = dy23/d23 - fabs(.5*d23*tem1)*(dy23>0. ? 1. : -1.);
+ if(fabs(sina)>=1.) {
+ *ierr = 1;
+ return;
+ }
+ cosa = sqrt(1.-sina*sina)*(dx23>0. ? 1. : -1.);
+ ds[1] = d23;
+ ds[0] = sqrt(d21sq)+d23;
+ }
+
+/* Parameters phi and sin(beta) at point 3 */
+
+ sinb = (sina * x[2] - cosa * y[2]) / rr[2];
+ if (fabs(sinb) >= 1.) {
+ *ierr = 2;
+ return;
+ }
+
+ *r3 = rr[2];
+ para[1] = rfz[7];
+ para[3] = sinb;
+ para[5] = rinv;
+
+/* Calculation of weight on phi, z, sin(beta), cot(theta) and 1/R */
+/* and adjustment on z, cot(theta) */
+
+ tanl = (z[1] - z[2]) / d23;
+ tnsb = sinb * tanl;
+
+ for(i=1;i<16;i++) wgc[i]=0.;
+
+/* Contribution of the point 3 */
+
+ sinb2 = sinb * sinb;
+ wgc[1] = *r3 * *r3 / (sigxy[3] * sigxy[3] +
+ sigr[3] * sigr[3] * sinb2 / (1. - sinb2));
+ r2 = sigr[3] * tanl;
+ wgc[3] = 1. / (sigz[3] * sigz[3] + r2 * r2 / (1. - sinb2));
+ wzz[2] =wgc[3];
+
+ if(tem==0) sinb22[0]=sinb22[1]=sinb2;
+
+ sumwz = wgc[3] * z[2];
+ sumwsz = 0.;
+
+/* Contribution of points 1 and 2 */
+
+ for (i = 1; i < 3; ++i) {
+
+/* Contribution at weight on phi and sin(beta) */
+
+ r1 = rr[i - 1];
+ wff[i- 1] = r1 * r1 / (sigxy[i] * sigxy[i] +
+ sigr[i] * sigr[i] * sinb22[i-1] / (1. - sinb22[i-1]));
+ r2 = sigr[i] * tanl;
+ wzz[i- 1] = 1. / (sigz[i] * sigz[i] + r2 * r2 / (1. - sinb22[i-1]));
+
+ fac = ds[i - 1] / (1. - sinb22[i-1]);
+ der24 = ds[i - 1];
+ der23 = fac * tnsb;
+ der13 = fac / rr[i - 1];
+ der15 = (rr[i - 1] - rr[2]) * .5 * der13;
+ der25 = der15 * tnsb;
+ wgc[1] += wff[i - 1];
+ wgc[4] += wff[i - 1] * der13;
+ wgc[6] += wff[i - 1] * der13 * der13;
+ wgc[11] += wff[i - 1] * der15;
+ wgc[13] += wff[i - 1] * der15 * der13;
+ wgc[15] += wff[i - 1] * der15 * der15;
+
+/* Contribution at weight on z and cot(theta) */
+
+ wgc[3] += wzz[i - 1];
+ wgc[8] += wzz[i - 1] * der24;
+ wgc[10] += wzz[i - 1] * der24 * der24;
+
+ sumwz += z[i - 1] * wzz[i - 1];
+ sumwsz += z[i - 1] * wzz[i - 1] * der24;
+
+/*Contribution of correlations between (phi,sin(beta),1/R) and (z,cot(
+theta))*/
+ wgc[5] += wzz[i - 1] * der23;
+ wgc[6] += wzz[i - 1] * der23 * der23;
+ wgc[9] += wzz[i - 1] * der23 * der24;
+ wgc[12] += wzz[i - 1] * der25;
+ wgc[13] += wzz[i - 1] * der25 * der23;
+ wgc[14] += wzz[i - 1] * der25 * der24;
+ wgc[15] += wzz[i - 1] * der25 * der25;
+ }
+
+/* Resolution of the system equation on z,cot(theta) */
+
+ sdet = wgc[3] * wgc[10] - wgc[8] * wgc[8];
+ if (sdet == 0.) {
+/* print *,' Tfit3p : sdet = 0. ' */
+ sdet = 0.;
+ } else {
+ sdet = 1. / sdet;
+ }
+/* wiw* SDET=1./(WGC(3)*WGC(10)-WGC(8)**2) */
+ z0 = (wgc[10] * sumwz - wgc[8] * sumwsz) * sdet;
+ tanl = (wgc[3] * sumwsz - wgc[8] * sumwz) * sdet;
+
+/* Calculation of chi2 */
+
+ r1 = z[2] - z0;
+ r2 = z[1] - z0 - tanl * ds[1];
+ r__3 = z[0] - z0 - tanl * ds[0];
+ *chi2c = wzz[2] * (r1 * r1) + wzz[1] * (r2 * r2) + wzz[0] * (r__3 * r__3);
+
+ para[2] = z0;
+ para[4] = tanl;
+
+/* print *,'chi2 in fit3=',chi2c */
+ *ierr = 0;
+
+ return;
+
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::tvtoxx(double *pari, double *wctr, double *par, double *wf)
+{
+ double r1,cf;
+
+/* ----------------------------------------------- */
+/* The routine for transformation from internal parameters to */
+/* cylindrical-polar parameters. */
+/* -By Sijin QIAN, 3/1994 */
+
+/* Input: pari --- internal track parameters (phi,Z,sinb,ctg(theta),1/Rtr)
+ */
+/* WCTR --- weight matrix of pari, packed in lower triangle. */
+
+/* Output: PAR(5) --- Cylindrical-polar track parameters */
+/* (Phi,Z,Theta,beta,1/Rtr) */
+/* wf(15) --- Weight matrix corresponding to PAR, */
+/* packed in lower triangle. */
+/* ----------------------------------------------- */
+
+/* Transformation for WEIGHT Matrix from (sin(beta),ctg(theta)) */
+/* to (theta, sin(beta)) */
+
+ /* Parameter adjustments */
+ --wf;
+ --par;
+ --wctr;
+ --pari;
+
+ // for(int i=0; i<5; i++) { par[i]=pari[i]; }
+ // for(int i=0; i<15; i++){ wf[i]=wctr[i]; }
+ par[2]=pari[2];
+ par[5]=pari[5];
+ for(long i=1; i<4; i++){ wf[i]=wctr[i]; }
+ wf[11]=wctr[11];
+ wf[12]=wctr[12];
+ wf[15]=wctr[15];
+
+ wf[ 4] = wctr[ 7];
+ wf[ 7] = wctr[ 4];
+ wf[ 5] = wctr[ 8];
+ wf[ 8] = wctr[ 5];
+ wf[ 6] = wctr[10];
+ wf[10] = wctr[ 6];
+ wf[13] = wctr[14];
+ wf[14] = wctr[13];
+
+ // had to modify the following to compile on HP-Unix
+ if (pari[4]==0.) {
+ par[3] = 0.5*M_PI;
+ } else {
+ double at = atan(1./pari[4]);
+ par[3] = fmod(at+M_PI, M_PI);
+ }
+ //par[3] = pari[4]==0. ? 0.5*pi : fmod(at+pi, pi);
+
+ r1 = sin(par[3]);
+ cf = -1. / (r1 * r1);
+
+ // vscale(&wf[4], &cf, &wf[4], &c3);
+ wf[4] = cf * wf[4];
+ wf[5] = cf * wf[5];
+ wf[6] = cf * wf[6] * cf;
+ wf[9] = cf * wctr[9];
+ wf[13] = cf * wf[13];
+
+/* Transformation from sin(beta) to beta */
+
+ par[1] = fmod(pari[1]+2*M_PI, 2*M_PI);
+ par[4] = asin(pari[3]);
+ cf = cos(par[4]);
+ // vscale(&wf[7], &cf, &wf[7], &c4);
+ wf[ 7] = cf * wf[ 7];
+ wf[ 8] = cf * wf[ 8];
+ wf[ 9] = cf * wf[ 9];
+ wf[10] = cf * wf[10] * cf;
+ wf[14] = cf * wf[14];
+
+ return ;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::txprcw(double *wgi, double *der, double *wgf)
+{
+ double w31, w41, w51, w52, w53, w54, w55, w44, w43, w42, w33, w32;
+/* *
+*/
+/* propagation of weight matrix from a cylinder to another one *
+*/
+/* Input : WGI : initial weight matrix *
+*/
+/* DER : inverse derivatives *
+*/
+/* Output : WGF : final weight matrix (may overwrite WGI) *
+*/
+/* ***********************************************************************
+*/
+/* computation of A = DER transp * WG */
+
+ /* Parameter adjustments */
+ --wgf;
+ --der;
+ --wgi;
+
+ /* Function Body */
+ w55 = wgi[11] * der[3] + wgi[12] * der[6] + wgi[14] * der[8] + wgi[15];
+ w54 = wgi[7] * der[3] + wgi[8] * der[6] + wgi[10] * der[8] + wgi[14];
+ w53 = wgi[4] * der[3] + wgi[5] * der[6] + wgi[9] * der[8] + wgi[13];
+ w52 = wgi[2] * der[3] + wgi[3] * der[6] + wgi[8] * der[8] + wgi[12];
+ w51 = wgi[1] * der[3] + wgi[2] * der[6] + wgi[7] * der[8] + wgi[11];
+
+ w44 = wgi[7] * der[2] + wgi[8] * der[5] + wgi[10] * der[7];
+ w43 = wgi[4] * der[2] + wgi[5] * der[5] + wgi[9] * der[7];
+ w42 = wgi[2] * der[2] + wgi[3] * der[5] + wgi[8] * der[7];
+ w41 = wgi[1] * der[2] + wgi[2] * der[5] + wgi[7] * der[7];
+
+ w33 = wgi[4] * der[1] + wgi[5] * der[4] + wgi[6];
+ w32 = wgi[2] * der[1] + wgi[3] * der[4] + wgi[5];
+ w31 = wgi[1] * der[1] + wgi[2] * der[4] + wgi[4];
+
+/* computation of A * DER */
+
+ wgf[1] = wgi[1];
+ wgf[2] = wgi[2];
+ wgf[3] = wgi[3];
+
+ wgf[4] = w31;
+ wgf[5] = w32;
+ wgf[6] = w31 * der[1] + w32 * der[4] + w33;
+
+ wgf[7] = w41;
+ wgf[8] = w42;
+ wgf[9] = w41 * der[1] + w42 * der[4] + w43;
+ wgf[10] = w41 * der[2] + w42 * der[5] + w44 * der[7];
+
+ wgf[11] = w51;
+ wgf[12] = w52;
+ wgf[13] = w51 * der[1] + w52 * der[4] + w53;
+ wgf[14] = w51 * der[2] + w52 * der[5] + w54 * der[7];
+ wgf[15] = w51 * der[3] + w52 * der[6] + w54 * der[8] + w55;
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::txprpw(double *wgi, double *der, double *wgf)
+{
+ double w31, w41, w51, w52, w53, w54, w55, w44, w43, w42, w34, w33, w32;
+
+/* propagation of weight matrix from a plane to another one *
+*/
+/* Input : WGI : initial weight matrix *
+*/
+/* DER : inverse derivatives *
+*/
+/* Output : WGF : final weight matrix (may overwrite WGI) *
+*/
+/* ***********************************************************************
+ */
+/* propagation of weight matrix from a plane to a plane */
+
+/* computation of W = DER transp * WG */
+
+ /* Parameter adjustments */
+ --wgf;
+ --der;
+ --wgi;
+
+ /* Function Body */
+ w55 = wgi[11] * der[3] + wgi[12] * der[6] + wgi[14] * der[8] + wgi[15];
+ w54 = wgi[7] * der[3] + wgi[8] * der[6] + wgi[10] * der[8] + wgi[14];
+ w53 = wgi[4] * der[3] + wgi[5] * der[6] + wgi[9] * der[8] + wgi[13];
+ w52 = wgi[2] * der[3] + wgi[3] * der[6] + wgi[8] * der[8] + wgi[12];
+ w51 = wgi[1] * der[3] + wgi[2] * der[6] + wgi[7] * der[8] + wgi[11];
+
+ w44 = wgi[7] * der[2] + wgi[8] * der[5] + wgi[10];
+ w43 = wgi[4] * der[2] + wgi[5] * der[5] + wgi[9];
+ w42 = wgi[2] * der[2] + wgi[3] * der[5] + wgi[8];
+ w41 = wgi[1] * der[2] + wgi[2] * der[5] + wgi[7];
+
+ w34 = wgi[7] * der[1] + wgi[8] * der[4] + wgi[9] + wgi[10] * der[7];
+ w33 = wgi[4] * der[1] + wgi[5] * der[4] + wgi[6] + wgi[9] * der[7];
+ w32 = wgi[2] * der[1] + wgi[3] * der[4] + wgi[5] + wgi[8] * der[7];
+ w31 = wgi[1] * der[1] + wgi[2] * der[4] + wgi[4] + wgi[7] * der[7];
+
+/* computation of W * DER */
+
+ wgf[1] = wgi[1];
+ wgf[2] = wgi[2];
+ wgf[3] = wgi[3];
+
+ wgf[4] = w31;
+ wgf[5] = w32;
+ wgf[6] = w31 * der[1] + w32 * der[4] + w33 + w34 * der[7];
+
+ wgf[7] = w41;
+ wgf[8] = w42;
+ wgf[9] = w41 * der[1] + w42 * der[4] + w43 + w44 * der[7];
+ wgf[10] = w41 * der[2] + w42 * der[5] + w44;
+
+ wgf[11] = w51;
+ wgf[12] = w52;
+ wgf[13] = w51 * der[1] + w52 * der[4] + w53 + w54 * der[7];
+ wgf[14] = w51 * der[2] + w52 * der[5] + w54;
+ wgf[15] = w51 * der[3] + w52 * der[6] + w54 * der[8] + w55;
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+
+/* *********************************************************************** */
+/* * */
+/* compute derivatives for a propagation cylinder -> cylinder *
+*/
+/* knowing the initial and final parameters *
+*/
+/* see routine TXTRPA for the definition of PARAMI,PARAMF,DER *
+*/
+/* and comments *
+*/
+/* ***********************************************************************
+ */
+
+void TrigInDetSctKFitter::txxdcy(double *parami, double *paramf, double *der)
+{
+/* Local variables */
+ double radf, fact, radi, dphi, cosb, sinb, rtrk, cdphi, cosbf,
+ ccpsf, sinbf, ccpsi, cpsif, rdphi, cpsii, sdphi, cotth,
+ scpsi, spsif, spsii, xc, yc, rc2;
+
+ double dphimn=0.01;
+
+/* radius of the helix, cot(theta) and sin(beta),cos(beta) at both ends
+*/
+ /* Parameter adjustments */
+ --der;
+ --paramf;
+ --parami;
+
+ /* Function Body */
+ radi = parami[1];
+ radf = paramf[1];
+ rtrk = 1. / parami[6];
+ cotth = 1. / tan(parami[4]);
+ sinb = sin(parami[5]);
+ cosb = cos(parami[5]);
+ sinbf = sin(paramf[5]);
+ cosbf = cos(paramf[5]);
+
+/* variation DPHI of phi angle */
+
+ dphi = paramf[2] + paramf[5] - parami[2] - parami[5];
+ dphi = fmod(dphi + 2. * 2*M_PI + M_PI, 2*M_PI) - M_PI;
+
+/* if |DPHI| > DPHIMN , use "exact" formulae */
+
+ if (fabs(dphi) >= dphimn) {
+ xc = radi - rtrk * sinb;
+ yc = rtrk * cosb;
+
+ rc2 = xc * xc + yc * yc;
+
+ ccpsi = radi - rtrk * sinb;
+ scpsi = rtrk * cosb;
+ ccpsf = radf - rtrk * sinbf;
+ // scpsf = rtrk * cosbf;
+
+ cpsii = rtrk - radi * sinb;
+ spsii = -radi * cosb;
+ cpsif = rtrk - radf * sinbf;
+ spsif = -radf * cosbf;
+
+ sdphi = sin(dphi);
+ cdphi = cos(dphi);
+
+ der[1] = 0.;
+ fact = -rtrk / spsif;
+ der[2] = sdphi * fact;
+ der[3] = fact * rtrk * (1. - cdphi);
+ der[4] = -rtrk * dphi * (1. + cotth * cotth);
+ der[5] = rtrk * cotth * (radf * ccpsf * spsii / spsif - radi * ccpsi)
+ / rc2;
+ der[6] = rtrk * rtrk * cotth * (-dphi + sinbf / cosbf - (radi * scpsi
+ + radf * ccpsf * cpsii / spsif) / rc2);
+ der[7] = spsii / spsif;
+ der[8] = rtrk * (cpsif - cpsii) / spsif;
+ }
+
+/* if |DPHI| < DPHIMN , use first order in 1/R */
+ else {
+ rdphi = rtrk * dphi;
+ der[1] = 0.;
+ der[2] = rdphi / (radf * cosbf);
+ der[3] = rdphi * .5 * der[2];
+ der[4] = -rdphi * (cotth * cotth + 1.);
+ der[5] = radi * cotth * sin(paramf[2] - parami[2]) / cosbf;
+ der[6] = rdphi * .5 * der[5];
+ der[7] = radi * cosb / (radf * cosbf);
+ der[8] = rdphi * .5 * (der[7] + 1.);
+ }
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+/* ******************************************************************** *
+*/
+/* compute derivatives for a propagation plane -> plane *
+*/
+/* knowing the initial and final parameters *
+*/
+/* see routine TXXPLA for the definition of PARAMI,PARAMF,DER *
+*/
+/* and comments *
+*/
+/* ***********************************************************************
+ */
+
+void TrigInDetSctKFitter::txxdpl(double *parami, double *paramf, double *der)
+{
+
+ /* Local variables */
+ double dphi, rtrk, cosf0, cosf1, sinf0, sinf1, dcosf, dsinf,
+ rdphi, tanth, dz;
+
+ /* Parameter adjustments */
+ --der;
+ --paramf;
+ --parami;
+
+ /* Function Body */
+ cosf0 = cos(parami[5]);
+ cosf1 = cos(paramf[5]);
+ sinf0 = sin(parami[5]);
+ sinf1 = sin(paramf[5]);
+ tanth = tan(parami[4]);
+ dz = paramf[1] - parami[1];
+ rdphi = tanth * dz;
+
+ double tem = (tanth * tanth + 1.) * dz;
+ der[1] = tem * cosf1;
+ der[4] = tem * sinf1;
+ der[7] = tem * parami[6];
+ der[8] = rdphi;
+
+ dphi = rdphi * parami[6];
+
+/* "exact" formulae if |DPHI| > DPHIMN */
+
+ if (fabs(dphi) >= .01) {
+ rtrk = 1. / parami[6];
+ dcosf = cosf1 - cosf0;
+ dsinf = sinf1 - sinf0;
+ der[2] = rtrk * dcosf;
+ tem = rtrk *rtrk;
+ der[3] = tem * (dphi * cosf1 - dsinf);
+ der[5] = rtrk * dsinf;
+ der[6] = tem * (dphi * sinf1 + dcosf);
+
+/* first order in 1/R if |DPHI| < DPHIMN */
+ } else {
+ der[2] = -rdphi * sinf0;
+ der[3] = rdphi * .5 * der[2];
+ der[5] = rdphi * cosf0;
+ der[6] = rdphi * .5 * der[5];
+ }
+
+ return ;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+/* **********************************************************************
+*/
+/* change from "plane" parameters to "barrel" parameters *
+*/
+/* at a given point (without propagation), or conversely, *
+*/
+/* and tranform the deviations from ref. traj. and the weight matrix *
+*/
+/* Input : ITYP : type to be changed ('PLAN'=1 or 'CYLI'=0) *
+*/
+/* PAR(1-6) : plane parameters *
+*/
+/* z(fixed),x,y,theta,phi,1/R *
+*/
+/* or cylinder parameters *
+*/
+/* r(fixed),Phi,z,theta,beta=phi-Phi,1/R *
+*/
+/** DQ(1-5) : deviations from reference trajectory
+**/
+/** on x,y,theta,phi,1/R (plane)
+**/
+/** or Phi,z,theta,beta,1/R (cylinder)
+**/
+/* WG(1-15) : weight matrix on these parameters *
+*/
+/* *
+*/
+/* Output : PAR(1-6) : parameters with the new type *
+*/
+/* DQ(1-5) : deviations from reference trajectory *
+*/
+/* WG(1-15) : weight matrix on these parameters *
+*/
+/************************************************************************/
+
+ void TrigInDetSctKFitter::tswtch(long *ityp, double *par, double *wg)
+{
+ double cosb, cosf, sinf, x, y, z, rcosb, tanth, d11, d21,
+ d12, d22, d42, d41, w11, w12, w13, w14, w15, w21, w22, capphi,
+ rad, phi;
+
+ /* Parameter adjustments */
+ --wg;
+ --par;
+
+ /* Function Body */
+ if (*ityp == 1) {
+
+/* transformation of parameters */
+
+ z = par[1];
+ x = par[2];
+ y = par[3];
+ phi = par[5];
+
+ par[1] = sqrt(x*x + y*y);
+ par[2] = fmod(atan2(y,x)+2*M_PI,2*M_PI);
+ par[3] = z;
+ par[5] = fmod(phi-par[2]+2*M_PI+M_PI, 2*M_PI)-M_PI;
+
+/* * */
+/* * transformation of deviations */
+/* * */
+/* RCOSB=PAR(1)*COS(PAR(5)) */
+
+ tanth = tan(par[4]);
+ cosf = cos(phi);
+ sinf = sin(phi);
+/* DQQ=(X*DQ(1)+Y*DQ(2))/RCOSB */
+/* DQ(1)= (-SINF*DQ(1)+COSF*DQ(2))/RCOSB */
+/* DQ(2)=-DQQ/TANTH */
+/* DQ(4)=DQ(4)-PAR(6)*DQQ-DQ(1) */
+
+/* transformation of weight matrix x,y,phi -> Phi,z,phi */
+
+ d11 = -y;
+ d21 = x;
+ d12 = -tanth * cosf;
+ d22 = -tanth * sinf;
+ d42 = -tanth * par[6];
+
+ w11 = d11 * wg[1] + d21 * wg[2];
+ w12 = d11 * wg[2] + d21 * wg[3];
+ w13 = d11 * wg[4] + d21 * wg[5];
+ w14 = d11 * wg[7] + d21 * wg[8];
+ w15 = d11 * wg[11] + d21 * wg[12];
+ w21 = d12 * wg[1] + d22 * wg[2] + d42 * wg[7];
+ w22 = d12 * wg[2] + d22 * wg[3] + d42 * wg[8];
+ wg[5] = d12 * wg[4] + d22 * wg[5] + d42 * wg[9];
+ wg[8] = d12 * wg[7] + d22 * wg[8] + d42 * wg[10];
+ wg[12] = d12 * wg[11] + d22 * wg[12] + d42 * wg[14];
+
+ wg[1] = w11 * d11 + w12 * d21;
+ wg[2] = w21 * d11 + w22 * d21;
+ wg[3] = w21 * d12 + w22 * d22 + wg[8] * d42;
+
+/* transformation Phi,phi -> Phi,beta */
+
+ wg[1] = wg[1] + w14 * 2. + wg[10];
+ wg[2] += wg[8];
+ wg[4] = w13 + wg[9];
+ wg[7] = w14 + wg[10];
+ wg[11] = w15 + wg[14];
+
+/* transformation from 'CYLI' to 'PLAN' -------------------------- */
+
+ } else {
+
+/* transformation of parameters */
+
+ rad = par[1];
+ capphi = par[2];
+ cosb = cos(par[5]);
+
+ par[1] = par[3];
+ par[2] = rad * cos(capphi);
+ par[3] = rad * sin(capphi);
+ par[5] = fmod(capphi+par[5]+2.*2*M_PI,2*M_PI);
+ cosf = cos(par[5]);
+ sinf = sin(par[5]);
+ tanth = tan(par[4]);
+/* * */
+/* * transformation of deviations */
+/* * */
+/* DQ(4)=DQ(4)-TANTH*PAR(6)*DQ(2)+DQ(1) */
+/* DQ1= -PAR(3)*DQ(1)-TANTH*COSF*DQ(2) */
+/* DQ(2)=PAR(2)*DQ(1)-TANTH*SINF*DQ(2) */
+/* DQ(1)=DQ1 */
+
+/* transformation of weight matrix Phi,beta -> Phi,phi */
+
+ wg[1] = wg[1] - wg[7] * 2. + wg[10];
+ wg[2] -= wg[8];
+ wg[4] -= wg[9];
+ wg[7] -= wg[10];
+ wg[11] -= wg[14];
+
+/* transformation of weight matrix Phi,z,phi -> x,y,phi */
+
+ rcosb = rad * cosb;
+ d11 = -sinf / rcosb;
+ d12 = cosf / rcosb;
+ double tem = tanth * rcosb;
+ d21 = -par[2] / tem;
+ d22 = -par[3] / tem;
+ tem = par[6] / rcosb;
+ d41 = -par[2] * tem;
+ d42 = -par[3] * tem;
+
+ w11 = d11 * wg[1] + d21 * wg[2] + d41 * wg[7];
+ w12 = d11 * wg[2] + d21 * wg[3] + d41 * wg[8];
+ w13 = d11 * wg[4] + d21 * wg[5] + d41 * wg[9];
+ w14 = d11 * wg[7] + d21 * wg[8] + d41 * wg[10];
+ w15 = d11 * wg[11] + d21 * wg[12] + d41 * wg[14];
+ w21 = d12 * wg[1] + d22 * wg[2] + d42 * wg[7];
+ w22 = d12 * wg[2] + d22 * wg[3] + d42 * wg[8];
+ wg[5] = d12 * wg[4] + d22 * wg[5] + d42 * wg[9];
+ wg[8] = d12 * wg[7] + d22 * wg[8] + d42 * wg[10];
+ wg[12] = d12 * wg[11] + d22 * wg[12] + d42 * wg[14];
+
+ wg[1] = w11 * d11 + w12 * d21 + w14 * d41;
+ wg[2] = w21 * d11 + w22 * d21 + wg[8] * d41;
+ wg[3] = w21 * d12 + w22 * d22 + wg[8] * d42;
+ wg[4] = w13;
+ wg[7] = w14;
+ wg[11] = w15;
+ }
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::txtrpa(double *parami, long *idir, double *radf,
+ double *zmin, double *zmax, double *sinbmx,
+ long *iopt, double *paramf, double *der,
+ double *alrphi, long *ierr)
+{
+ /* System generated locals */
+ double r__1;
+
+ double dphimn=0.0001;
+
+ /* Local variables */
+ double alff, beta, fact, radi, dphi, cosb, phii, delt, sinb,
+ cosf, phif, sinf, rtrk, cdphi, cosbf, ccpsf, dcosf, sinbf, ccpsi,
+ dsinf, cpsif, rdphi, cpsii, sdphi, rcosb, sphif, cotth,
+ scpsi, spsif, spsii, aa, xc, yc, xf, yf, zf, rc2, dr2, pph, rrr;
+
+/* ***********************************************************************
+ */
+/* AIM : * */
+/* ----- *
+*/
+/* Extrapolate a helix defined by the initial parameters PARAMI * */
+/* up to a given cylinder, and compute if requested the derivatives * */
+/* of the final parameters w.r.t. the initial ones * */
+/* The computation uses double precision on intermediate variables * */
+/* If the variation of phi angle is less than DPHIMN (.0001 in this * */
+/* version) the computation is done at first order in 1/R in order * */
+/* to avoid rounding errors, especially in the derivatives * */
+/* * */
+/* Input : PARAMI(1-6) : initial parameters * */
+/* (r,Phi,z,theta,beta,1/R) * */
+/* with beta = phi-Phi * */
+/* IDIR : if 1 : positive extrapolation only * */
+/* if -1 : negative " * */
+/* if 0 : extrapolation on both sides * */
+/* RADF : radius of the cylinder * */
+/* ZMIN : lower z limit of the cylinder * */
+/* ZMAX : upper z limit of the cylinder * */
+/* SINBMX : maximum allowed for |sin(beta)| at the * */
+/* intersection * */
+/* IOPT : 0 if derivatives not requested * */
+/* 1 if derivatives requested * */
+/* * */
+/* Output : IERR : 0 if intersection found * */
+/* 1 if no intersection with the cylinder * */
+/* 2 if SINBMX exceeded * */
+/* 3 if intersection outside of z limits * */
+/* 4 if 1/Rtr = 0 * */
+/* PARAMF(1-6) : final parameters * */
+/* DER(1-8) : deriv. of final w.r.t. initial param. * */
+/* DER(1) = d(1/R)/d(1/R) * */
+/* DER(2) = d(Phi)/d(beta) * */
+/* DER(3) = d(Phi)/d(1/R) * */
+/* DER(4) = d(z)/d(theta) * */
+/* DER(5) = d(z)/d(beta) * */
+/* DER(6) = d(z)/d(1/R) * */
+/* DER(7) = d(beta)/d(beta) * */
+/* DER(8) = d(beta)/d(1/R) * */
+/* ALRPHI : length (in r-phi projection) from start * */
+/* to extrapolation, with a sign (positive * */
+/* if the extrapolation is towards the * */
+/* direction defined by theta,phi) * */
+/* ***********************************************************************
+ */
+ /* Parameter adjustments */
+ --der;
+ --paramf;
+ --parami;
+
+ /* Function Body */
+ *ierr = 0;
+
+ if (parami[6] == 0.) {
+ *ierr = 4;
+ return;
+ }
+ radi = parami[1];
+ phii = parami[2];
+ beta = parami[5];
+ cosb = cos(beta);
+ sinb = sin(beta);
+ cotth = 1. / tan(parami[4]);
+ rtrk = 1. / parami[6];
+
+/* center and squared radius of the projected helix, in a rotated frame
+*/
+/* (x-axis through the starting point) */
+ xc = radi - rtrk * sinb;
+ yc = rtrk * cosb;
+ rc2 = xc * xc + yc * yc;
+
+/* resolution of a second order equation */
+/* Computing 2nd power */
+ r__1 = *radf;
+ rrr = (r__1 * r__1 - rtrk * rtrk - rc2) / (rtrk * 2.);
+ delt = rc2 - rrr * rrr;
+
+/* intersection exists if DELT > 0 */
+
+ if (delt <= 0.) {
+ *ierr = 1;
+ } else {
+ delt = sqrt(delt);
+
+/* choose intersection on the same side as the starting point */
+/* w.r.t. the plane containing the z axis and the axis of the helix */
+
+ sinf = (xc * rrr + yc * delt) / rc2;
+ cosf = (xc * delt - yc * rrr) / rc2;
+ xf = xc + rtrk * sinf;
+ yf = yc - rtrk * cosf;
+ sinbf = (sinf * xf - cosf * yf) / *radf;
+
+/* exit if beta too large at the intersection */
+ if (fabs(sinbf) > *sinbmx) {
+ *ierr = 2;
+ } else {
+ alff = atan2(sinf, cosf);
+ dphi = fmod(alff - beta + 2*M_PI + M_PI, 2*M_PI) - M_PI;
+ *alrphi = rtrk * dphi;
+
+/* select positive or negative extrapolations, or both */
+
+ if (*alrphi * *idir < 0.) {
+ *ierr = 1;
+ return;
+ }
+
+/* switch to approximate expressions if the variation of phi */
+/* is less than DPHIMN */
+
+/* "exact" expressions --------------------------------------- */
+
+ if (fabs(dphi) >= dphimn) {
+
+ zf = parami[3] + cotth * rtrk * dphi;
+
+/* exit if outside of limits in z,theta,Phi */
+
+ phif = atan2(yf, xf);
+ pph = phif;
+ if (pph < 0.) {
+ pph += 2*M_PI;
+ }
+ if (zf < *zmin || zf > *zmax) {
+ *ierr = 3;
+ } else {
+
+/* final parameters */
+ paramf[1] = *radf;
+ paramf[2] = fmod(phii + phif + 2*M_PI, 2*M_PI);
+ paramf[3] = zf;
+ paramf[4] = parami[4];
+ paramf[5] = fmod(alff - phif + 2*M_PI + M_PI, 2*M_PI) - M_PI;
+ paramf[6] = parami[6];
+
+/* computation of derivatives --------- */
+
+ if (*iopt == 1) {
+ cosbf = sqrt(1. - sinbf*sinbf);
+
+/* CCPSI = RC*cos(cap.psi) ; SCPSI = RC*sin(cap.psi) (initial point) */
+/* CCPSF = RC*cos(cap.psi) ; SCPSF = RC*sin(cap.psi) (final point) */
+ ccpsi = radi - rtrk * sinb;
+ scpsi = rtrk * cosb;
+ ccpsf = *radf - rtrk * sinbf;
+
+
+/* CPSII = sgn*RC*cos(psi) ; SPSII = sgn*RC*sin(psi) (initial point) */
+/* CPSIF = sgn*RC*cos(psi) ; SPSIF = sgn*RC*sin(psi) (final point) */
+ cpsii = rtrk - radi * sinb;
+ spsii = -radi * cosb;
+ cpsif = rtrk - *radf * sinbf;
+ spsif = -(*radf) * cosbf;
+
+ sdphi = sin(dphi);
+ cdphi = cos(dphi);
+
+ der[1] = 0.;
+ fact = -rtrk / spsif;
+ der[2] = sdphi * fact;
+ der[3] = fact * rtrk * (1. - cdphi);
+ der[4] = -rtrk * dphi * (cotth * cotth + 1.);
+ der[5] = rtrk * cotth * (*radf * ccpsf * spsii /
+ spsif - radi * ccpsi) / rc2;
+ der[6] = rtrk * rtrk * cotth * (-dphi + sinbf / cosbf
+ - (radi * scpsi + *radf * ccpsf * cpsii /
+ spsif) / rc2);
+ der[7] = spsii / spsif;
+ der[8] = rtrk * (cpsif - cpsii) / spsif;
+ }
+ }
+
+/* approximation at first order in 1/R --------------------
+------ */
+
+ } else {
+
+ dr2 = *radf * *radf - radi * radi;
+ rcosb = radi * cosb;
+ aa = 1. - radi * sinb / rtrk;
+ delt = rcosb * rcosb + aa * dr2;
+/* exit if no solution */
+ if (delt <= 0.) {
+ *ierr = 1;
+ } else {
+ rdphi = (sqrt(delt) - rcosb) / aa;
+ dphi = rdphi / rtrk;
+ dcosf = -sinb - cosb * .5 * dphi;
+ cosf = cosb + dcosf * dphi;
+ yf = -rdphi * dcosf;
+ dsinf = cosb - sinb * .5 * dphi;
+ sinf = sinb + dsinf * dphi;
+ xf = radi + rdphi * dsinf;
+ sinbf = (sinf * xf - cosf * yf) / *radf;
+ zf = parami[3] + cotth * rdphi;
+
+/* exit if outside of limits in z,theta,Phi */
+
+ phif = atan2(yf, xf);
+ pph = phif;
+ if (pph < 0.) {
+ pph += 2*M_PI;
+ }
+ if (zf < *zmin || zf > *zmax) {
+ *ierr = 3;
+ } else {
+ paramf[1] = *radf;
+ phif = atan2(yf, xf);
+ paramf[2] = fmod(parami[2] + phif + 2*M_PI, 2*M_PI);
+ paramf[3] = zf;
+ paramf[4] = parami[4];
+ paramf[5] = parami[5] + dphi - phif;
+ paramf[6] = parami[6];
+
+/* Computation of derivatives -------------- */
+
+ if (*iopt == 1) {
+ cosbf = sqrt(1. - sinbf * sinbf);
+ sphif = yf / *radf;
+
+ der[1] = 0.;
+ der[2] = rdphi / (*radf * cosbf);
+ der[3] = rdphi * .5 * der[2];
+ der[4] = -rdphi * (cotth * cotth + 1.);
+ der[5] = radi * cotth * sphif / cosbf;
+ der[6] = rdphi * .5 * der[5];
+ der[7] = radi * cosb / (*radf * cosbf);
+ der[8] = rdphi * .5 * (der[7] + 1.);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::txxpla(double *parami, long *idir, double *zf, double *rmin,
+ double *rmax, long *iopt, double *paramf,
+ double *der, double *alrphi, long *ierr)
+{
+
+ /* Local variables */
+ double dphi, rtrk, cosf0, cosf1, sinf0, sinf1, dcosf, dsinf,
+ rdphi, tanth, r1, x1, y1, ct2inv, xc, yc, dz, phi1;
+
+/* ***********************************************************************
+ */
+/* AIM : * */
+/* ----- *
+*/
+/* extrapolate a helix defined by the initial parameters PARAMI * */
+/* up to a given z-plane, and compute the derivatives of the * */
+/* final parameters w.r.t. the initial ones * */
+/* * */
+/* The computation uses double precision on intermediate variables * */
+/* If the variation of phi angle is less than DPHIMN (.0001 in this * */
+/* version) the computation is done at first order in 1/R in order * */
+/* to avoid rounding errors in the derivatives * */
+/* * */
+/* Input : PARAMI(1-6) : initial parameters * */
+/* (z,x,y,theta,phi,1/R) * */
+/* ZF : z of the final plane * */
+/* IDIR : if 1 : positive extrapolation only * */
+/* if -1 : negative " * */
+/* if 0 : extrapolation on both sides * */
+/* RMIN : lower limit of r on the plane * */
+/* RMAX : upper limit of r on the plane * */
+/* IOPT : 0 if derivatives not requested * */
+/* 1 if derivatives requested * */
+/* * */
+/* Output : IERR : 0 if OK * */
+/* 1 if no intersection found * */
+/* 3 if intersection outside of limits * */
+/* PARAMF(1-6) : final parameters * */
+/* DER(1-8) : deriv. of final w.r.t. initial param. * */
+/* DER(1) = d(x)/d(theta) * */
+/* DER(2) = d(x)/d(phi) * */
+/* DER(3) = d(x)/d(1/R) * */
+/* DER(4) = d(y)/d(theta) * */
+/* DER(5) = d(y)/d(phi) * */
+/* DER(6) = d(y)/d(1/R) * */
+/* DER(7) = d(phi)/d(theta) * */
+/* DER(8) = d(phi)/d(1/R) * */
+/* ALRPHI : length (in r-phi projection) from start * */
+/* to extrapolation, with a sign (positive * */
+/* if the extrapolation is towards the * */
+/* direction defined by theta,phi) * */
+/* ***********************************************************************
+ */
+/* REAL RZSURF,ZRMIN,ZRMAX,THXMIN,THXMAX,PHYMIN,PHYMAX */
+
+ /* Parameter adjustments */
+ --der;
+ --paramf;
+ --parami;
+
+ /* Function Body */
+ *ierr = 0;
+
+ dz = *zf - parami[1];
+ tanth = tan(parami[4]);
+ rdphi = dz * tanth;
+
+/* select positive or negative extrapolation, or both */
+ if (rdphi * *idir < 0.) {
+ *ierr = 1;
+ } else {
+ *alrphi = rdphi;
+
+/* x,y coordinates of intersection */
+ rtrk = 1. / parami[6];
+ cosf0 = cos((double) parami[5]);
+ sinf0 = sin((double) parami[5]);
+ xc = parami[2] - rtrk * sinf0;
+ yc = parami[3] + rtrk * cosf0;
+ dphi = parami[6] * rdphi;
+ phi1 = fmod(parami[5] + dphi, 2*M_PI);
+ cosf1 = cos(phi1);
+ sinf1 = sin(phi1);
+ x1 = xc + rtrk * sinf1;
+ y1 = yc - rtrk * cosf1;
+ r1 = sqrt(x1 * x1 + y1 * y1);
+
+/* intersection outside of limits in r,x,y */
+ if (r1 < *rmin || r1 > *rmax) {
+ *ierr = 3;
+
+/* parameters at the intersection */
+ } else {
+ paramf[1] = *zf;
+ paramf[2] = x1;
+ paramf[3] = y1;
+ paramf[4] = parami[4];
+ paramf[5] = phi1;
+ if (paramf[5] < 0.) paramf[5] += 2*M_PI;
+ paramf[6] = parami[6];
+
+/* computation of derivatives ----------------------------------- */
+
+ if (*iopt == 1) {
+ ct2inv = (tanth * tanth + 1.) * dz;
+ der[1] = ct2inv * cosf1;
+ der[4] = ct2inv * sinf1;
+ der[7] = parami[6] * ct2inv;
+ der[8] = rdphi;
+
+/* "exact" formulae if |DPHI| > DPHIMN */
+ if (fabs(dphi) >= 1e-4) {
+ dcosf = cosf1 - cosf0;
+ dsinf = sinf1 - sinf0;
+ der[2] = rtrk * dcosf;
+ der[3] = rtrk * rtrk * (dphi * cosf1 - dsinf);
+ der[5] = rtrk * dsinf;
+ der[6] = rtrk * rtrk * (dphi * sinf1 + dcosf);
+
+/* first order in 1/R if |DPHI| < DPHIMN */
+ } else {
+ der[2] = -rdphi * sinf0;
+ der[3] = rdphi * .5 * der[2];
+ der[5] = rdphi * cosf0;
+ der[6] = rdphi * .5 * der[5];
+ }
+ }
+ }
+ }
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+/* *******************************************************************
+
+ TMSCAT (float *SINTH,float *PINV,float *XRL,float *WG,float *dir)
+
+
+ * Transformation of weight matrix to account for multiple scatt. *
+* at a given point *
+* *
+* Input : SINTH : sin(theta) *
+* PINV : 1/p *
+* XRL : number of rad. lengths crossed at this point *
+* WG : initial weight matrix *
+* dir : direction of update (+1. = add a hit, *
+* -1. = subtract a hit) *
+* *
+* Output : WG : final weight matrix *
+* *
+***********************************************************************
+*/
+
+void TrigInDetSctKFitter::tmscat(double *sinth, double *pinv, double *xrl, double *wg,
+ double *dir)
+{ //local variable
+
+ double th2, xtt, xtb, xbb, q, ubb, utb, utt, s13, s14, s23, s24;
+ double s33, s34, s43, s44, s53, s54, wg4, wg5,wg9, wg13;
+
+ /* Parameter adjustments*/
+
+ --wg;
+/*
+* mean squared scattering angle in a direction perpendicular to
+* the trajectory (assuming large ratio momentum/mass)
+*/
+ th2 = 0.0136*(*pinv)*(1.+0.038*log(*xrl));
+ th2 = fabs(th2*th2*(*xrl))*(*dir);
+
+ // matrix (2x2) X = (cov.m.s.(theta,beta))**(-1) + TH2 * WG(theta,beta)
+ xtt = 1.+ th2*wg[6];
+ xtb = th2*wg[9];
+ xbb = (*sinth)*(*sinth) + th2*wg[10];
+
+ //matrix U = TH2 * X**(-1)
+ q=th2/(xbb*xtt - xtb*xtb);
+ ubb=q*xtt;
+ utb=-q*xtb;
+ utt=q*xbb;
+
+ //computation of WG - WG * (U ext. to 5x5) * WG
+
+ s13 = -wg[ 4]*utt - wg[ 7]*utb;
+ s14 = -wg[ 4]*utb - wg[ 7]*ubb;
+ s23 = -wg[ 5]*utt - wg[ 8]*utb;
+ s24 = -wg[ 5]*utb - wg[ 8]*ubb;
+ s53 = -wg[13]*utt - wg[14]*utb;
+ s54 = -wg[13]*utb - wg[14]*ubb;
+ wg[ 1] = wg[ 1] + s13*wg[ 4] + s14*wg[ 7];
+ wg[ 2] = wg[ 2] + s13*wg[ 5] + s14*wg[ 8];
+ wg[11] = wg[11] + s13*wg[13] + s14*wg[14];
+ wg[ 3] = wg[ 3] + s23*wg[ 5] + s24*wg[ 8];
+ wg[12] = wg[12] + s23*wg[13] + s24*wg[14];
+ wg[15] = wg[15] + s53*wg[13] + s54*wg[14];
+
+ s33 = 1.-wg[6]*utt-wg[ 9]*utb;
+ s34 = -wg[6]*utb-wg[ 9]*ubb;
+ s43 = -wg[9]*utt-wg[10]*utb;
+ s44 = 1.-wg[9]*utb-wg[10]*ubb;
+ wg4 = s33*wg[ 4] + s34*wg[ 7];
+ wg5 = s33*wg[ 5] + s34*wg[ 8];
+ wg[ 6] = s33*wg[ 6] + s34*wg[ 9];
+ wg9 = s33*wg[ 9] + s34*wg[10];
+ wg13 = s33*wg[13] + s34*wg[14];
+ wg[ 7] = s43*wg[ 4] + s44*wg[ 7];
+ wg[ 8] = s43*wg[ 5] + s44*wg[ 8];
+ wg[10] = s43*wg[ 9] + s44*wg[10];
+ wg[14] = s43*wg[13] + s44*wg[14];
+ wg[ 4] = wg4;
+ wg[ 5] = wg5;
+ wg[ 9] = wg9;
+ wg[13] = wg13;
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+// To add the energy loss effect in:-----------------------------------------
+// *
+// Input: x0 -- the material budget associated at a point in the unit of *
+// radiation lengths; *
+// dir -- direction of the reconstruction process *
+// (1. for going inwards, -1. for going outwards) *
+// *
+// Input and output: par6 -- the 6th track parameter, i.e. 1/Rtr *
+// wg[15] -- weight matrix *
+// *
+// Authors: S. Qian *
+// *
+// Date: Nov 16, 1999 *
+//--------------------------------------------------------------------------*
+
+void TrigInDetSctKFitter::add_E_loss(double *x0, double *dir, double *par6, double *wg) {
+
+ double q55, tem;
+ q55 = (*dir)*(*par6)*(*par6)*(*x0)*(0.415-0.744*(*x0));
+ if(q55==0.) return;
+ tem = 1./q55+wg[14];
+ if(tem==0.) {return;} else {tem=1./tem;}
+ *par6 *= (1. - (*dir)*(*x0)*(1.-0.5*(*x0)));
+ wg[0] -= tem*wg[10]*wg[10];
+ wg[1] -= tem*wg[10]*wg[11];
+ wg[2] -= tem*wg[11]*wg[11];
+ wg[3] -= tem*wg[10]*wg[12];
+ wg[4] -= tem*wg[11]*wg[12];
+ wg[5] -= tem*wg[12]*wg[12];
+ wg[6] -= tem*wg[10]*wg[13];
+ wg[7] -= tem*wg[11]*wg[13];
+ wg[8] -= tem*wg[12]*wg[13];
+ wg[9] -= tem*wg[13]*wg[13];
+ wg[10] -= tem*wg[10]*wg[14];
+ wg[11] -= tem*wg[11]*wg[14];
+ wg[12] -= tem*wg[12]*wg[14];
+ wg[13] -= tem*wg[13]*wg[14];
+ wg[14] -= tem*wg[14]*wg[14];
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+ void TrigInDetSctKFitter::trf2xy(long *idir, double *r, double *phi, double *wm) const
+{
+ /* transform weight matrix from (r,phi) to (x.y) (when idir= 1)
+ from (x,y) to (r,phi) (when idir= -1)
+
+ input: idir --- direction of the tranformation
+ r --- in (cm)
+ phi --- in (radian)
+ input and output: wm (15)
+
+ Auther: Sijin Qian, 4/1994
+ */
+
+ double d11, d21, d12, d22, wm1[12];
+
+ // d21 = sin(*phi) * pow((-(*r)), ((1-(*idir))/2));
+ // d22 = d11/r**(idir)
+
+ d11 = cos(*phi);
+ if((*idir) > 0) {
+ d21 = sin(*phi);
+ d22 = d11/(*r);
+ } else {
+ d21 = - sin(*phi) * (*r);
+ d22 = d11*(*r);
+ }
+ d12 = -d21/(*r);
+
+ for (long i=0; i<12; i++) {wm1[i]=wm[i];}
+
+ wm[ 0] = d11*d11*wm1[ 0] + 2.*d11*d12*wm1[1] + d12*d12*wm1[2];
+ wm[ 1] = d11*d21*wm1[ 0] + (d12*d21+d11*d22)*wm1[1] + d12*d22*wm1[2];
+ wm[ 2] = d21*d21*wm1[ 0] + 2.*d21*d22*wm1[1] + d22*d22*wm1[2];
+ wm[ 3] = d11*wm1[ 3] + d12*wm1[4];
+ wm[ 4] = d21*wm1[ 3] + d22*wm1[4];
+ wm[ 6] = d11*wm1[ 6] + d12*wm1[7];
+ wm[ 7] = d21*wm1[ 6] + d22*wm1[7];
+ wm[10] = d11*wm1[10] + d12*wm1[11];
+ wm[11] = d21*wm1[10] + d22*wm1[11];
+
+ return ;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::taddpt(double *df, double *dz, double *wff, double *wzz,
+ double *dp, double *wotr, double *achi2, long *ierr)
+{
+ double r__1, r__2;
+ long ier;
+
+/* ------------------------------------------------- */
+
+/* Subroutine for adding a point to a track */
+
+/*Input : df, dz -- Deviation of first two of 5 track parameters*/
+/* wff, wzz -- Weight on the two parameters */
+
+/* Input and output : wotr(1:15) -- weight matrix of parameters */
+
+/* Output : dp(1:5) -- Adjustment on track parameters */
+/* achi2 -- Incremental of Chi2 by adding this point*/
+/* ierr -- error code (0 = OK) */
+/* (1 = Chi2 increase too much)*/
+/* (2 = error in TSOLVE) */
+/* ------------------------------------------------- */
+
+ /* Parameter adjustments */
+ --wotr;
+ --dp;
+
+ /* Function Body */
+
+/* To add the weight of the point to the weght matrix */
+/* and to solve the linear system */
+
+ wotr[1] += *wff;
+ wotr[3] += *wzz;
+
+ if (wotr[1] <= 0. || wotr[3] <= 0.) {
+ *ierr = 3;
+ goto L20;
+ }
+ r__1 = *wff * *df;
+ r__2 = *wzz * *dz;
+ tsolve(&wotr[1], &r__1, &r__2, &dp[1], &ier);
+
+ if (ier != 0) {
+ *ierr = 2;
+ goto L20;
+ }
+ if (*wff < 0.) {
+ return;
+ }
+
+/* Increase of chi2 */
+
+ r__1 = *df - dp[1];
+ r__2 = *dz - dp[2];
+
+ *achi2 =
+
+/* Contribution of the added point */
+
+ *wff * (r__1 * r__1) + *wzz * (r__2 * r__2)
+
+/* Variation for previous ensembled points */
+
+ + (wotr[1] - *wff) * (dp[1] * dp[1])
+ + (2. * wotr[2] * dp[1] + (wotr[3] - *wzz) * dp[2]) * dp[2]
+ + (2. * (wotr[4] * dp[1] + wotr[5] * dp[2]) + wotr[6] * dp[3]) * dp[3]
+ + (2. * (wotr[7] * dp[1] + wotr[8] * dp[2] + wotr[9] * dp[3])
+ + wotr[10] * dp[4]) * dp[4]
+ + (2. * (wotr[11] * dp[1] + wotr[12] * dp[2] + wotr[13] * dp[3] +
+ wotr[14] * dp[4]) + wotr[15] * dp[5]) * dp[5];
+
+/* chi2 cut */
+
+ *ierr = (*achi2 <= maxDChi2[paramset]) ? 0 : 1;
+
+/* In the failure case, restore the previous weight matrix */
+
+L20:
+
+/* IF(IERR.NE.0.AND.WFF.GT.0.) THEN */
+ if (*ierr != 0) {
+ wotr[1] -= *wff;
+ wotr[3] -= *wzz;
+ }
+
+ return;
+}
+
+
+
+//-------------------------------------------------------------------------
+
+
+
+void TrigInDetSctKFitter::tsolve(double *w, double *vy, double *vz, double *d, long *ier)
+{
+ /* Local variables */
+ double q, r11, r12, r13, r14, r22, r23, r24, r33, r34, r44, tem,
+ piv3, piv4, piv5;
+
+/* ---------------------------------------------------------- */
+/* Subroutine to solve a linear system */
+/* ---------------------------------------------------------- */
+
+ /* Parameter adjustments */
+ --d;
+ --w;
+
+ /* Function Body */
+ *ier = 0;
+ piv5 = 1. / w[15];
+ q = w[11] * piv5;
+ r11 = w[1] - q * w[11];
+ r12 = w[2] - q * w[12];
+ r13 = w[4] - q * w[13];
+ r14 = w[7] - q * w[14];
+ q = w[12] * piv5;
+ r22 = w[3] - q * w[12];
+ r23 = w[5] - q * w[13];
+ r24 = w[8] - q * w[14];
+ q = w[13] * piv5;
+ r33 = w[6] - q * w[13];
+ r34 = w[9] - q * w[14];
+ r44 = w[10] - w[14] * w[14] * piv5;
+ if (r44 == 0.) {
+ *ier = 1;
+ return;
+ }
+ piv4 = 1. / r44;
+ q = r14 * piv4;
+ r11 -= q * r14;
+ r12 -= q * r24;
+ r13 -= q * r34;
+ q = r24 * piv4;
+ r22 -= q * r24;
+ r23 -= q * r34;
+ r33 -= r34 * r34 * piv4;
+ if (r33 == 0.) {
+ *ier = 1;
+ return;
+ }
+ piv3 = 1. / r33;
+ q = r13 * piv3;
+ r11 -= q * r13;
+ r12 -= q * r23;
+ r22 -= r23 * r23 * piv3;
+ tem = r11 * r22 - r12 * r12;
+ if (tem == 0.) {
+ *ier = 1;
+ return;
+ }
+ q = 1. / tem;
+ d[1] = q * ( r22 * *vy - r12 * *vz);
+ d[2] = q * (-r12 * *vy + r11 * *vz);
+ d[3] = -piv3 * (r13 * d[1] + r23 * d[2]);
+ d[4] = -piv4 * (r14 * d[1] + r24 * d[2] + r34 * d[3]);
+ d[5] = -piv5 * (w[11] * d[1] + w[12] * d[2] + w[13] * d[3] + w[14] * d[4]);
+
+ return;
+}
+
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetTrackFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetTrackFitter.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..f4856b4d1390ad1ec7f94ef3583f7082467c5fcb
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigInDetTrackFitter.cxx
@@ -0,0 +1,1110 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigInDetTrackFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 01.09.2005 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+
+#include <cmath>
+#include <iostream>
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "GaudiKernel/SystemOfUnits.h"
+#include "StoreGate/DataHandle.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+#include "TrigInDetEvent/TrigInDetTrackCollection.h"
+
+#include "TrkSurfaces/Surface.h"
+#include "TrkSurfaces/TrapezoidBounds.h"
+#include "TrkParameters/TrackParameters.h"
+
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+
+#include "InDetPrepRawData/SCT_Cluster.h"
+#include "InDetPrepRawData/PixelCluster.h"
+#include "TrigTimeAlgs/TrigTimerSvc.h"
+
+#include "TrigInDetToolInterfaces/ITrigInDetTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetTrackFitter.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+
+TrigInDetTrackFitter::TrigInDetTrackFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ): AthAlgTool(t,n,p),
+ m_MagFieldSvc("AtlasFieldSvc",this->name()),
+ m_trackMaker("TrigDkfTrackMakerTool")
+{
+ declareInterface< ITrigInDetTrackFitter >( this );
+
+ declareProperty( "AthenaFieldService", m_MagFieldSvc, "AlasFieldService");
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremm=false);
+ declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+ declareProperty( "OfflineClusters", m_offlineClusters = true);
+}
+
+StatusCode TrigInDetTrackFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ ATH_MSG_INFO("Using Athena magnetic field service");
+ sc = m_MagFieldSvc.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Unable to retrieve Athena MagFieldService");
+ return StatusCode::FAILURE;
+ }
+ sc=m_trackMaker.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_trackMaker);
+ return sc;
+ }
+
+ ITrigTimerSvc* timerSvc;
+ StatusCode scTime = service( "TrigTimerSvc", timerSvc);
+ if( scTime.isFailure() ) {
+ ATH_MSG_INFO("Unable to locate Service TrigTimerSvc ");
+ m_timers = false;
+ }
+ else{
+ m_timers = true;
+ }
+// add some timers:
+//
+ if ( m_timers ) {
+ m_timer[0] = timerSvc->addItem("CreateNodes");
+ m_timer[1] = timerSvc->addItem("ForwardFilter");
+ m_timer[2] = timerSvc->addItem("Smoother");
+ m_timer[3] = timerSvc->addItem("TrigTrackPars");
+ m_timer[4] = timerSvc->addItem("CleanUp");
+ }
+
+ m_fitStats.clear();
+ for(int i=0;i<13;i++) m_fitStats.push_back(FitStatStruct(i));
+ m_algorithmId=0;
+ ATH_MSG_INFO("TrigInDetTrackFitter constructed");
+ return sc;
+}
+
+StatusCode TrigInDetTrackFitter::finalize()
+{
+ MsgStream log(msgSvc(), name());
+ log << MSG::INFO <<"=============================================================="<<endreq ;
+ log << MSG::INFO <<"TrigInDetTrackFitter::finalize() - LVL2 Track fit Statistics: "<<endreq ;
+ for(std::vector<FitStatStruct>::iterator it=m_fitStats.begin();it!=m_fitStats.end();++it) {
+ if((*it).m_nTracksTotal==0) continue;
+ log << MSG::INFO <<"Algorithm Id="<<(*it).m_algorithmId<<" N tracks = "<<(*it).m_nTracksTotal<<endreq ;
+ log << MSG::INFO <<"Problems detected: "<<endreq ;
+ log << MSG::INFO <<"Unresolved spacepoints :"<<(*it).m_fitErrors[0]<<endreq ;
+ log << MSG::INFO <<"Extrapolator divergence:"<<(*it).m_fitErrors[1]<<endreq ;
+ log << MSG::INFO <<"pT falls below 200 MeV :"<<(*it).m_fitErrors[2]<<endreq ;
+ }
+ log << MSG::INFO <<"=============================================================="<<endreq ;
+ m_fitStats.clear();
+ StatusCode sc = AlgTool::finalize();
+ return sc;
+}
+
+void TrigInDetTrackFitter::m_getMagneticField(double r[3],double* B)
+{
+ B[0]=0.0;B[1]=0.0;B[2]=0.0;
+ double field[3];
+ m_MagFieldSvc->getField(r,field);//field is returned in kT
+ for(int i=0;i<3;i++) B[i]=field[i]/Gaudi::Units::kilogauss;//convert to kG
+}
+
+void TrigInDetTrackFitter::correctScale(Trk::TrkTrackState* pTS) {
+
+ double Rf[5];
+ double Gf[5][5];
+ int i,j;
+
+ for(i=0;i<4;i++) Rf[i] = pTS->m_getTrackState(i);
+ Rf[4] = 0.001*pTS->m_getTrackState(4);
+
+ for(i=0;i<4;i++)
+ for(j=0;j<4;j++) Gf[i][j] = pTS->m_getTrackCovariance(i,j);
+
+ Gf[0][4] = Gf[4][0] = pTS->m_getTrackCovariance(0,4)/1000.0;
+ Gf[1][4] = Gf[4][1] = pTS->m_getTrackCovariance(1,4)/1000.0;
+ Gf[2][4] = Gf[4][2] = pTS->m_getTrackCovariance(2,4)/1000.0;
+ Gf[3][4] = Gf[4][3] = pTS->m_getTrackCovariance(3,4)/1000.0;
+ Gf[4][4] = pTS->m_getTrackCovariance(4,4)/1000000.0;
+
+ pTS->m_setTrackState(Rf);
+ pTS->m_setTrackCovariance(Gf);
+}
+
+Trk::TrkTrackState* TrigInDetTrackFitter::m_extrapolate(Trk::TrkTrackState* pTS,
+ Trk::TrkPlanarSurface* pSB,
+ Trk::TrkPlanarSurface* pSE)
+{
+ const double C=0.02999975/1000.0;//using GeV internally
+ const double minStep=30.0;
+
+ double J[5][5],Rf[5],AG[5][5],Gi[5][5],Gf[5][5],A[5][5];
+ int i,j,m;
+
+ bool samePlane=false;
+
+ if(pSB!=NULL)
+ {
+ double diff=0.0;
+ for(i=0;i<4;i++) diff+=fabs(pSE->m_getPar(i)-pSB->m_getPar(i));
+ if(diff<1e-5) {
+ samePlane=true;
+ (m_fitStats[m_algorithmId]).m_fitErrors[0]++;
+ //std::cout<<"Starting plane and target plane are the same !"<<std::endl;
+ }
+ }
+
+ if(!samePlane) {
+
+ double gP[3],gPi[3],lP[3],gV[3],a,b,c,s,J0[7][5],descr,CQ,Ac,Av,Cc;
+ double V[3],P[3],M[3][3],D[4],Jm[7][7],
+ J1[5][7],gB[3],gBi[3],gBf[3],dBds[3],Buf[5][7],DVx,DVy,DVz;
+ int nStep,nStepMax;
+ double sl,ds,path=0.0;
+
+ //m_numericalJacobian(pTS,pSB,pSE,J);
+ double sint,cost,sinf,cosf;
+ sint=sin(pTS->m_getTrackState(3));cosf=cos(pTS->m_getTrackState(2));
+ sinf=sin(pTS->m_getTrackState(2));cost=cos(pTS->m_getTrackState(3));
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;CQ=C*pTS->m_getTrackState(4);
+
+ memset(&J0[0][0],0,sizeof(J0));
+
+ if(pSB!=NULL)
+ {
+ double L[3][3];
+ lP[0]=pTS->m_getTrackState(0);lP[1]=pTS->m_getTrackState(1);lP[2]=0.0;
+ pSB->m_transformPointToGlobal(lP,gP);
+ for(i=0;i<3;i++) for(j=0;j<3;j++) L[i][j]=pSB->m_getInvRotMatrix(i,j);
+
+ J0[0][0]=L[0][0];J0[0][1]=L[0][1];
+ J0[1][0]=L[1][0];J0[1][1]=L[1][1];
+ J0[2][0]=L[2][0];J0[2][1]=L[2][1];
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ else
+ {
+ gP[0]=-pTS->m_getTrackState(0)*sinf;
+ gP[1]= pTS->m_getTrackState(0)*cosf;
+ gP[2]= pTS->m_getTrackState(1);
+ J0[0][0]=-sinf;J0[0][2]=-pTS->m_getTrackState(0)*cosf;
+ J0[1][0]= cosf;J0[1][2]=-pTS->m_getTrackState(0)*sinf;
+ J0[2][1]=1.0;
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gPi[i]=gP[i];
+
+ m_getMagneticField(gP,gB);
+
+ for(i=0;i<3;i++) gBi[i]=gB[i];
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ descr=b*b-4.0*a*c;
+
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+
+ bool useExpansion=true;
+ double ratio = 4*a*c/(b*b);
+
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+
+ if(useExpansion) {
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+ }
+ else {
+ int signb = (b<0.0)?-1:1;
+ sl = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ if(fabs(sl)<minStep) nStepMax=1;
+ else
+ {
+ nStepMax=(int)(fabs(sl)/minStep)+1;
+ }
+ if((nStepMax<0)||(nStepMax>1000))
+ {
+ return NULL;
+ }
+ Av=sl*CQ;
+ Ac=0.5*sl*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*sl+Ac*DVx;
+ P[1]=gP[1]+gV[1]*sl+Ac*DVy;
+ P[2]=gP[2]+gV[2]*sl+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+
+ m_getMagneticField(P,gB);
+
+ for(i=0;i<3;i++) gBf[i]=gB[i];
+ for(i=0;i<3;i++)
+ {
+ dBds[i]=(gBf[i]-gBi[i])/sl;
+ gB[i]=gBi[i];
+ }
+ nStep=nStepMax;path=0.0;
+ while(nStep>0)
+ {
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ ratio = 4*a*c/(b*b);
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+ else useExpansion = true;
+
+ if(useExpansion) {
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+ }
+ else {
+ descr=b*b-4.0*a*c;
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+ int signb = (b<0.0)?-1:1;
+ sl = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ ds=sl/nStep;path+=ds;
+ Av=ds*CQ;
+ Ac=0.5*ds*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*ds+Ac*DVx;
+ P[1]=gP[1]+gV[1]*ds+Ac*DVy;
+ P[2]=gP[2]+gV[2]*ds+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+ for(i=0;i<3;i++)
+ {
+ gV[i]=V[i];gP[i]=P[i];
+ }
+ for(i=0;i<3;i++) gB[i]+=dBds[i]*ds;
+ nStep--;
+ }
+ pSE->m_transformPointToLocal(gP,lP);
+ Rf[0]=lP[0];Rf[1]=lP[1];
+ Rf[2]=atan2(V[1],V[0]);
+
+ if(fabs(V[2])>1.0)
+ {
+ return NULL;
+ }
+
+ Rf[3]=acos(V[2]);
+ Rf[4]=pTS->m_getTrackState(4);
+
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;
+
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gP[i]=gPi[i];
+
+ for(i=0;i<3;i++)
+ {
+ gB[i]=0.5*(gBi[i]+gBf[i]);
+ }
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ ratio = 4*a*c/(b*b);
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+ else useExpansion = true;
+
+ if(useExpansion) {
+ s=-c/b;
+ s=s*(1-a*s/b);
+ }
+ else {
+ descr=b*b-4.0*a*c;
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+ int signb = (b<0.0)?-1:1;
+ s = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ Av=s*CQ;
+ Ac=0.5*s*Av;
+ Cc=0.5*s*s*C;
+
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*s+Ac*DVx;
+ P[1]=gP[1]+gV[1]*s+Ac*DVy;
+ P[2]=gP[2]+gV[2]*s+Ac*DVz;
+
+ V[0]=gV[0]+Av*DVx;V[1]=gV[1]+Av*DVy;V[2]=gV[2]+Av*DVz;
+
+ pSE->m_transformPointToLocal(P,lP);
+
+ memset(&Jm[0][0],0,sizeof(Jm));
+
+ for(i=0;i<3;i++) for(j=0;j<3;j++) M[i][j]=pSE->m_getRotMatrix(i,j);
+
+ double coeff[3], dadVx,dadVy,dadVz,dadQ,dsdx,dsdy,dsdz,dsdVx,dsdVy,dsdVz,dsdQ;
+ coeff[0]=-c*c/(b*b*b);
+ coeff[1]=c*(1.0+3.0*c*a/(b*b))/(b*b);
+ coeff[2]=-(1.0+2.0*c*a/(b*b))/b;
+
+ dadVx=0.5*CQ*(-D[1]*gB[2]+D[2]*gB[1]);
+ dadVy=0.5*CQ*( D[0]*gB[2]-D[2]*gB[0]);
+ dadVz=0.5*CQ*(-D[0]*gB[1]+D[1]*gB[0]);
+ dadQ=0.5*C*(D[0]*DVx+D[1]*DVy+D[2]*DVz);
+
+ dsdx=coeff[2]*D[0];
+ dsdy=coeff[2]*D[1];
+ dsdz=coeff[2]*D[2];
+ dsdVx=coeff[0]*dadVx+coeff[1]*D[0];
+ dsdVy=coeff[0]*dadVy+coeff[1]*D[1];
+ dsdVz=coeff[0]*dadVz+coeff[1]*D[2];
+ dsdQ=coeff[0]*dadQ;
+
+ Jm[0][0]=1.0+V[0]*dsdx;
+ Jm[0][1]= V[0]*dsdy;
+ Jm[0][2]= V[0]*dsdz;
+
+ Jm[0][3]= s+V[0]*dsdVx;
+ Jm[0][4]= V[0]*dsdVy+Ac*gB[2];
+ Jm[0][5]= V[0]*dsdVz-Ac*gB[1];
+ Jm[0][6]= V[0]*dsdQ+Cc*DVx;
+
+ Jm[1][0]= V[1]*dsdx;
+ Jm[1][1]=1.0+V[1]*dsdy;
+ Jm[1][2]= V[1]*dsdz;
+
+ Jm[1][3]= V[1]*dsdVx-Ac*gB[2];
+ Jm[1][4]= s+V[1]*dsdVy;
+ Jm[1][5]= V[1]*dsdVz+Ac*gB[0];
+ Jm[1][6]= V[1]*dsdQ+Cc*DVy;
+
+ Jm[2][0]= V[2]*dsdx;
+ Jm[2][1]= V[2]*dsdy;
+ Jm[2][2]=1.0+V[2]*dsdz;
+ Jm[2][3]= V[2]*dsdVx+Ac*gB[1];
+ Jm[2][4]= V[2]*dsdVy-Ac*gB[0];
+ Jm[2][5]= s+V[2]*dsdVz;
+ Jm[2][6]= V[2]*dsdQ+Cc*DVz;
+
+ Jm[3][0]=dsdx*CQ*DVx;
+ Jm[3][1]=dsdy*CQ*DVx;
+ Jm[3][2]=dsdz*CQ*DVx;
+
+ Jm[3][3]=1.0+dsdVx*CQ*DVx;
+ Jm[3][4]=CQ*(dsdVy*DVx+s*gB[2]);
+ Jm[3][5]=CQ*(dsdVz*DVx-s*gB[1]);
+
+ Jm[3][6]=(CQ*dsdQ+C*s)*DVx;
+
+ Jm[4][0]=dsdx*CQ*DVy;
+ Jm[4][1]=dsdy*CQ*DVy;
+ Jm[4][2]=dsdz*CQ*DVy;
+
+ Jm[4][3]=CQ*(dsdVx*DVy-s*gB[2]);
+ Jm[4][4]=1.0+dsdVy*CQ*DVy;
+ Jm[4][5]=CQ*(dsdVz*DVy+s*gB[0]);
+
+ Jm[4][6]=(CQ*dsdQ+C*s)*DVy;
+
+ Jm[5][0]=dsdx*CQ*DVz;
+ Jm[5][1]=dsdy*CQ*DVz;
+ Jm[5][2]=dsdz*CQ*DVz;
+ Jm[5][3]=CQ*(dsdVx*DVz+s*gB[1]);
+ Jm[5][4]=CQ*(dsdVy*DVz-s*gB[0]);
+ Jm[5][5]=1.0+dsdVz*CQ*DVz;
+ Jm[5][6]=(CQ*dsdQ+C*s)*DVz;
+
+ Jm[6][6]=1.0;
+
+ memset(&J1[0][0],0,sizeof(J1));
+
+ J1[0][0]=M[0][0];J1[0][1]=M[0][1];J1[0][2]=M[0][2];
+ J1[1][0]=M[1][0];J1[1][1]=M[1][1];J1[1][2]=M[1][2];
+ J1[2][3]=-V[1]/(V[0]*V[0]+V[1]*V[1]);
+ J1[2][4]= V[0]/(V[0]*V[0]+V[1]*V[1]);
+ J1[3][5]=-1.0/sqrt(1-V[2]*V[2]);
+ J1[4][6]=1.0;
+
+ for(i=0;i<7;i++)
+ {
+ for(j=0;j<2;j++)
+ Buf[j][i]=J1[j][0]*Jm[0][i]+J1[j][1]*Jm[1][i]+J1[j][2]*Jm[2][i];
+ Buf[2][i]=J1[2][3]*Jm[3][i]+J1[2][4]*Jm[4][i];
+ Buf[3][i]=J1[3][5]*Jm[5][i];
+ Buf[4][i]=Jm[6][i];
+ }
+
+ if(pSB!=NULL)
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0]+Buf[i][2]*J0[2][0];
+ J[i][1]=Buf[i][0]*J0[0][1]+Buf[i][1]*J0[1][1]+Buf[i][2]*J0[2][1];
+ J[i][2]=Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ else
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0];
+ J[i][1]=Buf[i][2];
+ J[i][2]=Buf[i][0]*J0[0][2]+Buf[i][1]*J0[1][2]+Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ }
+ else {
+ Rf[0]=pTS->m_getTrackState(0);
+ Rf[1]=pTS->m_getTrackState(1);
+ Rf[2]=pTS->m_getTrackState(2);
+ Rf[3]=pTS->m_getTrackState(3);
+ Rf[4]=pTS->m_getTrackState(4);
+ memset(&J[0][0],0,sizeof(J));
+ for(i=0;i<5;i++) J[i][i]=1.0;
+ }
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ AG[i][j]=0.0;for(m=0;m<5;m++) AG[i][j]+=J[i][m]*pTS->m_getTrackCovariance(m,j);
+ }
+ for(i=0;i<5;i++) for(j=i;j<5;j++)
+ {
+ Gf[i][j]=0.0;
+ for(m=0;m<5;m++) Gf[i][j]+=AG[i][m]*J[j][m];
+ Gf[j][i]=Gf[i][j];
+ }
+
+ Trk::TrkTrackState* pTE=new Trk::TrkTrackState(pTS);
+
+ //workaround to keep using existing TrkTrackState code
+ double Rtmp[5];
+
+ for(i=0;i<4;i++) Rtmp[i] = Rf[i];
+ Rtmp[4] = 0.001*Rf[4];//GeV->MeV
+
+ pTE->m_setTrackState(Rtmp);
+ pTE->m_setTrackCovariance(Gf);
+ pTE->m_attachToSurface(pSE);
+
+ pTE->m_applyMaterialEffects();
+
+ pTE->m_setTrackState(Rf);//restore
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ Gi[i][j]=pTE->m_getTrackCovariance(i,j);
+ }
+
+ m_matrixInversion5x5(Gi);
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ A[i][j]=0.0;
+ for(m=0;m<5;m++) A[i][j]+=AG[m][i]*Gi[m][j];
+ }
+ pTE->m_setPreviousState(pTS);
+ pTE->m_setSmootherGain(A);
+
+ return pTE;
+}
+
+void TrigInDetTrackFitter::m_matrixInversion5x5(double a[5][5])
+{
+ /**** 5x5 matrix inversion by Gaussian elimination ****/
+ int i,j,k,l;
+ double factor;
+ double temp[5];
+ double b[5][5];
+ // Set b to I
+
+ memset(&b[0][0],0,sizeof(b));
+ b[0][0]=1.0;b[1][1]=1.0;b[2][2]=1.0;b[3][3]=1.0;b[4][4]=1.0;
+
+ for(i=0;i<5;i++)
+ {
+ for(j=i+1;j<5;j++)
+ if (fabs(a[i][i])<fabs(a[j][i]))
+ {
+ for(l=0;l<5;l++) temp[l]=a[i][l];
+ for(l=0;l<5;l++) a[i][l]=a[j][l];
+ for(l=0;l<5;l++) a[j][l]=temp[l];
+ for(l=0;l<5;l++) temp[l]=b[i][l];
+ for(l=0;l<5;l++) b[i][l]=b[j][l];
+ for(l=0;l<5;l++) b[j][l]=temp[l];
+ }
+ factor=a[i][i];
+ for(j=4;j>-1;j--)
+ {
+ b[i][j]/=factor;a[i][j]/=factor;
+ }
+ for(j=i+1;j<5;j++)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=4;i>0;i--)
+ {
+ for(j=i-1;j>-1;j--)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=0;i<5;i++) for(j=0;j<5;j++) a[i][j]=b[i][j];
+}
+
+
+void TrigInDetTrackFitter::fit(TrigInDetTrackCollection* recoTracks )
+{
+ if(m_timers)
+ {
+ m_timer[0]->start();
+ m_timer[0]->pause();
+ m_timer[1]->start();
+ m_timer[1]->pause();
+ m_timer[2]->start();
+ m_timer[2]->pause();
+ m_timer[3]->start();
+ m_timer[3]->pause();
+ m_timer[4]->start();
+ m_timer[4]->pause();
+ }
+
+ for(auto trIt = recoTracks->begin(); trIt != recoTracks->end(); ++trIt) {
+ fitTrack(**trIt);
+ }
+
+ if(m_timers)
+ {
+ m_timer[0]->stop();
+ m_timer[1]->stop();
+ m_timer[2]->stop();
+ m_timer[3]->stop();
+ m_timer[4]->stop();
+ }
+}
+
+TrackCollection* TrigInDetTrackFitter::fit(const TrackCollection& recoTracks, const Trk::ParticleHypothesis& matEffects)
+{
+ if(m_timers)
+ {
+ m_timer[0]->start();
+ m_timer[0]->pause();
+ m_timer[1]->start();
+ m_timer[1]->pause();
+ m_timer[2]->start();
+ m_timer[2]->pause();
+ m_timer[3]->start();
+ m_timer[3]->pause();
+ m_timer[4]->start();
+ m_timer[4]->pause();
+ }
+ TrackCollection* fittedTracks = new TrackCollection();
+ for(auto trIt = recoTracks.begin(); trIt != recoTracks.end(); ++trIt) {
+ Trk::Track* fittedTrack = fitTrack(**trIt, matEffects);
+ if (fittedTrack!=nullptr) {
+ fittedTracks->push_back(fittedTrack);
+ }
+ }
+ if(m_timers)
+ {
+ m_timer[0]->stop();
+ m_timer[1]->stop();
+ m_timer[2]->stop();
+ m_timer[3]->stop();
+ m_timer[4]->stop();
+ }
+ return fittedTracks;
+}
+
+void TrigInDetTrackFitter::fitTrack(TrigInDetTrack& recoTrack ) {
+
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>(recoTrack.param());
+ if(param==NULL)
+ {
+ ATH_MSG_WARNING("Fit Failed -- TrigInDetTrack has no parameters");
+ return;
+ }
+
+ // 1. Create initial track state:
+ double Rk[5];
+ Rk[0]=param->a0();
+ Rk[1]=param->z0();
+ Rk[2]=param->phi0();
+ double Theta=2.0*atan(exp(-param->eta()));
+ Rk[3]=Theta;
+ Rk[4]=1000.0*sin(Theta)/param->pT();//MeV->GeV
+ double Pt=param->pT();
+
+ if(fabs(Pt)<100.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ return;
+ }
+
+ // 2. Create filtering nodes
+
+ if(m_timers) m_timer[0]->resume();
+ std::vector<Trk::TrkBaseNode*> vpTrkNodes;
+ std::vector<Trk::TrkTrackState*> vpTrackStates;
+ bool trackResult = m_trackMaker->createDkfTrack(*(recoTrack.siSpacePoints()),vpTrkNodes, m_DChi2);
+ int nHits=vpTrkNodes.size();
+ if(m_timers)
+ {
+ m_timer[0]->pause();
+ m_timer[1]->resume();
+ }
+ ATH_MSG_VERBOSE(nHits<<" filtering nodes created");
+
+ if(!trackResult) return;
+
+ // 3. Main algorithm: filter and smoother (Rauch-Tung-Striebel)
+
+ m_algorithmId=recoTrack.algorithmId();
+ (m_fitStats[m_algorithmId]).m_nTracksTotal++;
+ //Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ double Gk[5][5] = {{100.0, 0, 0, 0, 0},
+ {0, 100.0, 0, 0, 0},
+ {0, 0, 0.01, 0, 0},
+ {0, 0, 0, 0.01, 0},
+ {0, 0, 0, 0, 0.1}};
+ pTS->m_setTrackCovariance(Gk);
+ if(m_doMultScatt)
+ pTS->m_setScatteringMode(1);
+ if(m_doBremm)
+ pTS->m_setScatteringMode(2);
+ vpTrackStates.push_back(pTS);
+
+ ATH_MSG_VERBOSE("Initial chi2: "<<recoTrack.chi2()<<" track authorId: "<<recoTrack.algorithmId());
+ ATH_MSG_VERBOSE("Initial params: locT="<<Rk[0]<<" locL="<<Rk[1]<<" phi="<<Rk[2]
+ <<" theta="<<Rk[3]<<" Q="<<Rk[4]<<" pT="<<sin(Rk[3])/Rk[4]<<" GeV");
+
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt(vpTrkNodes.begin()),
+ pnEnd(vpTrkNodes.end());
+
+ bool OK=true;
+
+ double chi2tot=0.0;
+ int ndoftot=-5;
+
+ Trk::TrkPlanarSurface* pSB=nullptr;
+ Trk::TrkPlanarSurface* pSE=nullptr;
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_extrapolate(pTS,pSB,pSE);
+
+ pSB=pSE;
+ if(pNS!=nullptr)
+ {
+ // pNS->m_report();
+ vpTrackStates.push_back(pNS);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_VERBOSE("dChi2="<<(*pnIt)->m_getChi2());
+ if((*pnIt)->m_isValidated())
+ {
+ chi2tot+=(*pnIt)->m_getChi2();
+ ndoftot+=(*pnIt)->m_getNdof();
+ }
+ (*pnIt)->m_updateTrackState(pNS);
+ pTS=pNS;
+ Pt=1000.0*sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_VERBOSE("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ (m_fitStats[m_algorithmId]).m_fitErrors[2]++;
+ OK=false;break;
+ }
+ }
+ else
+ {
+ (m_fitStats[m_algorithmId]).m_fitErrors[0]++;
+ OK=false;break;
+ }
+ }
+ if(m_timers) m_timer[1]->pause();
+ if(OK)
+ {
+ if(m_timers) m_timer[2]->resume();
+ std::vector<Trk::TrkTrackState*>::reverse_iterator ptsIt(vpTrackStates.rbegin()),
+ ptsEnd(vpTrackStates.rend());
+
+ for(;ptsIt!=ptsEnd;++ptsIt)
+ {
+ (*ptsIt)->m_runSmoother();
+ }
+ if(m_timers)
+ {
+ m_timer[2]->pause();
+ m_timer[3]->resume();
+ }
+
+ pTS=(*vpTrackStates.begin());
+ //correct GeV->MeV
+
+ correctScale(pTS);
+
+ double Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ double Phi0 = pTS->m_getTrackState(2);
+ if(Phi0>M_PI) Phi0-=2*M_PI;
+ if(Phi0<-M_PI) Phi0+=2*M_PI;
+ double Eta = -log(sin(0.5*pTS->m_getTrackState(3))/cos(0.5*pTS->m_getTrackState(3)));
+ double Z0 = pTS->m_getTrackState(1);
+ double D0 = pTS->m_getTrackState(0);
+
+ double errD0 = sqrt(pTS->m_getTrackCovariance(0,0));
+ double errZ0 = sqrt(pTS->m_getTrackCovariance(1,1));
+ double errPhi0 = sqrt(pTS->m_getTrackCovariance(2,2));
+ double errEta = sqrt(pTS->m_getTrackCovariance(3,3))/fabs(sin(pTS->m_getTrackState(3)));
+ double b=cos(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ double c=-Pt/pTS->m_getTrackState(4);
+ double a=-1.0/sin(pTS->m_getTrackState(3));
+ double errPt = sqrt(b*b*(pTS->m_getTrackCovariance(3,3))+c*c*(pTS->m_getTrackCovariance(4,4))+
+ 2.0*b*c*(pTS->m_getTrackCovariance(3,4)));
+
+ std::vector<double>* pCov=new std::vector<double>;
+ double CV[5][5];
+ CV[0][0]=pTS->m_getTrackCovariance(0,0);
+ CV[0][1]=pTS->m_getTrackCovariance(0,2);
+ CV[0][2]=pTS->m_getTrackCovariance(0,1);
+ CV[0][3]=a*(pTS->m_getTrackCovariance(0,3));
+ CV[0][4]=b*(pTS->m_getTrackCovariance(0,3))+c*(pTS->m_getTrackCovariance(0,4));
+ CV[1][1]=pTS->m_getTrackCovariance(2,2);
+
+ CV[1][2]=pTS->m_getTrackCovariance(1,2);
+ CV[1][3]=a*(pTS->m_getTrackCovariance(2,3));
+ CV[1][4]=b*(pTS->m_getTrackCovariance(2,3))+c*(pTS->m_getTrackCovariance(2,4));
+ CV[2][2]=pTS->m_getTrackCovariance(1,1);
+ CV[2][3]=a*(pTS->m_getTrackCovariance(1,3));
+ CV[2][4]=b*(pTS->m_getTrackCovariance(1,3))+c*(pTS->m_getTrackCovariance(1,4));
+ CV[3][3]=a*a*(pTS->m_getTrackCovariance(3,3));
+ CV[3][4]=a*(b*(pTS->m_getTrackCovariance(3,3))+c*(pTS->m_getTrackCovariance(3,4)));
+ CV[4][4]=b*b*(pTS->m_getTrackCovariance(3,3))+2.0*b*c*(pTS->m_getTrackCovariance(3,4))+
+ c*c*(pTS->m_getTrackCovariance(4,4));
+
+ for(int i=0;i<5;i++) {
+ for(int j=i;j<5;j++) {
+ pCov->push_back(CV[i][j]);
+ }
+ }
+ const TrigInDetTrackFitPar* tidtfp = new TrigInDetTrackFitPar(D0,Phi0,Z0,Eta, Pt,
+ errD0,errPhi0,errZ0,
+ errEta,errPt,pCov);
+
+ if(m_timers)
+ {
+ m_timer[3]->pause();
+ m_timer[4]->resume();
+ }
+
+ ATH_MSG_VERBOSE("Total chi2 ="<<chi2tot<<" NDOF="<<ndoftot);
+ ATH_MSG_VERBOSE("Fitted parameters: d0="<<D0<<" phi0="<<Phi0<<" z0="<<Z0
+ <<" eta0="<<Eta<<" pt="<<Pt);
+
+ if((ndoftot<0) || (fabs(Pt)<100.0) || (std::isnan(Pt)))
+ {
+ ATH_MSG_DEBUG("Fit failed - possibly floating point problem");;
+ delete tidtfp;
+ recoTrack.chi2(1e8);
+ }
+ else
+ {
+ delete param;
+ if(ndoftot>1) chi2tot/=ndoftot;
+ recoTrack.param(tidtfp);
+ recoTrack.chi2(chi2tot);
+ }
+ }
+ else
+ {
+ ATH_MSG_DEBUG("Forward Kalman filter: extrapolation failure ");
+ recoTrack.chi2(1e8);
+ }
+
+ pnIt=vpTrkNodes.begin();pnEnd=vpTrkNodes.end();
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ delete((*pnIt)->m_getSurface());
+ delete (*pnIt);
+ }
+ vpTrkNodes.clear();
+ for(std::vector<Trk::TrkTrackState*>::iterator ptsIt=vpTrackStates.begin();
+ ptsIt!=vpTrackStates.end();++ptsIt) delete (*ptsIt);
+ vpTrackStates.clear();
+
+ if(m_timers)
+ {
+ m_timer[4]->pause();
+ }
+}
+
+Trk::Track* TrigInDetTrackFitter::fitTrack(const Trk::Track& recoTrack, const Trk::ParticleHypothesis& matEffects) {
+
+ const Trk::TrackParameters* trackPars = recoTrack.perigeeParameters();
+ if(trackPars==nullptr) {
+ ATH_MSG_WARNING("Fit Failed -- TrkTrack has no parameters");
+ return nullptr;
+ }
+
+ // 1. Create initial track state:
+ double Rk[5];
+ Rk[0] = trackPars->parameters()[Trk::d0];
+ Rk[1] = trackPars->parameters()[Trk::z0];
+ Rk[2] = trackPars->parameters()[Trk::phi0];
+ if(Rk[2]>M_PI) Rk[2]-=2*M_PI;
+ if(Rk[2]<-M_PI) Rk[2]+=2*M_PI;
+ double trk_theta = trackPars->parameters()[Trk::theta];
+ Rk[3] = trk_theta;
+ double trk_qOverP = trackPars->parameters()[Trk::qOverP];
+ Rk[4] = 1000.0*trk_qOverP;//MeV->GeV
+ double trk_Pt = sin(trk_theta)/trk_qOverP;
+
+ if(fabs(trk_Pt)<100.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<trk_Pt<<" - skipping fit");
+ return nullptr;
+ }
+
+ // 2. Create filtering nodes
+
+ if(m_timers) m_timer[0]->resume();
+ std::vector<Trk::TrkBaseNode*> vpTrkNodes;
+ std::vector<Trk::TrkTrackState*> vpTrackStates;
+ bool trackResult = m_trackMaker->createDkfTrack(recoTrack,vpTrkNodes, m_DChi2);
+ int nHits=vpTrkNodes.size();
+ if(m_timers)
+ {
+ m_timer[0]->pause();
+ m_timer[1]->resume();
+ }
+ ATH_MSG_VERBOSE(nHits<<" filtering nodes created");
+
+ if(!trackResult) return nullptr;
+
+ // 3. Main algorithm: filter and smoother (Rauch-Tung-Striebel)
+
+ (m_fitStats[m_algorithmId]).m_nTracksTotal++;
+ Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ double Gk[5][5] = {{100.0, 0, 0, 0, 0},
+ {0, 100.0, 0, 0, 0},
+ {0, 0, 0.01, 0, 0},
+ {0, 0, 0, 0.01, 0},
+ {0, 0, 0, 0, 0.1}};
+ pTS->m_setTrackCovariance(Gk);
+ if(m_doMultScatt)
+ pTS->m_setScatteringMode(1);
+ if(m_doBremm)
+ pTS->m_setScatteringMode(2);
+ vpTrackStates.push_back(pTS);
+
+ //ATH_MSG_DEBUG("Initial chi2: "<<recoTrack.chi2()<<" track authorId: "<<recoTrack.algorithmId());
+ ATH_MSG_VERBOSE("Initial params: locT="<<Rk[0]<<" locL="<<Rk[1]<<" phi="<<Rk[2]
+ <<" theta="<<Rk[3]<<" Q="<<Rk[4]<<" pT="<<sin(Rk[3])/Rk[4]<<" GeV");
+
+ bool OK=true;
+
+ double chi2tot=0.0;
+ int ndoftot=-5;
+
+ Trk::TrkPlanarSurface* pSB=nullptr;
+ Trk::TrkPlanarSurface* pSE=nullptr;
+ for(auto pnIt = vpTrkNodes.begin(); pnIt!=vpTrkNodes.end(); ++pnIt) {
+ pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_extrapolate(pTS,pSB,pSE);
+
+ pSB=pSE;
+ if(pNS!=nullptr) {
+ vpTrackStates.push_back(pNS);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_VERBOSE("dChi2="<<(*pnIt)->m_getChi2());
+ if((*pnIt)->m_isValidated())
+ {
+ chi2tot+=(*pnIt)->m_getChi2();
+ ndoftot+=(*pnIt)->m_getNdof();
+ }
+ (*pnIt)->m_updateTrackState(pNS);
+ pTS=pNS;
+ double est_Pt = 1000.0*sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(est_Pt)<200.0)
+ {
+ ATH_MSG_VERBOSE("Estimated Pt is too low "<<est_Pt<<" - skipping fit");
+ (m_fitStats[m_algorithmId]).m_fitErrors[2]++;
+ OK=false;break;
+ }
+ }
+ else
+ {
+ (m_fitStats[m_algorithmId]).m_fitErrors[0]++;
+ OK=false;break;
+ }
+ }
+ if(m_timers) m_timer[1]->pause();
+ Trk::Track* fittedTrack = nullptr;
+ if(OK)
+ {
+ if(m_timers) m_timer[2]->resume();
+ for(auto ptsIt = vpTrackStates.rbegin();ptsIt!=vpTrackStates.rend();++ptsIt)
+ {
+ (*ptsIt)->m_runSmoother();
+ }
+ if(m_timers)
+ {
+ m_timer[2]->pause();
+ m_timer[3]->resume();
+ }
+
+ pTS=(*vpTrackStates.begin());
+ //correct GeV->MeV
+
+ correctScale(pTS);
+
+ double qOverP = pTS->m_getTrackState(4);
+ double pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ double phi0 = pTS->m_getTrackState(2);
+ if(phi0>M_PI) phi0-=2*M_PI;
+ if(phi0<-M_PI) phi0+=2*M_PI;
+ double theta = pTS->m_getTrackState(3);
+ double eta = -log(tan(0.5*theta));
+ double z0 = pTS->m_getTrackState(1);
+ double d0 = pTS->m_getTrackState(0);
+
+ if((ndoftot<0) || (fabs(pt)<100.0) || (std::isnan(pt)))
+ {
+ ATH_MSG_DEBUG("Fit failed - possibly floating point problem");
+ }
+ else
+ {
+
+ AmgSymMatrix(5)* cov = new AmgSymMatrix(5);
+ for(int i=0;i<5;i++) {
+ for(int j=i;j<5;j++)
+ {
+ cov->fillSymmetric(i, j, pTS->m_getTrackCovariance(i,j));
+ }
+ }
+ Trk::PerigeeSurface perigeeSurface;
+ Trk::Perigee* perigee = new Trk::Perigee(d0, z0, phi0, theta, qOverP, perigeeSurface, cov);
+
+ std::bitset<Trk::TrackStateOnSurface::NumberOfTrackStateOnSurfaceTypes> typePattern;
+ typePattern.set(Trk::TrackStateOnSurface::Perigee);
+ DataVector<const Trk::TrackStateOnSurface>* pParVec = new DataVector<const Trk::TrackStateOnSurface>;
+ pParVec->reserve(recoTrack.trackStateOnSurfaces()->size());
+ pParVec->push_back(new Trk::TrackStateOnSurface(0, perigee,0,0, typePattern));
+
+ for (auto tSOS = recoTrack.trackStateOnSurfaces()->begin(); tSOS != recoTrack.trackStateOnSurfaces()->end(); ++tSOS) {
+ //Don't store perigee - new perigee created above
+ if ((*tSOS)->type(Trk::TrackStateOnSurface::Perigee) == false) {
+ pParVec->push_back((*tSOS)->clone());
+ }
+ }
+ if(m_timers)
+ {
+ m_timer[3]->pause();
+ m_timer[4]->resume();
+ }
+
+ ATH_MSG_VERBOSE("Total chi2 ="<<chi2tot<<" NDOF="<<ndoftot);
+ ATH_MSG_VERBOSE("Fitted parameters: d0="<<d0<<" phi0="<<phi0<<" z0="<<z0
+ <<" eta0="<<eta<<" pt="<<pt);
+ Trk::FitQuality* pFQ=new Trk::FitQuality(chi2tot,ndoftot);
+ Trk::TrackInfo info(recoTrack.info());
+ info.setParticleHypothesis(matEffects);
+ fittedTrack = new Trk::Track(info, pParVec, pFQ);//fittedTrack now owns pParVec and pFQ
+ }
+ }
+ else
+ {
+ ATH_MSG_DEBUG("Forward Kalman filter: extrapolation failure ");
+ }
+
+ for(auto pnIt = vpTrkNodes.begin(); pnIt!=vpTrkNodes.end(); ++pnIt) {
+ delete((*pnIt)->m_getSurface());
+ delete (*pnIt);
+ }
+ vpTrkNodes.clear();
+ for(auto ptsIt = vpTrackStates.begin();ptsIt!=vpTrackStates.end();++ptsIt) {
+ delete (*ptsIt);
+ }
+ vpTrackStates.clear();
+
+ if(m_timers)
+ {
+ m_timer[4]->pause();
+ }
+ return fittedTrack;
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2FastExtrapolationTool.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2FastExtrapolationTool.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..18305c8f80b415cb9938274efaad902ba3ef2432
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2FastExtrapolationTool.cxx
@@ -0,0 +1,540 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigL2FastExtrapolationTool tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 01.09.2005 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "StoreGate/DataHandle.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+#include "MagFieldInterfaces/IMagFieldSvc.h"
+#include "AthenaBaseComps/AthMsgStreamMacros.h"
+
+#include "TrigInDetTrackFitter/ITrigL2FastExtrapolationTool.h"
+#include "TrigInDetTrackFitter/TrigL2FastExtrapolationTool.h"
+#include "GaudiKernel/SystemOfUnits.h"
+
+TrigL2FastExtrapolationTool::TrigL2FastExtrapolationTool(const std::string& t,
+ const std::string& n,
+ const IInterface* p ):
+ AthAlgTool(t,n,p),
+ m_MagFieldSvc("AtlasFieldSvc",this->name())
+{
+ declareInterface< ITrigL2FastExtrapolationTool >( this );
+}
+
+StatusCode TrigL2FastExtrapolationTool::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+ MsgStream athenaLog(msgSvc(), name());
+
+ StoreGateSvc* detStore;
+ sc = service("DetectorStore", detStore);
+ if ( sc.isFailure() ) {
+ ATH_MSG_FATAL("DetStore service not found");
+ return StatusCode::FAILURE;
+ }
+
+ ATH_MSG_INFO("Using Athena magnetic field service");
+ sc = m_MagFieldSvc.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Unable to retrieve Athena MagFieldService");
+ return StatusCode::FAILURE;
+ }
+ ATH_MSG_INFO("TrigL2FastExtrapolationTool initialized ");
+ return sc;
+}
+
+StatusCode TrigL2FastExtrapolationTool::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+
+void TrigL2FastExtrapolationTool::m_getMagneticField(double r[3],double* B)
+{
+ B[0]=0.0;B[1]=0.0;B[2]=0.0;
+ double field[3];
+ m_MagFieldSvc->getField(r,field);//field is returned in kT
+ for(int i=0;i<3;i++) B[i]=field[i]/Gaudi::Units::kilogauss;//convert to kG
+}
+
+Trk::TrkTrackState* TrigL2FastExtrapolationTool::extrapolate(Trk::TrkTrackState* pTS,
+ Trk::TrkPlanarSurface* pSB,
+ Trk::TrkPlanarSurface* pSE,
+ bool smooth)
+{
+ const double C=0.02999975;
+ const double minStep=30.0;
+
+ double sint,cost,sinf,cosf;
+ double gP[3],gPi[3],lP[3],gV[3],a,b,c,s,J0[7][5],Rf[5],descr,CQ,Ac,Av,Cc;
+ double V[3],P[3],M[3][3],AG[5][5],D[4],J[5][5],Jm[7][7],Gf[5][5],
+ J1[5][7],gB[3],gBi[3],gBf[3],dBds[3],Buf[5][7],DVx,DVy,DVz;
+ int i,j,m,nStep,nStepMax;
+ double sl,ds,path=0.0;
+
+
+ bool samePlane=false;
+
+ if(pSB!=NULL)
+ {
+ double diff=0.0;
+ for(i=0;i<4;i++) diff+=fabs(pSE->m_getPar(i)-pSB->m_getPar(i));
+ if(diff<1e-5) {
+ samePlane=true;
+ //(m_fitStats[m_algorithmId]).m_fitErrors[0]++;
+ //std::cout<<"Starting plane and target plane are the same !"<<std::endl;
+ }
+ }
+
+ if(!samePlane) {
+
+ //m_numericalJacobian(pTS,pSB,pSE,J);
+
+ sint=sin(pTS->m_getTrackState(3));cosf=cos(pTS->m_getTrackState(2));
+ sinf=sin(pTS->m_getTrackState(2));cost=cos(pTS->m_getTrackState(3));
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;CQ=C*pTS->m_getTrackState(4);
+
+ memset(&J0[0][0],0,sizeof(J0));
+
+ if(pSB!=NULL)
+ {
+ double L[3][3];
+ lP[0]=pTS->m_getTrackState(0);lP[1]=pTS->m_getTrackState(1);lP[2]=0.0;
+ pSB->m_transformPointToGlobal(lP,gP);
+ for(i=0;i<3;i++) for(j=0;j<3;j++) L[i][j]=pSB->m_getInvRotMatrix(i,j);
+
+ J0[0][0]=L[0][0];J0[0][1]=L[0][1];
+ J0[1][0]=L[1][0];J0[1][1]=L[1][1];
+ J0[2][0]=L[2][0];J0[2][1]=L[2][1];
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ else
+ {
+ gP[0]=-pTS->m_getTrackState(0)*sinf;
+ gP[1]= pTS->m_getTrackState(0)*cosf;
+ gP[2]= pTS->m_getTrackState(1);
+ J0[0][0]=-sinf;J0[0][2]=-pTS->m_getTrackState(0)*cosf;
+ J0[1][0]= cosf;J0[1][2]=-pTS->m_getTrackState(0)*sinf;
+ J0[2][1]=1.0;
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gPi[i]=gP[i];
+
+ m_getMagneticField(gP,gB);
+
+ for(i=0;i<3;i++) gBi[i]=gB[i];
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ descr=b*b-4.0*a*c;
+
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+
+ bool useExpansion=true;
+ double ratio = 4*a*c/(b*b);
+
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+
+ if(useExpansion) {
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+ }
+ else {
+ int signb = (b<0.0)?-1:1;
+ sl = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ if(fabs(sl)<minStep) nStepMax=1;
+ else
+ {
+ nStepMax=(int)(fabs(sl)/minStep)+1;
+ }
+ if((nStepMax<0)||(nStepMax>1000))
+ {
+ return NULL;
+ }
+ Av=sl*CQ;
+ Ac=0.5*sl*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*sl+Ac*DVx;
+ P[1]=gP[1]+gV[1]*sl+Ac*DVy;
+ P[2]=gP[2]+gV[2]*sl+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+
+ m_getMagneticField(P,gB);
+
+ for(i=0;i<3;i++) gBf[i]=gB[i];
+ for(i=0;i<3;i++)
+ {
+ dBds[i]=(gBf[i]-gBi[i])/sl;
+ gB[i]=gBi[i];
+ }
+ nStep=nStepMax;path=0.0;
+ while(nStep>0)
+ {
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ ratio = 4*a*c/(b*b);
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+ else useExpansion = true;
+
+ if(useExpansion) {
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+ }
+ else {
+ descr=b*b-4.0*a*c;
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+ int signb = (b<0.0)?-1:1;
+ sl = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ ds=sl/nStep;path+=ds;
+ Av=ds*CQ;
+ Ac=0.5*ds*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*ds+Ac*DVx;
+ P[1]=gP[1]+gV[1]*ds+Ac*DVy;
+ P[2]=gP[2]+gV[2]*ds+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+ for(i=0;i<3;i++)
+ {
+ gV[i]=V[i];gP[i]=P[i];
+ }
+ for(i=0;i<3;i++) gB[i]+=dBds[i]*ds;
+ nStep--;
+ }
+ pSE->m_transformPointToLocal(gP,lP);
+ Rf[0]=lP[0];Rf[1]=lP[1];
+ Rf[2]=atan2(V[1],V[0]);
+
+ if(fabs(V[2])>1.0) return NULL;
+
+ Rf[3]=acos(V[2]);
+ Rf[4]=pTS->m_getTrackState(4);
+
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;
+
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gP[i]=gPi[i];
+
+ for(i=0;i<3;i++)
+ {
+ gB[i]=0.5*(gBi[i]+gBf[i]);
+ }
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ ratio = 4*a*c/(b*b);
+ if(fabs(ratio)>0.1)
+ useExpansion = false;
+ else useExpansion = true;
+
+ if(useExpansion) {
+ s=-c/b;
+ s=s*(1-a*sl/b);
+ }
+ else {
+ descr=b*b-4.0*a*c;
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+ int signb = (b<0.0)?-1:1;
+ s = (-b+signb*sqrt(descr))/(2*a);
+ }
+
+ Av=s*CQ;
+ Ac=0.5*s*Av;
+ Cc=0.5*s*s*C;
+
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*s+Ac*DVx;
+ P[1]=gP[1]+gV[1]*s+Ac*DVy;
+ P[2]=gP[2]+gV[2]*s+Ac*DVz;
+
+ V[0]=gV[0]+Av*DVx;V[1]=gV[1]+Av*DVy;V[2]=gV[2]+Av*DVz;
+
+ pSE->m_transformPointToLocal(P,lP);
+
+ memset(&Jm[0][0],0,sizeof(Jm));
+
+ for(i=0;i<3;i++) for(j=0;j<3;j++) M[i][j]=pSE->m_getRotMatrix(i,j);
+
+ double coeff[3], dadVx,dadVy,dadVz,dadQ,dsdx,dsdy,dsdz,dsdVx,dsdVy,dsdVz,dsdQ;
+ coeff[0]=-c*c/(b*b*b);
+ coeff[1]=c*(1.0+3.0*c*a/(b*b))/(b*b);
+ coeff[2]=-(1.0+2.0*c*a/(b*b))/b;
+
+ dadVx=0.5*CQ*(-D[1]*gB[2]+D[2]*gB[1]);
+ dadVy=0.5*CQ*( D[0]*gB[2]-D[2]*gB[0]);
+ dadVz=0.5*CQ*(-D[0]*gB[1]+D[1]*gB[0]);
+ dadQ=0.5*C*(D[0]*DVx+D[1]*DVy+D[2]*DVz);
+
+ dsdx=coeff[2]*D[0];
+ dsdy=coeff[2]*D[1];
+ dsdz=coeff[2]*D[2];
+ dsdVx=coeff[0]*dadVx+coeff[1]*D[0];
+ dsdVy=coeff[0]*dadVy+coeff[1]*D[1];
+ dsdVz=coeff[0]*dadVz+coeff[1]*D[2];
+ dsdQ=coeff[0]*dadQ;
+
+ Jm[0][0]=1.0+V[0]*dsdx;
+ Jm[0][1]= V[0]*dsdy;
+ Jm[0][2]= V[0]*dsdz;
+
+ Jm[0][3]= s+V[0]*dsdVx;
+ Jm[0][4]= V[0]*dsdVy+Ac*gB[2];
+ Jm[0][5]= V[0]*dsdVz-Ac*gB[1];
+ Jm[0][6]= V[0]*dsdQ+Cc*DVx;
+
+ Jm[1][0]= V[1]*dsdx;
+ Jm[1][1]=1.0+V[1]*dsdy;
+ Jm[1][2]= V[1]*dsdz;
+
+ Jm[1][3]= V[1]*dsdVx-Ac*gB[2];
+ Jm[1][4]= s+V[1]*dsdVy;
+ Jm[1][5]= V[1]*dsdVz+Ac*gB[0];
+ Jm[1][6]= V[1]*dsdQ+Cc*DVy;
+
+ Jm[2][0]= V[2]*dsdx;
+ Jm[2][1]= V[2]*dsdy;
+ Jm[2][2]=1.0+V[2]*dsdz;
+ Jm[2][3]= V[2]*dsdVx+Ac*gB[1];
+ Jm[2][4]= V[2]*dsdVy-Ac*gB[0];
+ Jm[2][5]= s+V[2]*dsdVz;
+ Jm[2][6]= V[2]*dsdQ+Cc*DVz;
+
+ Jm[3][0]=dsdx*CQ*DVx;
+ Jm[3][1]=dsdy*CQ*DVx;
+ Jm[3][2]=dsdz*CQ*DVx;
+
+ Jm[3][3]=1.0+dsdVx*CQ*DVx;
+ Jm[3][4]=CQ*(dsdVy*DVx+s*gB[2]);
+ Jm[3][5]=CQ*(dsdVz*DVx-s*gB[1]);
+
+ Jm[3][6]=(CQ*dsdQ+C*s)*DVx;
+
+ Jm[4][0]=dsdx*CQ*DVy;
+ Jm[4][1]=dsdy*CQ*DVy;
+ Jm[4][2]=dsdz*CQ*DVy;
+
+ Jm[4][3]=CQ*(dsdVx*DVy-s*gB[2]);
+ Jm[4][4]=1.0+dsdVy*CQ*DVy;
+ Jm[4][5]=CQ*(dsdVz*DVy+s*gB[0]);
+
+ Jm[4][6]=(CQ*dsdQ+C*s)*DVy;
+
+ Jm[5][0]=dsdx*CQ*DVz;
+ Jm[5][1]=dsdy*CQ*DVz;
+ Jm[5][2]=dsdz*CQ*DVz;
+ Jm[5][3]=CQ*(dsdVx*DVz+s*gB[1]);
+ Jm[5][4]=CQ*(dsdVy*DVz-s*gB[0]);
+ Jm[5][5]=1.0+dsdVz*CQ*DVz;
+ Jm[5][6]=(CQ*dsdQ+C*s)*DVz;
+
+ Jm[6][6]=1.0;
+
+ memset(&J1[0][0],0,sizeof(J1));
+
+ J1[0][0]=M[0][0];J1[0][1]=M[0][1];J1[0][2]=M[0][2];
+ J1[1][0]=M[1][0];J1[1][1]=M[1][1];J1[1][2]=M[1][2];
+ J1[2][3]=-V[1]/(V[0]*V[0]+V[1]*V[1]);
+ J1[2][4]= V[0]/(V[0]*V[0]+V[1]*V[1]);
+ J1[3][5]=-1.0/sqrt(1-V[2]*V[2]);
+ J1[4][6]=1.0;
+
+ for(i=0;i<7;i++)
+ {
+ for(j=0;j<2;j++)
+ Buf[j][i]=J1[j][0]*Jm[0][i]+J1[j][1]*Jm[1][i]+J1[j][2]*Jm[2][i];
+ Buf[2][i]=J1[2][3]*Jm[3][i]+J1[2][4]*Jm[4][i];
+ Buf[3][i]=J1[3][5]*Jm[5][i];
+ Buf[4][i]=Jm[6][i];
+ }
+
+ if(pSB!=NULL)
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0]+Buf[i][2]*J0[2][0];
+ J[i][1]=Buf[i][0]*J0[0][1]+Buf[i][1]*J0[1][1]+Buf[i][2]*J0[2][1];
+ J[i][2]=Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ else
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0];
+ J[i][1]=Buf[i][2];
+ J[i][2]=Buf[i][0]*J0[0][2]+Buf[i][1]*J0[1][2]+Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ }
+ else {
+ Rf[0]=pTS->m_getTrackState(0);
+ Rf[1]=pTS->m_getTrackState(1);
+ Rf[2]=pTS->m_getTrackState(2);
+ Rf[3]=pTS->m_getTrackState(3);
+ Rf[4]=pTS->m_getTrackState(4);
+ memset(&J[0][0],0,sizeof(J));
+ for(i=0;i<5;i++) J[i][i]=1.0;
+ }
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ AG[i][j]=0.0;for(m=0;m<5;m++) AG[i][j]+=J[i][m]*pTS->m_getTrackCovariance(m,j);
+ }
+ for(i=0;i<5;i++) for(j=i;j<5;j++)
+ {
+ Gf[i][j]=0.0;
+ for(m=0;m<5;m++) Gf[i][j]+=AG[i][m]*J[j][m];
+ Gf[j][i]=Gf[i][j];
+ }
+
+ Trk::TrkTrackState* pTE=new Trk::TrkTrackState(pTS);
+
+ pTE->m_setTrackState(Rf);
+ pTE->m_setTrackCovariance(Gf);
+ pTE->m_attachToSurface(pSE);
+ pTE->m_applyMaterialEffects();
+
+ if(smooth)
+ {
+ double A[5][5],Gi[5][5];
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ Gi[i][j]=pTE->m_getTrackCovariance(i,j);
+ }
+ m_matrixInversion5x5(Gi);
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ A[i][j]=0.0;
+ for(m=0;m<5;m++) A[i][j]+=AG[m][i]*Gi[m][j];
+ }
+ pTE->m_setPreviousState(pTS);
+ pTE->m_setSmootherGain(A);
+ }
+
+ return pTE;
+}
+
+void TrigL2FastExtrapolationTool::m_matrixInversion5x5(double a[5][5])
+{
+ /**** 5x5 matrix inversion by Gaussian elimination ****/
+ int i,j,k,l;
+ double factor;
+ double temp[5];
+ double b[5][5];
+ // Set b to I
+
+ memset(&b[0][0],0,sizeof(b));
+ b[0][0]=1.0;b[1][1]=1.0;b[2][2]=1.0;b[3][3]=1.0;b[4][4]=1.0;
+
+ for(i=0;i<5;i++)
+ {
+ for(j=i+1;j<5;j++)
+ if (fabs(a[i][i])<fabs(a[j][i]))
+ {
+ for(l=0;l<5;l++) temp[l]=a[i][l];
+ for(l=0;l<5;l++) a[i][l]=a[j][l];
+ for(l=0;l<5;l++) a[j][l]=temp[l];
+ for(l=0;l<5;l++) temp[l]=b[i][l];
+ for(l=0;l<5;l++) b[i][l]=b[j][l];
+ for(l=0;l<5;l++) b[j][l]=temp[l];
+ }
+ factor=a[i][i];
+ for(j=4;j>-1;j--)
+ {
+ b[i][j]/=factor;a[i][j]/=factor;
+ }
+ for(j=i+1;j<5;j++)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=4;i>0;i--)
+ {
+ for(j=i-1;j>-1;j--)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=0;i<5;i++) for(j=0;j<5;j++) a[i][j]=b[i][j];
+}
+
+
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2HighPtTrackFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2HighPtTrackFitter.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..70ac59e2d49443430805779afef72a4f43dc4b62
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2HighPtTrackFitter.cxx
@@ -0,0 +1,171 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigL2HighPtTrackFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 16.06.2010 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+
+#include "GaudiKernel/ToolFactory.h"
+
+#include "TrkSurfaces/Surface.h"
+#include "TrkSurfaces/PlaneSurface.h"
+#include "TrkParameters/TrackParameters.h"
+
+#include "TrkPrepRawData/PrepRawData.h"
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+
+#include "TrkRIO_OnTrack/RIO_OnTrack.h"
+
+#include "TrigInDetTrackFitter/TrigL2HighPtTrackFitter.h"
+
+TrigL2HighPtTrackFitter::TrigL2HighPtTrackFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ):
+ AthAlgTool(t,n,p),
+ m_recalibrate(false),
+ m_fastExtrapolator("TrigL2FastExtrapolationTool"),
+ m_ROTcreator("Trk::RIO_OnTrackCreator/InDetTrigBroadInDetRotCreator")
+{
+ declareInterface< ITrigL2TrackFittingTool >( this );
+ declareProperty( "fastExtrapolator", m_fastExtrapolator, "TrigL2FastExtrapolationTool");
+ declareProperty( "useROTs",m_recalibrate = true);
+ declareProperty( "ROTcreator", m_ROTcreator, "Trk::RIO_OnTrackCreator/InDetTrigBroadInDetRotCreator");
+}
+
+StatusCode TrigL2HighPtTrackFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+ MsgStream athenaLog(msgSvc(), name());
+
+ if(m_recalibrate)
+ {
+ sc=m_ROTcreator.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve RIO_OnTrack creator tool "<<m_ROTcreator);
+ return StatusCode::FAILURE;
+ }
+ }
+ sc=m_fastExtrapolator.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve extrapolation tool"<<m_fastExtrapolator);
+ return sc;
+ }
+ ATH_MSG_INFO("TrigL2HighPtTrackFitter initialized ");
+ return sc;
+}
+
+StatusCode TrigL2HighPtTrackFitter::finalize()
+{
+ StatusCode sc = AlgTool::finalize();
+ return sc;
+}
+
+void TrigL2HighPtTrackFitter::m_recalibrateFilteringNode(Trk::TrkBaseNode* pN, Trk::TrkTrackState* pTS)
+{
+ if(pTS->m_getSurface()==NULL) return;
+ AmgSymMatrix(5)* pM = new AmgSymMatrix(5);
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ (*pM)(i,j)=pTS->m_getTrackCovariance(i,j);
+
+ const Trk::PlaneSurface& pTrkSB = dynamic_cast<const Trk::PlaneSurface&>(pN->m_getPrepRawData()->detectorElement()->surface());
+ Trk::TrackParameters* pTP=new Trk::AtaPlane(pTS->m_getTrackState(0),pTS->m_getTrackState(1),
+ pTS->m_getTrackState(2),pTS->m_getTrackState(3),
+ pTS->m_getTrackState(4),pTrkSB,
+ pM);
+
+ const Trk::RIO_OnTrack* pRIO = m_ROTcreator->correct(*(pN->m_getPrepRawData()),*pTP);
+ pN->m_updateWithRIO(pRIO);
+
+ delete pTP;
+ delete pRIO;
+}
+
+Trk::TrkTrackState* TrigL2HighPtTrackFitter::fit(Trk::TrkTrackState* pTS, std::vector<Trk::TrkBaseNode*>& vpTrkNodes,
+ bool runSmoother)
+{
+ std::vector<Trk::TrkTrackState*> vpTrackStates;
+
+ vpTrackStates.clear();
+ vpTrackStates.push_back(pTS);
+
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt(vpTrkNodes.begin()),
+ pnEnd(vpTrkNodes.end());
+
+ bool OK=true;
+ Trk::TrkPlanarSurface *pSB=NULL,*pSE;
+
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_fastExtrapolator->extrapolate(pTS,pSB,pSE,runSmoother);
+ pSB=pSE;
+ if(pNS!=NULL)
+ {
+ vpTrackStates.push_back(pNS);
+
+ if(m_recalibrate)
+ {
+ m_recalibrateFilteringNode((*pnIt), pNS);
+ }
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_DEBUG("dChi2="<<(*pnIt)->m_getChi2());
+ (*pnIt)->m_updateTrackState(pNS);
+ pTS=pNS;
+ double Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ OK=false;break;
+ }
+ }
+ else
+ {
+ OK=false;break;
+ }
+ }
+ if(OK)
+ {
+ if(runSmoother)
+ {
+ std::vector<Trk::TrkTrackState*>::reverse_iterator ptsIt(vpTrackStates.rbegin()),
+ ptsEnd(vpTrackStates.rend());
+ for(;ptsIt!=ptsEnd;++ptsIt)
+ {
+ (*ptsIt)->m_runSmoother();
+ }
+ pTS=new Trk::TrkTrackState( (*vpTrackStates.begin()));
+ }
+ else
+ pTS=new Trk::TrkTrackState( (*vpTrackStates.rbegin()));
+ }
+ else
+ {
+ pTS=NULL;
+ ATH_MSG_DEBUG("High Pt track fitter: extrapolation failure ");
+ }
+ for(std::vector<Trk::TrkTrackState*>::iterator ptsIt=vpTrackStates.begin();
+ ptsIt!=vpTrackStates.end();++ptsIt) delete (*ptsIt);
+ vpTrackStates.clear();
+ return pTS;
+
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2LowPtTrackFitter.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2LowPtTrackFitter.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..aa9592a07f5deff079472e78ceac167bc59d75a0
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2LowPtTrackFitter.cxx
@@ -0,0 +1,323 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigL2LowPtTrackFitter tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 17.06.2010 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+#include <string.h>
+
+#include "GaudiKernel/ToolFactory.h"
+
+#include "TrkSurfaces/Surface.h"
+#include "TrkSurfaces/PlaneSurface.h"
+#include "TrkParameters/TrackParameters.h"
+#include "TrkPrepRawData/PrepRawData.h"
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+
+#include "TrkRIO_OnTrack/RIO_OnTrack.h"
+
+#include "TrigInDetTrackFitter/TrigL2LowPtTrackFitter.h"
+
+TrigL2LowPtTrackFitter::TrigL2LowPtTrackFitter(const std::string& t,
+ const std::string& n,
+ const IInterface* p ):
+ AthAlgTool(t,n,p),
+ m_recalibrate(false),
+ m_fastExtrapolator("TrigL2FastExtrapolationTool"),
+ m_ROTcreator("Trk::RIO_OnTrackCreator/InDetTrigBroadInDetRotCreator"),
+ m_extrapolator("Trk::Extrapolator/InDetTrigExtrapolator")
+{
+ declareInterface< ITrigL2TrackFittingTool >( this );
+ declareProperty( "fastExtrapolator", m_fastExtrapolator, "TrigL2FastExtrapolationTool");
+ declareProperty( "useROTs",m_recalibrate = true);
+ declareProperty( "ROTcreator", m_ROTcreator, "Trk::RIO_OnTrackCreator/InDetTrigBroadInDetRotCreator");
+ declareProperty( "TrackExtrapolatorTool",m_extrapolator, "Trk::Extrapolator/InDetTrigExtrapolator");
+}
+
+StatusCode TrigL2LowPtTrackFitter::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ if(m_recalibrate)
+ {
+ sc=m_ROTcreator.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve RIO_OnTrack creator tool "<<m_ROTcreator);
+ return StatusCode::FAILURE;
+ }
+ }
+ sc=m_fastExtrapolator.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve extrapolation tool"<<m_fastExtrapolator);
+ return sc;
+ }
+ sc=m_extrapolator.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve extrapolation tool"<<m_extrapolator);
+ return sc;
+ }
+
+ ATH_MSG_INFO("TrigL2LowPtTrackFitter initialized ");
+ return sc;
+}
+
+StatusCode TrigL2LowPtTrackFitter::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+void TrigL2LowPtTrackFitter::m_recalibrateFilteringNode(Trk::TrkBaseNode* pN, Trk::TrkTrackState* pTS)
+{
+ if(pTS->m_getSurface()==NULL) return;
+
+ AmgSymMatrix(5)* pM = new AmgSymMatrix(5);
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ (*pM)(i,j)=pTS->m_getTrackCovariance(i,j);
+
+ const Trk::PlaneSurface& pTrkSB = dynamic_cast<const Trk::PlaneSurface&>(pN->m_getPrepRawData()->detectorElement()->surface());
+ Trk::TrackParameters* pTP=new Trk::AtaPlane(pTS->m_getTrackState(0),pTS->m_getTrackState(1),
+ pTS->m_getTrackState(2),pTS->m_getTrackState(3),
+ pTS->m_getTrackState(4),pTrkSB,
+ pM);
+
+ const Trk::RIO_OnTrack* pRIO = m_ROTcreator->correct(*(pN->m_getPrepRawData()),*pTP);
+ pN->m_updateWithRIO(pRIO);
+
+ delete pTP;
+ delete pRIO;
+}
+
+Trk::TrkTrackState* TrigL2LowPtTrackFitter::m_extrapolateOffline(Trk::TrkTrackState* pTS,
+ Trk::TrkPlanarSurface* pSB,
+ Trk::TrkPlanarSurface* pSE,
+ int dir)
+{
+ //1. create starting parameters
+ Trk::TrackParameters* pTP=NULL;
+ Trk::TrkTrackState* pTE=NULL;
+
+ if(pSB==NULL)
+ {
+ // 1a. MeasuredPerigee
+
+ AmgSymMatrix(5)* pM = new AmgSymMatrix(5);
+ pM->setZero();
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ (*pM)(i,j)=pTS->m_getTrackCovariance(i,j);
+ const Trk::PerigeeSurface perSurf;
+ pTP=new Trk::Perigee(pTS->m_getTrackState(0),pTS->m_getTrackState(1),
+ pTS->m_getTrackState(2),pTS->m_getTrackState(3),
+ pTS->m_getTrackState(4),perSurf, pM);
+ }
+ else
+ {
+ const Trk::PlaneSurface* pTrkSB = dynamic_cast<const Trk::PlaneSurface*>(pSB->m_getTrkSurface());
+ AmgSymMatrix(5)* pM = new AmgSymMatrix(5);
+ pM->setZero();
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ (*pM)(i,j)=pTS->m_getTrackCovariance(i,j);
+
+ pTP=new Trk::AtaPlane(pTS->m_getTrackState(0),pTS->m_getTrackState(1),
+ pTS->m_getTrackState(2),pTS->m_getTrackState(3),
+ pTS->m_getTrackState(4),*pTrkSB,pM);
+ }
+
+ // 2. Extrapolation
+
+ const Trk::TrackParameters* predPar = NULL;
+ if(dir>0)
+ {
+ predPar = m_extrapolator->extrapolate(*pTP,*pSE->m_getTrkSurface(),
+ Trk::alongMomentum,false,
+ Trk::pion);
+ }
+ else
+ {
+ if(pSE!=NULL)
+ predPar = m_extrapolator->extrapolate(*pTP,*pSE->m_getTrkSurface(),
+ Trk::oppositeMomentum,false,
+ Trk::pion);
+ else
+ {
+ Trk::PerigeeSurface perSurf;
+ predPar = m_extrapolator->extrapolate(*pTP,perSurf,
+ Trk::oppositeMomentum,false,
+ Trk::pion);
+ }
+ }
+
+ if(predPar!=NULL)
+ {
+ if(pSE!=NULL)
+ {
+ const Trk::AtaPlane* pTPE = dynamic_cast<const Trk::AtaPlane*>(predPar);
+ if(pTPE!=NULL) {
+ // 4. Create new TrackState
+ double Re[5],Ge[5][5];
+ Re[0]=pTPE->parameters()[Trk::locX];Re[1]=pTPE->parameters()[Trk::locY];
+ Re[2]=pTPE->parameters()[Trk::phi];Re[3]=pTPE->parameters()[Trk::theta];
+ Re[4]=pTPE->parameters()[Trk::qOverP];
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ Ge[i][j]=(*pTPE->covariance())(i,j);
+
+ pTE=new Trk::TrkTrackState(pTS);
+ pTE->m_setTrackState(Re);
+ pTE->m_setTrackCovariance(Ge);
+ pTE->m_attachToSurface(pSE);
+ }
+ else pTE=NULL;
+ }
+ else
+ {
+ const Trk::Perigee* pTPE = dynamic_cast<const Trk::Perigee*>(predPar);
+ if(pTPE!=NULL) {
+ // 4. Create new TrackState
+ double Re[5],Ge[5][5];
+ Re[0]=pTPE->parameters()[Trk::d0];Re[1]=pTPE->parameters()[Trk::z0];
+ Re[2]=pTPE->parameters()[Trk::phi];Re[3]=pTPE->parameters()[Trk::theta];
+ Re[4]=pTPE->parameters()[Trk::qOverP];
+
+ for(int i=0;i<5;i++)
+ for(int j=0;j<5;j++)
+ Ge[i][j]=(*pTPE->covariance())(i,j);
+
+ pTE=new Trk::TrkTrackState(pTS);
+ pTE->m_setTrackState(Re);
+ pTE->m_setTrackCovariance(Ge);
+ }
+ else pTE=NULL;
+ }
+ delete predPar;
+ }
+ delete pTP;
+ return pTE;
+}
+
+Trk::TrkTrackState* TrigL2LowPtTrackFitter::fit(Trk::TrkTrackState* pTS, std::vector<Trk::TrkBaseNode*>& vpTrkNodes,
+ bool runSmoother)
+{
+
+ bool OK=true;
+ Trk::TrkPlanarSurface *pSB=NULL;
+
+ // 1. Forward filter
+
+ for(std::vector<Trk::TrkBaseNode*>::iterator pnIt=vpTrkNodes.begin();pnIt!=vpTrkNodes.end();++pnIt)
+ {
+ Trk::TrkPlanarSurface* pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_fastExtrapolator->extrapolate(pTS,pSB,pSE,false);
+ pSB=pSE;
+ if(pNS!=NULL)
+ {
+ // m_recalibrateFilteringNode((*pnIt),pNS);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_DEBUG("dChi2="<<(*pnIt)->m_getChi2());
+ (*pnIt)->m_updateTrackState(pNS);
+ delete pTS;
+ pTS=pNS;
+ double Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ delete pTS;
+ OK=false;break;
+ }
+ }
+ else
+ {
+ delete pTS;OK=false;break;
+ }
+ }
+ if(!OK) return NULL;
+
+ if(!runSmoother) return pTS;
+
+ // 2. Backward filter
+ // reset covariance
+ double Gk[5][5];
+ memset(&Gk[0][0],0,sizeof(Gk));
+ Gk[0][0]=100.0;Gk[1][1]=100.0;Gk[2][2]=0.01;Gk[3][3]=0.01;Gk[4][4]=1e-6;
+ pTS->m_setTrackCovariance(Gk);
+
+ for(std::vector<Trk::TrkBaseNode*>::reverse_iterator pnrIt=vpTrkNodes.rbegin();pnrIt!=vpTrkNodes.rend();++pnrIt)
+ {
+ Trk::TrkPlanarSurface* pSE=(*pnrIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=NULL;
+ if(pSE!=pSB)
+ {
+ double C1,C2,dist=0.0;
+ for(int i=0;i<3;i++)
+ {
+ C1=pSB->m_getCenter()[i];C2=pSE->m_getCenter()[i];
+ dist+=(C2-C1)*(C2-C1);
+ }
+ dist=sqrt(dist);
+ if(dist>100.0)
+ pNS=m_extrapolateOffline(pTS,pSB,pSE,-1);
+ else
+ pNS=m_fastExtrapolator->extrapolate(pTS,pSB,pSE,false);
+ }
+ else
+ pNS=new Trk::TrkTrackState(pTS);
+
+ pSB=pSE;
+ if(pNS!=NULL)
+ {
+ if(m_recalibrate)
+ {
+ m_recalibrateFilteringNode((*pnrIt),pNS);
+ }
+ (*pnrIt)->m_validateMeasurement(pNS);
+ ATH_MSG_DEBUG("dChi2="<<(*pnrIt)->m_getChi2());
+ (*pnrIt)->m_updateTrackState(pNS);
+ delete pTS;
+ pTS=pNS;
+ double Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ delete pTS;
+ OK=false;break;
+ }
+ }
+ else
+ {
+ delete pTS;OK=false;break;
+ }
+ }
+ if(!OK) return NULL;
+
+ // 3. Extrapolating back to perigee
+ Trk::TrkTrackState* pNS=m_extrapolateOffline(pTS,pSB,NULL,-1);
+ delete pTS;pTS=pNS;
+
+ return pTS;
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2ResidualCalculator.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2ResidualCalculator.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..063f55b56b2dfe47ec6a79119df9bf3d2d5a0a37
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/TrigL2ResidualCalculator.cxx
@@ -0,0 +1,844 @@
+/*
+ Copyright (C) 2002-2017 CERN for the benefit of the ATLAS collaboration
+*/
+
+////////////////////////////////////////////////////////////////////////////////
+// TrigL2ResidualCalculator tool
+// -------------------------------
+// ATLAS Collaboration
+//
+// 11.11.2009 Package created
+//
+// Author: Dmitry Emeliyanov, RAL
+// e-mail: D.Emeliyanov@rl.ac.uk
+//
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <iostream>
+
+#include "StoreGate/StoreGateSvc.h"
+#include "GaudiKernel/ToolFactory.h"
+#include "GaudiKernel/SystemOfUnits.h"
+#include "StoreGate/DataHandle.h"
+
+#include "AtlasDetDescr/AtlasDetectorID.h"
+#include "InDetIdentifier/SCT_ID.h"
+#include "InDetIdentifier/PixelID.h"
+
+#include "TrigInDetEvent/TrigInDetTrack.h"
+
+//#include "TrkSurfaces/Surface.h"
+//#include "TrkSurfaces/TrapezoidBounds.h"
+//#include "TrkParameters/MeasuredPerigee.h"
+
+#include "TrkPrepRawData/PrepRawData.h"
+#include "TrkDistributedKalmanFilter/TrkBaseNode.h"
+#include "TrkDistributedKalmanFilter/TrkFilteringNodes.h"
+#include "TrkDistributedKalmanFilter/TrkTrackState.h"
+#include "TrkDistributedKalmanFilter/TrkPlanarSurface.h"
+#include "MagFieldInterfaces/IMagFieldSvc.h"
+
+//#include "TrigInDetToolInterfaces/ITrigL2ResidualCalculator.h"
+#include "TrigInDetToolInterfaces/TrigL2HitResidual.h"
+#include "TrigInDetTrackFitter/TrigL2ResidualCalculator.h"
+
+TrigL2ResidualCalculator::TrigL2ResidualCalculator(const std::string& t,
+ const std::string& n,
+ const IInterface* p ) :
+ AthAlgTool(t,n,p),
+ m_MagFieldSvc("AtlasFieldSvc",this->name()),
+ m_trackMaker("TrigDkfTrackMakerTool")
+{
+ declareInterface< ITrigL2ResidualCalculator >( this );
+
+ declareProperty( "AthenaFieldService", m_MagFieldSvc, "AtlasFieldSvc");
+ declareProperty( "doMultScattering", m_doMultScatt = true);
+ declareProperty( "doBremmCorrection", m_doBremm=false);
+ declareProperty( "Chi2Cut", m_DChi2 = 1000.0);
+ declareProperty( "OfflineClusters", m_offlineClusters = true);
+}
+
+StatusCode TrigL2ResidualCalculator::initialize()
+{
+ StatusCode sc = AthAlgTool::initialize();
+
+ StoreGateSvc* detStore;
+ sc = service("DetectorStore", detStore);
+ if ( sc.isFailure() ) {
+ ATH_MSG_FATAL("DetStore service not found");
+ return StatusCode::FAILURE;
+ }
+
+ if (detStore->retrieve(m_idHelper, "AtlasID").isFailure()) {
+ ATH_MSG_FATAL("Could not get AtlasDetectorID helper AtlasID");
+ return StatusCode::FAILURE;
+ }
+
+ if (detStore->retrieve(m_pixelId, "PixelID").isFailure()) {
+ ATH_MSG_FATAL("Could not get Pixel ID helper");
+ return StatusCode::FAILURE;
+ }
+ if (detStore->retrieve(m_sctId, "SCT_ID").isFailure()) {
+ ATH_MSG_FATAL("Could not get SCT ID helper");;
+ return StatusCode::FAILURE;
+ }
+ ATH_MSG_INFO("Using Athena magnetic field service");;
+ sc = m_MagFieldSvc.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Unable to retrieve Athena MagFieldService");
+ return StatusCode::FAILURE;
+ }
+ sc=m_trackMaker.retrieve();
+ if(sc.isFailure())
+ {
+ ATH_MSG_ERROR("Could not retrieve "<<m_trackMaker);
+ return sc;
+ }
+ ATH_MSG_INFO("TrigL2ResidualCalculator constructed ");
+ return sc;
+}
+
+StatusCode TrigL2ResidualCalculator::finalize()
+{
+ StatusCode sc = AthAlgTool::finalize();
+ return sc;
+}
+
+
+void TrigL2ResidualCalculator::m_getMagneticField(double r[3],double* B)
+{
+ B[0]=0.0;B[1]=0.0;B[2]=0.0;
+ double field[3];
+ m_MagFieldSvc->getField(r,field);//field is returned in kT
+ for(int i=0;i<3;i++) B[i]=field[i]/CLHEP::kilogauss;//convert to kG
+}
+
+Trk::TrkTrackState* TrigL2ResidualCalculator::m_extrapolate(Trk::TrkTrackState* pTS,
+ Trk::TrkPlanarSurface* pSB,
+ Trk::TrkPlanarSurface* pSE)
+{
+ const double C=0.02999975;
+ const double minStep=30.0;
+
+ double sint,cost,sinf,cosf;
+ double gP[3],gPi[3],lP[3],gV[3],a,b,c,s,J0[7][5],Rf[5],descr,CQ,Ac,Av,Cc;
+ double V[3],P[3],M[3][3],AG[5][5],D[4],J[5][5],Jm[7][7],Gi[5][5],Gf[5][5],
+ J1[5][7],A[5][5],gB[3],gBi[3],gBf[3],dBds[3],Buf[5][7],DVx,DVy,DVz;
+ int i,j,m,nStep,nStepMax;
+ double sl,ds,path=0.0;
+
+ //m_numericalJacobian(pTS,pSB,pSE,J);
+
+ sint=sin(pTS->m_getTrackState(3));cosf=cos(pTS->m_getTrackState(2));
+ sinf=sin(pTS->m_getTrackState(2));cost=cos(pTS->m_getTrackState(3));
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;CQ=C*pTS->m_getTrackState(4);
+
+ memset(&J0[0][0],0,sizeof(J0));
+
+ if(pSB!=NULL)
+ {
+ double L[3][3];
+ lP[0]=pTS->m_getTrackState(0);lP[1]=pTS->m_getTrackState(1);lP[2]=0.0;
+ pSB->m_transformPointToGlobal(lP,gP);
+ for(i=0;i<3;i++) for(j=0;j<3;j++) L[i][j]=pSB->m_getInvRotMatrix(i,j);
+
+ J0[0][0]=L[0][0];J0[0][1]=L[0][1];
+ J0[1][0]=L[1][0];J0[1][1]=L[1][1];
+ J0[2][0]=L[2][0];J0[2][1]=L[2][1];
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ else
+ {
+ gP[0]=-pTS->m_getTrackState(0)*sinf;
+ gP[1]= pTS->m_getTrackState(0)*cosf;
+ gP[2]= pTS->m_getTrackState(1);
+ J0[0][0]=-sinf;J0[0][2]=-pTS->m_getTrackState(0)*cosf;
+ J0[1][0]= cosf;J0[1][2]=-pTS->m_getTrackState(0)*sinf;
+ J0[2][1]=1.0;
+ J0[3][2]=-sinf*sint;J0[3][3]=cosf*cost;
+ J0[4][2]= cosf*sint;J0[4][3]=sinf*cost;
+ J0[5][3]=-sint;
+ J0[6][4]=1.0;
+ }
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gPi[i]=gP[i];
+
+ m_getMagneticField(gP,gB);
+
+ for(i=0;i<3;i++) gBi[i]=gB[i];
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+
+ descr=b*b-4.0*a*c;
+
+ if(descr<0.0)
+ {
+ // printf("D<0 - extrapolation failed\n");
+ return NULL;
+ }
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+
+ if(fabs(sl)<minStep) nStepMax=1;
+ else
+ {
+ nStepMax=(int)(fabs(sl)/minStep)+1;
+ }
+ if((nStepMax<0)||(nStepMax>1000))
+ {
+ return NULL;
+ }
+ Av=sl*CQ;
+ Ac=0.5*sl*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*sl+Ac*DVx;
+ P[1]=gP[1]+gV[1]*sl+Ac*DVy;
+ P[2]=gP[2]+gV[2]*sl+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+
+ m_getMagneticField(P,gB);
+
+ for(i=0;i<3;i++) gBf[i]=gB[i];
+ for(i=0;i<3;i++)
+ {
+ dBds[i]=(gBf[i]-gBi[i])/sl;
+ gB[i]=gBi[i];
+ }
+ nStep=nStepMax;path=0.0;
+ while(nStep>0)
+ {
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=0.5*CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+ sl=-c/b;
+ sl=sl*(1-a*sl/b);
+ ds=sl/nStep;path+=ds;
+ Av=ds*CQ;
+ Ac=0.5*ds*Av;
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*ds+Ac*DVx;
+ P[1]=gP[1]+gV[1]*ds+Ac*DVy;
+ P[2]=gP[2]+gV[2]*ds+Ac*DVz;
+ V[0]=gV[0]+Av*DVx;
+ V[1]=gV[1]+Av*DVy;
+ V[2]=gV[2]+Av*DVz;
+ for(i=0;i<3;i++)
+ {
+ gV[i]=V[i];gP[i]=P[i];
+ }
+ for(i=0;i<3;i++) gB[i]+=dBds[i]*ds;
+ nStep--;
+ }
+ pSE->m_transformPointToLocal(gP,lP);
+ Rf[0]=lP[0];Rf[1]=lP[1];
+ Rf[2]=atan2(V[1],V[0]);
+
+ if(fabs(V[2])>1.0) return NULL;
+
+ Rf[3]=acos(V[2]);
+ Rf[4]=pTS->m_getTrackState(4);
+
+ gV[0]=sint*cosf;gV[1]=sint*sinf;gV[2]=cost;
+
+ for(i=0;i<4;i++) D[i]=pSE->m_getPar(i);
+ for(i=0;i<3;i++) gP[i]=gPi[i];
+
+ for(i=0;i<3;i++)
+ {
+ gB[i]=0.5*(gBi[i]+gBf[i]);
+ }
+
+ c=D[0]*gP[0]+D[1]*gP[1]+D[2]*gP[2]+D[3];
+ b=D[0]*gV[0]+D[1]*gV[1]+D[2]*gV[2];
+ a=CQ*(gB[0]*(D[1]*gV[2]-D[2]*gV[1])+
+ gB[1]*(D[2]*gV[0]-D[0]*gV[2])+
+ gB[2]*(D[0]*gV[1]-D[1]*gV[0]));
+ s=-c/b;
+ s=s*(1-a*s/b);
+ Av=s*CQ;
+ Ac=0.5*s*Av;
+ Cc=0.5*s*s*C;
+
+ DVx=gV[1]*gB[2]-gV[2]*gB[1];
+ DVy=gV[2]*gB[0]-gV[0]*gB[2];
+ DVz=gV[0]*gB[1]-gV[1]*gB[0];
+
+ P[0]=gP[0]+gV[0]*s+Ac*DVx;
+ P[1]=gP[1]+gV[1]*s+Ac*DVy;
+ P[2]=gP[2]+gV[2]*s+Ac*DVz;
+
+ V[0]=gV[0]+Av*DVx;V[1]=gV[1]+Av*DVy;V[2]=gV[2]+Av*DVz;
+
+ pSE->m_transformPointToLocal(P,lP);
+
+ memset(&Jm[0][0],0,sizeof(Jm));
+
+ for(i=0;i<3;i++) for(j=0;j<3;j++) M[i][j]=pSE->m_getRotMatrix(i,j);
+
+ double coeff[3], dadVx,dadVy,dadVz,dadQ,dsdx,dsdy,dsdz,dsdVx,dsdVy,dsdVz,dsdQ;
+ coeff[0]=-c*c/(b*b*b);
+ coeff[1]=c*(1.0+3.0*c*a/(b*b))/(b*b);
+ coeff[2]=-(1.0+2.0*c*a/(b*b))/b;
+
+ dadVx=0.5*CQ*(-D[1]*gB[2]+D[2]*gB[1]);
+ dadVy=0.5*CQ*( D[0]*gB[2]-D[2]*gB[0]);
+ dadVz=0.5*CQ*(-D[0]*gB[1]+D[1]*gB[0]);
+ dadQ=0.5*C*(D[0]*DVx+D[1]*DVy+D[2]*DVz);
+
+ dsdx=coeff[2]*D[0];
+ dsdy=coeff[2]*D[1];
+ dsdz=coeff[2]*D[2];
+ dsdVx=coeff[0]*dadVx+coeff[1]*D[0];
+ dsdVy=coeff[0]*dadVy+coeff[1]*D[1];
+ dsdVz=coeff[0]*dadVz+coeff[1]*D[2];
+ dsdQ=coeff[0]*dadQ;
+
+ Jm[0][0]=1.0+V[0]*dsdx;
+ Jm[0][1]= V[0]*dsdy;
+ Jm[0][2]= V[0]*dsdz;
+
+ Jm[0][3]= s+V[0]*dsdVx;
+ Jm[0][4]= V[0]*dsdVy+Ac*gB[2];
+ Jm[0][5]= V[0]*dsdVz-Ac*gB[1];
+ Jm[0][6]= V[0]*dsdQ+Cc*DVx;
+
+ Jm[1][0]= V[1]*dsdx;
+ Jm[1][1]=1.0+V[1]*dsdy;
+ Jm[1][2]= V[1]*dsdz;
+
+ Jm[1][3]= V[1]*dsdVx-Ac*gB[2];
+ Jm[1][4]= s+V[1]*dsdVy;
+ Jm[1][5]= V[1]*dsdVz+Ac*gB[0];
+ Jm[1][6]= V[1]*dsdQ+Cc*DVy;
+
+ Jm[2][0]= V[2]*dsdx;
+ Jm[2][1]= V[2]*dsdy;
+ Jm[2][2]=1.0+V[2]*dsdz;
+ Jm[2][3]= V[2]*dsdVx+Ac*gB[1];
+ Jm[2][4]= V[2]*dsdVy-Ac*gB[0];
+ Jm[2][5]= s+V[2]*dsdVz;
+ Jm[2][6]= V[2]*dsdQ+Cc*DVz;
+
+ Jm[3][0]=dsdx*CQ*DVx;
+ Jm[3][1]=dsdy*CQ*DVx;
+ Jm[3][2]=dsdz*CQ*DVx;
+
+ Jm[3][3]=1.0+dsdVx*CQ*DVx;
+ Jm[3][4]=CQ*(dsdVy*DVx+s*gB[2]);
+ Jm[3][5]=CQ*(dsdVz*DVx-s*gB[1]);
+
+ Jm[3][6]=(CQ*dsdQ+C*s)*DVx;
+
+ Jm[4][0]=dsdx*CQ*DVy;
+ Jm[4][1]=dsdy*CQ*DVy;
+ Jm[4][2]=dsdz*CQ*DVy;
+
+ Jm[4][3]=CQ*(dsdVx*DVy-s*gB[2]);
+ Jm[4][4]=1.0+dsdVy*CQ*DVy;
+ Jm[4][5]=CQ*(dsdVz*DVy+s*gB[0]);
+
+ Jm[4][6]=(CQ*dsdQ+C*s)*DVy;
+
+ Jm[5][0]=dsdx*CQ*DVz;
+ Jm[5][1]=dsdy*CQ*DVz;
+ Jm[5][2]=dsdz*CQ*DVz;
+ Jm[5][3]=CQ*(dsdVx*DVz+s*gB[1]);
+ Jm[5][4]=CQ*(dsdVy*DVz-s*gB[0]);
+ Jm[5][5]=1.0+dsdVz*CQ*DVz;
+ Jm[5][6]=(CQ*dsdQ+C*s)*DVz;
+
+ Jm[6][6]=1.0;
+
+ memset(&J1[0][0],0,sizeof(J1));
+
+ J1[0][0]=M[0][0];J1[0][1]=M[0][1];J1[0][2]=M[0][2];
+ J1[1][0]=M[1][0];J1[1][1]=M[1][1];J1[1][2]=M[1][2];
+ J1[2][3]=-V[1]/(V[0]*V[0]+V[1]*V[1]);
+ J1[2][4]= V[0]/(V[0]*V[0]+V[1]*V[1]);
+ J1[3][5]=-1.0/sqrt(1-V[2]*V[2]);
+ J1[4][6]=1.0;
+
+ for(i=0;i<7;i++)
+ {
+ for(j=0;j<2;j++)
+ Buf[j][i]=J1[j][0]*Jm[0][i]+J1[j][1]*Jm[1][i]+J1[j][2]*Jm[2][i];
+ Buf[2][i]=J1[2][3]*Jm[3][i]+J1[2][4]*Jm[4][i];
+ Buf[3][i]=J1[3][5]*Jm[5][i];
+ Buf[4][i]=Jm[6][i];
+ }
+
+ if(pSB!=NULL)
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0]+Buf[i][2]*J0[2][0];
+ J[i][1]=Buf[i][0]*J0[0][1]+Buf[i][1]*J0[1][1]+Buf[i][2]*J0[2][1];
+ J[i][2]=Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ else
+ {
+ for(i=0;i<5;i++)
+ {
+ J[i][0]=Buf[i][0]*J0[0][0]+Buf[i][1]*J0[1][0];
+ J[i][1]=Buf[i][2];
+ J[i][2]=Buf[i][0]*J0[0][2]+Buf[i][1]*J0[1][2]+Buf[i][3]*J0[3][2]+Buf[i][4]*J0[4][2];
+ J[i][3]=Buf[i][3]*J0[3][3]+Buf[i][4]*J0[4][3]+Buf[i][5]*J0[5][3];
+ J[i][4]=Buf[i][6];
+ }
+ }
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ AG[i][j]=0.0;for(m=0;m<5;m++) AG[i][j]+=J[i][m]*pTS->m_getTrackCovariance(m,j);
+ }
+ for(i=0;i<5;i++) for(j=i;j<5;j++)
+ {
+ Gf[i][j]=0.0;
+ for(m=0;m<5;m++) Gf[i][j]+=AG[i][m]*J[j][m];
+ Gf[j][i]=Gf[i][j];
+ }
+ Trk::TrkTrackState* pTE=new Trk::TrkTrackState(pTS);
+ pTE->m_setTrackState(Rf);
+ pTE->m_setTrackCovariance(Gf);
+ pTE->m_attachToSurface(pSE);
+ pTE->m_applyMaterialEffects();
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ Gi[i][j]=pTE->m_getTrackCovariance(i,j);
+ }
+
+ m_matrixInversion5x5(Gi);
+
+ for(i=0;i<5;i++) for(j=0;j<5;j++)
+ {
+ A[i][j]=0.0;
+ for(m=0;m<5;m++) A[i][j]+=AG[m][i]*Gi[m][j];
+ }
+ pTE->m_setPreviousState(pTS);
+ pTE->m_setSmootherGain(A);
+
+ return pTE;
+}
+
+void TrigL2ResidualCalculator::m_matrixInversion5x5(double a[5][5])
+{
+ /**** 5x5 matrix inversion by Gaussian elimination ****/
+
+ int i,j,k,l;
+ double factor;
+ double temp[5];
+ double b[5][5];
+ // Set b to I
+
+ memset(&b[0][0],0,sizeof(b));
+ b[0][0]=1.0;b[1][1]=1.0;b[2][2]=1.0;b[3][3]=1.0;b[4][4]=1.0;
+
+ for(i=0;i<5;i++)
+ {
+ for(j=i+1;j<5;j++)
+ if (fabs(a[i][i])<fabs(a[j][i]))
+ {
+ for(l=0;l<5;l++) temp[l]=a[i][l];
+ for(l=0;l<5;l++) a[i][l]=a[j][l];
+ for(l=0;l<5;l++) a[j][l]=temp[l];
+ for(l=0;l<5;l++) temp[l]=b[i][l];
+ for(l=0;l<5;l++) b[i][l]=b[j][l];
+ for(l=0;l<5;l++) b[j][l]=temp[l];
+ }
+ factor=a[i][i];
+ for(j=4;j>-1;j--)
+ {
+ b[i][j]/=factor;a[i][j]/=factor;
+ }
+ for(j=i+1;j<5;j++)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=4;i>0;i--)
+ {
+ for(j=i-1;j>-1;j--)
+ {
+ factor=-a[j][i];
+ for(k=0;k<5;k++)
+ {
+ a[j][k]+=a[i][k]*factor;b[j][k]+=b[i][k]*factor;
+ }
+ }
+ }
+ for(i=0;i<5;i++) for(j=0;j<5;j++) a[i][j]=b[i][j];
+}
+
+
+StatusCode TrigL2ResidualCalculator::getResiduals(const TrigInDetTrack* pT, std::vector<TrigL2HitResidual>& vResid)
+{
+
+ double Rk[5],Pt,Theta;
+
+ std::vector<Trk::TrkBaseNode*> vpTrkNodes;
+ std::vector<Trk::TrkTrackState*> vpTrackStates;
+
+ vResid.clear();
+
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>(pT->param());
+ if(param==NULL)
+ {
+ ATH_MSG_WARNING("TrigL2ResidualCalculator failed -- TrigInDetTrack has no parameters");
+ return StatusCode::FAILURE;
+ }
+
+ // 1. Create initial track state:
+
+ Rk[0]=param->a0();
+ Rk[1]=param->z0();
+ Rk[2]=param->phi0();
+ Theta=2.0*atan(exp(-param->eta()));
+ Rk[3]=Theta;
+ Rk[4]=sin(Theta)/param->pT();
+ Pt=param->pT();
+
+ if(fabs(Pt)<100.0)
+ {
+ ATH_MSG_DEBUG("TrigL2ResidualCalculator failed -- Estimated Pt is too low "<<Pt);
+ return StatusCode::FAILURE;
+ }
+
+ // 2. Create filtering nodes
+
+ bool trackResult = m_trackMaker->createDkfTrack(*(pT->siSpacePoints()),vpTrkNodes, m_DChi2);
+ if(!trackResult)
+ {
+ ATH_MSG_DEBUG("TrigDkfTrackMaker failed");
+ return StatusCode::FAILURE;
+ }
+
+ // 3. Main algorithm: filter and smoother (Rauch-Tung-Striebel)
+
+ Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ double Gk[5][5];
+ memset(&Gk[0][0],0,sizeof(Gk));
+ Gk[0][0]=100.0;Gk[1][1]=100.0;Gk[2][2]=0.01;Gk[3][3]=0.01;Gk[4][4]=1e-6;
+ pTS->m_setTrackCovariance(Gk);
+ if(m_doMultScatt)
+ pTS->m_setScatteringMode(1);
+ if(m_doBremm)
+ pTS->m_setScatteringMode(2);
+ vpTrackStates.push_back(pTS);
+
+ ATH_MSG_DEBUG("Initial params: locT="<<Rk[0]<<" locL="<<Rk[1]<<" phi="<<Rk[2]
+ <<" theta="<<Rk[3]<<" Q="<<Rk[4]<<" pT="<<sin(Rk[3])/Rk[4]);
+
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt(vpTrkNodes.begin()),
+ pnEnd(vpTrkNodes.end());
+
+ bool OK=true;
+ Trk::TrkPlanarSurface *pSB=NULL,*pSE;
+
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_extrapolate(pTS,pSB,pSE);
+
+ pSB=pSE;
+ if(pNS!=NULL)
+ {
+ vpTrackStates.push_back(pNS);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_DEBUG("dChi2="<<(*pnIt)->m_getChi2());
+ (*pnIt)->m_updateTrackState(pNS);
+ pTS=pNS;
+ Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ OK=false;break;
+ }
+ }
+ else
+ {
+ OK=false;break;
+ }
+ }
+ if(OK)
+ {
+ std::vector<Trk::TrkTrackState*>::reverse_iterator ptsrIt(vpTrackStates.rbegin()),
+ ptsrEnd(vpTrackStates.rend());
+
+ for(;ptsrIt!=ptsrEnd;++ptsrIt)
+ {
+ (*ptsrIt)->m_runSmoother();
+ }
+
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt(vpTrkNodes.begin()),
+ pnEnd(vpTrkNodes.end());
+ std::vector<Trk::TrkTrackState*>::iterator ptsIt(vpTrackStates.begin());
+
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ ++ptsIt;
+
+ Trk::TrkTrackState* pNS=(*ptsIt);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+
+ double r[2],V[2][2];
+
+ int nSize=(*pnIt)->m_getResiduals(r);
+ nSize=(*pnIt)->m_getInverseResidualVariance(V);
+ const Trk::PrepRawData* pPRD = (*pnIt)->m_getPrepRawData();
+
+ Identifier id = pPRD->identify();
+
+ int Region=-1;
+ if(m_idHelper->is_pixel(id))
+ {
+ Region=(m_pixelId->is_barrel(id))?1:2;
+ }
+ if(m_idHelper->is_sct(id))
+ {
+ Region=(m_sctId->is_barrel(id))?3:4;
+ }
+ if(nSize==1) {
+ if(V[0][0]>0.0) {
+ vResid.push_back(TrigL2HitResidual(id,Region,r[0],r[0]*sqrt(V[0][0])));
+ }
+ else {
+ OK=false;
+ break;
+ }
+ }
+ else {
+ if((V[0][0]>0.0) && (V[1][1]>0.0)) {
+ vResid.push_back(TrigL2HitResidual(id,Region,r[0],r[0]*sqrt(V[0][0]),
+ r[1],r[1]*sqrt(V[1][1])));
+ }
+ else {
+ OK=false;
+ break;
+ }
+ }
+ }
+ }
+ else
+ {
+ ATH_MSG_DEBUG("Forward Kalman filter: extrapolation failure ");
+ }
+ pnIt=vpTrkNodes.begin();pnEnd=vpTrkNodes.end();
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ delete((*pnIt)->m_getSurface());
+ delete (*pnIt);
+ }
+ vpTrkNodes.clear();
+ for(std::vector<Trk::TrkTrackState*>::iterator ptsIt=vpTrackStates.begin();
+ ptsIt!=vpTrackStates.end();++ptsIt) delete (*ptsIt);
+ vpTrackStates.clear();
+ if(OK) return StatusCode::SUCCESS;
+ else return StatusCode::FAILURE;
+
+}
+
+StatusCode TrigL2ResidualCalculator::getUnbiassedResiduals(const TrigInDetTrack* pT,
+ std::vector<TrigL2HitResidual>& vResid)
+{
+
+ double Rk[5],Pt,Theta;
+
+ std::vector<Trk::TrkBaseNode*> vpTrkNodes;
+ std::vector<Trk::TrkTrackState*> vpTrackStates;
+
+ vResid.clear();
+
+ TrigInDetTrackFitPar* param=const_cast<TrigInDetTrackFitPar*>(pT->param());
+ if(param==NULL)
+ {
+ ATH_MSG_WARNING("TrigL2ResidualCalculator failed -- TrigInDetTrack has no parameters");
+ return StatusCode::FAILURE;
+ }
+
+ Pt=param->pT();
+ if(fabs(Pt)<100.0)
+ {
+ ATH_MSG_DEBUG("TrigL2ResidualCalculator failed -- Estimated Pt is too low "<<Pt);
+ return StatusCode::FAILURE;
+ }
+
+ // 1. Create filtering nodes
+
+ bool trackResult = m_trackMaker->createDkfTrack(*(pT->siSpacePoints()),vpTrkNodes, m_DChi2);
+ if(!trackResult)
+ {
+ ATH_MSG_DEBUG("TrigDkfTrackMaker failed");
+ return StatusCode::FAILURE;
+ }
+
+ int nNodeIndex=-1;
+ bool OK=true;
+ std::vector<Trk::TrkBaseNode*>::iterator pnIt,pnEnd(vpTrkNodes.end());
+
+ for(std::vector<Trk::TrkBaseNode*>::iterator pNodeIt=vpTrkNodes.begin();pNodeIt!=vpTrkNodes.end();
+ ++pNodeIt)
+ {
+ nNodeIndex++;
+ Trk::TrkBaseNode* pMaskedNode=(*pNodeIt);
+ Trk::TrkTrackState* pMaskedState=NULL;
+
+ // 2. Create initial track state:
+
+ Rk[0]=param->a0();
+ Rk[1]=param->z0();
+ Rk[2]=param->phi0();
+ Theta=2.0*atan(exp(-param->eta()));
+ Rk[3]=Theta;
+ Rk[4]=sin(Theta)/param->pT();
+
+ // 3. Main algorithm: filter and smoother (Rauch-Tung-Striebel)
+
+ Trk::TrkTrackState* pTS = new Trk::TrkTrackState(Rk);
+ double Gk[5][5];
+ memset(&Gk[0][0],0,sizeof(Gk));
+ Gk[0][0]=100.0;Gk[1][1]=100.0;Gk[2][2]=0.01;Gk[3][3]=0.01;Gk[4][4]=1e-6;
+ pTS->m_setTrackCovariance(Gk);
+ if(m_doMultScatt)
+ pTS->m_setScatteringMode(1);
+ if(m_doBremm)
+ pTS->m_setScatteringMode(2);
+ vpTrackStates.push_back(pTS);
+
+ ATH_MSG_DEBUG("Initial params: locT="<<Rk[0]<<" locL="<<Rk[1]<<" phi="<<Rk[2]
+ <<" theta="<<Rk[3]<<" Q="<<Rk[4]<<" pT="<<sin(Rk[3])/Rk[4]);
+
+ OK=true;
+ Trk::TrkPlanarSurface *pSB=NULL,*pSE;
+
+ for(pnIt=vpTrkNodes.begin();pnIt!=pnEnd;++pnIt)
+ {
+ pSE=(*pnIt)->m_getSurface();
+ Trk::TrkTrackState* pNS=m_extrapolate(pTS,pSB,pSE);
+
+ pSB=pSE;
+ if(pNS!=NULL)
+ {
+ vpTrackStates.push_back(pNS);
+
+ (*pnIt)->m_validateMeasurement(pNS);
+ ATH_MSG_DEBUG("dChi2="<<(*pnIt)->m_getChi2());
+ if((*pnIt)!=pMaskedNode)
+ {
+ (*pnIt)->m_updateTrackState(pNS);
+ }
+ else
+ {
+ pMaskedState=pNS;
+ }
+ pTS=pNS;
+ Pt=sin(pTS->m_getTrackState(3))/pTS->m_getTrackState(4);
+ if(fabs(Pt)<200.0)
+ {
+ ATH_MSG_DEBUG("Estimated Pt is too low "<<Pt<<" - skipping fit");
+ OK=false;break;
+ }
+ }
+ else
+ {
+ OK=false;break;
+ }
+ }
+ if(OK)
+ {
+ std::vector<Trk::TrkTrackState*>::reverse_iterator ptsrIt(vpTrackStates.rbegin()),
+ ptsrEnd(vpTrackStates.rend());
+
+ for(;ptsrIt!=ptsrEnd;++ptsrIt)
+ {
+ (*ptsrIt)->m_runSmoother();
+ }
+
+ pMaskedNode->m_validateMeasurement(pMaskedState);
+
+ double r[2],V[2][2];
+
+ int nSize=pMaskedNode->m_getResiduals(r);
+ nSize=pMaskedNode->m_getInverseResidualVariance(V);
+ const Trk::PrepRawData* pPRD = pMaskedNode->m_getPrepRawData();
+
+ Identifier id = pPRD->identify();
+
+ int Region=-1;
+ if(m_idHelper->is_pixel(id))
+ {
+ Region=(m_pixelId->is_barrel(id))?1:2;
+ }
+ if(m_idHelper->is_sct(id))
+ {
+ Region=(m_sctId->is_barrel(id))?3:4;
+ }
+ if(nSize==1) {
+ if(V[0][0]>0.0) {
+ vResid.push_back(TrigL2HitResidual(id,Region,r[0],r[0]*sqrt(V[0][0])));
+ }
+ else {
+ OK=false;
+ break;
+ }
+ }
+ else {
+ if((V[0][0]>0.0) && (V[1][1]>0.0)) {
+ vResid.push_back(TrigL2HitResidual(id,Region,r[0],r[0]*sqrt(V[0][0]),
+ r[1],r[1]*sqrt(V[1][1])));
+ }
+ else {
+ OK=false;
+ break;
+ }
+ }
+ }
+ else
+ {
+ ATH_MSG_DEBUG("Forward Kalman filter: extrapolation failure ");
+ vResid.clear();
+ }
+ for(std::vector<Trk::TrkTrackState*>::iterator ptsIt=vpTrackStates.begin();
+ ptsIt!=vpTrackStates.end();++ptsIt) delete (*ptsIt);
+ vpTrackStates.clear();
+ if(!OK) break;
+ }
+ pnIt=vpTrkNodes.begin();pnEnd=vpTrkNodes.end();
+ for(;pnIt!=pnEnd;++pnIt)
+ {
+ delete((*pnIt)->m_getSurface());
+ delete (*pnIt);
+ }
+ vpTrkNodes.clear();
+
+ if(OK) return StatusCode::SUCCESS;
+ else return StatusCode::FAILURE;
+
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_entries.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_entries.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..174251b1f08c854289c90e46571bb27881075f61
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_entries.cxx
@@ -0,0 +1,43 @@
+#include "TrigInDetTrackFitter/TrigInDetTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetPerigeeFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetSctKFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetKarimakiFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetBremDetectionTool.h"
+#include "TrigInDetTrackFitter/TrigDkfTrackMakerTool.h"
+#include "TrigInDetTrackFitter/TrigL2ResidualCalculator.h"
+#include "TrigInDetTrackFitter/TrigInDetOfflineTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigInDetCombinedTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigL2HighPtTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigL2LowPtTrackFitter.h"
+#include "TrigInDetTrackFitter/TrigL2FastExtrapolationTool.h"
+
+#include "GaudiKernel/DeclareFactoryEntries.h"
+
+DECLARE_TOOL_FACTORY(TrigInDetTrackFitter)
+DECLARE_TOOL_FACTORY(TrigInDetPerigeeFitter)
+DECLARE_TOOL_FACTORY(TrigInDetSctKFitter)
+DECLARE_TOOL_FACTORY(TrigInDetKarimakiFitter)
+DECLARE_TOOL_FACTORY(TrigInDetBremDetectionTool)
+DECLARE_TOOL_FACTORY(TrigDkfTrackMakerTool)
+DECLARE_TOOL_FACTORY(TrigL2ResidualCalculator)
+DECLARE_TOOL_FACTORY(TrigInDetOfflineTrackFitter)
+DECLARE_TOOL_FACTORY(TrigInDetCombinedTrackFitter)
+DECLARE_TOOL_FACTORY(TrigL2HighPtTrackFitter)
+DECLARE_TOOL_FACTORY(TrigL2LowPtTrackFitter)
+DECLARE_TOOL_FACTORY(TrigL2FastExtrapolationTool)
+
+DECLARE_FACTORY_ENTRIES(TrigInDetTrackFitter)
+{
+ DECLARE_TOOL(TrigInDetTrackFitter)
+ DECLARE_TOOL(TrigInDetPerigeeFitter)
+ DECLARE_TOOL(TrigInDetSctKFitter)
+ DECLARE_TOOL(TrigInDetKarimakiFitter)
+ DECLARE_TOOL(TrigInDetBremDetectionTool)
+ DECLARE_TOOL(TrigDkfTrackMakerTool)
+ DECLARE_TOOL(TrigL2ResidualCalculator)
+ DECLARE_TOOL(TrigInDetOfflineTrackFitter)
+ DECLARE_TOOL(TrigInDetCombinedTrackFitter)
+ DECLARE_TOOL(TrigL2HighPtTrackFitter)
+ DECLARE_TOOL(TrigL2LowPtTrackFitter)
+ DECLARE_TOOL(TrigL2FastExtrapolationTool)
+}
diff --git a/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_load.cxx b/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_load.cxx
new file mode 100755
index 0000000000000000000000000000000000000000..871f3f548572937299ca49cb94bb5a94b24dc7b0
--- /dev/null
+++ b/Trigger/TrigTools/TrigInDetTrackFitter/src/components/TrigInDetTrackFitter_load.cxx
@@ -0,0 +1,4 @@
+#include "GaudiKernel/LoadFactoryEntries.h"
+
+LOAD_FACTORY_ENTRIES( TrigInDetTrackFitter )
+