PatternTools/src/TwoTrackMinimumDistanceHelixHelix.cc

#include "TrackingTools/PatternTools/interface/TwoTrackMinimumDistanceHelixHelix.h"
#include "DataFormats/GeometryVector/interface/GlobalVector.h"
#include "MagneticField/Engine/interface/MagneticField.h"
#include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

using namespace std;

namespace {
  inline GlobalPoint operator - ( const GlobalPoint & a, const GlobalPoint & b ){
    return GlobalPoint ( a.basicVector() - b.basicVector() );
  }

  inline GlobalPoint operator + ( const GlobalPoint & a, const GlobalPoint & b ){
    return GlobalPoint ( a.basicVector() + b.basicVector() );
  }

  inline GlobalPoint operator / ( const GlobalPoint & a, const double b ){
    return GlobalPoint ( a.basicVector() / b );
  }

  inline GlobalPoint operator * ( const GlobalPoint & a, const float b ){
    return GlobalPoint ( a.basicVector() * b );
  }

  inline GlobalPoint operator * ( const float b , const GlobalPoint & a ){
    return GlobalPoint ( a.basicVector() * b );
  }

  inline double square ( const double s ) { return s*s; }
}

TwoTrackMinimumDistanceHelixHelix::TwoTrackMinimumDistanceHelixHelix():
theH(0), theG(0), pointsUpdated(false), themaxjump(20),thesingjacI(1./0.1), themaxiter(4)
{ }

TwoTrackMinimumDistanceHelixHelix::~TwoTrackMinimumDistanceHelixHelix() {}

bool TwoTrackMinimumDistanceHelixHelix::updateCoeffs(
    const GlobalPoint & gpH, const GlobalPoint & gpG ) {

  const double Bc2kH = theH->magneticField().inInverseGeV(gpH).z();
  const double Bc2kG = theG->magneticField().inInverseGeV(gpG).z();
  const double Ht= theH->momentum().perp();
  const double Gt= theG->momentum().perp();
  //  thelambdaH=asin ( theH->momentum().z() / Hn );
  
  if ( Ht == 0. || Gt == 0. ) {
    edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
      << "transverse momentum of input trajectory is zero.";
    return true;
  };
  
  if ( theH->charge() == 0. || theG->charge() == 0. ) {
    edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
      << "charge of input track is zero.";
    return true;
  };
  
  if ( Bc2kG == 0.) {
    edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
      << "magnetic field at point " << gpG << " is zero.";
    return true;
  };
  
  if ( Bc2kH == 0. ) {
    edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
      << "magnetic field at point " << gpH << " is zero.";
    return true;
  };
  
  theh= Ht / (theH->charge() * Bc2kH );
  thesinpH0= - theH->momentum().y() / ( Ht );
  thecospH0= - theH->momentum().x() / ( Ht );
  thetanlambdaH = - theH->momentum().z() / ( Ht);
  thepH0 = atan2 ( thesinpH0 , thecospH0 );
  
  // thelambdaG=asin ( theG->momentum().z()/( Gn ) );
  
  theg= Gt / (theG->charge() * Bc2kG );
  thesinpG0= - theG->momentum().y() / ( Gt);
  thecospG0= - theG->momentum().x() / (Gt);
  thetanlambdaG = - theG->momentum().z() / ( Gt);
  thepG0 = atan2 ( thesinpG0 , thecospG0 );
  
  thea=theH->position().x() - theG->position().x() + theg * thesinpG0 -
    theh * thesinpH0;
  theb=theH->position().y() - theG->position().y() - theg * thecospG0 +
    theh * thecospH0;
  thec1=  theh * thetanlambdaH * thetanlambdaH;
  thec2= -theg * thetanlambdaG * thetanlambdaG;
  thed1= -theg * thetanlambdaG * thetanlambdaH;
  thed2=  theh * thetanlambdaG * thetanlambdaH;
  thee1= thetanlambdaH * ( theH->position().z() - theG->position().z() -
                           theh * thepH0 * thetanlambdaH + theg * thepG0 * thetanlambdaG );
  thee2= thetanlambdaG * ( theH->position().z() - theG->position().z() -
                           theh * thepH0 * thetanlambdaH + theg * thepG0 * thetanlambdaG );
  return false;
}

bool TwoTrackMinimumDistanceHelixHelix::oneIteration(double & dH, double & dG ) const {
  thesinpH=sin(thepH);
  thecospH=cos(thepH);
  thesinpG=sin(thepG);
  thecospG=cos(thepG);
  
  const double A11= theh * ( - thesinpH * ( thea - theg * thesinpG ) +
                             thecospH * ( theb + theg * thecospG ) + thec1);
  if (A11 < 0) { return true; };
  const double A22= -theg * (- thesinpG * ( thea + theh * thesinpH ) +
                             thecospG*( theb - theh * thecospH ) + thec2);
  if (A22 < 0) { return true; };
  const double A12= theh * (-theg * thecospG * thecospH -
                            theg * thesinpH * thesinpG + thed1);
  const double A21= -theg * (theh * thecospG * thecospH +
                             theh * thesinpH * thesinpG + thed2);
  const double detaI = 1./(A11 * A22 - A12 * A21);
  const double z1=theh * ( thecospH * ( thea - theg * thesinpG ) + thesinpH *
                           ( theb + theg*thecospG ) + thec1 * thepH + thed1 * thepG + thee1);
  const double z2=-theg * (thecospG * ( thea + theh * thesinpH ) + thesinpG *
                           ( theb - theh*thecospH ) + thec2 * thepG + thed2 * thepH + thee2);
  
  dH=( z1 * A22 - z2 * A12 ) * detaI;
  dG=( z2 * A11 - z1 * A21 ) * detaI;
  if ( fabs(detaI) > thesingjacI ) { return true; };
  
  thepH -= dH;
  thepG -= dG;
  return false;
}

/*
bool TwoTrackMinimumDistanceHelixHelix::parallelTracks() const {
  return (fabs(theH->momentum().x() - theG->momentum().x()) < .00000001 )
    && (fabs(theH->momentum().y() - theG->momentum().y()) < .00000001 )
    && (fabs(theH->momentum().z() - theG->momentum().z()) < .00000001 )
    && (theH->charge()==theG->charge()) 
    ;
}
*/

bool TwoTrackMinimumDistanceHelixHelix::calculate(
                                                  const GlobalTrajectoryParameters & G,
                                                  const GlobalTrajectoryParameters & H, const float qual ) {
  pointsUpdated = false;
  theG= &G;
  theH= &H;
  bool retval=false;
  
  if ( updateCoeffs ( theG->position(), theH->position() ) ) return true;
  
  thepG = thepG0;
  thepH = thepH0;
  
  int counter=0;
  double pH=0; double pG=0;
  do {
    retval=oneIteration ( pG, pH );
    if ( std::isinf(pG) || std::isinf(pH) ) retval=true;
    if ( counter++>themaxiter ) retval=true;
  } while ( (!retval) && ( fabs(pG) > qual || fabs(pH) > qual ));
  if ( fabs ( theg * ( thepG - thepG0 ) ) > themaxjump ) retval=true;
  if ( fabs ( theh * ( thepH - thepH0 ) ) > themaxjump ) retval=true;
  return retval;
}


void TwoTrackMinimumDistanceHelixHelix::finalPoints() const {
  GlobalVector tmpG( sin(thepG) - thesinpG0,
                   - cos(thepG) + thecospG0,
                   thetanlambdaG * ( thepG- thepG0 ) 
                   );
  pointG = theG->position() + theg * tmpG;
  pathG = ( thepG- thepG0 ) * (theg*sqrt(1+thetanlambdaG*thetanlambdaG)) ;

  GlobalVector tmpH( sin(thepH) - thesinpH0,
                   - cos(thepH) + thecospH0,
                   thetanlambdaH * ( thepH- thepH0 ) 
                   );
  pointH = theH->position() + theh * tmpH;
  pathH = ( thepH- thepH0 ) * (theh*sqrt(1+thetanlambdaH*thetanlambdaH)) ;

  pointsUpdated = true;
}

Revision:	1.1
Committed:	Fri Nov 25 16:38:25 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/PatternTools/interface/TwoTrackMinimumDistanceHelixHelix.h"
2			#include "DataFormats/GeometryVector/interface/GlobalVector.h"
3			#include "MagneticField/Engine/interface/MagneticField.h"
4			#include "TrackingTools/TrajectoryParametrization/interface/GlobalTrajectoryParameters.h"
5			#include "FWCore/MessageLogger/interface/MessageLogger.h"
6
7			using namespace std;
8
9			namespace {
10			inline GlobalPoint operator - ( const GlobalPoint & a, const GlobalPoint & b ){
11			return GlobalPoint ( a.basicVector() - b.basicVector() );
12			}
13
14			inline GlobalPoint operator + ( const GlobalPoint & a, const GlobalPoint & b ){
15			return GlobalPoint ( a.basicVector() + b.basicVector() );
16			}
17
18			inline GlobalPoint operator / ( const GlobalPoint & a, const double b ){
19			return GlobalPoint ( a.basicVector() / b );
20			}
21
22			inline GlobalPoint operator * ( const GlobalPoint & a, const float b ){
23			return GlobalPoint ( a.basicVector() * b );
24			}
25
26			inline GlobalPoint operator * ( const float b , const GlobalPoint & a ){
27			return GlobalPoint ( a.basicVector() * b );
28			}
29
30			inline double square ( const double s ) { return s*s; }
31			}
32
33			TwoTrackMinimumDistanceHelixHelix::TwoTrackMinimumDistanceHelixHelix():
34			theH(0), theG(0), pointsUpdated(false), themaxjump(20),thesingjacI(1./0.1), themaxiter(4)
35			{ }
36
37			TwoTrackMinimumDistanceHelixHelix::~TwoTrackMinimumDistanceHelixHelix() {}
38
39			bool TwoTrackMinimumDistanceHelixHelix::updateCoeffs(
40			const GlobalPoint & gpH, const GlobalPoint & gpG ) {
41
42			const double Bc2kH = theH->magneticField().inInverseGeV(gpH).z();
43			const double Bc2kG = theG->magneticField().inInverseGeV(gpG).z();
44			const double Ht= theH->momentum().perp();
45			const double Gt= theG->momentum().perp();
46			// thelambdaH=asin ( theH->momentum().z() / Hn );
47
48			if ( Ht == 0. \|\| Gt == 0. ) {
49			edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
50			<< "transverse momentum of input trajectory is zero.";
51			return true;
52			};
53
54			if ( theH->charge() == 0. \|\| theG->charge() == 0. ) {
55			edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
56			<< "charge of input track is zero.";
57			return true;
58			};
59
60			if ( Bc2kG == 0.) {
61			edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
62			<< "magnetic field at point " << gpG << " is zero.";
63			return true;
64			};
65
66			if ( Bc2kH == 0. ) {
67			edm::LogWarning ("TwoTrackMinimumDistanceHelixHelix")
68			<< "magnetic field at point " << gpH << " is zero.";
69			return true;
70			};
71
72			theh= Ht / (theH->charge() * Bc2kH );
73			thesinpH0= - theH->momentum().y() / ( Ht );
74			thecospH0= - theH->momentum().x() / ( Ht );
75			thetanlambdaH = - theH->momentum().z() / ( Ht);
76			thepH0 = atan2 ( thesinpH0 , thecospH0 );
77
78			// thelambdaG=asin ( theG->momentum().z()/( Gn ) );
79
80			theg= Gt / (theG->charge() * Bc2kG );
81			thesinpG0= - theG->momentum().y() / ( Gt);
82			thecospG0= - theG->momentum().x() / (Gt);
83			thetanlambdaG = - theG->momentum().z() / ( Gt);
84			thepG0 = atan2 ( thesinpG0 , thecospG0 );
85
86			thea=theH->position().x() - theG->position().x() + theg * thesinpG0 -
87			theh * thesinpH0;
88			theb=theH->position().y() - theG->position().y() - theg * thecospG0 +
89			theh * thecospH0;
90			thec1= theh * thetanlambdaH * thetanlambdaH;
91			thec2= -theg * thetanlambdaG * thetanlambdaG;
92			thed1= -theg * thetanlambdaG * thetanlambdaH;
93			thed2= theh * thetanlambdaG * thetanlambdaH;
94			thee1= thetanlambdaH * ( theH->position().z() - theG->position().z() -
95			theh * thepH0 * thetanlambdaH + theg * thepG0 * thetanlambdaG );
96			thee2= thetanlambdaG * ( theH->position().z() - theG->position().z() -
97			theh * thepH0 * thetanlambdaH + theg * thepG0 * thetanlambdaG );
98			return false;
99			}
100
101			bool TwoTrackMinimumDistanceHelixHelix::oneIteration(double & dH, double & dG ) const {
102			thesinpH=sin(thepH);
103			thecospH=cos(thepH);
104			thesinpG=sin(thepG);
105			thecospG=cos(thepG);
106
107			const double A11= theh * ( - thesinpH * ( thea - theg * thesinpG ) +
108			thecospH * ( theb + theg * thecospG ) + thec1);
109			if (A11 < 0) { return true; };
110			const double A22= -theg * (- thesinpG * ( thea + theh * thesinpH ) +
111			thecospG( theb - theh thecospH ) + thec2);
112			if (A22 < 0) { return true; };
113			const double A12= theh * (-theg * thecospG * thecospH -
114			theg * thesinpH * thesinpG + thed1);
115			const double A21= -theg * (theh * thecospG * thecospH +
116			theh * thesinpH * thesinpG + thed2);
117			const double detaI = 1./(A11 * A22 - A12 * A21);
118			const double z1=theh * ( thecospH * ( thea - theg * thesinpG ) + thesinpH *
119			( theb + thegthecospG ) + thec1 thepH + thed1 * thepG + thee1);
120			const double z2=-theg * (thecospG * ( thea + theh * thesinpH ) + thesinpG *
121			( theb - thehthecospH ) + thec2 thepG + thed2 * thepH + thee2);
122
123			dH=( z1 * A22 - z2 * A12 ) * detaI;
124			dG=( z2 * A11 - z1 * A21 ) * detaI;
125			if ( fabs(detaI) > thesingjacI ) { return true; };
126
127			thepH -= dH;
128			thepG -= dG;
129			return false;
130			}
131
132			/*
133			bool TwoTrackMinimumDistanceHelixHelix::parallelTracks() const {
134			return (fabs(theH->momentum().x() - theG->momentum().x()) < .00000001 )
135			&& (fabs(theH->momentum().y() - theG->momentum().y()) < .00000001 )
136			&& (fabs(theH->momentum().z() - theG->momentum().z()) < .00000001 )
137			&& (theH->charge()==theG->charge())
138			;
139			}
140			*/
141
142			bool TwoTrackMinimumDistanceHelixHelix::calculate(
143			const GlobalTrajectoryParameters & G,
144			const GlobalTrajectoryParameters & H, const float qual ) {
145			pointsUpdated = false;
146			theG= &G;
147			theH= &H;
148			bool retval=false;
149
150			if ( updateCoeffs ( theG->position(), theH->position() ) ) return true;
151
152			thepG = thepG0;
153			thepH = thepH0;
154
155			int counter=0;
156			double pH=0; double pG=0;
157			do {
158			retval=oneIteration ( pG, pH );
159			if ( std::isinf(pG) \|\| std::isinf(pH) ) retval=true;
160			if ( counter++>themaxiter ) retval=true;
161			} while ( (!retval) && ( fabs(pG) > qual \|\| fabs(pH) > qual ));
162			if ( fabs ( theg * ( thepG - thepG0 ) ) > themaxjump ) retval=true;
163			if ( fabs ( theh * ( thepH - thepH0 ) ) > themaxjump ) retval=true;
164			return retval;
165			}
166
167
168			void TwoTrackMinimumDistanceHelixHelix::finalPoints() const {
169			GlobalVector tmpG( sin(thepG) - thesinpG0,
170			- cos(thepG) + thecospG0,
171			thetanlambdaG * ( thepG- thepG0 )
172			);
173			pointG = theG->position() + theg * tmpG;
174			pathG = ( thepG- thepG0 ) * (thegsqrt(1+thetanlambdaGthetanlambdaG)) ;
175
176			GlobalVector tmpH( sin(thepH) - thesinpH0,
177			- cos(thepH) + thecospH0,
178			thetanlambdaH * ( thepH- thepH0 )
179			);
180			pointH = theH->position() + theh * tmpH;
181			pathH = ( thepH- thepH0 ) * (thehsqrt(1+thetanlambdaHthetanlambdaH)) ;
182
183			pointsUpdated = true;
184			}
185