CMS 3D CMS Logo

/data/refman/pasoursint/CMSSW_5_3_9_patch3/src/RecoMuon/TrackerSeedGenerator/src/L1MuonPixelTrackFitter.cc

Go to the documentation of this file.
00001 #include "RecoMuon/TrackerSeedGenerator/interface/L1MuonPixelTrackFitter.h"
00002 
00003 #include "DataFormats/L1GlobalMuonTrigger/interface/L1MuGMTCand.h"
00004 
00005 #include "RecoTracker/TkMSParametrization/interface/PixelRecoLineRZ.h"
00006 #include "RecoTracker/TkMSParametrization/interface/PixelRecoUtilities.h"
00007 #include "RecoPixelVertexing/PixelTrackFitting/interface/PixelTrackBuilder.h"
00008 #include "DataFormats/GeometryCommonDetAlgo/interface/Measurement1D.h"
00009 #include "MagneticField/Engine/interface/MagneticField.h"
00010 #include "MagneticField/Records/interface/IdealMagneticFieldRecord.h"
00011 #include "RecoTracker/TkSeedingLayers/interface/SeedingHitSet.h"
00012 
00013 #include <math.h>
00014 
00015 template <class T> T sqr( T t) {return t*t;}
00016 
00017 L1MuonPixelTrackFitter::L1MuonPixelTrackFitter(const edm::ParameterSet& cfg)
00018   : theConfig(cfg)
00019 { }
00020 
00021 void L1MuonPixelTrackFitter::setL1Constraint(const L1MuGMTCand & muon)
00022 {
00023   thePhiL1 = muon.phiValue()+0.021817;
00024   theEtaL1 = muon.etaValue();
00025   theChargeL1 = muon.charge();
00026 }
00027 
00028 void L1MuonPixelTrackFitter::setPxConstraint(const SeedingHitSet & hits)
00029 {
00030   theHit1 = hits[0]->globalPosition();
00031   theHit1 = hits[1]->globalPosition();
00032 }
00033 
00034 reco::Track* L1MuonPixelTrackFitter::run( const edm::EventSetup& es, 
00035     const std::vector<const TrackingRecHit *>& hits, const TrackingRegion& region) const
00036 {
00037   edm::ESHandle<MagneticField> fieldESH;
00038   es.get<IdealMagneticFieldRecord>().get(fieldESH);
00039 
00040   double phi_vtx_fromHits = (theHit2-theHit1).phi();
00041 
00042   double invPt    = valInversePt(phi_vtx_fromHits, thePhiL1, theEtaL1);
00043   double invPtErr = errInversePt(invPt, theEtaL1);
00044 
00045   int charge = (invPt > 0.) ? 1 : -1;
00046   double  curvature = PixelRecoUtilities::curvature(invPt, es);
00047 
00048   Circle circle(theHit1, theHit2, curvature);
00049 
00050   double valPhi = this->valPhi(circle, charge);
00051   double valTip = this->valTip(circle, curvature); 
00052   double valZip = this->valZip(curvature, theHit1,theHit2);
00053   double valCotTheta = this->valCotTheta(PixelRecoLineRZ(theHit1,theHit2)); 
00054 
00055   static double invPtErrorScale = theConfig.getParameter<double>("invPtErrorScale");
00056   static double phiErrorScale= theConfig.getParameter<double>("phiErrorScale");
00057   static double cotThetaErrorScale = theConfig.getParameter<double>("cotThetaErrorScale");
00058   static double tipErrorScale = theConfig.getParameter<double>("tipErrorScale");
00059   static double zipErrorScale = theConfig.getParameter<double>("zipErrorScale");
00060 
00061 //  if ( (fabs(invPt)-0.1)/invPtErr > 3.) return 0;
00062 
00063   PixelTrackBuilder builder;
00064   double valPt =  (fabs(invPt) > 1.e-4) ? 1./fabs(invPt) : 1.e4;
00065   double errPt = invPtErrorScale*invPtErr * valPt*valPt;
00066   double eta = asinh(valCotTheta);
00067   double errPhi = this->errPhi(invPt, eta)           * phiErrorScale;
00068   double errCotTheta = this->errCotTheta(invPt, eta) * cotThetaErrorScale;
00069   double errTip = this->errTip(invPt, eta)           * tipErrorScale; 
00070   double errZip = this->errZip(invPt, eta)           * zipErrorScale;
00071     
00072   Measurement1D pt(valPt, errPt);
00073   Measurement1D phi(valPhi, errPhi);
00074   Measurement1D cotTheta(valCotTheta, errCotTheta);
00075   Measurement1D tip(valTip, errTip);
00076   Measurement1D zip(valZip, errZip);
00077 
00078   float chi2 = 0.;
00079 
00080 /*
00081   std::ostringstream str;
00082     str <<"\t ipt: " << invPt <<"+/-"<<invPtErr
00083         <<"\t pt:  " << pt.value() <<"+/-"<<pt.error()
00084         <<"\t phi: " << phi.value() <<"+/-"<<phi.error()
00085         <<"\t cot: " << cotTheta.value() <<"+/-"<<cotTheta.error()
00086         <<"\t tip: " << tip.value() <<"+/-"<<tip.error()
00087         <<"\t zip: " << zip.value() <<"+/-"<<zip.error()
00088         <<"\t charge: " << charge;
00089   std::cout <<str.str()<< std::endl;
00090 */
00091 
00092  
00093   return  builder.build(pt, phi, cotTheta, tip, zip, chi2, charge, hits, fieldESH.product());
00094 }
00095 
00096 
00097 double L1MuonPixelTrackFitter::valPhi(const Circle &circle, int charge) const
00098 {
00099   Circle::Point zero(0.,0.,0.);
00100   Circle::Vector center = circle.center()-zero;
00101   long double radius = center.perp();
00102   center /= radius;
00103   Circle::Vector dir = center.cross(-charge*Circle::Vector(0.,0.,1.));
00104   return dir.phi();
00105 }
00106 
00107 double L1MuonPixelTrackFitter::errPhi(double invPt, double eta) const {
00108   //
00109   // sigma = p0+p1/|pt|;
00110   //
00111   double p0,p1;
00112   int ieta = int (10*fabs(eta));
00113   switch (ieta) {
00114   case 0:  { p0 = 0.000597506; p1 = 0.00221057; break; }
00115   case 1:  { p0 = 0.000591867; p1 = 0.00278744; break; }
00116   case 2:  { p0 = 0.000635666; p1 = 0.00207433; break; }
00117   case 3:  { p0 = 0.000619086; p1 = 0.00255121; break; }
00118   case 4:  { p0 = 0.000572067; p1 = 0.00310618; break; }
00119   case 5:  { p0 = 0.000596239; p1 = 0.00288442; break; }
00120   case 6:  { p0 = 0.000607608; p1 = 0.00282996; break; }
00121   case 7:  { p0 = 0.000606446; p1 = 0.00281118; break; }
00122   case 8:  { p0 = 0.000594076; p1 = 0.00280546; break; }
00123   case 9:  { p0 = 0.000579615; p1 = 0.00335534; break; }
00124   case 10:  { p0 = 0.000659546; p1 = 0.00340443; break; }
00125   case 11:  { p0 = 0.000659031; p1 = 0.00343151; break; }
00126   case 12:  { p0 = 0.000738391; p1 = 0.00337297; break; }
00127   case 13:  { p0 = 0.000798966; p1 = 0.00330008; break; }
00128   case 14:  { p0 = 0.000702997; p1 = 0.00562643; break; }
00129   case 15:  { p0 = 0.000973417; p1 = 0.00312666; break; }
00130   case 16:  { p0 = 0.000995213; p1 = 0.00564278; break; }
00131   case 17:  { p0 = 0.00121436; p1 = 0.00572704; break; }
00132   case 18:  { p0 = 0.00119216; p1 = 0.00760204; break; }
00133   case 19:  { p0 = 0.00141204; p1 = 0.0093777; break; }
00134   default:  { p0 = 0.00153161; p1 = 0.00940265; break; }
00135   }
00136   return p0+p1*fabs(invPt);
00137 }
00138 
00139 
00140 double L1MuonPixelTrackFitter::valCotTheta(const PixelRecoLineRZ& line) const
00141 {
00142   return line.cotLine();
00143 }
00144 
00145 double L1MuonPixelTrackFitter::errCotTheta(double invPt, double eta) const 
00146 {
00147   //
00148   // sigma = p0+p1/|pt|;
00149   //
00150   double p0,p1;
00151   int ieta = int (10*fabs(eta));
00152   switch (ieta) {
00153   case 0:  { p0 = 0.00166115; p1 = 5.75533e-05; break; }
00154   case 1:  { p0 = 0.00157525; p1 = 0.000707437; break; }
00155   case 2:  { p0 = 0.00122246; p1 = 0.000325456; break; }
00156   case 3:  { p0 = 0.000852422; p1 = 0.000429216; break; }
00157   case 4:  { p0 = 0.000637561; p1 = 0.00122298; break; }
00158   case 5:  { p0 = 0.000555766; p1 = 0.00158096; break; }
00159   case 6:  { p0 = 0.000641202; p1 = 0.00143339; break; }
00160   case 7:  { p0 = 0.000803207; p1 = 0.000648816; break; }
00161   case 8:  { p0 = 0.000741394; p1 = 0.0015289; break; }
00162   case 9:  { p0 = 0.000652019; p1 = 0.00168873; break; }
00163   case 10:  { p0 = 0.000716902; p1 = 0.00257556; break; }
00164   case 11:  { p0 = 0.000800409; p1 = 0.00190563; break; }
00165   case 12:  { p0 = 0.000808778; p1 = 0.00264139; break; }
00166   case 13:  { p0 = 0.000775757; p1 = 0.00318478; break; }
00167   case 14:  { p0 = 0.000705781; p1 = 0.00460576; break; }
00168   case 15:  { p0 = 0.000580679; p1 = 0.00748248; break; }
00169   case 16:  { p0 = 0.000561667; p1 = 0.00767487; break; }
00170   case 17:  { p0 = 0.000521626; p1 = 0.0100178; break; }
00171   case 18:  { p0 = 0.00064253; p1 = 0.0106062; break; }
00172   case 19:  { p0 = 0.000636868; p1 = 0.0140047; break; }
00173   default:  { p0 = 0.000682478; p1 = 0.0163569; break; }
00174   }
00175   return p0+p1*fabs(invPt);
00176 }
00177 
00178 
00179 double L1MuonPixelTrackFitter::valTip(const Circle &circle, double curvature) const
00180 {
00181   Circle::Point zero(0.,0.,0.);
00182   Circle::Vector center = circle.center()-zero;
00183   long double radius = center.perp();
00184   return radius-1./fabs(curvature);
00185 }
00186 
00187 double L1MuonPixelTrackFitter::errTip(double invPt, double eta) const {
00188   //
00189   // sigma = p0+p1/|pt|;
00190   //
00191   double p0,p1;
00192   int ieta = int (10*fabs(eta));
00193   switch (ieta) {
00194   case 0:  { p0 = 0.00392416; p1 = 0.00551809; break; }
00195   case 1:  { p0 = 0.00390391; p1 = 0.00543244; break; }
00196   case 2:  { p0 = 0.0040651; p1 = 0.00406496; break; }
00197   case 3:  { p0 = 0.00387782; p1 = 0.00797637; break; }
00198   case 4:  { p0 = 0.00376798; p1 = 0.00866894; break; }
00199   case 5:  { p0 = 0.0042131; p1 = 0.00462184; break; }
00200   case 6:  { p0 = 0.00392579; p1 = 0.00784685; break; }
00201   case 7:  { p0 = 0.00370472; p1 = 0.00790174; break; }
00202   case 8:  { p0 = 0.00364433; p1 = 0.00928368; break; }
00203   case 9:  { p0 = 0.00387578; p1 = 0.00640431; break; }
00204   case 10:  { p0 = 0.00382464; p1 = 0.00960763; break; }
00205   case 11:  { p0 = 0.0038907; p1 = 0.0104562; break; }
00206   case 12:  { p0 = 0.00392525; p1 = 0.0106442; break; }
00207   case 13:  { p0 = 0.00400634; p1 = 0.011218; break; }
00208   case 14:  { p0 = 0.0036229; p1 = 0.0156403; break; }
00209   case 15:  { p0 = 0.00444317; p1 = 0.00832987; break; }
00210   case 16:  { p0 = 0.00465492; p1 = 0.0179908; break; }
00211   case 17:  { p0 = 0.0049652; p1 = 0.0216647; break; }
00212   case 18:  { p0 = 0.0051395; p1 = 0.0233692; break; }
00213   case 19:  { p0 = 0.0062917; p1 = 0.0262175; break; }
00214   default:  { p0 = 0.00714444; p1 = 0.0253856; break; }
00215   }
00216   return p0+p1*fabs(invPt);
00217 }
00218 
00219 
00220 double L1MuonPixelTrackFitter::valZip(double curv,
00221     const GlobalPoint& pinner, const GlobalPoint& pouter) const
00222 {
00223   //
00224   // phi = asin(r*rho/2) with asin(x) ~= x+x**3/(2*3)
00225   //
00226   double rho3 = curv*curv*curv;
00227   double r1 = pinner.perp();
00228   double phi1 = r1*curv/2 + pinner.perp2()*r1*rho3/48.;
00229   double r2 = pouter.perp();
00230   double phi2 = r2*curv/2 + pouter.perp2()*r2*rho3/48.;
00231   double z1 = pinner.z();
00232   double z2 = pouter.z();
00233 
00234   return z1 - phi1/(phi1-phi2)*(z1-z2);
00235 }
00236 
00237 
00238 double L1MuonPixelTrackFitter::errZip(double invPt, double eta) const {
00239   //
00240   // sigma = p0+p1/pt;
00241   //
00242   double p0,p1;
00243   int ieta = int (10*fabs(eta));
00244   switch (ieta) {
00245   case 0:  { p0 = 0.0120743; p1 = 0; break; }
00246   case 1:  { p0 = 0.0110343; p1 = 0.0051199; break; }
00247   case 2:  { p0 = 0.00846487; p1 = 0.00570084; break; }
00248   case 3:  { p0 = 0.00668726; p1 = 0.00331165; break; }
00249   case 4:  { p0 = 0.00467126; p1 = 0.00578239; break; }
00250   case 5:  { p0 = 0.0043042; p1 = 0.00598517; break; }
00251   case 6:  { p0 = 0.00515392; p1 = 0.00495422; break; }
00252   case 7:  { p0 = 0.0060843; p1 = 0.00320512; break; }
00253   case 8:  { p0 = 0.00564942; p1 = 0.00478876; break; }
00254   case 9:  { p0 = 0.00532111; p1 = 0.0073239; break; }
00255   case 10:  { p0 = 0.00579429; p1 = 0.00952782; break; }
00256   case 11:  { p0 = 0.00614229; p1 = 0.00977795; break; }
00257   case 12:  { p0 = 0.00714661; p1 = 0.00550482; break; }
00258   case 13:  { p0 = 0.0066593; p1 = 0.00999362; break; }
00259   case 14:  { p0 = 0.00634922; p1 = 0.0148156; break; }
00260   case 15:  { p0 = 0.00600586; p1 = 0.0318022; break; }
00261   case 16:  { p0 = 0.00676919; p1 = 0.027456; break; }
00262   case 17:  { p0 = 0.00670066; p1 = 0.0317005; break; }
00263   case 18:  { p0 = 0.00752392; p1 = 0.0347714; break; }
00264   case 19:  { p0 = 0.00791425; p1 = 0.0566665; break; }
00265   default:  { p0 = 0.00882372; p1 = 0.0596858; break; }
00266   }
00267   return  p0+p1*fabs(invPt);
00268 }
00269   
00270 
00271 double L1MuonPixelTrackFitter::valInversePt( double phi0, double phiL1, double eta) const
00272 {
00273    double result = 0.;   
00274    
00275   // solve equtaion p3*result^3 + p1*result + dphi = 0;
00276   // where:  result = 1/pt 
00277   //         dphi = phiL1 - phi0
00278   // Cardan way is used
00279   
00280   double p1,p2,p3;        //parameters p1,p3 are negative by parameter fix in fit!
00281   param(eta, p1, p2, p3);
00282 
00283   double dphi = deltaPhi(phiL1,phi0);    // phi0-phiL1
00284   if ( fabs(dphi) < 0.01) {
00285     result = -dphi/p1; 
00286   } else {
00287     double q = dphi/2./p3;
00288     double p = p1/3./p3;                 // positive
00289     double D = q*q + p*p*p;              // positive 
00290     double u = pow(-q+sqrt(D), 1./3.);
00291     double v = -pow(q+sqrt(D), 1./3.);
00292     result = u+v;
00293   }
00294   return result;
00295 }
00296 
00297 
00298 double L1MuonPixelTrackFitter::errInversePt(double invPt, double eta) const {
00299   //
00300   // pt*sigma(1/pt) = p0+p1*pt;
00301   //
00302   double p0,p1;
00303   int ieta = int (10*fabs(eta));
00304   switch (ieta) {
00305   case 0:  { p0 = 0.0196835; p1 = 0.00517533; break; }
00306   case 1:  { p0 = 0.0266583; p1 = 0.00478101; break; }
00307   case 2:  { p0 = 0.0217164; p1 = 0.00545425; break; }
00308   case 3:  { p0 = 0.0197547; p1 = 0.00552263; break; }
00309   case 4:  { p0 = 0.0208778; p1 = 0.00536009; break; }
00310   case 5:  { p0 = 0.024192; p1 = 0.00521709; break; }
00311   case 6:  { p0 = 0.0265315; p1 = 0.0051897; break; }
00312   case 7:  { p0 = 0.0198071; p1 = 0.00566822; break; }
00313   case 8:  { p0 = 0.0361955; p1 = 0.00486352; break; }
00314   case 9:  { p0 = 0.037864; p1 = 0.00509094; break; }
00315   case 10:  { p0 = 0.0382968; p1 = 0.00612354; break; }
00316   case 11:  { p0 = 0.0308326; p1 = 0.0074234; break; }
00317   case 12:  { p0 = 0.0248577; p1 = 0.00883049; break; }
00318   case 13:  { p0 = 0.0279965; p1 = 0.00888293; break; }
00319   case 14:  { p0 = 0.0372582; p1 = 0.00950252; break; }
00320   case 15:  { p0 = 0.0281366; p1 = 0.0111501; break; }
00321   case 16:  { p0 = 0.0421483; p1 = 0.0109413; break; }
00322   case 17:  { p0 = 0.0461798; p1 = 0.0125824; break; }
00323   case 18:  { p0 = 0.0530603; p1 = 0.0132638; break; }
00324   case 19:  { p0 = 0.0601148; p1 = 0.0147911; break; }
00325   default:  { p0 = 0.0552377; p1 = 0.0155574; break; }
00326   }
00327   return p1+p0*fabs(invPt);
00328 }
00329 
00330 double L1MuonPixelTrackFitter::findPt(double phi0, double phiL1, double eta, int charge) const {
00331 
00332   double dphi_min = fabs(deltaPhi(phi0,phiL1));
00333   double pt_best = 1.;
00334   double pt_cur = 1;
00335   while ( pt_cur < 10000.) {
00336     double phi_exp = phi0+getBending(1./pt_cur, eta, charge);
00337     double dphi = fabs(deltaPhi(phi_exp,phiL1));
00338     if ( dphi < dphi_min) {
00339       pt_best = pt_cur;
00340       dphi_min = dphi;
00341     }
00342     if (pt_cur < 10.) pt_cur += 0.01; 
00343     else if (pt_cur < 20.) pt_cur+= 0.025; 
00344     else if( pt_cur < 100.) pt_cur+= 0.1;
00345     else pt_cur +=1;
00346   };
00347   return pt_best;
00348 }
00349 
00350 
00351 double L1MuonPixelTrackFitter::getBending(double invPt, double eta, int charge) 
00352 {
00353   double p1, p2, p3;
00354   param(eta,p1,p2,p3);
00355   return charge*p1*invPt + charge*p2*invPt*invPt + charge*p3*invPt*invPt*invPt;
00356 }
00357 
00358 double L1MuonPixelTrackFitter::getBendingError(double invPt, double eta) 
00359 {
00360   int ieta = int (10*fabs(eta));
00361   double p0,p1;
00362   switch (ieta) {
00363   case 0:  { p0 = 0.0196835; p1 = 0.00517533; break; }
00364   case 1:  { p0 = 0.0266583; p1 = 0.00478101; break; }
00365   case 2:  { p0 = 0.0217164; p1 = 0.00545425; break; }
00366   case 3:  { p0 = 0.0197547; p1 = 0.00552263; break; }
00367   case 4:  { p0 = 0.0208778; p1 = 0.00536009; break; }
00368   case 5:  { p0 = 0.024192; p1 = 0.00521709; break; }
00369   case 6:  { p0 = 0.0265315; p1 = 0.0051897; break; }
00370   case 7:  { p0 = 0.0198071; p1 = 0.00566822; break; }
00371   case 8:  { p0 = 0.0361955; p1 = 0.00486352; break; }
00372   case 9:  { p0 = 0.037864; p1 = 0.00509094; break; }
00373   case 10:  { p0 = 0.0382968; p1 = 0.00612354; break; }
00374   case 11:  { p0 = 0.0308326; p1 = 0.0074234; break; }
00375   case 12:  { p0 = 0.0248577; p1 = 0.00883049; break; }
00376   case 13:  { p0 = 0.0279965; p1 = 0.00888293; break; }
00377   case 14:  { p0 = 0.0372582; p1 = 0.00950252; break; }
00378   case 15:  { p0 = 0.0281366; p1 = 0.0111501; break; }
00379   case 16:  { p0 = 0.0421483; p1 = 0.0109413; break; }
00380   case 17:  { p0 = 0.0461798; p1 = 0.0125824; break; }
00381   case 18:  { p0 = 0.0530603; p1 = 0.0132638; break; }
00382   case 19:  { p0 = 0.0601148; p1 = 0.0147911; break; }
00383   default:  { p0 = 0.0552377; p1 = 0.0155574; break; }
00384   }
00385   return p0+p1*sqr(invPt);
00386 }
00387 
00388 
00389 void L1MuonPixelTrackFitter::param(double eta, double &p1, double& p2, double& p3) 
00390 {
00391   int ieta = int (10*fabs(eta));
00392   switch (ieta) {
00393   case 0:  { p1 = -2.68016; p2 = 0; p3 = -12.9653; break; }
00394   case 1:  { p1 = -2.67864; p2 = 0; p3 = -12.0036; break; }
00395   case 2:  { p1 = -2.72997; p2 = 0; p3 = -10.3468; break; }
00396   case 3:  { p1 = -2.68836; p2 = 0; p3 = -12.3369; break; }
00397   case 4:  { p1 = -2.66885; p2 = 0; p3 = -11.589; break; }
00398   case 5:  { p1 = -2.64932; p2 = 0; p3 = -12.7176; break; }
00399   case 6:  { p1 = -2.64513; p2 = 0; p3 = -11.3912; break; }
00400   case 7:  { p1 = -2.64487; p2 = 0; p3 = -9.83239; break; }
00401   case 8:  { p1 = -2.58875; p2 = 0; p3 = -10.0541; break; }
00402   case 9:  { p1 = -2.5551; p2 = 0; p3 = -9.75757; break; }
00403   case 10:  { p1 = -2.55294; p2 = 0; p3 = -7.25238; break; }
00404   case 11:  { p1 = -2.45333; p2 = 0; p3 = -7.966; break; }
00405   case 12:  { p1 = -2.29283; p2 = 0; p3 = -7.03231; break; }
00406   case 13:  { p1 = -2.13167; p2 = 0; p3 = -4.29182; break; }
00407   case 14:  { p1 = -1.9102; p2 = 0; p3 = -3.21295; break; }
00408   case 15:  { p1 = -1.74552; p2 = 0; p3 = -0.531827; break; }
00409   case 16:  { p1 = -1.53642; p2 = 0; p3 = -1.74057; break; }
00410   case 17:  { p1 = -1.39446; p2 = 0; p3 = -1.56819; break; }
00411   case 18:  { p1 = -1.26176; p2 = 0; p3 = -0.598631; break; }
00412   case 19:  { p1 = -1.14133; p2 = 0; p3 = -0.0941055; break; }
00413   default:  { p1 = -1.02086; p2 = 0; p3 = -0.100491; break; }
00414   }
00415 }
00416 
00417 double L1MuonPixelTrackFitter::deltaPhi(double phi1, double phi2) const
00418 {
00419   while ( phi1 >= 2*M_PI) phi1 -= 2*M_PI;
00420   while ( phi2 >= 2*M_PI) phi2 -= 2*M_PI;
00421   while ( phi1 < 0) phi1 += 2*M_PI;
00422   while ( phi2 < 0) phi2 += 2*M_PI;
00423   double dPhi = phi2-phi1;
00424 
00425   if ( dPhi > M_PI ) dPhi -= 2*M_PI;
00426   if ( dPhi < -M_PI ) dPhi += 2*M_PI;
00427 
00428   return dPhi;
00429 }
00430