TrajectoryState/src/PerigeeConversions.cc

#include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
#include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
#include "MagneticField/Engine/interface/MagneticField.h" 
#include <cmath>

PerigeeTrajectoryParameters PerigeeConversions::ftsToPerigeeParameters
  (const FTS& originalFTS, const GlobalPoint& referencePoint, double & pt) const

{
  GlobalVector impactDistance = originalFTS.position() - referencePoint;

  pt = originalFTS.momentum().perp();
  if (pt==0.) throw cms::Exception("PerigeeConversions", "Track with pt=0");
  
  double theta = originalFTS.momentum().theta();
  double phi = originalFTS.momentum().phi();
  double field  = originalFTS.parameters().magneticField().inInverseGeV(originalFTS.position()).z();
//   if (field==0.) throw cms::Exception("PerigeeConversions", "Field is 0") << " at " << originalFTS.position() << "\n" ;

  double positiveMomentumPhi = ( (phi>0) ? phi : (2*M_PI+phi) );
  double positionPhi = impactDistance.phi();
  double positivePositionPhi =
   ( (positionPhi>0) ? positionPhi : (2*M_PI+positionPhi) );
  double phiDiff = positiveMomentumPhi - positivePositionPhi;
  if (phiDiff<0.0) phiDiff+= (2*M_PI);
  double signEpsilon = ( (phiDiff > M_PI) ? -1.0 : 1.0);

  double epsilon = signEpsilon *
                     sqrt ( impactDistance.x()*impactDistance.x() +
                            impactDistance.y()*impactDistance.y() );

  // The track parameters:
  AlgebraicVector5 theTrackParameters;

  double signTC = - originalFTS.charge();
  bool isCharged = (signTC!=0) && (fabs(field)>1.e-10);
  if (isCharged) {
    theTrackParameters[0] = field / pt*signTC;
  } else {
    theTrackParameters[0] = 1 / pt;
  }
  theTrackParameters[1] = theta;
  theTrackParameters[2] = phi;
  theTrackParameters[3] = epsilon;
  theTrackParameters[4] = impactDistance.z();
  return PerigeeTrajectoryParameters(theTrackParameters, isCharged);
}

// PerigeeTrajectoryParameters PerigeeConversions::helixToPerigeeParameters
//   (const reco::helix::Parameters & helixPar, const GlobalPoint& referencePoint) const
// {
//   AlgebraicVector theTrackParameters = AlgebraicVector(5);
//   double field  = TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3;
//   theTrackParameters[0] = - field*helixPar.omega();
//   theTrackParameters[1] = atan(1/helixPar.tanDip());
//   theTrackParameters[2] = helixPar.phi0() - M_PI/2;
//   theTrackParameters[3] = helixPar.d0();
//   theTrackParameters[4] = helixPar.dz();
//   return PerigeeTrajectoryParameters(theTrackParameters, helixPar.pt(), true);
// }

PerigeeTrajectoryError PerigeeConversions::ftsToPerigeeError
  (const FTS& originalFTS) const
{
  AlgebraicSymMatrix55 errorMatrix = originalFTS.curvilinearError().matrix();
  AlgebraicMatrix55 curv2perigee = jacobianCurvilinear2Perigee(originalFTS);
  return PerigeeTrajectoryError(ROOT::Math::Similarity(curv2perigee,errorMatrix));
}

// PerigeeTrajectoryError PerigeeConversions::helixToPerigeeError
//   (const reco::helix::Parameters & helixPar, 
//      const reco::helix::Covariance & helixCov) const
// {
// //FIXME: verify that the order of the parameters are correct
//   AlgebraicSymMatrix55 helixCovMatrix;
//   helixCovMatrix(0,0) = helixCov(reco::helix::i_d0,reco::helix::i_d0);
//   helixCovMatrix(1,1) = helixCov(reco::helix::i_phi0,reco::helix::i_phi0);
//   helixCovMatrix(2,2) = helixCov(reco::helix::i_omega,reco::helix::i_omega);
//   helixCovMatrix(3,3) = helixCov(reco::helix::i_dz,reco::helix::i_dz);
//   helixCovMatrix(4,4) = helixCov(reco::helix::i_tanDip,reco::helix::i_tanDip);
// 
//   helixCovMatrix(0,1) = helixCov(reco::helix::i_d0,reco::helix::i_phi0);
//   helixCovMatrix(0,2) = helixCov(reco::helix::i_d0,reco::helix::i_omega);
//   helixCovMatrix(0,3) = helixCov(reco::helix::i_d0,reco::helix::i_dz);
//   helixCovMatrix(0,4) = helixCov(reco::helix::i_d0,reco::helix::i_tanDip);
// 
//   helixCovMatrix(1,2) = helixCov(reco::helix::i_phi0,reco::helix::i_omega);
//   helixCovMatrix(1,3) = helixCov(reco::helix::i_phi0,reco::helix::i_dz);
//   helixCovMatrix(1,4) = helixCov(reco::helix::i_phi0,reco::helix::i_tanDip);
// 
//   helixCovMatrix(2,3) = helixCov(reco::helix::i_omega,reco::helix::i_dz);
//   helixCovMatrix(2,4) = helixCov(reco::helix::i_omega,reco::helix::i_tanDip);
// 
//   helixCovMatrix(3,4) = helixCov(reco::helix::i_dz,reco::helix::i_tanDip);
// 
//   AlgebraicMatrix helix2perigee = jacobianHelix2Perigee(helixPar, helixCov);
//   return PerigeeTrajectoryError(helixCovMatrix.similarity(helix2perigee));
// }


CurvilinearTrajectoryError PerigeeConversions::curvilinearError
  (const PerigeeTrajectoryError& perigeeError, const GlobalTrajectoryParameters& gtp) const
{
  AlgebraicMatrix55 perigee2curv = jacobianPerigee2Curvilinear(gtp);
  return CurvilinearTrajectoryError(ROOT::Math::Similarity(perigee2curv, perigeeError.covarianceMatrix()));
}

GlobalPoint PerigeeConversions::positionFromPerigee
  (const PerigeeTrajectoryParameters& parameters, const GlobalPoint& referencePoint) const
{
  AlgebraicVector5 theVector = parameters.vector();
  return GlobalPoint(theVector[3]*sin(theVector[2])+referencePoint.x(),
                     -theVector[3]*cos(theVector[2])+referencePoint.y(),
                     theVector[4]+referencePoint.z());
}


GlobalVector PerigeeConversions::momentumFromPerigee
  (const PerigeeTrajectoryParameters& parameters, double pt, const GlobalPoint& referencePoint) const
{
  return GlobalVector(cos(parameters.phi()) * pt,
                      sin(parameters.phi()) * pt,
                      pt / tan(parameters.theta()));
}


GlobalVector PerigeeConversions::momentumFromPerigee
  (const AlgebraicVector3& momentum, const TrackCharge& charge, 
   const GlobalPoint& referencePoint, const MagneticField* field) const
{
  double pt;
  if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");

  double bz = fabs(field->inInverseGeV(referencePoint).z());
  if ( charge!=0 && bz>1.e-10 ) {
    pt = std::abs(bz/momentum[0]);
    if (pt<1.e-10) throw cms::Exception("PerigeeConversions", "pt is 0");
  } else {
    pt = 1 / momentum[0];
  }

  return GlobalVector(cos(momentum[2]) * pt,
                      sin(momentum[2]) * pt,
                      pt/tan(momentum[1]));
}

TrackCharge PerigeeConversions::chargeFromPerigee
  (const PerigeeTrajectoryParameters& parameters) const
{
  return parameters.charge();
}

TrajectoryStateClosestToPoint PerigeeConversions::trajectoryStateClosestToPoint
        (const AlgebraicVector3& momentum, const GlobalPoint& referencePoint,
         const TrackCharge& charge, const AlgebraicSymMatrix66& theCovarianceMatrix,
         const MagneticField* field) const
{
  AlgebraicMatrix66 param2cart = jacobianParameters2Cartesian
        (momentum, referencePoint, charge, field);
  CartesianTrajectoryError cartesianTrajErr(ROOT::Math::Similarity(param2cart, theCovarianceMatrix));

  FTS theFTS(GlobalTrajectoryParameters(referencePoint,
            momentumFromPerigee(momentum, charge, referencePoint, field), charge,
            field), cartesianTrajErr);

  return TrajectoryStateClosestToPoint(theFTS, referencePoint);
}


AlgebraicMatrix66
PerigeeConversions::jacobianParameters2Cartesian(
        const AlgebraicVector3& momentum, const GlobalPoint& position,
        const TrackCharge& charge, const MagneticField* field) const
{
  if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");
  double factor = 1.;
  double bField = field->inInverseGeV(position).z();
  if (charge!=0 && fabs(bField)>1.e-10) {
//     if (bField==0.) throw cms::Exception("PerigeeConversions", "Field is 0");
    factor = -bField*charge;
  }
  AlgebraicMatrix66 frameTransJ;
  frameTransJ(0,0) = 1;
  frameTransJ(1,1) = 1;
  frameTransJ(2,2) = 1;
  frameTransJ(3,3) = - factor * cos(momentum[2]) / (momentum[0]*momentum[0]);
  frameTransJ(4,3) = - factor * sin(momentum[2]) / (momentum[0]*momentum[0]);
  frameTransJ(5,3) = - factor / tan(momentum[1]) / (momentum[0]*momentum[0]);

  frameTransJ(3,5) = - factor * sin(momentum[2])  / (momentum[0]);
  frameTransJ(4,5) = factor * cos(momentum[2]) / (momentum[0]);
  frameTransJ(5,4) = - factor / (momentum[0]*sin(momentum[1])*sin(momentum[1]));

  return frameTransJ;
}


AlgebraicMatrix55
PerigeeConversions::jacobianCurvilinear2Perigee(const FreeTrajectoryState& fts) const {

  GlobalVector p = fts.momentum();

  GlobalVector Z = GlobalVector(0.,0.,1.);
  GlobalVector T = p.unit();
  GlobalVector U = Z.cross(T).unit();; 
  GlobalVector V = T.cross(U);

  GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.); //opposite to track dir.
  I = I.unit();
  GlobalVector J(-I.y(), I.x(),0.); //counterclockwise rotation
  GlobalVector K(Z);
  GlobalPoint x = fts.position();
  GlobalVector B  = fts.parameters().magneticField().inInverseGeV(x);
  GlobalVector H = B.unit();
  GlobalVector HxT = H.cross(T);
  GlobalVector N = HxT.unit();
  double alpha = HxT.mag();
  double qbp = fts.signedInverseMomentum();
  double Q = -B.mag() * qbp;
  double alphaQ = alpha * Q;

  double lambda = 0.5 * M_PI - p.theta();
  double coslambda = cos(lambda), tanlambda = tan(lambda);

  double TI = T.dot(I);
  double NU = N.dot(U);
  double NV = N.dot(V);
  double UI = U.dot(I);
  double VI = V.dot(I);
  double UJ = U.dot(J);
  double VJ = V.dot(J);
  double UK = U.dot(K);
  double VK = V.dot(K);

  AlgebraicMatrix55 jac;

  if( fabs(fts.transverseCurvature())<1.e-10 ) {
    jac(0,0) = 1/coslambda;
    jac(0,1) = tanlambda/coslambda/fts.parameters().momentum().mag();
  }else{
    double Bz = B.z();
    jac(0,0) = -Bz/coslambda;
    jac(0,1) = -Bz * tanlambda/coslambda*qbp;
    jac(1,3) = alphaQ * NV * UI/TI;
    jac(1,4) = alphaQ * NV * VI/TI;
    jac(0,3) = -jac(0,1) * jac(1,3);
    jac(0,4) = -jac(0,1) * jac(1,4);
    jac(2,3) = -alphaQ/coslambda * NU * UI/TI;
    jac(2,4) = -alphaQ/coslambda * NU * VI/TI;
  }
  jac(1,1) = -1.;
  jac(2,2) = 1.;
  jac(3,3) = VK/TI;
  jac(3,4) = -UK/TI;
  jac(4,3) = -VJ/TI;
  jac(4,4) = UJ/TI;
  
  return jac;
  
}


AlgebraicMatrix55 
PerigeeConversions::jacobianPerigee2Curvilinear(const GlobalTrajectoryParameters& gtp) const {

  GlobalVector p = gtp.momentum();

  GlobalVector Z = GlobalVector(0.f,0.f,1.f);
  GlobalVector T = p.unit();
  GlobalVector U = Z.cross(T).unit();; 
  GlobalVector V = T.cross(U);

  GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.f); //opposite to track dir.
  I = I.unit();
  GlobalVector J(-I.y(), I.x(),0.f); //counterclockwise rotation
  GlobalVector K(Z);
  GlobalPoint x = gtp.position();
  GlobalVector B  = gtp.magneticField().inInverseGeV(x);
  GlobalVector H = B.unit();
  GlobalVector HxT = H.cross(T);
  GlobalVector N = HxT.unit();
  double alpha = HxT.mag();
  double qbp = gtp.signedInverseMomentum();
  double Q = -B.mag() * qbp;
  double alphaQ = alpha * Q;

  double lambda = 0.5 * M_PI - p.theta();
  double coslambda = cos(lambda), sinlambda = sin(lambda);
  double mqbpt = -1./coslambda * qbp;

  double TJ = T.dot(J);
  double TK = T.dot(K);
  double NU = N.dot(U);
  double NV = N.dot(V);
  double UJ = U.dot(J);
  double VJ = V.dot(J);
  double UK = U.dot(K);
  double VK = V.dot(K);

  AlgebraicMatrix55 jac;

  if( fabs(gtp.transverseCurvature())<1.e-10f ) {
    jac(0,0) = coslambda;
    jac(0,1) = sinlambda/coslambda/gtp.momentum().mag();
  }else{
    jac(0,0) = -coslambda/B.z();
    jac(0,1) = -sinlambda * mqbpt;
    jac(1,3) = -alphaQ * NV * TJ;
    jac(1,4) = -alphaQ * NV * TK;
    jac(2,3) = -alphaQ/coslambda * NU * TJ;
    jac(2,4) = -alphaQ/coslambda * NU * TK;
  }
  jac(1,1) = -1.;
  jac(2,2) = 1.;
  jac(3,3) = UJ;
  jac(3,4) = UK;
  jac(4,3) = VJ;
  jac(4,4) = VK;
  
  return jac;
}

// AlgebraicMatrix 
// PerigeeConversions::jacobianHelix2Perigee(const reco::helix::Parameters & helixPar, 
//      const reco::helix::Covariance & helixCov) const
// {
//   AlgebraicMatrix55 jac;
// 
//   jac(3,0) = 1.;
//   jac(2,1) = 1.;
// //   jac(0,2) = - 1. / magField.inTesla(helixPar.vertex()).z() * 2.99792458e-3;
//   jac(0,2) = - 1. / (TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3);
//   jac(4,3) = 1.;
//   jac(1,4) = -(1. + helixPar.tanDip()*helixPar.tanDip());
// std::std::cout << jac;
//   return jac;
// }
Revision:	1.1
Committed:	Fri Nov 25 16:38:28 2011 UTC (13 years, 5 months ago) by econte
Content type:	text/plain
Branch:	MAIN
CVS Tags:	TBD2011, TBD_2011, HEAD
Log Message:	new IPHC alignment
#	User	Rev	Content
1	econte	1.1	#include "TrackingTools/TrajectoryState/interface/PerigeeConversions.h"
2			#include "TrackingTools/TrajectoryState/interface/TrajectoryStateClosestToPoint.h"
3			#include "MagneticField/Engine/interface/MagneticField.h"
4			#include <cmath>
5
6			PerigeeTrajectoryParameters PerigeeConversions::ftsToPerigeeParameters
7			(const FTS& originalFTS, const GlobalPoint& referencePoint, double & pt) const
8
9			{
10			GlobalVector impactDistance = originalFTS.position() - referencePoint;
11
12			pt = originalFTS.momentum().perp();
13			if (pt==0.) throw cms::Exception("PerigeeConversions", "Track with pt=0");
14
15			double theta = originalFTS.momentum().theta();
16			double phi = originalFTS.momentum().phi();
17			double field = originalFTS.parameters().magneticField().inInverseGeV(originalFTS.position()).z();
18			// if (field==0.) throw cms::Exception("PerigeeConversions", "Field is 0") << " at " << originalFTS.position() << "\n" ;
19
20			double positiveMomentumPhi = ( (phi>0) ? phi : (2*M_PI+phi) );
21			double positionPhi = impactDistance.phi();
22			double positivePositionPhi =
23			( (positionPhi>0) ? positionPhi : (2*M_PI+positionPhi) );
24			double phiDiff = positiveMomentumPhi - positivePositionPhi;
25			if (phiDiff<0.0) phiDiff+= (2*M_PI);
26			double signEpsilon = ( (phiDiff > M_PI) ? -1.0 : 1.0);
27
28			double epsilon = signEpsilon *
29			sqrt ( impactDistance.x()*impactDistance.x() +
30			impactDistance.y()*impactDistance.y() );
31
32			// The track parameters:
33			AlgebraicVector5 theTrackParameters;
34
35			double signTC = - originalFTS.charge();
36			bool isCharged = (signTC!=0) && (fabs(field)>1.e-10);
37			if (isCharged) {
38			theTrackParameters[0] = field / pt*signTC;
39			} else {
40			theTrackParameters[0] = 1 / pt;
41			}
42			theTrackParameters[1] = theta;
43			theTrackParameters[2] = phi;
44			theTrackParameters[3] = epsilon;
45			theTrackParameters[4] = impactDistance.z();
46			return PerigeeTrajectoryParameters(theTrackParameters, isCharged);
47			}
48
49			// PerigeeTrajectoryParameters PerigeeConversions::helixToPerigeeParameters
50			// (const reco::helix::Parameters & helixPar, const GlobalPoint& referencePoint) const
51			// {
52			// AlgebraicVector theTrackParameters = AlgebraicVector(5);
53			// double field = TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3;
54			// theTrackParameters[0] = - field*helixPar.omega();
55			// theTrackParameters[1] = atan(1/helixPar.tanDip());
56			// theTrackParameters[2] = helixPar.phi0() - M_PI/2;
57			// theTrackParameters[3] = helixPar.d0();
58			// theTrackParameters[4] = helixPar.dz();
59			// return PerigeeTrajectoryParameters(theTrackParameters, helixPar.pt(), true);
60			// }
61
62			PerigeeTrajectoryError PerigeeConversions::ftsToPerigeeError
63			(const FTS& originalFTS) const
64			{
65			AlgebraicSymMatrix55 errorMatrix = originalFTS.curvilinearError().matrix();
66			AlgebraicMatrix55 curv2perigee = jacobianCurvilinear2Perigee(originalFTS);
67			return PerigeeTrajectoryError(ROOT::Math::Similarity(curv2perigee,errorMatrix));
68			}
69
70			// PerigeeTrajectoryError PerigeeConversions::helixToPerigeeError
71			// (const reco::helix::Parameters & helixPar,
72			// const reco::helix::Covariance & helixCov) const
73			// {
74			// //FIXME: verify that the order of the parameters are correct
75			// AlgebraicSymMatrix55 helixCovMatrix;
76			// helixCovMatrix(0,0) = helixCov(reco::helix::i_d0,reco::helix::i_d0);
77			// helixCovMatrix(1,1) = helixCov(reco::helix::i_phi0,reco::helix::i_phi0);
78			// helixCovMatrix(2,2) = helixCov(reco::helix::i_omega,reco::helix::i_omega);
79			// helixCovMatrix(3,3) = helixCov(reco::helix::i_dz,reco::helix::i_dz);
80			// helixCovMatrix(4,4) = helixCov(reco::helix::i_tanDip,reco::helix::i_tanDip);
81			//
82			// helixCovMatrix(0,1) = helixCov(reco::helix::i_d0,reco::helix::i_phi0);
83			// helixCovMatrix(0,2) = helixCov(reco::helix::i_d0,reco::helix::i_omega);
84			// helixCovMatrix(0,3) = helixCov(reco::helix::i_d0,reco::helix::i_dz);
85			// helixCovMatrix(0,4) = helixCov(reco::helix::i_d0,reco::helix::i_tanDip);
86			//
87			// helixCovMatrix(1,2) = helixCov(reco::helix::i_phi0,reco::helix::i_omega);
88			// helixCovMatrix(1,3) = helixCov(reco::helix::i_phi0,reco::helix::i_dz);
89			// helixCovMatrix(1,4) = helixCov(reco::helix::i_phi0,reco::helix::i_tanDip);
90			//
91			// helixCovMatrix(2,3) = helixCov(reco::helix::i_omega,reco::helix::i_dz);
92			// helixCovMatrix(2,4) = helixCov(reco::helix::i_omega,reco::helix::i_tanDip);
93			//
94			// helixCovMatrix(3,4) = helixCov(reco::helix::i_dz,reco::helix::i_tanDip);
95			//
96			// AlgebraicMatrix helix2perigee = jacobianHelix2Perigee(helixPar, helixCov);
97			// return PerigeeTrajectoryError(helixCovMatrix.similarity(helix2perigee));
98			// }
99
100
101			CurvilinearTrajectoryError PerigeeConversions::curvilinearError
102			(const PerigeeTrajectoryError& perigeeError, const GlobalTrajectoryParameters& gtp) const
103			{
104			AlgebraicMatrix55 perigee2curv = jacobianPerigee2Curvilinear(gtp);
105			return CurvilinearTrajectoryError(ROOT::Math::Similarity(perigee2curv, perigeeError.covarianceMatrix()));
106			}
107
108			GlobalPoint PerigeeConversions::positionFromPerigee
109			(const PerigeeTrajectoryParameters& parameters, const GlobalPoint& referencePoint) const
110			{
111			AlgebraicVector5 theVector = parameters.vector();
112			return GlobalPoint(theVector[3]*sin(theVector[2])+referencePoint.x(),
113			-theVector[3]*cos(theVector[2])+referencePoint.y(),
114			theVector[4]+referencePoint.z());
115			}
116
117
118			GlobalVector PerigeeConversions::momentumFromPerigee
119			(const PerigeeTrajectoryParameters& parameters, double pt, const GlobalPoint& referencePoint) const
120			{
121			return GlobalVector(cos(parameters.phi()) * pt,
122			sin(parameters.phi()) * pt,
123			pt / tan(parameters.theta()));
124			}
125
126
127			GlobalVector PerigeeConversions::momentumFromPerigee
128			(const AlgebraicVector3& momentum, const TrackCharge& charge,
129			const GlobalPoint& referencePoint, const MagneticField* field) const
130			{
131			double pt;
132			if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");
133
134			double bz = fabs(field->inInverseGeV(referencePoint).z());
135			if ( charge!=0 && bz>1.e-10 ) {
136			pt = std::abs(bz/momentum[0]);
137			if (pt<1.e-10) throw cms::Exception("PerigeeConversions", "pt is 0");
138			} else {
139			pt = 1 / momentum[0];
140			}
141
142			return GlobalVector(cos(momentum[2]) * pt,
143			sin(momentum[2]) * pt,
144			pt/tan(momentum[1]));
145			}
146
147			TrackCharge PerigeeConversions::chargeFromPerigee
148			(const PerigeeTrajectoryParameters& parameters) const
149			{
150			return parameters.charge();
151			}
152
153			TrajectoryStateClosestToPoint PerigeeConversions::trajectoryStateClosestToPoint
154			(const AlgebraicVector3& momentum, const GlobalPoint& referencePoint,
155			const TrackCharge& charge, const AlgebraicSymMatrix66& theCovarianceMatrix,
156			const MagneticField* field) const
157			{
158			AlgebraicMatrix66 param2cart = jacobianParameters2Cartesian
159			(momentum, referencePoint, charge, field);
160			CartesianTrajectoryError cartesianTrajErr(ROOT::Math::Similarity(param2cart, theCovarianceMatrix));
161
162			FTS theFTS(GlobalTrajectoryParameters(referencePoint,
163			momentumFromPerigee(momentum, charge, referencePoint, field), charge,
164			field), cartesianTrajErr);
165
166			return TrajectoryStateClosestToPoint(theFTS, referencePoint);
167			}
168
169
170			AlgebraicMatrix66
171			PerigeeConversions::jacobianParameters2Cartesian(
172			const AlgebraicVector3& momentum, const GlobalPoint& position,
173			const TrackCharge& charge, const MagneticField* field) const
174			{
175			if (momentum[0]==0.) throw cms::Exception("PerigeeConversions", "Track with rho=0");
176			double factor = 1.;
177			double bField = field->inInverseGeV(position).z();
178			if (charge!=0 && fabs(bField)>1.e-10) {
179			// if (bField==0.) throw cms::Exception("PerigeeConversions", "Field is 0");
180			factor = -bField*charge;
181			}
182			AlgebraicMatrix66 frameTransJ;
183			frameTransJ(0,0) = 1;
184			frameTransJ(1,1) = 1;
185			frameTransJ(2,2) = 1;
186			frameTransJ(3,3) = - factor * cos(momentum[2]) / (momentum[0]*momentum[0]);
187			frameTransJ(4,3) = - factor * sin(momentum[2]) / (momentum[0]*momentum[0]);
188			frameTransJ(5,3) = - factor / tan(momentum[1]) / (momentum[0]*momentum[0]);
189
190			frameTransJ(3,5) = - factor * sin(momentum[2]) / (momentum[0]);
191			frameTransJ(4,5) = factor * cos(momentum[2]) / (momentum[0]);
192			frameTransJ(5,4) = - factor / (momentum[0]sin(momentum[1])sin(momentum[1]));
193
194			return frameTransJ;
195			}
196
197
198			AlgebraicMatrix55
199			PerigeeConversions::jacobianCurvilinear2Perigee(const FreeTrajectoryState& fts) const {
200
201			GlobalVector p = fts.momentum();
202
203			GlobalVector Z = GlobalVector(0.,0.,1.);
204			GlobalVector T = p.unit();
205			GlobalVector U = Z.cross(T).unit();;
206			GlobalVector V = T.cross(U);
207
208			GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.); //opposite to track dir.
209			I = I.unit();
210			GlobalVector J(-I.y(), I.x(),0.); //counterclockwise rotation
211			GlobalVector K(Z);
212			GlobalPoint x = fts.position();
213			GlobalVector B = fts.parameters().magneticField().inInverseGeV(x);
214			GlobalVector H = B.unit();
215			GlobalVector HxT = H.cross(T);
216			GlobalVector N = HxT.unit();
217			double alpha = HxT.mag();
218			double qbp = fts.signedInverseMomentum();
219			double Q = -B.mag() * qbp;
220			double alphaQ = alpha * Q;
221
222			double lambda = 0.5 * M_PI - p.theta();
223			double coslambda = cos(lambda), tanlambda = tan(lambda);
224
225			double TI = T.dot(I);
226			double NU = N.dot(U);
227			double NV = N.dot(V);
228			double UI = U.dot(I);
229			double VI = V.dot(I);
230			double UJ = U.dot(J);
231			double VJ = V.dot(J);
232			double UK = U.dot(K);
233			double VK = V.dot(K);
234
235			AlgebraicMatrix55 jac;
236
237			if( fabs(fts.transverseCurvature())<1.e-10 ) {
238			jac(0,0) = 1/coslambda;
239			jac(0,1) = tanlambda/coslambda/fts.parameters().momentum().mag();
240			}else{
241			double Bz = B.z();
242			jac(0,0) = -Bz/coslambda;
243			jac(0,1) = -Bz * tanlambda/coslambda*qbp;
244			jac(1,3) = alphaQ * NV * UI/TI;
245			jac(1,4) = alphaQ * NV * VI/TI;
246			jac(0,3) = -jac(0,1) * jac(1,3);
247			jac(0,4) = -jac(0,1) * jac(1,4);
248			jac(2,3) = -alphaQ/coslambda * NU * UI/TI;
249			jac(2,4) = -alphaQ/coslambda * NU * VI/TI;
250			}
251			jac(1,1) = -1.;
252			jac(2,2) = 1.;
253			jac(3,3) = VK/TI;
254			jac(3,4) = -UK/TI;
255			jac(4,3) = -VJ/TI;
256			jac(4,4) = UJ/TI;
257
258			return jac;
259
260			}
261
262
263
264			AlgebraicMatrix55
265			PerigeeConversions::jacobianPerigee2Curvilinear(const GlobalTrajectoryParameters& gtp) const {
266
267			GlobalVector p = gtp.momentum();
268
269			GlobalVector Z = GlobalVector(0.f,0.f,1.f);
270			GlobalVector T = p.unit();
271			GlobalVector U = Z.cross(T).unit();;
272			GlobalVector V = T.cross(U);
273
274			GlobalVector I = GlobalVector(-p.x(), -p.y(), 0.f); //opposite to track dir.
275			I = I.unit();
276			GlobalVector J(-I.y(), I.x(),0.f); //counterclockwise rotation
277			GlobalVector K(Z);
278			GlobalPoint x = gtp.position();
279			GlobalVector B = gtp.magneticField().inInverseGeV(x);
280			GlobalVector H = B.unit();
281			GlobalVector HxT = H.cross(T);
282			GlobalVector N = HxT.unit();
283			double alpha = HxT.mag();
284			double qbp = gtp.signedInverseMomentum();
285			double Q = -B.mag() * qbp;
286			double alphaQ = alpha * Q;
287
288			double lambda = 0.5 * M_PI - p.theta();
289			double coslambda = cos(lambda), sinlambda = sin(lambda);
290			double mqbpt = -1./coslambda * qbp;
291
292			double TJ = T.dot(J);
293			double TK = T.dot(K);
294			double NU = N.dot(U);
295			double NV = N.dot(V);
296			double UJ = U.dot(J);
297			double VJ = V.dot(J);
298			double UK = U.dot(K);
299			double VK = V.dot(K);
300
301			AlgebraicMatrix55 jac;
302
303			if( fabs(gtp.transverseCurvature())<1.e-10f ) {
304			jac(0,0) = coslambda;
305			jac(0,1) = sinlambda/coslambda/gtp.momentum().mag();
306			}else{
307			jac(0,0) = -coslambda/B.z();
308			jac(0,1) = -sinlambda * mqbpt;
309			jac(1,3) = -alphaQ * NV * TJ;
310			jac(1,4) = -alphaQ * NV * TK;
311			jac(2,3) = -alphaQ/coslambda * NU * TJ;
312			jac(2,4) = -alphaQ/coslambda * NU * TK;
313			}
314			jac(1,1) = -1.;
315			jac(2,2) = 1.;
316			jac(3,3) = UJ;
317			jac(3,4) = UK;
318			jac(4,3) = VJ;
319			jac(4,4) = VK;
320
321			return jac;
322			}
323
324			// AlgebraicMatrix
325			// PerigeeConversions::jacobianHelix2Perigee(const reco::helix::Parameters & helixPar,
326			// const reco::helix::Covariance & helixCov) const
327			// {
328			// AlgebraicMatrix55 jac;
329			//
330			// jac(3,0) = 1.;
331			// jac(2,1) = 1.;
332			// // jac(0,2) = - 1. / magField.inTesla(helixPar.vertex()).z() * 2.99792458e-3;
333			// jac(0,2) = - 1. / (TrackingTools::FakeField::Field::inTesla(helixPar.vertex()).z() * 2.99792458e-3);
334			// jac(4,3) = 1.;
335			// jac(1,4) = -(1. + helixPar.tanDip()*helixPar.tanDip());
336			// std::std::cout << jac;
337			// return jac;
338			// }