EIC Software
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
RKTrackRep.cxx
Go to the documentation of this file. Or view the newest version in sPHENIX GitHub for file RKTrackRep.cxx
1 /* Copyright 2008-2009, Technische Universitaet Muenchen,
2  Authors: Christian Hoeppner & Sebastian Neubert
3 
4  This file is part of GENFIT.
5 
6  GENFIT is free software: you can redistribute it and/or modify
7  it under the terms of the GNU Lesser General Public License as published
8  by the Free Software Foundation, either version 3 of the License, or
9  (at your option) any later version.
10 
11  GENFIT is distributed in the hope that it will be useful,
12  but WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  GNU Lesser General Public License for more details.
15 
16  You should have received a copy of the GNU Lesser General Public License
17  along with GENFIT. If not, see <http://www.gnu.org/licenses/>.
18 */
19 
20 /* The Runge Kutta implementation stems from GEANT3 originally (R. Brun et al.)
21  Porting to C goes back to Igor Gavrilenko @ CERN
22  The code was taken from the Phast analysis package of the COMPASS experiment
23  (Sergei Gerrassimov @ CERN)
24 */
25 
26 #include "RKTrackRep.h"
27 #include <iostream>
28 #include <sstream>
29 #include <iomanip>
30 #include <assert.h>
31 #include "stdlib.h"
32 #include <math.h>
33 #include "TMath.h"
34 #include "TBuffer.h"
35 #include "TGeoManager.h"
36 #include "TDatabasePDG.h"
37 #include "GFException.h"
38 #include "GFFieldManager.h"
39 #include "GFMaterialEffects.h"
40 #include "GFPointPath.h"
41 
42 #define MINSTEP 0.001 // minimum step [cm] for Runge Kutta and iteration to POCA
43 //#define DEBUG
44 
45 
46 RKTrackRep::~RKTrackRep(){
47 }
48 
49 void RKTrackRep::Streamer(TBuffer &R__b)
50 {
51  // Stream an object of class RKTrackRep.
52 
53  if (R__b.IsReading()) {
54  R__b.ReadClassBuffer(RKTrackRep::Class(),this);
55  initArrays();
56  fCachePlane = fRefPlane;
57  fCacheSpu = fSpu;
58 
59  } else {
60  R__b.WriteClassBuffer(RKTrackRep::Class(),this);
61  }
62 }
63 
64 
65 RKTrackRep::RKTrackRep() : GFAbsTrackRep(5), fDirection(0), fNoMaterial(false), fPdg(0), fMass(0.), fCharge(-1), fCachePlane(), fCacheSpu(1), fSpu(1), fAuxInfo(1,2) {
66  initArrays();
67 }
68 
69 
70 RKTrackRep::RKTrackRep(const TVector3& pos,
71  const TVector3& mom,
72  const TVector3& poserr,
73  const TVector3& momerr,
74  const int& PDGCode) :
75  GFAbsTrackRep(5), fDirection(0), fNoMaterial(false), fCachePlane(), fCacheSpu(1), fAuxInfo(1,2) {
76 
77  initArrays();
78  setPDG(PDGCode); // also sets charge and mass
79  calcStateCov(pos, mom, poserr, momerr);
80 }
81 
82 
83 RKTrackRep::RKTrackRep(const TVector3& pos,
84  const TVector3& mom,
85  const int& PDGCode) :
86  GFAbsTrackRep(5), fDirection(0), fNoMaterial(false), fCachePlane(), fCacheSpu(1), fAuxInfo(1,2) {
87 
88  initArrays();
89  setPDG(PDGCode); // also sets charge and mass
90  calcState(pos, mom);
91 
92  // set covariance diagonal elements to large number
93  static const double value(1.E4);
94 
95  fCov(0,0) = value;
96  fCov(1,1) = value;
97  fCov(2,2) = value;
98  fCov(3,3) = value;
99  fCov(4,4) = value;
100 }
101 
102 
103 RKTrackRep::RKTrackRep(const GFTrackCand* const aGFTrackCandPtr, int pdgCode) :
104  GFAbsTrackRep(5), fDirection(0), fNoMaterial(false), fCachePlane(), fCacheSpu(1), fAuxInfo(1,2) {
105 
106  if (pdgCode == 0){
107  pdgCode = aGFTrackCandPtr->getPdgCode();
108  }
109 
110  initArrays();
111 
112  setPDG(pdgCode); // also sets charge and mass
113 
114  TMatrixD state6D = aGFTrackCandPtr->getStateSeed();
115  TMatrixD cov6D = aGFTrackCandPtr->getCovSeed();
116  TVector3 posError;
117  TVector3 momError;
118  if( cov6D[0][0] < 0.0 ){ // no valid cov was set in the trackCand so just set a large one
119  posError.SetXYZ(sqrt(cov6D[0][0]),sqrt(cov6D[1][1]),sqrt(cov6D[2][2]));
120  momError.SetXYZ(sqrt(cov6D[3][3]),sqrt(cov6D[4][4]),sqrt(cov6D[5][5]));
121  } else {
122  posError.SetXYZ(sqrt(cov6D[0][0]),sqrt(cov6D[1][1]),sqrt(cov6D[2][2]));
123  momError.SetXYZ(sqrt(cov6D[3][3]),sqrt(cov6D[4][4]),sqrt(cov6D[5][5]));
124  }
125 
126  TVector3 pos(state6D[0][0],state6D[1][0],state6D[2][0]);
127  TVector3 mom(state6D[3][0],state6D[4][0],state6D[5][0]);
128  calcStateCov(pos, mom, posError, momError);
129 }
130 
131 
132 void RKTrackRep::initArrays(){
133  memset(fStateJac,0x00,(7+7*7)*sizeof(double));
134  memset(fNoise,0x00,7*7*sizeof(double));
135  memset(fStateJac,0x00,7*7*sizeof(double));
136 
137  memset(fJ_pM_5x7,0x00,5*7*sizeof(double));
138  memset(fJ_pM_5x6,0x00,5*6*sizeof(double));
139  memset(fJ_Mp_7x5,0x00,7*5*sizeof(double));
140  memset(fJ_Mp_6x5,0x00,6*5*sizeof(double));
141 }
142 
143 
144 void RKTrackRep::setData(const TMatrixD& st, const GFDetPlane& pl, const TMatrixD* cov, const TMatrixD* aux){
145  if(aux != NULL) {
146  fCacheSpu = (*aux)(0,0);
147  fDirection = (*aux)(0,1);
148  }
149  else {
150  if(pl!=fCachePlane){
151  GFException exc("RKTrackRep::setData() was called with a reference plane which is not the same as the one from the last extrapolate(plane,state,cov).",__LINE__,__FILE__);
152  throw exc;
153  }
154  }
155  GFAbsTrackRep::setData(st,pl,cov);
156  if (fCharge*fState(0,0) < 0) fCharge *= -1; // set charge accordingly! (fState[0][0] = q/p)
157  fSpu = fCacheSpu;
158 }
159 
160 
161 const TMatrixD* RKTrackRep::getAuxInfo(const GFDetPlane& pl) {
162 
163  if(pl!=fCachePlane) {
164  GFException exc("RKTrackRep::getAuxInfo() - Trying to get auxiliary information with planes mismatch (Information returned does not belong to requested plane)!",__LINE__,__FILE__);
165  throw exc;
166  }
167  fAuxInfo.ResizeTo(1,2);
168  fAuxInfo(0,0) = fCacheSpu;
169  fAuxInfo(0,1) = fDirection;
170  return &fAuxInfo;
171 }
172 
173 
174 void RKTrackRep::setPDG(int i){
175  fPdg = i;
176  TParticlePDG * part = TDatabasePDG::Instance()->GetParticle(fPdg);
177  if(part == 0){
178  GFException exc("RKTrackRep::setPDG ==> particle id not known to TDatabasePDG",__LINE__,__FILE__);
179  throw exc;
180  }
181  fMass = part->Mass();
182  fCharge = part->Charge()/(3.);
183 }
184 
185 
186 
187 void RKTrackRep::calcStateCov(const TVector3& pos,
188  const TVector3& mom,
189  const TVector3& poserr,
190  const TVector3& momerr){
191 
192  calcState(pos, mom);
193 
194  double pw = mom.Mag();
195  double pu = 0.;
196  double pv = 0.;
197 
198  fU = fRefPlane.getU();
199  fV = fRefPlane.getV();
200  fW = fRefPlane.getNormal();
201 
202 
203  fCov(0,0) = fCharge*fCharge/pow(mom.Mag(),6.) *
204  (mom.X()*mom.X() * momerr.X()*momerr.X()+
205  mom.Y()*mom.Y() * momerr.Y()*momerr.Y()+
206  mom.Z()*mom.Z() * momerr.Z()*momerr.Z());
207 
208  fCov(1,1) = pow((fU.X()/pw - fW.X()*pu/(pw*pw)),2.) * momerr.X()*momerr.X() +
209  pow((fU.Y()/pw - fW.Y()*pu/(pw*pw)),2.) * momerr.Y()*momerr.Y() +
210  pow((fU.Z()/pw - fW.Z()*pu/(pw*pw)),2.) * momerr.Z()*momerr.Z();
211 
212  fCov(2,2) = pow((fV.X()/pw - fW.X()*pv/(pw*pw)),2.) * momerr.X()*momerr.X() +
213  pow((fV.Y()/pw - fW.Y()*pv/(pw*pw)),2.) * momerr.Y()*momerr.Y() +
214  pow((fV.Z()/pw - fW.Z()*pv/(pw*pw)),2.) * momerr.Z()*momerr.Z();
215 
216  fCov(3,3) = poserr.X()*poserr.X() * fU.X()*fU.X() +
217  poserr.Y()*poserr.Y() * fU.Y()*fU.Y() +
218  poserr.Z()*poserr.Z() * fU.Z()*fU.Z();
219 
220  fCov(4,4) = poserr.X()*poserr.X() * fV.X()*fV.X() +
221  poserr.Y()*poserr.Y() * fV.Y()*fV.Y() +
222  poserr.Z()*poserr.Z() * fV.Z()*fV.Z();
223 }
224 
225 
226 void RKTrackRep::calcState(const TVector3& pos,
227  const TVector3& mom){
228 
229  fRefPlane.setON(pos, mom);
230  fSpu=1.;
231 
232  fState(0,0) = fCharge/mom.Mag();
233 
234  //u' and v'
235  fState(1,0) = 0.;
236  fState(2,0) = 0.;
237 
238  //u and v
239  fState(3,0) = 0.;
240  fState(4,0) = 0.;
241 }
242 
243 
244 
245 void RKTrackRep::getState7(M1x7& state7) {
246  getState7(state7, fState, fRefPlane, fSpu);
247 }
248 
249 
250 void RKTrackRep::getState7(M1x7& state7, const TMatrixD& state5, const GFDetPlane& pl, const double& spu) {
251 
252  fU = pl.getU();
253  fV = pl.getV();
254  fO = pl.getO();
255  fW = pl.getNormal();
256 
257  state7[0] = fO.X() + state5(3,0)*fU.X() + state5(4,0)*fV.X(); // x
258  state7[1] = fO.Y() + state5(3,0)*fU.Y() + state5(4,0)*fV.Y(); // y
259  state7[2] = fO.Z() + state5(3,0)*fU.Z() + state5(4,0)*fV.Z(); // z
260 
261  state7[3] = spu * (fW.X() + state5(1,0)*fU.X() + state5(2,0)*fV.X()); // a_x
262  state7[4] = spu * (fW.Y() + state5(1,0)*fU.Y() + state5(2,0)*fV.Y()); // a_y
263  state7[5] = spu * (fW.Z() + state5(1,0)*fU.Z() + state5(2,0)*fV.Z()); // a_z
264 
265  // normalize dir
266  double norm = 1. / sqrt(state7[3]*state7[3] + state7[4]*state7[4] + state7[5]*state7[5]);
267  for (unsigned int i=3; i<6; ++i) state7[i] *= norm;
268 
269  state7[6] = state5(0,0); // q/p
270 }
271 
272 
273 TMatrixD RKTrackRep::getState5(const M1x7& state7, const GFDetPlane& pl, double& spu) {
274 
275  fU = pl.getU();
276  fV = pl.getV();
277 
278  fPos.SetXYZ(state7[0], state7[1], state7[2]);
279  fPos -= pl.getO();
280 
281  fDir.SetXYZ(state7[3], state7[4], state7[5]);
282 
283  // force A to be in normal direction and set spu accordingly
284  double AtW = fDir * pl.getNormal();
285  spu = 1.;
286  if (AtW < 0) {
287  fDir *= -1.;
288  AtW *= -1.;
289  spu = -1.;
290  }
291 
292  TMatrixD state5(5,1);
293  state5(0,0) = state7[6];
294  state5(1,0) = fDir*fU / AtW;
295  state5(2,0) = fDir*fV / AtW;
296  state5(3,0) = fPos*fU;
297  state5(4,0) = fPos*fV;
298 
299  return state5;
300 }
301 
302 
303 
304 void RKTrackRep::transformPM7(const TMatrixD& in5x5, M7x7& out7x7,
305  const GFDetPlane& pl, const TMatrixD& state5, const double& spu,
306  TMatrixD* Jac) {
307 
308  // get vectors and aux variables
309  fU = pl.getU();
310  fV = pl.getV();
311  fW = pl.getNormal();
312 
313  fpTilde.SetXYZ(spu * (fW.X() + state5(1,0)*fU.X() + state5(2,0)*fV.X()), // a_x
314  spu * (fW.Y() + state5(1,0)*fU.Y() + state5(2,0)*fV.Y()), // a_y
315  spu * (fW.Z() + state5(1,0)*fU.Z() + state5(2,0)*fV.Z()));// a_z
316 
317 
318  const double pTildeMag = fpTilde.Mag();
319  const double pTildeMag2 = pTildeMag*pTildeMag;
320 
321  const double utpTildeOverpTildeMag2 = fU*fpTilde / pTildeMag2;
322  const double vtpTildeOverpTildeMag2 = fV*fpTilde / pTildeMag2;
323 
324  //J_pM matrix is d(x,y,z,ax,ay,az,q/p) / d(q/p,u',v',u,v) (out is 7x7)
325 
326  // d(x,y,z)/d(u)
327  fJ_pM_5x7[21] = fU.X(); // [3][0]
328  fJ_pM_5x7[22] = fU.Y(); // [3][1]
329  fJ_pM_5x7[23] = fU.Z(); // [3][2]
330  // d(x,y,z)/d(v)
331  fJ_pM_5x7[28] = fV.X(); // [4][2]
332  fJ_pM_5x7[29] = fV.Y(); // [4][2]
333  fJ_pM_5x7[30] = fV.Z(); // [4][2]
334  // d(q/p)/d(q/p)
335  fJ_pM_5x7[6] = 1.; // not needed for array matrix multiplication
336  // d(ax,ay,az)/d(u')
337  double fact = spu / pTildeMag;
338  fJ_pM_5x7[10] = fact * ( fU.X() - fpTilde.X()*utpTildeOverpTildeMag2 ); // [1][3]
339  fJ_pM_5x7[11] = fact * ( fU.Y() - fpTilde.Y()*utpTildeOverpTildeMag2 ); // [1][4]
340  fJ_pM_5x7[12] = fact * ( fU.Z() - fpTilde.Z()*utpTildeOverpTildeMag2 ); // [1][5]
341  // d(ax,ay,az)/d(v')
342  fJ_pM_5x7[17] = fact * ( fV.X() - fpTilde.X()*vtpTildeOverpTildeMag2 ); // [2][3]
343  fJ_pM_5x7[18] = fact * ( fV.Y() - fpTilde.Y()*vtpTildeOverpTildeMag2 ); // [2][4]
344  fJ_pM_5x7[19] = fact * ( fV.Z() - fpTilde.Z()*vtpTildeOverpTildeMag2 ); // [2][5]
345 
346 
347  // since the Jacobian contains a lot of zeros, and the resulting cov has to be symmetric,
348  // the multiplication can be done much faster directly on array level
349  // out = J_pM^T * in5x5 * J_pM
350  const M5x5& in5x5_ = *((M5x5*) in5x5.GetMatrixArray());
351  RKTools::J_pMTxcov5xJ_pM(fJ_pM_5x7, in5x5_, out7x7);
352 
353  if (Jac!=NULL){
354  Jac->ResizeTo(5,7);
355  *Jac = TMatrixD(5,7, &(fJ_pM_5x7[0]));
356  }
357 }
358 
359 
360 void RKTrackRep::transformPM6(const TMatrixD& in5x5, M6x6& out6x6,
361  const GFDetPlane& pl, const TMatrixD& state5, const double& spu,
362  TMatrixD* Jac) {
363 
364  // get vectors and aux variables
365  fU = pl.getU();
366  fV = pl.getV();
367 
368  fpTilde.SetXYZ(spu * (fW.X() + state5(1,0)*fU.X() + state5(2,0)*fV.X()), // a_x
369  spu * (fW.Y() + state5(1,0)*fU.Y() + state5(2,0)*fV.Y()), // a_y
370  spu * (fW.Z() + state5(1,0)*fU.Z() + state5(2,0)*fV.Z()));// a_z
371 
372  const double pTildeMag = fpTilde.Mag();
373  const double pTildeMag2 = pTildeMag*pTildeMag;
374 
375  const double utpTildeOverpTildeMag2 = fU*fpTilde / pTildeMag2;
376  const double vtpTildeOverpTildeMag2 = fV*fpTilde / pTildeMag2;
377 
378  //J_pM matrix is d(x,y,z,px,py,pz) / d(q/p,u',v',u,v) (out is 6x6)
379 
380  const double qop = state5(0,0);
381  const double p = fCharge/qop; // momentum
382 
383  // d(px,py,pz)/d(q/p)
384  double fact = -1. * p / (pTildeMag * qop);
385  fJ_pM_5x6[3] = fact * fpTilde.X(); // [0][3]
386  fJ_pM_5x6[4] = fact * fpTilde.Y(); // [0][4]
387  fJ_pM_5x6[5] = fact * fpTilde.Z(); // [0][5]
388  // d(px,py,pz)/d(u')
389  fact = p * spu / pTildeMag;
390  fJ_pM_5x6[9] = fact * ( fU.X() - fpTilde.X()*utpTildeOverpTildeMag2 ); // [1][3]
391  fJ_pM_5x6[10] = fact * ( fU.Y() - fpTilde.Y()*utpTildeOverpTildeMag2 ); // [1][4]
392  fJ_pM_5x6[11] = fact * ( fU.Z() - fpTilde.Z()*utpTildeOverpTildeMag2 ); // [1][5]
393  // d(px,py,pz)/d(v')
394  fJ_pM_5x6[15] = fact * ( fV.X() - fpTilde.X()*vtpTildeOverpTildeMag2 ); // [2][3]
395  fJ_pM_5x6[16] = fact * ( fV.Y() - fpTilde.Y()*vtpTildeOverpTildeMag2 ); // [2][4]
396  fJ_pM_5x6[17] = fact * ( fV.Z() - fpTilde.Z()*vtpTildeOverpTildeMag2 ); // [2][5]
397  // d(x,y,z)/d(u)
398  fJ_pM_5x6[18] = fU.X(); // [3][0]
399  fJ_pM_5x6[19] = fU.Y(); // [3][1]
400  fJ_pM_5x6[20] = fU.Z(); // [3][2]
401  // d(x,y,z)/d(v)
402  fJ_pM_5x6[24] = fV.X(); // [4][0]
403  fJ_pM_5x6[25] = fV.Y(); // [4][1]
404  fJ_pM_5x6[26] = fV.Z(); // [4][2]
405 
406 
407  // do the transformation
408  // out = J_pM^T * in5x5 * J_pM
409  const M5x5& in5x5_ = *((M5x5*) in5x5.GetMatrixArray());
410  RKTools::J_pMTxcov5xJ_pM(fJ_pM_5x6, in5x5_, out6x6);
411 
412  if (Jac!=NULL){
413  Jac->ResizeTo(5,6);
414  *Jac = TMatrixD(5,6, &(fJ_pM_5x6[0]));
415  }
416 }
417 
418 
419 void RKTrackRep::transformM7P(const M7x7& in7x7, TMatrixD& out5x5,
420  const GFDetPlane& pl, const M1x7& state7,
421  TMatrixD* Jac) {
422 
423  out5x5.ResizeTo(5, 5);
424 
425  // get vectors and aux variables
426  fU = pl.getU();
427  fV = pl.getV();
428  fW = pl.getNormal();
429 
430  fDir.SetXYZ(state7[3], state7[4], state7[5]);
431 
432  const double AtU = fDir*fU;
433  const double AtV = fDir*fV;
434  const double AtW = fDir*fW;
435 
436  // J_Mp matrix is d(q/p,u',v',u,v) / d(x,y,z,ax,ay,az,q/p) (in is 7x7)
437 
438  // d(u')/d(ax,ay,az)
439  double fact = 1./(AtW*AtW);
440  fJ_Mp_7x5[16] = fact * (fU.X()*AtW - fW.X()*AtU); // [3][1]
441  fJ_Mp_7x5[21] = fact * (fU.Y()*AtW - fW.Y()*AtU); // [4][1]
442  fJ_Mp_7x5[26] = fact * (fU.Z()*AtW - fW.Z()*AtU); // [5][1]
443  // d(v')/d(ax,ay,az)
444  fJ_Mp_7x5[17] = fact * (fV.X()*AtW - fW.X()*AtV); // [3][2]
445  fJ_Mp_7x5[22] = fact * (fV.Y()*AtW - fW.Y()*AtV); // [4][2]
446  fJ_Mp_7x5[27] = fact * (fV.Z()*AtW - fW.Z()*AtV); // [5][2]
447  // d(q/p)/d(q/p)
448  fJ_Mp_7x5[30] = 1.; // [6][0] - not needed for array matrix multiplication
449  //d(u)/d(x,y,z)
450  fJ_Mp_7x5[3] = fU.X(); // [0][3]
451  fJ_Mp_7x5[8] = fU.Y(); // [1][3]
452  fJ_Mp_7x5[13] = fU.Z(); // [2][3]
453  //d(v)/d(x,y,z)
454  fJ_Mp_7x5[4] = fV.X(); // [0][4]
455  fJ_Mp_7x5[9] = fV.Y(); // [1][4]
456  fJ_Mp_7x5[14] = fV.Z(); // [2][4]
457 
458 
459  // since the Jacobian contains a lot of zeros, and the resulting cov has to be symmetric,
460  // the multiplication can be done much faster directly on array level
461  // out5x5 = J_Mp^T * in * J_Mp
462  M5x5& out5x5_ = *((M5x5*) out5x5.GetMatrixArray());
463  RKTools::J_MpTxcov7xJ_Mp(fJ_Mp_7x5, in7x7, out5x5_);
464 
465  if (Jac!=NULL){
466  Jac->ResizeTo(7,5);
467  *Jac = TMatrixD(7,5, &(fJ_Mp_7x5[0]));
468  }
469 }
470 
471 
472 void RKTrackRep::transformM6P(const M6x6& in6x6, TMatrixD& out5x5,
473  const GFDetPlane& pl, const M1x7& state7,
474  TMatrixD* Jac) {
475 
476  out5x5.ResizeTo(5, 5);
477 
478  // get vectors and aux variables
479  fU = pl.getU();
480  fV = pl.getV();
481  fW = pl.getNormal();
482 
483  fDir.SetXYZ(state7[3], state7[4], state7[5]);
484 
485  const double AtU = fDir*fU;
486  const double AtV = fDir*fV;
487  const double AtW = fDir*fW;
488 
489  // J_Mp matrix is d(q/p,u',v',u,v) / d(x,y,z,px,py,pz) (in is 6x6)
490 
491  const double qop = state7[6];
492  const double p = fCharge/qop; // momentum
493 
494  //d(u)/d(x,y,z)
495  fJ_Mp_6x5[3] = fU.X(); // [0][3]
496  fJ_Mp_6x5[8] = fU.Y(); // [1][3]
497  fJ_Mp_6x5[13] = fU.Z(); // [2][3]
498  //d(v)/d(x,y,z)
499  fJ_Mp_6x5[4] = fV.X(); // [0][4]
500  fJ_Mp_6x5[9] = fV.Y(); // [1][4]
501  fJ_Mp_6x5[14] = fV.Z(); // [2][4]
502  // d(q/p)/d(px,py,pz)
503  double fact = (-1.) * qop / p;
504  fJ_Mp_6x5[15] = fact * fDir.X(); // [3][0]
505  fJ_Mp_6x5[20] = fact * fDir.Y(); // [4][0]
506  fJ_Mp_6x5[25] = fact * fDir.Z(); // [5][0]
507  // d(u')/d(px,py,pz)
508  fact = 1./(p*AtW*AtW);
509  fJ_Mp_6x5[16] = fact * (fU.X()*AtW - fW.X()*AtU); // [3][1]
510  fJ_Mp_6x5[21] = fact * (fU.Y()*AtW - fW.Y()*AtU); // [4][1]
511  fJ_Mp_6x5[26] = fact * (fU.Z()*AtW - fW.Z()*AtU); // [5][1]
512  // d(v')/d(px,py,pz)
513  fJ_Mp_6x5[17] = fact * (fV.X()*AtW - fW.X()*AtV); // [3][2]
514  fJ_Mp_6x5[22] = fact * (fV.Y()*AtW - fW.Y()*AtV); // [4][2]
515  fJ_Mp_6x5[27] = fact * (fV.Z()*AtW - fW.Z()*AtV); // [5][2]
516 
517  // do the transformation
518  // out5x5 = J_Mp^T * in * J_Mp
519  M5x5& out5x5_ = *((M5x5*) out5x5.GetMatrixArray());
520  RKTools::J_MpTxcov6xJ_Mp(fJ_Mp_6x5, in6x6, out5x5_);
521 
522  if (Jac!=NULL){
523  Jac->ResizeTo(6,5);
524  *Jac = TMatrixD(6,5, &(fJ_Mp_6x5[0]));;
525  }
526 }
527 
528 
529 
530 TVector3 RKTrackRep::getPos(const GFDetPlane& pl){
531 #ifdef DEBUG
532  std::cout << "RKTrackRep::getPos()\n";
533 #endif
534  M1x7 state7;
535  getState7(state7);
536  if(pl!=fRefPlane) Extrap(pl, state7);
537  return TVector3(state7[0], state7[1], state7[2]);
538 }
539 
540 
541 TVector3 RKTrackRep::getMom(const GFDetPlane& pl){
542 #ifdef DEBUG
543  std::cout << "RKTrackRep::getMom()\n";
544 #endif
545  M1x7 state7;
546  getState7(state7);
547  if(pl!=fRefPlane) Extrap(pl, state7);
548 
549  TVector3 mom(state7[3], state7[4], state7[5]);
550  mom.SetMag(fCharge/state7[6]);
551  return mom;
552 }
553 
554 
555 void RKTrackRep::getPosMom(const GFDetPlane& pl,TVector3& pos, TVector3& mom){
556 #ifdef DEBUG
557  std::cout << "RKTrackRep::getPosMom()\n";
558 #endif
559  M1x7 state7;
560  getState7(state7);
561  if(pl!=fRefPlane) Extrap(pl, state7);
562 
563  pos.SetXYZ(state7[0], state7[1], state7[2]);
564  mom.SetXYZ(state7[3], state7[4], state7[5]);
565  mom.SetMag(fCharge/state7[6]);
566 }
567 
568 
569 void RKTrackRep::getPosMomCov(const GFDetPlane& pl, TVector3& pos, TVector3& mom, TMatrixD& cov6x6){
570  TMatrixD statePred(fState);
571  TMatrixD covPred(fCov);
572  double spu(fSpu);
573 
574  if(pl != fRefPlane) {
575  extrapolate(pl, statePred, covPred);
576  spu = fCacheSpu;
577  }
578 
579  M1x7 state7;
580  getState7(state7, statePred, pl, spu);
581 
582  // cov
583  cov6x6.ResizeTo(6, 6); // make sure cov has correct dimensions
584  M6x6& cov6x6_ = *((M6x6*) cov6x6.GetMatrixArray());
585  transformPM6(covPred, cov6x6_, pl, statePred, spu);
586 
587  pos.SetXYZ(state7[0], state7[1], state7[2]);
588  mom.SetXYZ(state7[3], state7[4], state7[5]);
589  mom.SetMag(fCharge/state7[6]);
590 }
591 
592 
593 void RKTrackRep::setPosMomCov(const TVector3& pos, const TVector3& mom, const TMatrixD& cov6x6){
594 
595  if (cov6x6.GetNcols()!=6 || cov6x6.GetNrows()!=6){
596  GFException exc("RKTrackRep::setPosMomCov ==> cov has to be 6x6 (x, y, z, px, py, pz)",__LINE__,__FILE__);
597  throw exc;
598  }
599 
600  // fCharge does not change!
601  calcState(pos, mom);
602 
603  fCachePlane = fRefPlane;
604  fCacheSpu = 1.;
605 
606  M1x7 state7;
607  getState7(state7);
608 
609  const M6x6& cov6x6_ = *((M6x6*) cov6x6.GetMatrixArray());
610 
611  transformM6P(cov6x6_, fCov, fRefPlane, state7);
612 }
613 
614 
615 
616 void RKTrackRep::extrapolateToPoint(const TVector3& pos,
617  TVector3& poca,
618  TVector3& dirInPoca){
619 
620 #ifdef DEBUG
621  std::cout << "RKTrackRep::extrapolateToPoint()\n";
622 #endif
623 
624  static const unsigned int maxIt(1000);
625 
626  M1x7 state7;
627  getState7(state7);
628  fDir.SetXYZ(state7[3], state7[4], state7[5]);
629 
630  double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0);
631  TVector3 lastDir(0,0,0);
632 
633  GFDetPlane pl;
634  unsigned int iterations(0);
635 
636  while(true){
637  lastStep = step;
638  lastDir = fDir;
639 
640  pl.setON(pos, fDir);
641  step = this->Extrap(pl, state7, NULL, true, maxStep);
642  fDir.SetXYZ(state7[3], state7[4], state7[5]);
643 
644  // check break conditions
645  poca.SetXYZ(state7[0], state7[1], state7[2]);
646  angle = fabs(fDir.Angle((pos-poca))-TMath::PiOver2()); // angle between direction and connection to point - 90 deg
647  distToPoca = (pos-poca).Mag();
648  if (angle*distToPoca < 0.1*MINSTEP) break;
649  if(++iterations == maxIt) {
650  GFException exc("RKTrackRep::extrapolateToPoint ==> extrapolation to point failed, maximum number of iterations reached",__LINE__,__FILE__);
651  throw exc;
652  }
653 
654  // if lastStep and step have opposite sign, the real normal vector lies somewhere between the last two normal vectors (i.e. the directions)
655  // -> try mean value of the two (normalization not needed)
656  if (lastStep*step < 0){
657  fDir += lastDir;
658  maxStep = 0.5*fabs(lastStep); // make it converge!
659  }
660  }
661 
662  dirInPoca.SetXYZ(state7[3], state7[4], state7[5]);
663 
664 #ifdef DEBUG
665  std::cout << "RKTrackRep::extrapolateToPoint(): Reached POCA after " << iterations+1 << " iterations. Distance: " << (pos-poca).Mag() << " cm. Angle deviation: " << dirInPoca.Angle((pos-poca))-TMath::PiOver2() << " rad \n";
666 #endif
667 }
668 
669 
670 TVector3 RKTrackRep::poca2Line(const TVector3& extr1,const TVector3& extr2,const TVector3& point) const {
671 
672  TVector3 theWire = extr2-extr1;
673  if(theWire.Mag()<1.E-8){
674  GFException exc("RKTrackRep::poca2Line ==> try to find POCA between line and point, but the line is really just a point",__LINE__,__FILE__);
675  throw exc;
676  }
677 
678  double t = 1./(theWire*theWire) * (point*theWire + extr1*extr1 - extr1*extr2);
679  return (extr1 + t*theWire);
680 }
681 
682 
683 void RKTrackRep::extrapolateToLine(const TVector3& point1,
684  const TVector3& point2,
685  TVector3& poca,
686  TVector3& dirInPoca,
687  TVector3& poca_onwire){
688 
689 #ifdef DEBUG
690  std::cout << "RKTrackRep::extrapolateToLine(), (x,y) = (" << point1.X() << ", " << point1.Y() << ")\n";
691 #endif
692 
693  static const unsigned int maxIt(1000);
694 
695  M1x7 state7;
696  getState7(state7);
697  fDir.SetXYZ(state7[3], state7[4], state7[5]);
698 
699  double step(0.), lastStep(0.), maxStep(1.E99), angle(0), distToPoca(0);
700  TVector3 lastDir(0,0,0);
701 
702  GFDetPlane pl;
703  unsigned int iterations(0);
704 
705  while(true){
706  lastStep = step;
707  lastDir = fDir;
708 
709  pl.setO(point1);
710  pl.setU(fDir.Cross(point2-point1));
711  pl.setV(point2-point1);
712  step = this->Extrap(pl, state7, NULL, true, maxStep);
713  fDir.SetXYZ(state7[3], state7[4], state7[5]);
714 
715  // check break conditions
716  poca.SetXYZ(state7[0], state7[1], state7[2]);
717  poca_onwire = poca2Line(point1,point2,poca);
718  angle = fabs(fDir.Angle((poca_onwire-poca))-TMath::PiOver2()); // angle between direction and connection to point - 90 deg
719  distToPoca = (poca_onwire-poca).Mag();
720  if (angle*distToPoca < 0.1*MINSTEP) break;
721  if(++iterations == maxIt) {
722  GFException exc("RKTrackRep::extrapolateToLine ==> extrapolation to line failed, maximum number of iterations reached",__LINE__,__FILE__);
723  throw exc;
724  }
725 
726  // if lastStep and step have opposite sign, the real normal vector lies somewhere between the last two normal vectors (i.e. the directions)
727  // -> try mean value of the two (normalization not needed)
728  if (lastStep*step <0){
729  fDir += lastDir;
730  maxStep = 0.5*fabs(lastStep); // make it converge!
731  }
732  }
733 
734  dirInPoca.SetXYZ(state7[3], state7[4], state7[5]);
735 
736 #ifdef DEBUG
737  std::cout << "RKTrackRep::extrapolateToLine(): Reached POCA after " << iterations+1 << " iterations. Distance: " << (poca_onwire-poca).Mag() << " cm. Angle deviation: " << dirInPoca.Angle((poca_onwire-poca))-TMath::PiOver2() << " rad \n";
738 #endif
739 }
740 
741 
742 double RKTrackRep::extrapolate(const GFDetPlane& pl,
743  TMatrixD& statePred,
744  TMatrixD& covPred){
745 
746 #ifdef DEBUG
747  std::cout << "RKTrackRep::extrapolate(pl, statePred, covPred)\n";
748 #endif
749 
750  M1x7 state7;
751  getState7(state7);
752  M7x7 cov7x7;
753 
754  transformPM7(fCov, cov7x7, fRefPlane, fState, fSpu);
755 
756  double coveredDistance = Extrap(pl, state7, &cov7x7);
757 
758  statePred.ResizeTo(5,1);
759  statePred = getState5(state7, pl, fCacheSpu);
760  fCachePlane = pl;
761 
762  covPred.ResizeTo(5,5);
763  transformM7P(cov7x7, covPred, pl, state7);
764 
765  return coveredDistance;
766 }
767 
768 
769 double RKTrackRep::extrapolate(const GFDetPlane& pl,
770  TMatrixD& statePred){
771 
772 #ifdef DEBUG
773  std::cout << "RKTrackRep::extrapolate(pl, statePred)\n";
774 #endif
775 
776  M1x7 state7;
777  getState7(state7);
778  double coveredDistance = Extrap(pl, state7);
779  double spu;
780  statePred.ResizeTo(5,1);
781  statePred = getState5(state7, pl, spu);
782 
783  return coveredDistance;
784 }
785 
786 
787 double RKTrackRep::stepalong(double h, TVector3& pos, TVector3& dir){
788 
789 #ifdef DEBUG
790  std::cout << "RKTrackRep::stepalong()\n";
791 #endif
792 
793  TVector3 dest;
794 
795  static const unsigned int maxIt(30);
796  double coveredDistance(0.);
797 
798  M1x7 state7;
799  getState7(state7);
800 
801  GFDetPlane pl;
802  unsigned int iterations(0);
803 
804  while(true){
805  pos.SetXYZ(state7[0], state7[1], state7[2]);
806  dir.SetXYZ(state7[3], state7[4], state7[5]);
807  dir.SetMag(1.);
808 
809  dest = pos + (h - coveredDistance) * dir;
810 
811  pl.setON(dest, dir);
812  coveredDistance += this->Extrap(pl, state7);
813 
814  if(fabs(h - coveredDistance)<MINSTEP) break;
815  if(++iterations == maxIt) {
816  GFException exc("RKTrackRep::stepalong ==> maximum number of iterations reached",__LINE__,__FILE__);
817  throw exc;
818  }
819  }
820 
821  pos.SetXYZ(state7[0], state7[1], state7[2]);
822  dir.SetXYZ(state7[3], state7[4], state7[5]);
823 
824  return coveredDistance;
825 }
826 
827 
828 
829 double RKTrackRep::Extrap( const GFDetPlane& plane, M1x7& state7, M7x7* cov, bool onlyOneStep, double maxStep) {
830 
831  static const unsigned int maxNumIt(500);
832  unsigned int numIt(0);
833 
834  const bool calcCov(cov!=NULL);
835 
836  // set initial state
837  memcpy(fStateJac, state7, 7*sizeof(double));
838 
839  double coveredDistance(0.);
840  double sumDistance(0.);
841 
842  while(true){
843 
844  #ifdef DEBUG
845  std::cout << "\n============ RKTrackRep::Extrap loop nr. " << numIt << " ============\n";
846  #endif
847 
848  if(numIt++ > maxNumIt){
849  GFException exc("RKTrackRep::Extrap ==> maximum number of iterations exceeded",__LINE__,__FILE__);
850  exc.setFatal();
851  throw exc;
852  }
853 
854  // initialize fStateJac with unit matrix; last entry is not 1 but q/p
855  if(calcCov){
856  memset(&fStateJac[7],0x00,49*sizeof(double));
857  for(int i=0; i<7; ++i){
858  fStateJac[(i+1)*7+i] = 1.;
859  }
860  }
861 
862  fDirectionBefore.SetXYZ(fStateJac[3], fStateJac[4], fStateJac[5]); // direction before propagation
863 
864  // propagation
865  std::vector<GFPointPath> points;
866 
867  bool checkJacProj = true;
868 
869  if( ! this->RKutta(plane, fStateJac, coveredDistance, points, checkJacProj, calcCov, onlyOneStep, maxStep) ) {
870  GFException exc("RKTrackRep::Extrap ==> Runge Kutta propagation failed",__LINE__,__FILE__);
871  exc.setFatal();
872  throw exc;
873  }
874 
875  fPos.SetXYZ(fStateJac[0], fStateJac[1], fStateJac[2]);
876  if (!fNoMaterial) points.push_back(GFPointPath(fPos, 0)); // add last point
877 
878  #ifdef DEBUG
879  std::cout<<"Original points \n";
880  for (unsigned int i=0; i<points.size(); ++i){
881  points[i].Print();
882  }
883  std::cout<<"\n";
884  #endif
885 
886  fDirectionAfter.SetXYZ(fStateJac[3], fStateJac[4], fStateJac[5]); // direction after propagation
887 
888  sumDistance+=coveredDistance;
889 
890  // filter points
891  if (!fNoMaterial) { // points are only filled if mat fx are on
892  if(points.size() > 2){ // check if there are at least three points
893  double firstStep(points[0].getPath());
894  for (unsigned int i=points.size()-2; i>0; --i){
895  if (points[i].getPath() * firstStep < 0 || fabs(points[i].getPath()) < MINSTEP){
896  points[i-1].addToPath(points[i].getPath());
897  points.erase(points.begin()+i);
898  }
899  }
900  }
901  #ifdef DEBUG
902  std::cout<<"Filtered points \n";
903  for (unsigned int i=0; i<points.size(); ++i){
904  points[i].Print();
905  }
906  std::cout<<"\n";
907  #endif
908  }
909 
910 
911  if(calcCov) memset(fNoise,0x00,7*7*sizeof(double)); // set fNoise to 0
912 
913 
914  // call MatFX
915  unsigned int nPoints(points.size());
916  if (!fNoMaterial && nPoints>0){
917  // momLoss has a sign - negative loss means momentum gain
918  double momLoss = GFMaterialEffects::getInstance()->effects(points,
919  fabs(fCharge/fStateJac[6]), // momentum
920  fPdg,
921  fXX0,
922  calcCov,
923  fNoise,
924  &(fStateJac[7]),
925  &fDirectionBefore,
926  &fDirectionAfter);
927 
928  #ifdef DEBUG
929  std::cout << "momLoss: " << momLoss << " GeV; relative: " << momLoss/fabs(fCharge/fStateJac[6]) << "\n";
930  #endif
931 
932  // do momLoss only for defined 1/momentum .ne.0
933  if(fabs(fStateJac[6])>1.E-10) fStateJac[6] = fCharge/(fabs(fCharge/fStateJac[6])-momLoss);
934  } // finished MatFX
935 
936  if(calcCov){ // propagate cov and add noise
937  memcpy(fOldCov, (*cov), 7*7*sizeof(double));
938  M7x7& cov_ = (*cov);
939 
940  // numerical check:
941  for(unsigned int i=0; i<7*7; ++i){
942  if(fabs((*cov)[i]) > 1.E100){
943  GFException exc("RKTrackRep::Extrap ==> covariance matrix exceeds numerical limits",__LINE__,__FILE__);
944  exc.setFatal();
945  throw exc;
946  }
947  }
948 
949  // cov = Jac^T * oldCov * Jac;
950  // last column of jac is [0,0,0,0,0,0,1]
951  // cov is symmetric
952  M7x7& J_MM = *((M7x7*) &(fStateJac[7]));
953  RKTools::J_MMTxcov7xJ_MM(J_MM, fOldCov, cov_);
954 
955  // add noise to cov
956  for (int i=0; i<7*7; ++i) cov_[i] += fNoise[i];
957 
958  } // finished propagate cov and add noise
959 
960  if (onlyOneStep) break;
961 
962  //we arrived at the destination plane, if we point to the active area of the plane (if it is finite), and the distance is below threshold
963  if( plane.distance(fPos) < MINSTEP && plane.inActive(fPos, fDirectionAfter)) {
964  // check if Jacobian has been projected onto plane; Otherwise make another iteration
965  if (calcCov && !checkJacProj && nPoints>0){
966  #ifdef DEBUG
967  std::cout << "Jacobian was not projected onto destination plane -> one more iteration. \n";
968  #endif
969  continue;
970  }
971  #ifdef DEBUG
972  std::cout << "arrived at plane with a distance of " << plane.distance(fPos) << " cm left. \n";
973  #endif
974  break;
975  }
976 
977  }
978 
979  memcpy(state7, fStateJac, 7*sizeof(double));
980 
981  return sumDistance;
982 }
983 
984 
985 //
986 // Runge-Kutta method for tracking a particles through a magnetic field.
987 // Uses Nystroem algorithm (See Handbook Nat. Bur. of Standards, procedure 25.5.20)
988 //
989 // Input parameters:
990 // SU - plane parameters
991 // SU[0] - direction cosines normal to surface Ex
992 // SU[1] - ------- Ey
993 // SU[2] - ------- Ez; Ex*Ex+Ey*Ey+Ez*Ez=1
994 // SU[3] - distance to surface from (0,0,0) > 0 cm
995 //
996 // ND - number of variables for derivatives calculation
997 // P - initial parameters (coordinates(cm), direction,
998 // charge/momentum (Gev-1) and derivatives this parameters (8x7)
999 //
1000 // X Y Z Ax Ay Az q/P
1001 // P[ 0] P[ 1] P[ 2] P[ 3] P[ 4] P[ 5] P[ 6]
1002 //
1003 // dX/dp dY/dp dZ/dp dAx/dp dAy/dp dAz/dp d(q/P)/dp
1004 // P[ 7] P[ 8] P[ 9] P[10] P[11] P[12] P[13] d()/dp1
1005 //
1006 // P[14] P[15] P[16] P[17] P[18] P[19] P[20] d()/dp2
1007 // ............................................................................ d()/dpND
1008 //
1009 // Authors: R.Brun, M.Hansroul, V.Perevoztchikov (Geant3)
1010 //
1011 bool RKTrackRep::RKutta (const GFDetPlane& plane,
1012  M8x7& P,
1013  double& coveredDistance,
1014  std::vector<GFPointPath>& points,
1015  bool& checkJacProj,
1016  bool calcCov,
1017  bool onlyOneStep,
1018  double maxStep) {
1019 
1020  // important fixed numbers
1021  static const double EC = 0.000149896229; // c/(2*10^12) resp. c/2Tera
1022  static const double P3 = 1./3.; // 1/3
1023  static const int ND = 56; // number of variables for derivatives calculation
1024  static const int ND1 = ND-7; // = 49
1025  // limits, check-values, etc. Can be tuned!
1026  static const double Wmax = 3000.; // max. way allowed [cm]
1027  static const double AngleMax = 6.3; // max. total angle change of momentum. Prevents extrapolating a curler round and round if no active plane is found.
1028  static const double Pmin = 4.E-3; // minimum momentum for propagation [GeV]
1029  static const unsigned int maxNumIt = 1000; // maximum number of iterations in main loop
1030  // Aux parameters
1031  M1x3& R = *((M1x3*) &P[0]); // Start coordinates [cm] (x, y, z)
1032  M1x3& A = *((M1x3*) &P[3]); // Start directions (ax, ay, az); ax^2+ay^2+az^2=1
1033  M1x3 SA = {0.,0.,0.}; // Start directions derivatives dA/S
1034  double Pinv = P[6]*EC; // P[6] is charge/momentum in e/(GeV/c)
1035  double Way = 0.; // Sum of absolute values of all extrapolation steps [cm]
1036  bool atPlane = false; // stepper thinks that the plane will be reached in that step -> linear extrapolation and projection of jacobian
1037  bool momLossExceeded = false; // stepper had to limit stepsize due to momentum loss -> no next RKutta loop, no linear extrapolation
1038  fPos.SetXYZ(R[0],R[1],R[2]); // position
1039  fDir.SetXYZ(A[0],A[1],A[2]); // direction
1040  double momentum = fabs(fCharge/P[6]); // momentum [GeV]
1041  double relMomLoss = 0; // relative momentum loss in RKutta
1042  double deltaAngle = 0.; // total angle by which the momentum has changed during extrapolation
1043  double An(0), S(0), Sl(0), S3(0), S4(0), PS2(0), CBA(0);
1044  // Variables for RKutta main loop
1045  M1x4 SU = {0.,0.,0.,0.};
1046  M1x3 H0 = {0.,0.,0.}, H1 = {0.,0.,0.}, H2 = {0.,0.,0.}, r = {0.,0.,0.};
1047  double A0(0), A1(0), A2(0), A3(0), A4(0), A5(0), A6(0);
1048  double B0(0), B1(0), B2(0), B3(0), B4(0), B5(0), B6(0);
1049  double C0(0), C1(0), C2(0), C3(0), C4(0), C5(0), C6(0);
1050  double dA0(0), dA2(0), dA3(0), dA4(0), dA5(0), dA6(0);
1051  double dB0(0), dB2(0), dB3(0), dB4(0), dB5(0), dB6(0);
1052  double dC0(0), dC2(0), dC3(0), dC4(0), dC5(0), dC6(0);
1053 
1054  #ifdef DEBUG
1055  std::cout << "RKTrackRep::RKutta \n";
1056  std::cout << "position: "; fPos.Print();
1057  std::cout << "direction: "; fDir.Print();
1058  std::cout << "momentum: " << momentum << " GeV\n";
1059  std::cout << "destination: "; plane.Print();
1060  #endif
1061 
1062  checkJacProj = false;
1063 
1064  // check momentum
1065  if(momentum < Pmin){
1066  std::ostringstream sstream;
1067  sstream << "RKTrackRep::RKutta ==> momentum too low: " << fabs(fCharge/P[6])*1000. << " MeV";
1068  GFException exc(sstream.str(),__LINE__,__FILE__);
1069  exc.setFatal();
1070  throw exc;
1071  }
1072 
1073 
1074  // make SU vector point away from origin
1075  const TVector3 W = plane.getNormal();
1076  if(W*plane.getO() > 0){SU[0] = W.X(); SU[1] = W.Y(); SU[2] = W.Z();}
1077  else {SU[0] = -1.*W.X(); SU[1] = -1.*W.Y(); SU[2] = -1.*W.Z(); }
1078  SU[3] = plane.distance(0., 0., 0.);
1079 
1080  unsigned int counter(0);
1081 
1082  // Step estimation (signed)
1083  fH = GFFieldManager::getFieldVal(fPos);
1084  S = estimateStep(points, fPos, fDir, SU, fH, plane, momentum, relMomLoss, deltaAngle, momLossExceeded, atPlane, maxStep);
1085  if (fabs(S) < 0.001*MINSTEP) {
1086  #ifdef DEBUG
1087  std::cout << " RKutta - step too small -> break \n";
1088  #endif
1089  counter += 1; // skip the main loop, go to linear extrapolation step (will be skipped) and just project jacobian
1090  }
1091 
1092  //
1093  // Main loop of Runge-Kutta method
1094  //
1095  while (fabs(S) >= MINSTEP || counter == 0) {
1096 
1097  if(++counter > maxNumIt){
1098  GFException exc("RKTrackRep::RKutta ==> maximum number of iterations exceeded",__LINE__,__FILE__);
1099  exc.setFatal();
1100  throw exc;
1101  }
1102 
1103  #ifdef DEBUG
1104  std::cout << "------ RKutta main loop nr. " << counter-1 << " ------\n";
1105  #endif
1106 
1107  //
1108  // Runge Kutta Extrapolation
1109  //
1110  S3 = P3*S;
1111  S4 = 0.25*S;
1112  PS2 = Pinv*S;
1113 
1114  // First point
1115  r[0] = R[0]; r[1] = R[1]; r[2]=R[2];
1116  fPos.SetXYZ(r[0], r[1], r[2]); // vector of start coordinates R0 (x, y, z)
1117  fH = GFFieldManager::getFieldVal(fPos); // magnetic field in 10^-4 T = kGauss
1118  H0[0] = PS2*fH.X(); H0[1] = PS2*fH.Y(); H0[2] = PS2*fH.Z(); // H0 is PS2*(Hx, Hy, Hz) @ R0
1119  A0 = A[1]*H0[2]-A[2]*H0[1]; B0 = A[2]*H0[0]-A[0]*H0[2]; C0 = A[0]*H0[1]-A[1]*H0[0]; // (ax, ay, az) x H0
1120  A2 = A[0]+A0 ; B2 = A[1]+B0 ; C2 = A[2]+C0 ; // (A0, B0, C0) + (ax, ay, az)
1121  A1 = A2+A[0] ; B1 = B2+A[1] ; C1 = C2+A[2] ; // (A0, B0, C0) + 2*(ax, ay, az)
1122 
1123  // Second point
1124  r[0] += A1*S4; r[1] += B1*S4; r[2] += C1*S4;
1125  fPos.SetXYZ(r[0], r[1], r[2]);
1126  fH = GFFieldManager::getFieldVal(fPos);
1127  H1[0] = fH.X()*PS2; H1[1] = fH.Y()*PS2; H1[2] = fH.Z()*PS2; // H1 is PS2*(Hx, Hy, Hz) @ (x, y, z) + 0.25*S * [(A0, B0, C0) + 2*(ax, ay, az)]
1128  A3 = B2*H1[2]-C2*H1[1]+A[0]; B3 = C2*H1[0]-A2*H1[2]+A[1]; C3 = A2*H1[1]-B2*H1[0]+A[2]; // (A2, B2, C2) x H1 + (ax, ay, az)
1129  A4 = B3*H1[2]-C3*H1[1]+A[0]; B4 = C3*H1[0]-A3*H1[2]+A[1]; C4 = A3*H1[1]-B3*H1[0]+A[2]; // (A3, B3, C3) x H1 + (ax, ay, az)
1130  A5 = A4-A[0]+A4 ; B5 = B4-A[1]+B4 ; C5 = C4-A[2]+C4 ; // 2*(A4, B4, C4) - (ax, ay, az)
1131 
1132  // Last point
1133  r[0]=R[0]+S*A4; r[1]=R[1]+S*B4; r[2]=R[2]+S*C4; //setup.Field(r,H2);
1134  fPos.SetXYZ(r[0], r[1], r[2]);
1135  fH = GFFieldManager::getFieldVal(fPos);
1136  H2[0] = fH.X()*PS2; H2[1] = fH.Y()*PS2; H2[2] = fH.Z()*PS2; // H2 is PS2*(Hx, Hy, Hz) @ (x, y, z) + 0.25*S * (A4, B4, C4)
1137  A6 = B5*H2[2]-C5*H2[1]; B6 = C5*H2[0]-A5*H2[2]; C6 = A5*H2[1]-B5*H2[0]; // (A5, B5, C5) x H2
1138 
1139  #ifdef DEBUG
1140  std::cout << "Mag field: "; fH.Print();
1141  #endif
1142 
1143  // update paths
1144  coveredDistance += S; // add stepsize to way (signed)
1145  Way += fabs(S);
1146 
1147  // check way limit
1148  if(Way > Wmax){
1149  std::ostringstream sstream;
1150  sstream << "RKTrackRep::RKutta ==> Total extrapolation length is longer than length limit : " << Way << " cm !";
1151  GFException exc(sstream.str(),__LINE__,__FILE__);
1152  exc.setFatal();
1153  throw exc;
1154  }
1155 
1156  //
1157  // Derivatives of track parameters in last point
1158  //
1159  if(calcCov){
1160  // d(x, y, z)/d(x, y, z) submatrix is unit matrix
1161  P[7] = 1; P[15] = 1; P[23] = 1;
1162  // d(ax, ay, az)/d(ax, ay, az) submatrix is 0
1163  // start with d(x, y, z)/d(ax, ay, az)
1164  for(int i=4*7; i<ND; i+=7) { // i = 28, 35, 42, 49; ND = 56; ND1 = 49; rows of Jacobian
1165 
1166  M1x3& dR = *((M1x3*) &P[i]); // dR = (dX/dpN, dY/dpN, dZ/dpN)
1167  M1x3& dA = *((M1x3*) &P[i+3]); // dA = (dAx/dpN, dAy/dpN, dAz/dpN); N = X,Y,Z,Ax,Ay,Az,q/p
1168 
1169  if(i==ND1) {dA[0]*=P[6]; dA[1]*=P[6]; dA[2]*=P[6];}
1170 
1171  //first point
1172  dA0 = H0[2]*dA[1]-H0[1]*dA[2]; // dA0/dp }
1173  dB0 = H0[0]*dA[2]-H0[2]*dA[0]; // dB0/dp } = dA x H0
1174  dC0 = H0[1]*dA[0]-H0[0]*dA[1]; // dC0/dp }
1175 
1176  if(i==ND1) {dA0+=A0; dB0+=B0; dC0+=C0;} // if last row: (dA0, dB0, dC0) := (dA0, dB0, dC0) + (A0, B0, C0)
1177 
1178  dA2 = dA0+dA[0]; // }
1179  dB2 = dB0+dA[1]; // } = (dA0, dB0, dC0) + dA
1180  dC2 = dC0+dA[2]; // }
1181 
1182  //second point
1183  dA3 = dA[0]+dB2*H1[2]-dC2*H1[1]; // dA3/dp }
1184  dB3 = dA[1]+dC2*H1[0]-dA2*H1[2]; // dB3/dp } = dA + (dA2, dB2, dC2) x H1
1185  dC3 = dA[2]+dA2*H1[1]-dB2*H1[0]; // dC3/dp }
1186 
1187  if(i==ND1) {dA3+=A3-A[0]; dB3+=B3-A[1]; dC3+=C3-A[2];} // if last row: (dA3, dB3, dC3) := (dA3, dB3, dC3) + (A3, B3, C3) - (ax, ay, az)
1188 
1189  dA4 = dA[0]+dB3*H1[2]-dC3*H1[1]; // dA4/dp }
1190  dB4 = dA[1]+dC3*H1[0]-dA3*H1[2]; // dB4/dp } = dA + (dA3, dB3, dC3) x H1
1191  dC4 = dA[2]+dA3*H1[1]-dB3*H1[0]; // dC4/dp }
1192 
1193  if(i==ND1) {dA4+=A4-A[0]; dB4+=B4-A[1]; dC4+=C4-A[2];} // if last row: (dA4, dB4, dC4) := (dA4, dB4, dC4) + (A4, B4, C4) - (ax, ay, az)
1194 
1195  //last point
1196  dA5 = dA4+dA4-dA[0]; // }
1197  dB5 = dB4+dB4-dA[1]; // } = 2*(dA4, dB4, dC4) - dA
1198  dC5 = dC4+dC4-dA[2]; // }
1199 
1200  dA6 = dB5*H2[2]-dC5*H2[1]; // dA6/dp }
1201  dB6 = dC5*H2[0]-dA5*H2[2]; // dB6/dp } = (dA5, dB5, dC5) x H2
1202  dC6 = dA5*H2[1]-dB5*H2[0]; // dC6/dp }
1203 
1204  if(i==ND1) {dA6+=A6; dB6+=B6; dC6+=C6;} // if last row: (dA6, dB6, dC6) := (dA6, dB6, dC6) + (A6, B6, C6)
1205 
1206  if(i==ND1) {
1207  dR[0] += (dA2+dA3+dA4)*S3/P[6]; dA[0] = (dA0+dA3+dA3+dA5+dA6)*P3/P[6]; // dR := dR + S3*[(dA2, dB2, dC2) + (dA3, dB3, dC3) + (dA4, dB4, dC4)]
1208  dR[1] += (dB2+dB3+dB4)*S3/P[6]; dA[1] = (dB0+dB3+dB3+dB5+dB6)*P3/P[6]; // dA := 1/3*[(dA0, dB0, dC0) + 2*(dA3, dB3, dC3) + (dA5, dB5, dC5) + (dA6, dB6, dC6)]
1209  dR[2] += (dC2+dC3+dC4)*S3/P[6]; dA[2] = (dC0+dC3+dC3+dC5+dC6)*P3/P[6];
1210  }
1211  else {
1212  dR[0] += (dA2+dA3+dA4)*S3; dA[0] = (dA0+dA3+dA3+dA5+dA6)*P3; // dR := dR + S3*[(dA2, dB2, dC2) + (dA3, dB3, dC3) + (dA4, dB4, dC4)]
1213  dR[1] += (dB2+dB3+dB4)*S3; dA[1] = (dB0+dB3+dB3+dB5+dB6)*P3; // dA := 1/3*[(dA0, dB0, dC0) + 2*(dA3, dB3, dC3) + (dA5, dB5, dC5) + (dA6, dB6, dC6)]
1214  dR[2] += (dC2+dC3+dC4)*S3; dA[2] = (dC0+dC3+dC3+dC5+dC6)*P3;
1215  }
1216  }
1217  }
1218 
1219  //
1220  // Track parameters in last point
1221  //
1222  R[0] += (A2+A3+A4)*S3; A[0] += (SA[0]=(A0+A3+A3+A5+A6)*P3-A[0]); // R = R0 + S3*[(A2, B2, C2) + (A3, B3, C3) + (A4, B4, C4)]
1223  R[1] += (B2+B3+B4)*S3; A[1] += (SA[1]=(B0+B3+B3+B5+B6)*P3-A[1]); // A = 1/3*[(A0, B0, C0) + 2*(A3, B3, C3) + (A5, B5, C5) + (A6, B6, C6)]
1224  R[2] += (C2+C3+C4)*S3; A[2] += (SA[2]=(C0+C3+C3+C5+C6)*P3-A[2]); // SA = A_new - A_old
1225  fPos.SetXYZ(R[0], R[1], R[2]);
1226 
1227  // normalize A
1228  CBA = 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]); // 1/|A|
1229  A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1230  fDir.SetXYZ(A[0], A[1], A[2]);
1231 
1232  if (onlyOneStep) return(true);
1233 
1234  // if stepsize has been limited by material, break the loop and return. No linear extrapolation!
1235  if (momLossExceeded) {
1236  #ifdef DEBUG
1237  std::cout<<" momLossExceeded -> return(true); \n";
1238  #endif
1239  return(true);
1240  }
1241 
1242  // estimate Step for next loop or linear extrapolation
1243  Sl = S; // last S used
1244  S = estimateStep(points, fPos, fDir, SU, fH, plane, momentum, relMomLoss, deltaAngle, momLossExceeded, atPlane, maxStep);
1245 
1246  if (atPlane && fabs(S) < MINSTEP) {
1247  #ifdef DEBUG
1248  std::cout<<" (atPlane && fabs(S) < MINSTEP) -> break; \n";
1249  #endif
1250  break; // else if at plane: do linear extrapolation
1251  }
1252  if (momLossExceeded && fabs(S) < MINSTEP) {
1253  #ifdef DEBUG
1254  std::cout<<" (momLossExceeded && fabs(S) < MINSTEP) -> return(true); \n";
1255  #endif
1256  return(true); // no linear extrapolation!
1257  }
1258 
1259  // check if total angle is bigger than AngleMax. Can happen if a curler should be fitted and it does not hit the active area of the next plane.
1260  if (fabs(deltaAngle) > AngleMax){
1261  std::ostringstream sstream;
1262  sstream << "RKTrackRep::RKutta ==> Do not get to an active plane! Already extrapolated " << deltaAngle * 180 / TMath::Pi() << "°.";
1263  GFException exc(sstream.str(),__LINE__,__FILE__);
1264  exc.setFatal();
1265  throw exc;
1266  }
1267 
1268  // check if we went back and forth multiple times -> we don't come closer to the plane!
1269  if (counter > 3){
1270  if (S *points[counter-1].getPath() < 0 &&
1271  points[counter-1].getPath()*points[counter-2].getPath() < 0 &&
1272  points[counter-2].getPath()*points[counter-3].getPath() < 0){
1273  GFException exc("RKTrackRep::RKutta ==> Do not get closer to plane!",__LINE__,__FILE__);
1274  exc.setFatal();
1275  throw exc;
1276  }
1277  }
1278 
1279  } //end of main loop
1280 
1281 
1282  //
1283  // linear extrapolation to surface
1284  //
1285  if (atPlane) {
1286 
1287  if (fabs(Sl) > 0.001*MINSTEP){
1288  #ifdef DEBUG
1289  std::cout << " RKutta - linear extrapolation to surface\n";
1290  #endif
1291  Sl = 1./Sl; // Sl = inverted last Stepsize Sl
1292 
1293  // normalize SA
1294  SA[0]*=Sl; SA[1]*=Sl; SA[2]*=Sl; // SA/Sl = delta A / delta way; local derivative of A with respect to the length of the way
1295 
1296  // calculate A
1297  A[0] += SA[0]*S; // S = distance to surface
1298  A[1] += SA[1]*S; // A = A + S * SA*Sl
1299  A[2] += SA[2]*S;
1300 
1301  // normalize A
1302  CBA = 1./sqrt(A[0]*A[0]+A[1]*A[1]+A[2]*A[2]); // 1/|A|
1303  A[0] *= CBA; A[1] *= CBA; A[2] *= CBA;
1304 
1305  R[0] += S*(A[0]-0.5*S*SA[0]); // P = R + S*(A - 0.5*S*SA); approximation for final point on surface
1306  R[1] += S*(A[1]-0.5*S*SA[1]);
1307  R[2] += S*(A[2]-0.5*S*SA[2]);
1308  }
1309 #ifdef DEBUG
1310  else {
1311  std::cout << " RKutta - last stepsize too small -> can't do linear extrapolation! \n";
1312  }
1313 #endif
1314 
1315  //
1316  // Project Jacobian of extrapolation onto destination plane
1317  //
1318  if(calcCov){
1319  if (checkJacProj && points.size()>0){
1320  GFException exc("RKTrackRep::Extrap ==> covariance is projected onto destination plane again",__LINE__,__FILE__);
1321  throw exc;
1322  }
1323  checkJacProj = true;
1324  #ifdef DEBUG
1325  std::cout << " Project Jacobian of extrapolation onto destination plane\n";
1326  #endif
1327  An = A[0]*SU[0] + A[1]*SU[1] + A[2]*SU[2];
1328  fabs(An) > 1.E-7 ? An=1./An : An = 0; // 1/A_normal
1329  double norm;
1330  for(int i=7; i<ND; i+=7) {
1331  M1x3& dR = *((M1x3*) &P[i]);
1332  M1x3& dA = *((M1x3*) &P[i+3]);
1333  norm = (dR[0]*SU[0] + dR[1]*SU[1] + dR[2]*SU[2])*An; // dR_normal / A_normal
1334  dR[0] -= norm*A [0]; dR[1] -= norm*A [1]; dR[2] -= norm*A [2];
1335  dA[0] -= norm*SA[0]; dA[1] -= norm*SA[1]; dA[2] -= norm*SA[2];
1336  }
1337  }
1338 
1339  coveredDistance += S;
1340  Way += fabs(S);
1341  } // end of linear extrapolation to surface
1342 
1343  return(true);
1344 }
1345 
1346 
1347 double RKTrackRep::estimateStep(std::vector<GFPointPath>& points,
1348  const TVector3& pos,
1349  const TVector3& dir,
1350  const M1x4& SU,
1351  const TVector3& MagField,
1352  const GFDetPlane& plane,
1353  const double& momentum,
1354  double& relMomLoss,
1355  double& deltaAngle,
1356  bool& momLossExceeded,
1357  bool& atPlane,
1358  double maxStep) const {
1359 
1360  static const double Smax = 10.; // max. step allowed [cm]
1361  static const double dAngleMax = 0.05; // max. deviation of angle between direction before and after the step [rad]
1362  double Step;
1363  bool improveEstimation (true);
1364 
1365  momLossExceeded = false;
1366  atPlane = false;
1367 
1368  #ifdef DEBUG
1369  std::cout << " RKTrackRep::estimateStep \n";
1370  std::cout << " position: "; pos.Print();
1371  std::cout << " direction: "; dir.Print();
1372  #endif
1373 
1374 
1375  // calculate distance to surface
1376  double Dist = SU[3] - (pos.X()*SU[0] + pos.Y()*SU[1] + pos.Z()*SU[2]); // Distance between start coordinates and surface
1377  double An = dir.X()*SU[0] + dir.Y()*SU[1] + dir.Z()*SU[2]; // An = dir * N; component of dir normal to surface
1378 
1379  if (fabs(An) > 1.E-10) Step = Dist/An;
1380  else {
1381  Step = Dist*1.E10;
1382  if (An<0) Step *= -1.;
1383  }
1384 
1385  // see if dir points towards surface (1) or not (-1)
1386  double StepSign(1);
1387  if (Step<0) StepSign = -1;
1388 
1389  #ifdef DEBUG
1390  std::cout << " Distance to plane: " << Dist << "\n";
1391  std::cout << " guess for Step: " << Step << "\n";
1392  if (StepSign>0) std::cout << " Direction is pointing towards surface.\n";
1393  else std::cout << " Direction is pointing away from surface.\n";
1394  #endif
1395 
1396  // calculate way SmaxAngle after which momentum angle has changed AngleMax
1397  double Hmag(MagField.Mag()), SmaxAngle(Smax), radius(0), p_perp(0);
1398  if (Hmag > 1E-5){
1399  p_perp = ( dir - MagField*((dir*MagField)/(Hmag*Hmag)) ).Mag() * momentum; // [GeV]
1400  radius = p_perp/(0.3E-3*Hmag); // [cm]
1401  double sinAngle = fabs(sin(dir.Angle(MagField)));
1402  if (sinAngle > 1E-10) SmaxAngle = fabs(dAngleMax * radius / sinAngle); // [cm]
1403  }
1404 
1405 
1406  //
1407  // Select direction
1408  //
1409  // auto select
1410  if (fDirection == 0){
1411  #ifdef DEBUG
1412  std::cout << " auto select direction. \n";
1413  #endif
1414  }
1415  // see if straight line approximation is ok
1416  else if ( fabs(Step) < 0.2*SmaxAngle ){
1417  #ifdef DEBUG
1418  std::cout << " straight line approximation is fine. Delta angle until surface is reached is approx " << Step/SmaxAngle * dAngleMax * 180 / TMath::Pi() << " deg \n";
1419  #endif
1420 
1421  // if direction is pointing to active part of surface
1422  if( plane.inActive(pos, dir) ) {
1423  #ifdef DEBUG
1424  std::cout << " direction is pointing to active part of surface. \n";
1425  #endif
1426  }
1427  // if we are near the plane, but not pointing to the active area, make a big step!
1428  else {
1429  Step = fDirection*SmaxAngle;
1430  improveEstimation = false;
1431  #ifdef DEBUG
1432  std::cout << " we are near the plane, but not pointing to the active area. make a big step! \n";
1433  #endif
1434  }
1435  }
1436  // fDirection decides!
1437  else {
1438  Step = fabs(Step)*fDirection;
1439  improveEstimation = false;
1440  #ifdef DEBUG
1441  std::cout << " select direction according to fDirection. \n";
1442  #endif
1443  }
1444 
1445  #ifdef DEBUG
1446  std::cout << " guess for Step (signed): " << Step << "\n";
1447  #endif
1448 
1449  // re-check sign of Step
1450  if (Step>=0) StepSign = 1;
1451  else StepSign = -1;
1452 
1453  //
1454  // Limit stepsize
1455  //
1456  // reduce maximum stepsize Step to Smax
1457  if (fabs(Step) > Smax) {
1458  Step = StepSign*Smax;
1459  improveEstimation = false;
1460  }
1461  if (fabs(Step) > maxStep) {
1462  Step = StepSign*maxStep;
1463  improveEstimation = false;
1464  }
1465 
1466  // also limit stepsize according to the change of the momentum direction!
1467  if (fabs(Step) > SmaxAngle) {
1468  Step = StepSign*SmaxAngle;
1469  improveEstimation = false;
1470  }
1471 
1472  #ifdef DEBUG
1473  std::cout << " limit from maxangle: " << SmaxAngle << ", radius: " << radius << "\n";
1474  #endif
1475 
1476 
1477  // call stepper and reduce stepsize if step not too small
1478  if (!fNoMaterial){
1479 
1480  if(fabs(Step) > MINSTEP){ // only call stepper if step estimation big enough
1481  double StepMat = GFMaterialEffects::getInstance()->stepper(fabs(Step),
1482  SmaxAngle,
1483  pos.X(), pos.Y(), pos.Z(),
1484  StepSign*dir.X(), StepSign*dir.Y(), StepSign*dir.Z(),
1485  momentum,
1486  relMomLoss,
1487  fPdg);
1488  if (fabs(Step) > StepMat) {
1489  Step = StepSign*StepMat;
1490  momLossExceeded = true;
1491  }
1492 
1493  #ifdef DEBUG
1494  std::cout << " limit from stepper: " << Step << "\n";
1495  #endif
1496  }
1497  }
1498 
1499 
1500  if (!momLossExceeded && improveEstimation){
1501  atPlane = true;
1502 
1503  // improve step estimation to surface according to curvature
1504  if (Hmag > 1E-5 && fabs(Step) > 0.1*SmaxAngle){
1505  //
1506  // simplified Runge Kutta Extrapolation
1507  //
1508  double S3 = Step/3.;
1509  double PS2 = fCharge/momentum*0.000149896229 * Step;
1510  M1x3 H0 = {0.,0.,0.};
1511  double A0(0), A2(0), A3(0), A4(0), A5(0), A6(0);
1512  double B0(0), B2(0), B3(0), B4(0), B5(0), B6(0);
1513  double C0(0), C2(0), C3(0), C4(0), C5(0), C6(0);
1514 
1515  // First point
1516  H0[0] = PS2*MagField.X(); H0[1] = PS2*MagField.Y(); H0[2] = PS2*MagField.Z(); // H0 is PS2*(Hx, Hy, Hz) @ R0
1517  A0 = dir.Y()*H0[2]-dir.Z()*H0[1]; B0 = dir.Z()*H0[0]-dir.X()*H0[2]; C0 = dir.X()*H0[1]-dir.Y()*H0[0]; // (ax, ay, az) x H0
1518  A2 = dir.X()+A0 ; B2 = dir.Y()+B0 ; C2 = dir.Z()+C0 ; // (A0, B0, C0) + (ax, ay, az)
1519 
1520  // Second point
1521  A3 = B2*H0[2]-C2*H0[1]+dir.X(); B3 = C2*H0[0]-A2*H0[2]+dir.Y(); C3 = A2*H0[1]-B2*H0[0]+dir.Z(); // (A2, B2, C2) x H0 + (ax, ay, az)
1522  A4 = B3*H0[2]-C3*H0[1]+dir.X(); B4 = C3*H0[0]-A3*H0[2]+dir.Y(); C4 = A3*H0[1]-B3*H0[0]+dir.Z(); // (A3, B3, C3) x H0 + (ax, ay, az)
1523  A5 = A4-dir.X()+A4 ; B5 = B4-dir.Y()+B4 ; C5 = C4-dir.Z()+C4 ; // 2*(A4, B4, C4) - (ax, ay, az)
1524 
1525  // Last point
1526  A6 = B5*H0[2]-C5*H0[1]; B6 = C5*H0[0]-A5*H0[2]; C6 = A5*H0[1]-B5*H0[0]; // (A5, B5, C5) x H0
1527 
1528  // calculate distance to surface
1529  Dist = SU[3] - ((pos.X()+(A2+A3+A4)*S3) * SU[0] +
1530  (pos.Y()+(B2+B3+B4)*S3) * SU[1] +
1531  (pos.Z()+(C2+C3+C4)*S3) * SU[2]); // Distance between start coordinates and surface
1532 
1533  An = (A0+A3+A3+A5+A6)/3. * SU[0] +
1534  (B0+B3+B3+B5+B6)/3. * SU[1] +
1535  (C0+C3+C3+C5+C6)/3. * SU[2]; // An = dir * N; component of dir normal to surface
1536 
1537  Step += Dist/An;
1538 
1539  #ifdef DEBUG
1540  std::cout << " Improved step estimation taking curvature into account: " << Step << "\n";
1541  #endif
1542  }
1543 
1544  }
1545 
1546  deltaAngle += Step/SmaxAngle * dAngleMax;
1547  points.push_back(GFPointPath(pos, Step));
1548 
1549  #ifdef DEBUG
1550  std::cout << " --> Step to be used: " << Step << "\n";
1551  #endif
1552 
1553  return Step;
1554 }
1555 
1556 
1557 void RKTrackRep::setPropDir(int dir){
1558  // make sure fDirection is -1, 0 or 1
1559  if (dir>0) fDirection = 1;
1560  else if (dir<0) fDirection = -1;
1561  else fDirection = 0;
1562 }
1563 
1564 
1565 
1566 ClassImp(RKTrackRep)