Bug Summary

File:libraries/TRACKING/DReferenceTrajectory.cc
Location:line 2020, column 30
Description:Dereference of null pointer (loaded from variable 's')

Annotated Source Code

1// $Id$
2//
3// File: DReferenceTrajectory.cc
4// Created: Wed Jul 19 13:42:58 EDT 2006
5// Creator: davidl (on Darwin swire-b241.jlab.org 8.7.0 powerpc)
6//
7
8#include <signal.h>
9#include <memory>
10#include <cmath>
11
12#include <DVector3.h>
13using namespace std;
14#include <math.h>
15#include <algorithm>
16
17#include "DReferenceTrajectory.h"
18#include "DTrackCandidate.h"
19#include "DMagneticFieldStepper.h"
20#include "HDGEOMETRY/DRootGeom.h"
21#define ONE_THIRD0.33333333333333333 0.33333333333333333
22#define TWO_THIRD0.66666666666666667 0.66666666666666667
23#define EPS1e-8 1e-8
24#define NaNstd::numeric_limits<double>::quiet_NaN() std::numeric_limits<double>::quiet_NaN()
25
26struct StepStruct {DReferenceTrajectory::swim_step_t steps[256];};
27
28//---------------------------------
29// DReferenceTrajectory (Constructor)
30//---------------------------------
31DReferenceTrajectory::DReferenceTrajectory(const DMagneticFieldMap *bfield
32 , double q
33 , swim_step_t *swim_steps
34 , int max_swim_steps
35 , double step_size)
36{
37 // Copy some values into data members
38 this->q = q;
39 this->step_size = step_size;
40 this->bfield = bfield;
41 this->Nswim_steps = 0;
42 this->dist_to_rt_depth = 0;
43 this->mass = 0.13957; // assume pion mass until otherwise specified
44 this->mass_sq=this->mass*this->mass;
45 this->hit_cdc_endplate = false;
46 this->RootGeom=NULL__null;
47 this->geom = NULL__null;
48 this->ploss_direction = kForward;
49 this->check_material_boundaries = true;
50 this->zmin_track_boundary = -100.0; // boundary at which to stop swimming
51 this->zmax_track_boundary = 670.0; // boundary at which to stop swimming
52 this->Rmax_interior = 65.0; // Maximum radius (in cm) corresponding to inside of BCAL
53 this->Rmax_exterior = 88.0; // Maximum radius (in cm) corresponding to outside of BCAL
54
55 this->last_phi = 0.0;
56 this->last_swim_step = NULL__null;
57 this->last_dist_along_wire = 0.0;
58 this->last_dz_dphi = 0.0;
59
60 this->debug_level = 0;
61
62 // Initialize some values from configuration parameters
63 BOUNDARY_STEP_FRACTION = 0.80;
64 MIN_STEP_SIZE = 0.1; // cm
65 MAX_STEP_SIZE = 3.0; // cm
66 int MAX_SWIM_STEPS = 2500;
67
68 gPARMS->SetDefaultParameter("TRK:BOUNDARY_STEP_FRACTION" , BOUNDARY_STEP_FRACTION, "Fraction of estimated distance to boundary to use as step size");
69 gPARMS->SetDefaultParameter("TRK:MIN_STEP_SIZE" , MIN_STEP_SIZE, "Minimum step size in cm to take when swimming a track with adaptive step sizes");
70 gPARMS->SetDefaultParameter("TRK:MAX_STEP_SIZE" , MAX_STEP_SIZE, "Maximum step size in cm to take when swimming a track with adaptive step sizes");
71 gPARMS->SetDefaultParameter("TRK:MAX_SWIM_STEPS" , MAX_SWIM_STEPS, "Number of swim steps for DReferenceTrajectory to allocate memory for (when not using external buffer)");
72
73 // It turns out that the greatest bottleneck in speed here comes from
74 // allocating/deallocating the large block of memory required to hold
75 // all of the trajectory info. The preferred way of calling this is
76 // with a pointer allocated once at program startup. This code block
77 // though allows it to be allocated here if necessary.
78 if(!swim_steps){
79 own_swim_steps = true;
80 this->max_swim_steps = MAX_SWIM_STEPS;
81 this->swim_steps = new swim_step_t[this->max_swim_steps];
82 }else{
83 own_swim_steps = false;
84 this->max_swim_steps = max_swim_steps;
85 this->swim_steps = swim_steps;
86 }
87}
88
89//---------------------------------
90// DReferenceTrajectory (Copy Constructor)
91//---------------------------------
92DReferenceTrajectory::DReferenceTrajectory(const DReferenceTrajectory& rt)
93{
94 /// The copy constructor will always allocate its own memory for the
95 /// swim steps and set its internal flag to indicate that is owns them
96 /// regardless of the owner of the source trajectory's.
97
98 this->Nswim_steps = rt.Nswim_steps;
99 this->q = rt.q;
100 this->max_swim_steps = rt.max_swim_steps;
101 this->own_swim_steps = true;
102 this->step_size = rt.step_size;
103 this->bfield = rt.bfield;
104 this->last_phi = rt.last_phi;
105 this->last_dist_along_wire = rt.last_dist_along_wire;
106 this->last_dz_dphi = rt.last_dz_dphi;
107 this->RootGeom = rt.RootGeom;
108 this->geom = rt.geom;
109 this->dist_to_rt_depth = 0;
110 this->mass = rt.GetMass();
111 this->mass_sq=this->mass*this->mass;
112 this->ploss_direction = rt.ploss_direction;
113 this->check_material_boundaries = rt.GetCheckMaterialBoundaries();
114 this->BOUNDARY_STEP_FRACTION = rt.GetBoundaryStepFraction();
115 this->MIN_STEP_SIZE = rt.GetMinStepSize();
116 this->MAX_STEP_SIZE = rt.GetMaxStepSize();
117 this->debug_level=rt.debug_level;
118 this->zmin_track_boundary = -100.0; // boundary at which to stop swimming
119 this->zmax_track_boundary = 670.0; // boundary at which to stop swimming
120 this->Rmax_interior = 65.0; // Maximum radius (in cm) corresponding to inside of BCAL
121 this->Rmax_exterior = 88.0; // Maximum radius (in cm) corresponding to outside of BCAL
122
123
124 this->swim_steps = new swim_step_t[this->max_swim_steps];
125 this->last_swim_step = NULL__null;
126 for(int i=0; i<Nswim_steps; i++)
127 {
128 swim_steps[i] = rt.swim_steps[i];
129 if(&(rt.swim_steps[i]) == rt.last_swim_step)
130 this->last_swim_step = &(swim_steps[i]);
131 }
132
133}
134
135//---------------------------------
136// operator= (Assignment operator)
137//---------------------------------
138DReferenceTrajectory& DReferenceTrajectory::operator=(const DReferenceTrajectory& rt)
139{
140 /// The assignment operator will always make sure the memory allocated
141 /// for the swim_steps is owned by the object being copied into.
142 /// If it already owns memory of sufficient size, then it will be
143 /// reused. If it owns memory that is too small, it will be freed and
144 /// a new block allocated. If it does not own its swim_steps coming
145 /// in, then it will allocate memory so that it does own it on the
146 /// way out.
147
148 if(&rt == this)return *this; // protect against self copies
149
150 // Free memory if block is too small
151 if(own_swim_steps==true && max_swim_steps<rt.Nswim_steps){
152 delete[] swim_steps;
153 swim_steps=NULL__null;
154 }
155
156 // Forget memory block if we don't currently own it
157 if(!own_swim_steps){
158 swim_steps=NULL__null;
159 }
160
161 this->Nswim_steps = rt.Nswim_steps;
162 this->q = rt.q;
163 this->max_swim_steps = rt.max_swim_steps;
164 this->own_swim_steps = true;
165 this->step_size = rt.step_size;
166 this->bfield = rt.bfield;
167 this->last_phi = rt.last_phi;
168 this->last_dist_along_wire = rt.last_dist_along_wire;
169 this->last_dz_dphi = rt.last_dz_dphi;
170 this->RootGeom = rt.RootGeom;
171 this->geom = rt.geom;
172 this->dist_to_rt_depth = rt.dist_to_rt_depth;
173 this->mass = rt.GetMass();
174 this->mass_sq=this->mass*this->mass;
175 this->ploss_direction = rt.ploss_direction;
176 this->check_material_boundaries = rt.GetCheckMaterialBoundaries();
177 this->BOUNDARY_STEP_FRACTION = rt.GetBoundaryStepFraction();
178 this->MIN_STEP_SIZE = rt.GetMinStepSize();
179 this->MAX_STEP_SIZE = rt.GetMaxStepSize();
180
181 // Allocate memory if needed
182 if(swim_steps==NULL__null)this->swim_steps = new swim_step_t[this->max_swim_steps];
183
184 // Copy swim steps
185 this->last_swim_step = NULL__null;
186 for(int i=0; i<Nswim_steps; i++)
187 {
188 swim_steps[i] = rt.swim_steps[i];
189 if(&(rt.swim_steps[i]) == rt.last_swim_step)
190 this->last_swim_step = &(swim_steps[i]);
191 }
192
193
194 return *this;
195}
196
197//---------------------------------
198// ~DReferenceTrajectory (Destructor)
199//---------------------------------
200DReferenceTrajectory::~DReferenceTrajectory()
201{
202 if(own_swim_steps){
203 delete[] swim_steps;
204 }
205}
206
207//---------------------------------
208// CopyWithShift
209//---------------------------------
210void DReferenceTrajectory::CopyWithShift(const DReferenceTrajectory *rt, DVector3 shift)
211{
212 // First, do a straight copy
213 *this = *rt;
214
215 // Second, shift all positions
216 for(int i=0; i<Nswim_steps; i++)swim_steps[i].origin += shift;
217}
218
219
220//---------------------------------
221// Reset
222//---------------------------------
223void DReferenceTrajectory::Reset(void){
224 //reset DReferenceTrajectory for re-use
225 this->Nswim_steps = 0;
226 this->ploss_direction = kForward;
227 this->mass = 0.13957; // assume pion mass until otherwise specified
228 this->mass_sq=this->mass*this->mass;
229 this->hit_cdc_endplate = false;
230 this->last_phi = 0.0;
231 this->last_swim_step = NULL__null;
232 this->last_dist_along_wire = 0.0;
233 this->last_dz_dphi = 0.0;
234 this->dist_to_rt_depth = 0;
235 this->check_material_boundaries = true;
236}
237
238//---------------------------------
239// FastSwim -- light-weight swim to a wire that does not treat multiple
240// scattering but does handle energy loss.
241// No checks for distance to boundaries are done.
242//---------------------------------
243void DReferenceTrajectory::FastSwim(const DVector3 &pos, const DVector3 &mom,
244 DVector3 &last_pos,DVector3 &last_mom,
245 double q,double smax,
246 const DCoordinateSystem *wire){
247 DVector3 mypos(pos);
248 DVector3 mymom(mom);
249
250 // Initialize the stepper
251 DMagneticFieldStepper stepper(bfield, q, &pos, &mom);
252 double s=0,doca=1000.,old_doca=1000.,dP_dx=0.;
253 double mass=GetMass();
254 while (s<smax){
255 // Save old value of doca
256 old_doca=doca;
257
258 // Adjust step size to take smaller steps in regions of high momentum loss
259 if(mass>0. && step_size<0.0 && geom){
260 double KrhoZ_overA=0.0;
261 double rhoZ_overA=0.0;
262 double LogI=0.0;
263 double X0=0.0;
264 if (geom->FindMatALT1(mypos,mymom,KrhoZ_overA,rhoZ_overA,LogI,X0)
265 ==NOERROR){
266 // Calculate momentum loss due to ionization
267 dP_dx = dPdx(mymom.Mag(), KrhoZ_overA, rhoZ_overA,LogI);
268 double my_step_size = 0.0001/fabs(dP_dx);
269
270 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE; // maximum step size in cm
271 if(my_step_size<MIN_STEP_SIZE)my_step_size=MIN_STEP_SIZE; // minimum step size in cm
272
273 stepper.SetStepSize(my_step_size);
274 }
275 }
276 // Swim to next
277 double ds=stepper.Step(NULL__null);
278 s+=ds;
279
280 stepper.GetPosMom(mypos,mymom);
281 if (mass>0 && dP_dx<0.){
282 double ptot=mymom.Mag();
283 if (ploss_direction==kForward) ptot+=dP_dx*ds;
284 else ptot-=dP_dx*ds;
285 mymom.SetMag(ptot);
286 stepper.SetStartingParams(q, &mypos, &mymom);
287 }
288
289 // Break if we have passed the wire
290 DVector3 wirepos=wire->origin;
291 if (fabs(wire->udir.z())>0.){ // for CDC wires
292 wirepos+=((mypos.z()-wire->origin.z())/wire->udir.z())*wire->udir;
293 }
294 doca=(wirepos-mypos).Mag();
295 if (doca>old_doca) break;
296
297 // Store the position and momentum for this step
298 last_pos=mypos;
299 last_mom=mymom;
300 }
301}
302
303// Faster version of the swimmer that uses an alternate stepper and does not
304// check for material boundaries.
305void DReferenceTrajectory::FastSwim(const DVector3 &pos, const DVector3 &mom, double q,double smax, double zmin,double zmax){
306
307 /// (Re)Swim the trajectory starting from pos with momentum mom.
308 /// This will use the charge and step size (if given) passed to
309 /// the constructor when the object was created. It will also
310 /// (re)use the swim_step buffer, replacing it's contents.
311
312 // If the charged passed to us is greater that 10, it means use the charge
313 // already stored in the class. Otherwise, use what was passed to us.
314 if(fabs(q)>10)
315 q = this->q;
316 else
317 this->q = q;
318
319 DMagneticFieldStepper stepper(bfield, q, &pos, &mom);
320 if(step_size>0.0)stepper.SetStepSize(step_size);
321
322 // Step until we hit a boundary (don't track more than 20 meters)
323 swim_step_t *swim_step = this->swim_steps;
324 double t=0.;
325 Nswim_steps = 0;
326 double itheta02 = 0.0;
327 double itheta02s = 0.0;
328 double itheta02s2 = 0.0;
329 double X0sum=0.0;
330 swim_step_t *last_step=NULL__null;
331 double old_radius=10000.;
332
333 // Variables used to tag the step at which the track passes into one one of
334 // the outer detectors
335 index_at_bcal=-1;
336 index_at_tof=-1;
337 index_at_fcal=-1;
338 bool hit_bcal=false,hit_fcal=false,hit_tof=false;
339
340 for(double s=0; fabs(s)<smax; Nswim_steps++, swim_step++){
341
342 if(Nswim_steps>=this->max_swim_steps){
343 if (debug_level>0){
344 jerr<<__FILE__"libraries/TRACKING/DReferenceTrajectory.cc"<<":"<<__LINE__344<<" Too many steps in trajectory. Truncating..."<<endl;
345 }
346 break;
347 }
348
349 stepper.GetDirs(swim_step->sdir, swim_step->tdir, swim_step->udir);
350 stepper.GetPosMom(swim_step->origin, swim_step->mom);
351 swim_step->Ro = stepper.GetRo();
352 swim_step->s = s;
353 swim_step->t = t;
354
355 // Magnetic field at current position
356 bfield->GetField(swim_step->origin,swim_step->B);
357
358 //magnitude of momentum and beta
359 double p_sq=swim_step->mom.Mag2();
360 double one_over_beta_sq=1.+mass_sq/p_sq;
361
362 // Add material if geom or RootGeom is not NULL
363 // If both are non-NULL, then use RootGeom
364 double dP = 0.0;
365 double dP_dx=0.0;
366 if(RootGeom || geom){
367 double KrhoZ_overA=0.0;
368 double rhoZ_overA=0.0;
369 double LogI=0.0;
370 double X0=0.0;
371 jerror_t err;
372 if(RootGeom){
373 double rhoZ_overA,rhoZ_overA_logI;
374 err = RootGeom->FindMatLL(swim_step->origin,
375 rhoZ_overA,
376 rhoZ_overA_logI,
377 X0);
378 KrhoZ_overA=0.1535e-3*rhoZ_overA;
379 LogI=rhoZ_overA_logI/rhoZ_overA;
380 }else{
381 err = geom->FindMatALT1(swim_step->origin, swim_step->mom, KrhoZ_overA, rhoZ_overA,LogI, X0);
382 }
383 if(err == NOERROR){
384 if(X0>0.0){
385 double p=sqrt(p_sq);
386 double delta_s = s;
387 if(last_step)delta_s -= last_step->s;
388 double radlen = delta_s/X0;
389
390 if(radlen>1.0E-5){ // PDG 2008 pg 271, second to last paragraph
391
392 // double theta0 = 0.0136*sqrt(one_over_beta_sq)/p*sqrt(radlen)*(1.0+0.038*log(radlen)); // From PDG 2008 eq 27.12
393 //double theta02 = theta0*theta0;
394 double factor=1.0+0.038*log(radlen);
395 double theta02=1.8496e-4*factor*factor*radlen*one_over_beta_sq/p_sq;
396
397 itheta02 += theta02;
398 itheta02s += s*theta02;
399 itheta02s2 += s*s*theta02;
400 X0sum+=X0;
401 }
402
403 // Calculate momentum loss due to ionization
404 dP_dx = dPdx(p, KrhoZ_overA, rhoZ_overA,LogI);
405 }
406 }
407 last_step = swim_step;
408 }
409 swim_step->itheta02 = itheta02;
410 swim_step->itheta02s = itheta02s;
411 swim_step->itheta02s2 = itheta02s2;
412 swim_step->invX0=Nswim_steps/X0sum;
413
414 if(step_size<0.0){ // step_size<0 indicates auto-calculated step size
415 // Adjust step size to take smaller steps in regions of high momentum loss
416 double my_step_size = 0.0001/fabs(dP_dx);
417 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE; // maximum step size in cm
418 if(my_step_size<MIN_STEP_SIZE)my_step_size=MIN_STEP_SIZE; // minimum step size in cm
419
420 stepper.SetStepSize(my_step_size);
421 }
422
423 // Swim to next
424 double ds=stepper.FastStep(swim_step->B);
425
426 // Calculate momentum loss due to the step we're about to take
427 dP = ds*dP_dx;
428
429 // Adjust momentum due to ionization losses
430 if(dP!=0.0){
431 DVector3 pos, mom;
432 stepper.GetPosMom(pos, mom);
433 double ptot = mom.Mag() - dP; // correct for energy loss
434 if (ptot<0) {Nswim_steps++; break;}
435 mom.SetMag(ptot);
436 stepper.SetStartingParams(q, &pos, &mom);
437 }
438
439 // update flight time
440 t+=ds*sqrt(one_over_beta_sq)/SPEED_OF_LIGHT29.9792;
441 s += ds;
442
443 // Mark places along trajectory where we pass into one of the
444 // main detectors
445 double R=swim_step->origin.Perp();
446 double z=swim_step->origin.Z();
447 if (hit_bcal==false && R>65. && z<407 &&z>0){
448 index_at_bcal=Nswim_steps-1;
449 hit_bcal=true;
450 }
451 if (hit_tof==false && z>606.){
452 index_at_tof=Nswim_steps-1;
453 hit_tof=true;
454 }
455 if (hit_fcal==false && z>625.){
456 index_at_fcal=Nswim_steps-1;
457 hit_fcal=true;
458 }
459
460 // Exit the loop if we are already inside the volume of the BCAL
461 // and the radius is decreasing
462 if (R<old_radius && R>65.0 && z<407.0 && z>-100.0){
463 Nswim_steps++; break;
464 }
465
466 // Exit loop if we leave the tracking volume
467 if (z>zmax){Nswim_steps++; break;}
468 if(R>88.0 && z<407.0){Nswim_steps++; break;} // ran into BCAL
469 if (fabs(swim_step->origin.X())>129.
470 || fabs(swim_step->origin.Y())>129.)
471 {Nswim_steps++; break;} // left extent of TOF
472 if(z>670.0){Nswim_steps++; break;} // ran into FCAL
473 if(z<zmin){Nswim_steps++; break;} // exit upstream
474
475 old_radius=swim_step->origin.Perp();
476 }
477
478 // OK. At this point the positions of the trajectory in the lab
479 // frame have been recorded along with the momentum of the
480 // particle and the directions of reference trajectory
481 // coordinate system at each point.
482}
483
484
485
486
487
488
489//---------------------------------
490// Swim
491//---------------------------------
492void DReferenceTrajectory::Swim(const DVector3 &pos, const DVector3 &mom, double q, const DMatrixDSym *cov,double smax, const DCoordinateSystem *wire)
493{
494 /// (Re)Swim the trajectory starting from pos with momentum mom.
495 /// This will use the charge and step size (if given) passed to
496 /// the constructor when the object was created. It will also
497 /// (re)use the sim_step buffer, replacing it's contents.
498
499 // If the charged passed to us is greater that 10, it means use the charge
500 // already stored in the class. Otherwise, use what was passed to us.
501 if(fabs(q)>10)
502 q = this->q;
503 else
504 this->q = q;
505
506 DMagneticFieldStepper stepper(bfield, q, &pos, &mom);
507 if(step_size>0.0)stepper.SetStepSize(step_size);
508
509 // Step until we hit a boundary (don't track more than 20 meters)
510 swim_step_t *swim_step = this->swim_steps;
511 double t=0.;
512 Nswim_steps = 0;
513 double itheta02 = 0.0;
514 double itheta02s = 0.0;
515 double itheta02s2 = 0.0;
516 double X0sum=0.0;
517 swim_step_t *last_step=NULL__null;
518 double old_radius=10000.;
519
520 DMatrixDSym mycov(7);
521 if (cov!=NULL__null){
522 mycov=*cov;
523 }
524
525 // Reset flag indicating whether we hit the CDC endplate
526 // and get the parameters of the endplate so we can check
527 // if we hit it while swimming.
528 //hit_cdc_endplate = false;
529 /*
530#if 0 // The GetCDCEndplate call goes all the way back to the XML and slows down
531 // overall tracking by a factor of 20. Therefore, we skip finding it
532 // and just hard-code the values instead. 1/28/2011 DL
533 double cdc_endplate_z=150+17; // roughly, from memory
534 double cdc_endplate_dz=5.0; // roughly, from memory
535 double cdc_endplate_rmin=10.0; // roughly, from memory
536 double cdc_endplate_rmax=55.0; // roughly, from memory
537 if(geom)geom->GetCDCEndplate(cdc_endplate_z, cdc_endplate_dz, cdc_endplate_rmin, cdc_endplate_rmax);
538 double cdc_endplate_zmin = cdc_endplate_z - cdc_endplate_dz/2.0;
539 double cdc_endplate_zmax = cdc_endplate_zmin + cdc_endplate_dz;
540#else
541 double cdc_endplate_rmin=10.0; // roughly, from memory
542 double cdc_endplate_rmax=55.0; // roughly, from memory
543 double cdc_endplate_zmin = 167.6;
544 double cdc_endplate_zmax = 168.2;
545#endif
546 */
547
548#if 0
549 // Get Bfield from stepper to initialize Bz_old
550 DVector3 B;
551 stepper.GetBField(B);
552 double Bz_old = B.z();
553#endif
554
555 // Variables used to tag the step at which the track passes into one
556 // one of the outer detectors
557 index_at_bcal=-1;
558 index_at_tof=-1;
559 index_at_fcal=-1;
560 bool hit_bcal=false,hit_fcal=false,hit_tof=false;
561
562 for(double s=0; fabs(s)<smax; Nswim_steps++, swim_step++){
563
564 if(Nswim_steps>=this->max_swim_steps){
565 if (debug_level>0){
566 jerr<<__FILE__"libraries/TRACKING/DReferenceTrajectory.cc"<<":"<<__LINE__566<<" Too many steps in trajectory. Truncating..."<<endl;
567 }
568 break;
569 }
570
571 stepper.GetDirs(swim_step->sdir, swim_step->tdir, swim_step->udir);
572 stepper.GetPosMom(swim_step->origin, swim_step->mom);
573 swim_step->Ro = stepper.GetRo();
574 swim_step->s = s;
575 swim_step->t = t;
576
577 //magnitude of momentum and beta
578 double p_sq=swim_step->mom.Mag2();
579 double one_over_beta_sq=1.+mass_sq/p_sq;
580
581 // Add material if geom or RootGeom is not NULL
582 // If both are non-NULL, then use RootGeom
583 double dP = 0.0;
584 double dP_dx=0.0;
585 double s_to_boundary=1.0E6; // initialize to "infinity" in case we don't set this below
586 if(RootGeom || geom){
587 double KrhoZ_overA=0.0;
588 double rhoZ_overA=0.0;
589 double LogI=0.0;
590 double X0=0.0;
591 jerror_t err;
592 if(RootGeom){
593 double rhoZ_overA,rhoZ_overA_logI;
594 err = RootGeom->FindMatLL(swim_step->origin,
595 rhoZ_overA,
596 rhoZ_overA_logI,
597 X0);
598 KrhoZ_overA=0.1535e-3*rhoZ_overA;
599 LogI=rhoZ_overA_logI/rhoZ_overA;
600 }else{
601 if(check_material_boundaries){
602 err = geom->FindMatALT1(swim_step->origin, swim_step->mom, KrhoZ_overA, rhoZ_overA,LogI, X0, &s_to_boundary);
603 }else{
604 err = geom->FindMatALT1(swim_step->origin, swim_step->mom, KrhoZ_overA, rhoZ_overA,LogI, X0);
605 }
606
607 // Check if we hit the CDC endplate
608 //double z = swim_step->origin.Z();
609 //if(z>=cdc_endplate_zmin && z<=cdc_endplate_zmax){
610 // double r = swim_step->origin.Perp();
611 // if(r>=cdc_endplate_rmin && r<=cdc_endplate_rmax){
612 // hit_cdc_endplate = true;
613 //}
614 //}
615 }
616
617 if(err == NOERROR){
618 if(X0>0.0){
619 double p=sqrt(p_sq);
620 double delta_s = s;
621 if(last_step)delta_s -= last_step->s;
622 double radlen = delta_s/X0;
623
624 if(radlen>1.0E-5){ // PDG 2008 pg 271, second to last paragraph
625
626 // double theta0 = 0.0136*sqrt(one_over_beta_sq)/p*sqrt(radlen)*(1.0+0.038*log(radlen)); // From PDG 2008 eq 27.12
627 //double theta02 = theta0*theta0;
628 double factor=1.0+0.038*log(radlen);
629 double theta02=1.8496e-4*factor*factor*radlen*one_over_beta_sq/p_sq;
630
631 itheta02 += theta02;
632 itheta02s += s*theta02;
633 itheta02s2 += s*s*theta02;
634 X0sum+=X0;
635
636 if (cov){
637
638 }
639 }
640
641 // Calculate momentum loss due to ionization
642 dP_dx = dPdx(p, KrhoZ_overA, rhoZ_overA,LogI);
643 }
644 }
645 last_step = swim_step;
646 }
647 swim_step->itheta02 = itheta02;
648 swim_step->itheta02s = itheta02s;
649 swim_step->itheta02s2 = itheta02s2;
650 swim_step->invX0=Nswim_steps/X0sum;
651
652 // Adjust step size to take smaller steps in regions of high momentum loss or field gradient
653 if(step_size<0.0){ // step_size<0 indicates auto-calculated step size
654 // Take step so as to change momentum by 100keV
655 //double my_step_size=p/fabs(dP_dx)*0.01;
656 double my_step_size = 0.0001/fabs(dP_dx);
657
658 // Now check the field gradient
659#if 0
660 stepper.GetBField(B);
661 double Bz = B.z();
662 if (fabs(Bz-Bz_old)>EPS1e-8){
663 double my_step_size_B=0.01*my_step_size
664 *fabs(Bz/(Bz_old-Bz));
665 if (my_step_size_B<my_step_size)
666 my_step_size=my_step_size_B;
667 }
668 Bz_old=Bz; // Save old z-component of B-field
669#endif
670 // Use the estimated distance to the boundary to make sure we don't overstep
671 // into a high density region and miss some material. Use half the estimated
672 // distance since it's only an estimate. Note that even though this would lead
673 // to infinitely small steps, there is a minimum step size imposed below to
674 // ensure the step size is reasonable.
675 /*
676 double step_size_to_boundary = BOUNDARY_STEP_FRACTION*s_to_boundary;
677 if(step_size_to_boundary < my_step_size)my_step_size = step_size_to_boundary;
678 */
679
680 if(my_step_size>MAX_STEP_SIZE)my_step_size=MAX_STEP_SIZE; // maximum step size in cm
681 if(my_step_size<MIN_STEP_SIZE)my_step_size=MIN_STEP_SIZE; // minimum step size in cm
682
683 stepper.SetStepSize(my_step_size);
684 }
685
686 // Swim to next
687 double ds=stepper.Step(NULL__null,&swim_step->B);
688 if (cov){
689 PropagateCovariance(ds,q,mass_sq,mom,pos,swim_step->B,mycov);
690 swim_step->cov_t_t=mycov(6,6);
691 swim_step->cov_px_t=mycov(6,0);
692 swim_step->cov_py_t=mycov(6,1);
693 swim_step->cov_pz_t=mycov(6,2);
694 }
695
696 // Calculate momentum loss due to the step we're about to take
697 dP = ds*dP_dx;
698
699 // Adjust momentum due to ionization losses
700 if(dP!=0.0){
701 DVector3 pos, mom;
702 stepper.GetPosMom(pos, mom);
703 double ptot = mom.Mag() - dP; // correct for energy loss
704 bool ranged_out = false;
705 /*
706 if (ptot<0.05){
707 swim_step->origin.Print();
708 cout<<"N: " << Nswim_steps <<" x " << pos.x() <<" y " <<pos.y() <<" z " << pos.z() <<" r " << pos.Perp()<< " s " << s << " p " << ptot << endl;
709 }
710 */
711 if(ptot<0.0)ranged_out=true;
712 if(dP<0.0 && ploss_direction==kForward)ranged_out=true;
713 if(dP>0.0 && ploss_direction==kBackward)ranged_out=true;
714 if(mom.Mag()==0.0)ranged_out=true;
715 if(ranged_out){
716 Nswim_steps++; // This will at least allow for very low momentum particles to have 1 swim step
717 break;
718 }
719 mom.SetMag(ptot);
720 stepper.SetStartingParams(q, &pos, &mom);
721 }
722
723 // update flight time
724 t+=ds*sqrt(one_over_beta_sq)/SPEED_OF_LIGHT29.9792;
725 s += ds;
726
727 // Mark places along trajectory where we pass into one of the
728 // main detectors
729 double R=swim_step->origin.Perp();
730 double z=swim_step->origin.Z();
731 if (hit_bcal==false && R>65. && z<407 &&z>0){
732 index_at_bcal=Nswim_steps-1;
733 hit_bcal=true;
734 }
735 if (hit_tof==false && z>618.){
736 index_at_tof=Nswim_steps-1;
737 hit_tof=true;
738 }
739 if (hit_fcal==false && z>625.){
740 index_at_fcal=Nswim_steps-1;
741 hit_fcal=true;
742 }
743
744 // Exit the loop if we are already inside the volume of the BCAL
745 // and the radius is decreasing
746 if (R<old_radius && R>Rmax_interior && z<407.0 && z>-100.0){
747 Nswim_steps++; break;
748 }
749
750
751 // Exit loop if we leave the tracking volume
752 if(R>Rmax_exterior && z<407.0){Nswim_steps++; break;} // ran into BCAL
753 if (fabs(swim_step->origin.X())>129.
754 || fabs(swim_step->origin.Y())>129.)
755 {Nswim_steps++; break;} // left extent of TOF
756 if(z>zmax_track_boundary){Nswim_steps++; break;} // ran into FCAL
757 if(z<zmin_track_boundary){Nswim_steps++; break;} // exit upstream
758 if(wire && Nswim_steps>0){ // optionally check if we passed a wire we're supposed to be swimming to
759 swim_step_t *closest_step = FindClosestSwimStep(wire);
760 if(++closest_step!=swim_step){Nswim_steps++; break;}
761 }
762
763 old_radius=swim_step->origin.Perp();
764 }
765
766 // OK. At this point the positions of the trajectory in the lab
767 // frame have been recorded along with the momentum of the
768 // particle and the directions of reference trajectory
769 // coordinate system at each point.
770}
771
772// Routine to find position on the trajectory where the track crosses a radial
773// position R. Also returns the path length to this position.
774jerror_t DReferenceTrajectory::GetIntersectionWithRadius(double R,
775 DVector3 &mypos,
776 double *s,
777 double *t,
778 DVector3 *p_at_intersection) const{
779 mypos.SetXYZ(NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN());
780 if(p_at_intersection)
781 p_at_intersection->SetXYZ(NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN());
782
783 if(Nswim_steps<1){
784 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<784<<" "<<"No swim steps! You must \"Swim\" the track before calling GetIntersectionWithRadius(...)"<<endl;
785 }
786 // Return early if the radius at the end of the reference trajectory is still less than R
787 double outer_radius=swim_steps[Nswim_steps-1].origin.Perp();
788 if (outer_radius<R){
789 if (s) *s=0.;
790 if (t) *t=0.;
791 return VALUE_OUT_OF_RANGE;
792 }
793 // Return early if the radius at the beginning of the trajectory is outside
794 // the radius to which we are trying to match
795 double inner_radius=swim_steps[0].origin.Perp();
796 if (inner_radius>R){
797 if (s) *s=0.;
798 if (t) *t=0.;
799 return VALUE_OUT_OF_RANGE;
800 }
801
802
803 // Loop over swim steps and find the one that crosses the radius
804 swim_step_t *swim_step = swim_steps;
805 swim_step_t *step=NULL__null;
806 swim_step_t *last_step=NULL__null;
807
808 // double inner_radius=swim_step->origin.Perp();
809 for(int i=0; i<Nswim_steps; i++, swim_step++){
810 if (swim_step->origin.Perp()>R){
811 step=swim_step;
812 break;
813 }
814 if (swim_step->origin.Z()>407.0) return VALUE_OUT_OF_RANGE;
815 last_step=swim_step;
816 }
817 if (step==NULL__null||last_step==NULL__null) return VALUE_OUT_OF_RANGE;
818 if (p_at_intersection!=NULL__null){
819 *p_at_intersection=last_step->mom;
820 }
821
822 // At this point, the location where the track intersects the cyclinder
823 // is somewhere between last_step and step. For simplicity, we're going
824 // to just find the intersection of the cylinder with the line that joins
825 // the 2 positions. We do this by working in the X/Y plane only and
826 // finding the value of "alpha" which is the fractional distance the
827 // intersection point is between last_pos and mypos. We'll then apply
828 // the alpha found in the 2D X/Y space to the 3D x/y/Z space to find
829 // the actual intersection point.
830 DVector2 x1(last_step->origin.X(), last_step->origin.Y());
831 DVector2 x2(step->origin.X(), step->origin.Y());
832 DVector2 dx = x2-x1;
833 double A = dx.Mod2();
834 double B = 2.0*(x1.X()*dx.X() + x1.Y()*dx.Y());
835 double C = x1.Mod2() - R*R;
836
837 double sqrt_D=sqrt(B*B-4.0*A*C);
838 double one_over_denom=0.5/A;
839 double alpha1 = (-B + sqrt_D)*one_over_denom;
840 double alpha2 = (-B - sqrt_D)*one_over_denom;
841 double alpha = alpha1;
842 if(alpha1<0.0 || alpha1>1.0)alpha=alpha2;
843 if(!isfinite(alpha))return VALUE_OUT_OF_RANGE;
844
845 DVector3 delta = step->origin - last_step->origin;
846 mypos = last_step->origin + alpha*delta;
847
848 // The value of s actually represents the pathlength
849 // to the outside point. Adjust it back to the
850 // intersection point (approximately).
851 if (s) *s = step->s-(1.0-alpha)*delta.Mag();
852
853 // flight time
854 if (t){
855 double p_sq=step->mom.Mag2();
856 double one_over_beta=sqrt(1.+mass_sq/p_sq);
857 *t = step->t-(1.0-alpha)*delta.Mag()*one_over_beta/SPEED_OF_LIGHT29.9792;
858 }
859
860 return NOERROR;
861}
862
863//---------------------------------
864// GetIntersectionWithPlane
865//---------------------------------
866jerror_t DReferenceTrajectory::GetIntersectionWithPlane(const DVector3 &origin, const DVector3 &norm, DVector3 &pos, double *s,double *t,double *var_t,DetectorSystem_t detector) const{
867 DVector3 dummy;
868 return GetIntersectionWithPlane(origin,norm,pos,dummy,s,t,var_t,detector);
869}
870jerror_t DReferenceTrajectory::GetIntersectionWithPlane(const DVector3 &origin, const DVector3 &norm, DVector3 &pos, DVector3 &p_at_intersection, double *s,
871 double *t,double *var_t,DetectorSystem_t detector) const
872{
873 /// Get the intersection point of this trajectory with a plane.
874 /// The plane is specified by <i>origin</i> and <i>norm</i>. The
875 /// <i>origin</i> vector should give the coordinates of any point
876 /// on the plane and <i>norm</i> should give a vector normal to
877 /// the plane. The <i>norm</i> vector will be copied and normalized
878 /// so it can be of any magnitude upon entry.
879 ///
880 /// The coordinates of the intersection point will copied into
881 /// the supplied <i>pos</i> vector. If a non-NULL pointer for <i>s</i>
882 /// is passed in, the pathlength of the trajectory from its begining
883 /// to the intersection point is copied into location pointed to.
884
885 // Set reasonable defaults
886 pos.SetXYZ(0,0,0);
887 if(s)*s=0.0;
888 if(t)*t=0.0;
889 p_at_intersection.SetXYZ(0,0,0);
890
891 // Return early if the z-position of the plane with which we are
892 // intersecting is beyong the reference trajectory.
893 if (origin.z()>swim_steps[Nswim_steps-1].origin.z()){
894 return VALUE_OUT_OF_RANGE;
895 }
896 // Find the closest swim step to the position where the track crosses
897 // the plane
898 int first_i=0;
899 switch(detector){
900 case SYS_FCAL:
901 if (index_at_fcal<0) return VALUE_OUT_OF_RANGE;
902 first_i=index_at_fcal;
903 break;
904 case SYS_TOF:
905 if (index_at_tof<0) return VALUE_OUT_OF_RANGE;
906 first_i=index_at_tof;
907 break;
908 default:
909 break;
910 }
911 swim_step_t *step=FindPlaneCrossing(origin,norm,first_i);
912 if(!step){
913 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<913<<" "<<"Could not find closest swim step!"<<endl;
914 return RESOURCE_UNAVAILABLE;
915 }
916
917 // Here we follow a scheme described in more detail in the
918 // DistToRT(DVector3 hit) method below. The basic idea is to
919 // express a point on the helix in terms of a single variable
920 // and then solve for that variable by setting the distance
921 // to zero.
922 //
923 // x = Ro*(cos(phi) - 1)
924 // y = Ro*sin(phi)
925 // z = phi*(dz/dphi)
926 //
927 // As is done below, we work in the RT coordinate system. Well,
928 // sort of. The distance to the plane is given by:
929 //
930 // d = ( x - c ).n
931 //
932 // where x is a point on the helix, c is the "origin" point
933 // which lies somewhere in the plane and n is the "norm"
934 // vector. Since we want a point in the plane, we set d=0
935 // and solve for phi (with the components of x expressed in
936 // terms of phi as given in the DistToRT method below).
937 //
938 // Thus, the equation we need to solve is:
939 //
940 // x.n - c.n = 0
941 //
942 // notice that "c" only gets dotted into "n" so that
943 // dot product can occur in any coordinate system. Therefore,
944 // we do that in the lab coordinate system to avoid the
945 // overhead of transforming "c" to the RT system. The "n"
946 // vector, however, still must be transformed.
947 //
948 // Expanding the trig functions to 2nd order in phi, performing
949 // the x.n dot product, and gathering equal powers of phi
950 // leads us to he following:
951 //
952 // (-Ro*nx/2)*phi^2 + (Ro*ny+dz_dphi*nz)*phi - c.n = 0
953 //
954 // which is quadratic in phi. We solve for both roots, but use
955 // the one with the smller absolute value (if both are finite).
956
957 double &Ro = step->Ro;
958
959 // OK, having said all of that, it turns out that the above
960 // mechanism will tend to fail in regions of low or no
961 // field because the value of Ro is very large. Thus, we need to
962 // use a straight line projection in such cases. We also
963 // want to use a straight line projection if the helical intersection
964 // fails for some other reason.
965 //
966 // The algorthim is then to only try the helical calculation
967 // for small (<10m) values of Ro and then do the straight line
968 // if R is larger than that OR the helical calculation fails.
969
970 // Try helical calculation
971 if(Ro<1000.0){
972 double nx = norm.Dot(step->sdir);
973 double ny = norm.Dot(step->tdir);
974 double nz = norm.Dot(step->udir);
975
976 double delta_z = step->mom.Dot(step->udir);
977 double delta_phi = step->mom.Dot(step->tdir)/Ro;
978 double dz_dphi = delta_z/delta_phi;
979
980 double A = -Ro*nx/2.0;
981 double B = Ro*ny + dz_dphi*nz;
982 double C = norm.Dot(step->origin-origin);
983 double sqroot=sqrt(B*B-4.0*A*C);
984 double twoA=2.0*A;
985
986 double phi_1 = (-B + sqroot)/(twoA);
987 double phi_2 = (-B - sqroot)/(twoA);
988
989 double phi = fabs(phi_1)<fabs(phi_2) ? phi_1:phi_2;
990 if(!isfinite(phi_1))phi = phi_2;
991 if(!isfinite(phi_2))phi = phi_1;
992 if(isfinite(phi)){
993
994 double my_s = -Ro/2.0 * phi*phi;
995 double my_t = Ro * phi;
996 double my_u = dz_dphi * phi;
997
998 pos = step->origin + my_s*step->sdir + my_t*step->tdir + my_u*step->udir;
999 p_at_intersection = step->mom;
1000 if(s){
1001 double delta_s = sqrt(my_t*my_t + my_u*my_u);
1002 *s = step->s + (phi>0 ? +delta_s:-delta_s);
1003 }
1004 // flight time
1005 if (t){
1006 double delta_s = sqrt(my_t*my_t + my_u*my_u);
1007 double ds=(phi>0 ? +delta_s:-delta_s);
1008 double p_sq=step->mom.Mag2();
1009 double one_over_beta=sqrt(1.+mass_sq/p_sq);
1010 *t = step->t+ds*one_over_beta/SPEED_OF_LIGHT29.9792;
1011 }
1012 if (var_t){
1013 *var_t=step->cov_t_t;
1014 }
1015
1016 // Success. Go ahead and return
1017 return NOERROR;
1018 }
1019 }
1020
1021 // If we got here then we need to try a straight line calculation
1022 double p_sq=step->mom.Mag2();
1023 double dz_over_pz=(origin.z()-step->origin.z())/step->mom.z();
1024 double ds=sqrt(p_sq)*dz_over_pz;
1025 pos.SetXYZ(step->origin.x()+dz_over_pz*step->mom.x(),
1026 step->origin.y()+dz_over_pz*step->mom.y(),
1027 origin.z());
1028 p_at_intersection = step->mom;
1029
1030 if(s){
1031 *s = step->s + ds;
1032 }
1033 // flight time
1034 if (t){
1035 double one_over_beta=sqrt(1.+mass_sq/p_sq);
1036 *t = step->t+ds*one_over_beta/SPEED_OF_LIGHT29.9792;
1037 }
1038 // Flight time variance
1039 if (var_t){
1040 *var_t=step->cov_t_t;
1041 }
1042
1043 return NOERROR;
1044}
1045
1046//---------------------------------
1047// InsertSteps
1048//---------------------------------
1049int DReferenceTrajectory::InsertSteps(const swim_step_t *start_step, double delta_s, double step_size)
1050{
1051 /// Insert additional steps into the reference trajectory starting
1052 /// at start_step and swimming for at least delta_s by step_size
1053 /// sized steps. Both delta_s and step_size are in centimeters.
1054 /// If the value of delta_s is negative then the particle's momentum
1055 /// and charge are reversed before swimming. This could be a problem
1056 /// if energy loss is implemented.
1057
1058 if(!start_step)return -1;
1059
1060 // We do this by creating another, temporary DReferenceTrajectory object
1061 // on the stack and swimming it.
1062 DVector3 pos = start_step->origin;
1063 DVector3 mom = start_step->mom;
1064 double my_q = q;
1065 int direction = +1;
1066 if(delta_s<0.0){
1067 mom *= -1.0;
1068 my_q = -q;
1069 direction = -1;
1070 }
1071
1072 // Here I allocate the steps using an auto_ptr so I don't have to mess with
1073 // deleting them at all of the possible exits. The problem with auto_ptr
1074 // is it can't handle arrays so it has to be wrapped in a struct.
1075 auto_ptr<StepStruct> steps_aptr(new StepStruct);
1076 DReferenceTrajectory::swim_step_t *steps = steps_aptr->steps;
1077 DReferenceTrajectory rt(bfield , my_q , steps , 256);
1078 rt.SetStepSize(step_size);
1079 rt.Swim(pos, mom, my_q,NULL__null,fabs(delta_s));
1080 if(rt.Nswim_steps==0)return 1;
1081
1082 // Check that there is enough space to add these points
1083 if((Nswim_steps+rt.Nswim_steps)>max_swim_steps){
1084 //_DBG_<<"Not enough swim steps available to add new ones! Max="<<max_swim_steps<<" had="<<Nswim_steps<<" new="<<rt.Nswim_steps<<endl;
1085 return 2;
1086 }
1087
1088 // At this point, we may have swum forward or backwards so the points
1089 // will need to be added either before start_step or after it. We also
1090 // may want to replace an old step that overlaps our high density steps
1091 // since they are presumably more accurate. Find the indexes of the
1092 // existing steps that the new steps will be inserted between.
1093 double sdiff = rt.swim_steps[rt.Nswim_steps-1].s;
1094 double s1 = start_step->s;
1095 double s2 = start_step->s + (double)direction*sdiff;
1096 double smin = s1<s2 ? s1:s2;
1097 double smax = s1<s2 ? s2:s1;
1098 int istep_start = 0;
1099 int istep_end = 0;
1100 for(int i=0; i<Nswim_steps; i++){
1101 if(swim_steps[i].s < smin)istep_start = i;
1102 if(swim_steps[i].s <= smax)istep_end = i+1;
1103 }
1104
1105 // istep_start and istep_end now point to the steps we want to keep.
1106 // All steps between them (exclusive) will be overwritten. Note that
1107 // the original start_step should be in the "overwrite" range since
1108 // it is included already in the new trajectory.
1109 int steps_to_overwrite = istep_end - istep_start - 1;
1110 int steps_to_shift = rt.Nswim_steps - steps_to_overwrite;
1111
1112 // Shift the steps down (or is it up?) starting with istep_end.
1113 for(int i=Nswim_steps-1; i>=istep_end; i--)swim_steps[i+steps_to_shift] = swim_steps[i];
1114
1115 // Copy the new steps into this object
1116 double s_0 = start_step->s;
1117 double itheta02_0 = start_step->itheta02;
1118 double itheta02s_0 = start_step->itheta02s;
1119 double itheta02s2_0 = start_step->itheta02s2;
1120 for(int i=0; i<rt.Nswim_steps; i++){
1121 int index = direction>0 ? (istep_start+1+i):(istep_start+1+rt.Nswim_steps-1-i);
1122 swim_steps[index] = rt.swim_steps[i];
1123 swim_steps[index].s = s_0 + (double)direction*swim_steps[index].s;
1124 swim_steps[index].itheta02 = itheta02_0 + (double)direction*swim_steps[index].itheta02;
1125 swim_steps[index].itheta02s = itheta02s_0 + (double)direction*swim_steps[index].itheta02s;
1126 swim_steps[index].itheta02s2 = itheta02s2_0 + (double)direction*swim_steps[index].itheta02s2;
1127 if(direction<0.0){
1128 swim_steps[index].sdir *= -1.0;
1129 swim_steps[index].tdir *= -1.0;
1130 }
1131 }
1132 Nswim_steps += rt.Nswim_steps-steps_to_overwrite;
1133
1134 // Note that the above procedure may leave us with "kinks" in the itheta0
1135 // variables. It may be that we need to recalculate those for all of the
1136 // new points and the ones after them by making one more pass. I'm hoping
1137 // it is a realitively small correction though so we can skip it here.
1138 return 0;
1139}
1140
1141//---------------------------------
1142// DistToRTwithTime
1143//---------------------------------
1144double DReferenceTrajectory::DistToRTwithTime(DVector3 hit, double *s,double *t,
1145 double *var_t,
1146 DetectorSystem_t detector) const{
1147 double dist=DistToRT(hit,s,detector);
1148 if (s!=NULL__null && t!=NULL__null)
1149 {
1150 if(last_swim_step==NULL__null)
1151 {
1152 *s = 1.0E6;
1153 *t = 1.0E6;
1154 if (var_t!=NULL__null){
1155 *var_t=1.0E6;
1156 }
1157 }
1158 else
1159 {
1160 double p_sq=last_swim_step->mom.Mag2();
1161 double one_over_beta=sqrt(1.+mass_sq/p_sq);
1162 *t=last_swim_step->t+(*s-last_swim_step->s)*one_over_beta/SPEED_OF_LIGHT29.9792;
1163 if (var_t!=NULL__null){
1164 *var_t=last_swim_step->cov_t_t;
1165 }
1166 }
1167 }
1168 return dist;
1169}
1170
1171//---------------------------------
1172// DistToRT
1173//---------------------------------
1174double DReferenceTrajectory::DistToRT(DVector3 hit, double *s,
1175 DetectorSystem_t detector) const
1176{
1177 last_swim_step=NULL__null;
1178 if(Nswim_steps<1)_DBG__std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1178<<std::endl;
1179
1180 int start_index=0;
1181 switch(detector){
1182 case SYS_BCAL:
1183 if (index_at_bcal<0) return numeric_limits<double>::quiet_NaN();
1184 start_index=index_at_bcal;
1185 break;
1186 case SYS_FCAL:
1187 if (index_at_fcal<0) return numeric_limits<double>::quiet_NaN();
1188 start_index=index_at_fcal;
1189 break;
1190 case SYS_TOF:
1191 if (index_at_tof<0) return numeric_limits<double>::quiet_NaN();
1192 start_index=index_at_tof;
1193 break;
1194 default:
1195 break;
1196 }
1197
1198 // First, find closest step to point
1199 swim_step_t *swim_step = &swim_steps[start_index];
1200 swim_step_t *step=NULL__null;
1201
1202 //double min_delta2 = 1.0E6;
1203 double old_delta2=10.e6,delta2=1.0e6;
1204
1205 // Check if we should start at the end of the reference trajectory
1206 // or the beginning...
1207 int last_index=Nswim_steps-1;
1208 double forward_delta2=(swim_step->origin - hit).Mag2();
1209 double backward_delta2=(swim_steps[last_index].origin-hit).Mag2();
1210
1211 if (forward_delta2<backward_delta2){ // start at the beginning
1212 for(int i=start_index; i<Nswim_steps; i++, swim_step++){
1213
1214 DVector3 pos_diff = swim_step->origin - hit;
1215 delta2 = pos_diff.Mag2();
1216
1217 if (delta2>old_delta2){
1218 break;
1219 }
1220
1221 //if(delta2 < min_delta2){
1222 //min_delta2 = delta2;
1223
1224 step = swim_step;
1225 old_delta2=delta2;
1226 //}
1227 }
1228 }
1229 else{// start at the end
1230 for(int i=last_index; i>=start_index; i--){
1231 swim_step=&swim_steps[i];
1232 DVector3 pos_diff = swim_step->origin - hit;
1233 delta2 = pos_diff.Mag2();
1234 if (delta2>old_delta2) break;
1235
1236 //if(delta2 < min_delta2){
1237 //min_delta2 = delta2;
1238
1239 step = swim_step;
1240 old_delta2=delta2;
1241 //}
1242 }
1243
1244 }
1245
1246 if(step==NULL__null){
1247 // It seems to occasionally occur that we have 1 swim step
1248 // and it's values are invalid. Supress warning messages
1249 // for these as they are "known" (even if not fully understood!)
1250 if(s != NULL__null)
1251 *s = 1.0E6;
1252 if(Nswim_steps>1){
1253 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1253<<" "<<"\"hit\" passed to DistToRT(DVector3) out of range!"<<endl;
1254 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1254<<" "<<"hit x,y,z = "<<hit.x()<<", "<<hit.y()<<", "<<hit.z()<<" Nswim_steps="<<Nswim_steps<<" min_delta2="<<delta2<<endl;
1255 }
1256 return 1.0E6;
1257 }
1258
1259 // store last step
1260 last_swim_step=step;
1261
1262
1263 // Next, define a point on the helical segment defined by the
1264 // swim step it the RT coordinate system. The directions of
1265 // the RT coordinate system are defined by step->xdir, step->ydir,
1266 // and step->zdir. The coordinates of a point on the helix
1267 // in this coordinate system are:
1268 //
1269 // x = Ro*(cos(phi) - 1)
1270 // y = Ro*sin(phi)
1271 // z = phi*(dz/dphi)
1272 //
1273 // where phi is the phi angle of the point in this coordinate system.
1274 // phi=0 corresponds to the swim step point itself
1275 //
1276 // Transform the given coordinates to the RT coordinate system
1277 // and call these x0,y0,z0. Then, the distance of point to a
1278 // point on the helical segment is given by:
1279 //
1280 // d^2 = (x0-x)^2 + (y0-y)^2 + (z0-z)^2
1281 //
1282 // where x,y,z are all functions of phi as given above.
1283 //
1284 // writing out d^2 in terms of phi, but using the small angle
1285 // approximation for the trig functions, an equation for the
1286 // distance in only phi is obtained. Taking the derivative
1287 // and setting it equal to zero leaves a 3rd order polynomial
1288 // in phi whose root corresponds to the minimum distance.
1289 // Skipping some math, this equation has the form:
1290 //
1291 // d(d^2)/dphi = 0 = Ro^2*phi^3 + 2*alpha*phi + beta
1292 //
1293 // where:
1294 // alpha = x0*Ro + Ro^2 + (dz/dphi)^2
1295 //
1296 // beta = -2*y0*Ro - 2*z0*(dz/dphi)
1297 //
1298 // The above 3rd order poly is convenient in that it does not
1299 // contain a phi^2 term. This means we can skip the step
1300 // done in the general case where a change of variables is
1301 // made such that the 2nd order term disappears.
1302 //
1303 // In general, an equation of the form
1304 //
1305 // w^3 + 3.0*b*w + 2*c = 0
1306 //
1307 // has one real root:
1308 //
1309 // w0 = q - p
1310 //
1311 // where:
1312 // q^3 = d - c
1313 // p^3 = d + c
1314 // d^2 = b^3 + c^2 (don't confuse with d^2 above!)
1315 //
1316 // So for us ...
1317 //
1318 // 3b = 2*alpha/(Ro^2)
1319 // 2c = beta/(Ro^2)
1320
1321 hit -= step->origin;
1322 double x0 = hit.Dot(step->sdir);
1323 double y0 = hit.Dot(step->tdir);
1324 double z0 = hit.Dot(step->udir);
1325 double &Ro = step->Ro;
1326 double Ro2 = Ro*Ro;
1327 double delta_z = step->mom.Dot(step->udir);
1328 double delta_phi = step->mom.Dot(step->tdir)/Ro;
1329 double dz_dphi = delta_z/delta_phi;
1330
1331 // double alpha = x0*Ro + Ro2 + pow(dz_dphi,2.0);
1332 double alpha=x0*Ro + Ro2 +dz_dphi*dz_dphi;
1333 // double beta = -2.0*y0*Ro - 2.0*z0*dz_dphi;
1334 double beta = -2.0*(y0*Ro + z0*dz_dphi);
1335 // double b = (2.0*alpha/Ro2)/3.0;
1336 double b= TWO_THIRD0.66666666666666667*alpha/Ro2;
1337 // double c = (beta/Ro2)/2.0;
1338 double c = 0.5*(beta/Ro2);
1339 // double d = sqrt(pow(b,3.0) + pow(c,2.0));
1340 double d2=b*b*b+c*c;
1341 double phi=0.,dist2=1e8;
1342 if (d2>=0){
1343 double d=sqrt(d2);
1344 //double q = pow(d-c, ONE_THIRD);
1345 //double p = pow(d+c, ONE_THIRD);
1346 double p=cbrt(d+c);
1347 double q=cbrt(d-c);
1348 phi = q - p;
1349 if (fabs(phi)<0.2){ // check small angle approximation
1350 double phisq=phi*phi;
1351
1352 dist2 = 0.25*Ro2*phisq*phisq + alpha*phisq + beta*phi
1353 + x0*x0 + y0*y0 + z0*z0;
1354 }
1355 else{
1356 return numeric_limits<double>::quiet_NaN();
1357 }
1358 }
1359 else{
1360 // Use DeMoivre's theorem to find the cube root of a complex
1361 // number. In this case there are three real solutions.
1362 double d=sqrt(-d2);
1363 c*=-1.;
1364 double temp=sqrt(cbrt(c*c+d*d));
1365 double theta1=ONE_THIRD0.33333333333333333*atan2(d,c);
1366 double sum_over_2=temp*cos(theta1);
1367 double diff_over_2=-temp*sin(theta1);
1368
1369 double phi0=2.*sum_over_2;
1370 double phi0sq=phi0*phi0;
1371 double phi1=-sum_over_2+sqrt(3)*diff_over_2;
1372 double phi1sq=phi1*phi1;
1373 double phi2=-sum_over_2-sqrt(3)*diff_over_2;
1374 double phi2sq=phi2*phi2;
1375 double d2_2 = 0.25*Ro2*phi2sq*phi2sq + alpha*phi2sq + beta*phi2 + x0*x0 + y0*y0 + z0*z0;
1376 double d2_1 = 0.25*Ro2*phi1sq*phi1sq + alpha*phi1sq + beta*phi1 + x0*x0 + y0*y0 + z0*z0;
1377 double d2_0 = 0.25*Ro2*phi0sq*phi0sq + alpha*phi0sq + beta*phi0 + x0*x0 + y0*y0 + z0*z0;
1378
1379 if (d2_0<d2_1 && d2_0<d2_2){
1380 phi=phi0;
1381 dist2=d2_0;
1382 }
1383 else if (d2_1<d2_0 && d2_1<d2_2){
1384 phi=phi1;
1385 dist2=d2_1;
1386 }
1387 else{
1388 phi=phi2;
1389 dist2=d2_2;
1390 }
1391 if (fabs(phi)<0.2){ // Check small angle approximation
1392 return numeric_limits<double>::quiet_NaN();
1393 }
1394
1395 if (std::isnan(Ro))
1396 {
1397 }
1398 }
1399
1400 // Calculate distance along track ("s")
1401 if(s!=NULL__null){
1402 double dz = dz_dphi*phi;
1403 double Rodphi = Ro*phi;
1404 double ds = sqrt(dz*dz + Rodphi*Rodphi);
1405 *s = step->s + (phi>0.0 ? ds:-ds);
1406 }
1407
1408 this->last_phi = phi;
1409 this->last_swim_step = step;
1410 this->last_dz_dphi = dz_dphi;
1411
1412 return sqrt(dist2);
1413}
1414
1415//---------------------------------
1416// FindClosestSwimStep
1417//---------------------------------
1418DReferenceTrajectory::swim_step_t* DReferenceTrajectory::FindClosestSwimStep(const DCoordinateSystem *wire, int *istep_ptr) const
1419{
1420 /// Find the closest swim step to the given wire. The value of
1421 /// "L" should be the active wire length. The coordinate system
1422 /// defined by "wire" should have its origin at the center of
1423 /// the wire with the wire running in the direction of udir.
1424
1425 if(istep_ptr)*istep_ptr=-1;
1426
1427 if(Nswim_steps<1){
1428 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1428<<" "<<"No swim steps! You must \"Swim\" the track before calling FindClosestSwimStep(...)"<<endl;
1429 }
1430
1431 // Make sure we have a wire first!
1432 if(!wire)return NULL__null;
1433
1434 // Loop over swim steps and find the one closest to the wire
1435 swim_step_t *swim_step = swim_steps;
1436 swim_step_t *step=NULL__null;
1437 //double min_delta2 = 1.0E6;
1438 double old_delta2=1.0e6;
1439 double L_over_2 = wire->L/2.0; // half-length of wire in cm
1440 int istep=-1;
1441
1442 double dx, dy, dz;
1443
1444 // w is a vector to the origin of the wire
1445 // u is a unit vector along the wire
1446
1447 double wx, wy, wz;
1448 double ux, uy, uz;
1449
1450 wx = wire->origin.X();
1451 wy = wire->origin.Y();
1452 wz = wire->origin.Z();
1453
1454 ux = wire->udir.X();
1455 uy = wire->udir.Y();
1456 uz = wire->udir.Z();
1457
1458 int i;
1459 for(i=0; i<Nswim_steps; i++, swim_step++){
1460 // Find the point's position along the wire. If the point
1461 // is past the end of the wire, calculate the distance
1462 // from the end of the wire.
1463 // DVector3 pos_diff = swim_step->origin - wire->origin;
1464
1465 dx = swim_step->origin.X() - wx;
1466 dy = swim_step->origin.Y() - wy;
1467 dz = swim_step->origin.Z() - wz;
1468
1469 // double u = wire->udir.Dot(pos_diff);
1470 double u = ux * dx + uy * dy + uz * dz;
1471
1472 // Find distance perpendicular to wire
1473 // double delta2 = pos_diff.Mag2() - u*u;
1474 double delta2 = dx*dx + dy*dy + dz*dz - u*u;
1475
1476 // If point is past end of wire, calculate distance
1477 // from wire's end by adding on distance along wire direction.
1478 if( fabs(u)>L_over_2){
1479 // delta2 += pow(fabs(u)-L_over_2, 2.0);
1480 double u_minus_L_over_2=fabs(u)-L_over_2;
1481 delta2 += ( u_minus_L_over_2*u_minus_L_over_2 );
1482 // printf("step %d\n",i);
1483 }
1484
1485 if(debug_level>3)_DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1485<<" "<<"delta2="<<delta2<<" old_delta2="<<old_delta2<<endl;
1486 if (delta2>old_delta2) break;
1487
1488 //if(delta2 < min_delta2){
1489 // min_delta2 = delta2;
1490 step = swim_step;
1491 istep=i;
1492
1493 //}
1494 //printf("%d delta %f min %f\n",i,delta2,min_delta2);
1495 old_delta2=delta2;
1496 }
1497
1498 if(istep_ptr)*istep_ptr=istep;
1499
1500 if(debug_level>3)_DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1500<<" "<<"found closest step at i="<<i<<" istep_ptr="<<istep_ptr<<endl;
1501
1502 return step;
1503}
1504
1505//---------------------------------
1506// FindClosestSwimStep
1507//---------------------------------
1508DReferenceTrajectory::swim_step_t* DReferenceTrajectory::FindClosestSwimStep(const DVector3 &origin, DVector3 norm, int *istep_ptr) const
1509{
1510 /// Find the closest swim step to the plane specified by origin
1511 /// and norm. origin should indicate any point in the plane and
1512 /// norm a vector normal to the plane.
1513 if(istep_ptr)*istep_ptr=-1;
1514
1515 if(Nswim_steps<1){
1516 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1516<<" "<<"No swim steps! You must \"Swim\" the track before calling FindClosestSwimStep(...)"<<endl;
1517 }
1518
1519 // Make sure normal vector is unit lenght
1520 norm.SetMag(1.0);
1521
1522 // Loop over swim steps and find the one closest to the plane
1523 swim_step_t *swim_step = swim_steps;
1524 swim_step_t *step=NULL__null;
1525 //double min_dist = 1.0E6;
1526 double old_dist=1.0e6;
1527 int istep=-1;
1528
1529 for(int i=0; i<Nswim_steps; i++, swim_step++){
1530
1531 // Distance to plane is dot product of normal vector with any
1532 // vector pointing from the current step to a point in the plane
1533 double dist = fabs(norm.Dot(swim_step->origin-origin));
1534
1535 if (dist>old_dist) break;
1536
1537 // Check if we're the closest step
1538 //if(dist < min_dist){
1539 //min_dist = dist;
1540
1541 step = swim_step;
1542 istep=i;
1543 //}
1544 old_dist=dist;
1545
1546 // We should probably have a break condition here so we don't
1547 // waste time looking all the way to the end of the track after
1548 // we've passed the plane.
1549 }
1550
1551 if(istep_ptr)*istep_ptr=istep;
1552
1553 return step;
1554}
1555
1556
1557//---------------------------------
1558// FindPlaneCrossing
1559//---------------------------------
1560DReferenceTrajectory::swim_step_t* DReferenceTrajectory::FindPlaneCrossing(const DVector3 &origin, DVector3 norm,int first_i,int *istep_ptr) const
1561{
1562 /// Find the closest swim step to the position where the track crosses
1563 /// the plane specified by origin
1564 /// and norm. origin should indicate any point in the plane and
1565 /// norm a vector normal to the plane.
1566 if(istep_ptr)*istep_ptr=-1;
1567
1568 if(Nswim_steps<1){
1569 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1569<<" "<<"No swim steps! You must \"Swim\" the track before calling FindPlaneCrossing(...)"<<endl;
1570 raise(SIGSEGV11);// force seg. fault
1571 }
1572
1573 // Make sure normal vector is unit lenght
1574 norm.SetMag(1.0);
1575
1576 // Loop over swim steps and find the one closest to the plane
1577 swim_step_t *swim_step = &swim_steps[first_i];
1578 swim_step_t *step=NULL__null;
1579 //double min_dist = 1.0E6;
1580 int istep=-1;
1581 double old_dist=1.0e6;
1582
1583 // Check if we should start from the beginning of the reference
1584 // trajectory or the end
1585 int last_index=Nswim_steps-1;
1586 double forward_dist= norm.Dot(swim_step->origin-origin);
1587 double backward_dist= norm.Dot(swim_steps[last_index].origin-origin);
1588 if (fabs(forward_dist)<fabs(backward_dist)){ // start at beginning
1589 for(int i=first_i; i<Nswim_steps; i++, swim_step++){
1590
1591 // Distance to plane is dot product of normal vector with any
1592 // vector pointing from the current step to a point in the plane
1593 //double dist = fabs(norm.Dot(swim_step->origin-origin));
1594 double dist = norm.Dot(swim_step->origin-origin);
1595
1596 // We've crossed the plane when the sign of dist changes
1597 if (dist*old_dist<0 && i>0) {
1598 if (fabs(dist)<fabs(old_dist)){
1599 step=swim_step;
1600 istep=i;
1601 }
1602 break;
1603 }
1604 step = swim_step;
1605 istep=i;
1606 old_dist=dist;
1607 }
1608 }
1609 else{ // start at end
1610 for(int i=last_index; i>=0; i--){
1611 swim_step=&swim_steps[i];
1612 double dist = norm.Dot(swim_step->origin-origin);
1613 // We've crossed the plane when the sign of dist changes
1614 if (dist*old_dist<0 && i<last_index) {
1615 if (fabs(dist)<fabs(old_dist)){
1616 step=swim_step;
1617 istep=i;
1618 }
1619 break;
1620 }
1621 step = swim_step;
1622 istep=i;
1623 old_dist=dist;
1624 }
1625
1626 }
1627
1628 if(istep_ptr)*istep_ptr=istep;
1629
1630 return step;
1631}
1632
1633
1634
1635
1636//---------------------------------
1637// DistToRT
1638//---------------------------------
1639double DReferenceTrajectory::DistToRT(const DCoordinateSystem *wire, double *s) const
1640{
1641 /// Find the closest distance to the given wire in cm. The value of
1642 /// "L" should be the active wire length (in cm). The coordinate system
1643 /// defined by "wire" should have its origin at the center of
1644 /// the wire with the wire running in the direction of udir.
1645 swim_step_t *step=FindClosestSwimStep(wire);
1646
1647 return (step && step->s>0) ? DistToRT(wire, step, s):std::numeric_limits<double>::quiet_NaN();
1648}
1649
1650//---------------------------------
1651// DistToRTBruteForce
1652//---------------------------------
1653double DReferenceTrajectory::DistToRTBruteForce(const DCoordinateSystem *wire, double *s) const
1654{
1655 /// Find the closest distance to the given wire in cm. The value of
1656 /// "L" should be the active wire length (in cm). The coordinate system
1657 /// defined by "wire" should have its origin at the center of
1658 /// the wire with the wire running in the direction of udir.
1659 swim_step_t *step=FindClosestSwimStep(wire);
1660
1661 return step ? DistToRTBruteForce(wire, step, s):std::numeric_limits<double>::quiet_NaN();
1662}
1663
1664//------------------
1665// DistToRT
1666//------------------
1667double DReferenceTrajectory::DistToRT(const DCoordinateSystem *wire, const swim_step_t *step, double *s) const
1668{
1669 /// Calculate the distance of the given wire(in the lab
1670 /// reference frame) to the Reference Trajectory which the
1671 /// given swim step belongs to. This uses the momentum directions
1672 /// and positions of the swim step
1673 /// to define a curve and calculate the distance of the hit
1674 /// from it. The swim step should be the closest one to the wire.
1675 /// IMPORTANT: This approximates the helix locally by a parabola.
1676 /// This means the swim step should be fairly close
1677 /// to the wire so that this approximation is valid. If the
1678 /// reference trajectory from which the swim step came is too
1679 /// sparse, the results will not be nearly as good.
1680
1681 // Interestingly enough, this is one of the harder things to figure
1682 // out in the tracking code which is why the explanations may be
1683 // a bit long.
1684
1685 // The general idea is to define the helix in a coordinate system
1686 // in which the wire runs along the z-axis. The distance to the
1687 // wire is then defined just in the X/Y plane of this coord. system.
1688 // The distance is expressed as a function of the phi angle in the
1689 // natural coordinate system of the helix. This way, phi=0 corresponds
1690 // to the swim step point itself and the DOCA point should be
1691 // at a small phi angle.
1692 //
1693 // The minimum distance between the helical segment and the wire
1694 // will be a function of sin(phi), cos(phi) and phi. Approximating
1695 // sin(phi) by phi and cos(phi) by (1-phi^2) leaves a 4th order
1696 // polynomial in phi. Taking the derivative leaves a 3rd order
1697 // polynomial whose root is the phi corresponding to the
1698 // Distance Of Closest Approach(DOCA) point on the helix. Plugging
1699 // that value of phi back into the distance formula gives
1700 // us the minimum distance between the track and the wire.
1701
1702 // First, we need to define the coordinate system in which the
1703 // wire runs along the z-axis. This is actually done already
1704 // in the CDC package for each wire once, at program start.
1705 // The directions of the axes are defined in wire->sdir,
1706 // wire->tdir, and wire->udir.
1707
1708 // Next, define a point on the helical segment defined by the
1709 // swim step it the RT coordinate system. The directions of
1710 // the RT coordinate system are defined by step->xdir, step->ydir,
1711 // and step->zdir. The coordinates of a point on the helix
1712 // in this coordinate system are:
1713 //
1714 // x = Ro*(cos(phi) - 1)
1715 // y = Ro*sin(phi)
1716 // z = phi*(dz/dphi)
1717 //
1718 // where phi is the phi angle of the point in this coordinate system.
1719
1720 // Now, a vector describing the helical point in the LAB coordinate
1721 // system is:
1722 //
1723 // h = x*xdir + y*ydir + z*zdir + pos
1724 //
1725 // where h,xdir,ydir,zdir and pos are all 3-vectors.
1726 // xdir,ydir,zdir are unit vectors defining the directions
1727 // of the RT coord. system axes in the lab coord. system.
1728 // pos is a vector defining the position of the swim step
1729 // in the lab coord.system
1730
1731 // Now we just need to find the extent of "h" in the wire's
1732 // coordinate system (period . means dot product):
1733 //
1734 // s = (h-wpos).sdir
1735 // t = (h-wpos).tdir
1736 // u = (h-wpos).udir
1737 //
1738 // where wpos is the position of the center of the wire in
1739 // the lab coord. system and is given by wire->wpos.
1740
1741 // At this point, the values of s,t, and u repesent a point
1742 // on the helix in the coord. system of the wire with the
1743 // wire in the "u" direction and positioned at the origin.
1744 // The distance(squared) from the wire to the point on the helix
1745 // is given by:
1746 //
1747 // d^2 = s^2 + t^2
1748 //
1749 // where s and t are both functions of phi.
1750
1751 // So, we'll define the values of "s" and "t" above as:
1752 //
1753 // s = A*x + B*y + C*z + D
1754 // t = E*x + F*y + G*z + H
1755 //
1756 // where A,B,C,D,E,F,G, and H are constants defined below
1757 // and x,y,z are all functions of phi defined above.
1758 // (period . means dot product)
1759 //
1760 // A = sdir.xdir
1761 // B = sdir.ydir
1762 // C = sdir.zdir
1763 // D = sdir.(pos-wpos)
1764 //
1765 // E = tdir.xdir
1766 // F = tdir.ydir
1767 // G = tdir.zdir
1768 // H = tdir.(pos-wpos)
1769 const DVector3 &xdir = step->sdir;
1770 const DVector3 &ydir = step->tdir;
1771 const DVector3 &zdir = step->udir;
1772 const DVector3 &sdir = wire->sdir;
1773 const DVector3 &tdir = wire->tdir;
1774 const DVector3 &udir = wire->udir;
1775 DVector3 pos_diff = step->origin - wire->origin;
1776
1777 double A = sdir.Dot(xdir);
1778 double B = sdir.Dot(ydir);
1779 double C = sdir.Dot(zdir);
1780 double D = sdir.Dot(pos_diff);
1781
1782 double E = tdir.Dot(xdir);
1783 double F = tdir.Dot(ydir);
1784 double G = tdir.Dot(zdir);
1785 double H = tdir.Dot(pos_diff);
1786
1787 // OK, here is the dirty part. Using the approximations given above
1788 // to write the x and y functions in terms of phi^2 and phi (instead
1789 // of cos and sin) we put them into the equations for s and t above.
1790 // Then, inserting those into the equation for d^2 above that, we
1791 // get a very long equation in terms of the constants A,...H and
1792 // phi up to 4th order. Combining coefficients for similar powers
1793 // of phi yields an equation of the form:
1794 //
1795 // d^2 = Q*phi^4 + R*phi^3 + S*phi^2 + T*phi + U
1796 //
1797 // The dirty part is that it takes the better part of a sheet of
1798 // paper to work out the relations for Q,...U in terms of
1799 // A,...H, and Ro, dz/dphi. You can work it out yourself on
1800 // paper to verify that the equations below are correct.
1801 double Ro = step->Ro;
1802 double Ro2 = Ro*Ro;
1803 double delta_z = step->mom.Dot(step->udir);
1804 double delta_phi = step->mom.Dot(step->tdir)/Ro;
1805 double dz_dphi = delta_z/delta_phi;
1806 double dz_dphi2=dz_dphi*dz_dphi;
1807 double Ro_dz_dphi=Ro*dz_dphi;
1808
1809 // double Q = pow(A*Ro/2.0, 2.0) + pow(E*Ro/2.0, 2.0);
1810 double Q=0.25*Ro2*(A*A+E*E);
1811 // double R = -(2.0*A*B*Ro2 + 2.0*A*C*Ro_dz_dphi + 2.0*E*F*Ro2 + 2.0*E*G*Ro_dz_dphi)/2.0;
1812 double R = -((A*B+E*F)*Ro2 + (A*C+E*G)*Ro_dz_dphi);
1813 // double S = pow(B*Ro, 2.0) + pow(C*dz_dphi,2.0) + 2.0*B*C*Ro_dz_dphi - 2.0*A*D*Ro/2.0
1814 //+ pow(F*Ro, 2.0) + pow(G*dz_dphi,2.0) + 2.0*F*G*Ro_dz_dphi - 2.0*E*H*Ro/2.0;
1815 double S= (B*B+F*F)*Ro2+(C*C+G*G)*dz_dphi2+2.0*(B*C+F*G)*Ro_dz_dphi
1816 -(A*D+E*H)*Ro;
1817 // double T = 2.0*B*D*Ro + 2.0*C*D*dz_dphi + 2.0*F*H*Ro + 2.0*G*H*dz_dphi;
1818 double T = 2.0*((B*D+F*H)*Ro + (C*D+G*H)*dz_dphi);
1819 double U = D*D + H*H;
1820
1821 // Aaarghh! my fingers hurt just from typing all of that!
1822 //
1823 // OK, now we differentiate the above equation for d^2 to get:
1824 //
1825 // d(d^2)/dphi = 4*Q*phi^3 + 3*R*phi^2 + 2*S*phi + T
1826 //
1827 // NOTE: don't confuse "R" with "Ro" in the above equations!
1828 //
1829 // Now we have to solve the 3rd order polynomial for the phi value of
1830 // the point of closest approach on the RT. This is a well documented
1831 // procedure. Essentially, when you have an equation of the form:
1832 //
1833 // x^3 + a2*x^2 + a1*x + a0 = 0;
1834 //
1835 // a change of variables is made such that w = x + a2/3 which leads
1836 // to a third order poly with no w^2 term:
1837 //
1838 // w^3 + 3.0*b*w + 2*c = 0
1839 //
1840 // where:
1841 // b = a1/3 - (a2^2)/9
1842 // c = a0/2 - a1*a2/6 + (a2^3)/27
1843 //
1844 // The one real root of this is:
1845 //
1846 // w0 = q - p
1847 //
1848 // where:
1849 // q^3 = d - c
1850 // p^3 = d + c
1851 // d^2 = b^3 + c^2 (don't confuse with d^2 above!)
1852 //
1853 // For us this means that:
1854 // a2 = 3*R/(4*Q)
1855 // a1 = 2*S/(4*Q)
1856 // a0 = T/(4*Q)
1857 //
1858 // A potential problem could occur if Q is at or very close to zero.
1859 // This situation occurs when both A and E are zero. This would mean
1860 // that both sdir and tdir are perpendicular to xdir which means
1861 // xdir is in the same direction as udir (got that?). Physically,
1862 // this corresponds to the situation when both the momentum and
1863 // the magnetic field are perpendicular to the wire (though not
1864 // necessarily perpendicular to each other). This situation can't
1865 // really occur in the CDC detector where the chambers are well
1866 // contained in a region where the field is essentially along z as
1867 // are the wires.
1868 //
1869 // Just to be safe, we check that Q is greater than
1870 // some minimum before solving for phi. If it is too small, we fall
1871 // back to solving the quadratic equation for phi.
1872 double phi =0.0;
1873 if(fabs(Q)>1.0E-6){
1
Taking false branch
→
1874 /*
1875 double fourQ = 4.0*Q;
1876 double a2 = 3.0*R/fourQ;
1877 double a1 = 2.0*S/fourQ;
1878 double a0 = T/fourQ;
1879 */
1880 double one_over_fourQ=0.25/Q;
1881 double a2=3.0*R*one_over_fourQ;
1882 double a1=2.0*S*one_over_fourQ;
1883 double a0=T*one_over_fourQ;
1884 double a2sq=a2*a2;
1885 /*
1886 double b = a1/3.0 - a2*a2/9.0;
1887 double c = a0/2.0 - a1*a2/6.0 + a2*a2*a2/27.0;
1888 */
1889 double b=ONE_THIRD0.33333333333333333*(a1-ONE_THIRD0.33333333333333333*a2sq);
1890 double c=0.5*(a0-ONE_THIRD0.33333333333333333*a1*a2)+a2*a2sq/27.0;
1891 double my_d2=b*b*b+c*c;
1892 if (my_d2>0){
1893 //double d = sqrt(pow(b, 3.0) + pow(c, 2.0)); // occasionally, this is zero. See below
1894 double d=sqrt(my_d2);
1895 //double q = pow(d - c, ONE_THIRD);
1896 //double p = pow(d + c, ONE_THIRD);
1897 double q=cbrt(d-c);
1898 double p=cbrt(d+c);
1899
1900 double w0 = q - p;
1901 //phi = w0 - a2/3.0;
1902 phi = w0 - ONE_THIRD0.33333333333333333*a2;
1903 }
1904 else{
1905 // Use DeMoivre's theorem to find the cube root of a complex
1906 // number. In this case there are three real solutions.
1907 double d=sqrt(-my_d2);
1908 c*=-1.;
1909 double temp=sqrt(cbrt(c*c+d*d));
1910 double theta1=ONE_THIRD0.33333333333333333*atan2(d,c);
1911 double sum_over_2=temp*cos(theta1);
1912 double diff_over_2=-temp*sin(theta1);
1913
1914 double phi0=-a2/3+2.*sum_over_2;
1915 double phi1=-a2/3-sum_over_2+sqrt(3)*diff_over_2;
1916 double phi2=-a2/3-sum_over_2-sqrt(3)*diff_over_2;
1917
1918 double d2_0 = U + phi0*(T + phi0*(S + phi0*(R + phi0*Q)));
1919 double d2_1 = U + phi1*(T + phi1*(S + phi1*(R + phi1*Q)));
1920 double d2_2 = U + phi2*(T + phi2*(S + phi2*(R + phi2*Q)));
1921
1922 if (d2_0<d2_1 && d2_0<d2_2){
1923 phi=phi0;
1924 }
1925 else if (d2_1<d2_0 && d2_1<d2_2){
1926 phi=phi1;
1927 }
1928 else{
1929 phi=phi2;
1930 }
1931 }
1932 }
1933
1934 if(fabs(Q)<=1.0E-6 || !isfinite(phi)){
2
←
Taking false branch
→
1935 double a = 3.0*R;
1936 double b = 2.0*S;
1937 double c = 1.0*T;
1938 phi = (-b + sqrt(b*b - 4.0*a*c))/(2.0*a);
1939 }
1940
1941 // The accuracy of this method is limited by how close the step is to the
1942 // actual minimum. If the value of phi is large then the step size is
1943 // not too close and we should add another couple of steps in the right
1944 // place in order to get a more accurate value. Note that while this will
1945 // increase the time it takes this round, presumably the fitter will be
1946 // calling this often for each wire and having a high density of points
1947 // near the wires will just make subsequent calls go quicker. This also
1948 // allows larger initial step sizes with the high density regions getting
1949 // filled in as needed leading to overall faster tracking.
1950#if 0
1951 if(isfinite(phi) && fabs(phi)>2.0E-4){
1952 if(dist_to_rt_depth>=3){
1953 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<1953<<" "<<"3 or more recursive calls to DistToRT(). Something is wrong! bailing ..."<<endl;
1954 //for(int k=0; k<Nswim_steps; k++){
1955 // DVector3 &v = swim_steps[k].origin;
1956 // _DBG_<<" "<<k<<": "<<v.X()<<", "<<v.Y()<<", "<<v.Z()<<endl;
1957 //}
1958 //exit(-1);
1959 return std::numeric_limits<double>::quiet_NaN();
1960 }
1961 double scale_step = 1.0;
1962 double s_range = 1.0*scale_step;
1963 double step_size = 0.02*scale_step;
1964 int err = InsertSteps(step, phi>0.0 ? +s_range:-s_range, step_size); // Add new steps near this step by swimming in the direction of phi
1965 if(!err){
1966 step=FindClosestSwimStep(wire); // Find the new closest step
1967 if(!step)return std::numeric_limits<double>::quiet_NaN();
1968 dist_to_rt_depth++;
1969 double doca = DistToRT(wire, step, s); // re-call ourself with the new step
1970 dist_to_rt_depth--;
1971 return doca;
1972 }else{
1973 if(err<0)return std::numeric_limits<double>::quiet_NaN();
1974
1975 // If InsertSteps() returns an error > 0 then it indicates that it
1976 // was unable to add additional steps (perhaps because there
1977 // aren't enough spaces available). In that case, we just go ahead
1978 // and use the phi we have and make the best estimate possible.
1979 }
1980 }
1981#endif
1982
1983 // It is possible at this point that the value of phi corresponds to
1984 // a point past the end of the wire. We should check for this here and
1985 // recalculate, if necessary, the DOCA at the end of the wire. First,
1986 // calculate h (the vector defined way up above) and dot it into the
1987 // wire's u-direction to get the position of the DOCA point along the
1988 // wire.
1989 double x = -0.5*Ro*phi*phi;
1990 double y = Ro*phi;
1991 double z = dz_dphi*phi;
1992 DVector3 h = pos_diff + x*xdir + y*ydir + z*zdir;
1993 double u = h.Dot(udir);
1994 if(fabs(u) > wire->L/2.0){
3
←
Taking false branch
→
1995 // Looks like our DOCA point is past the end of the wire.
1996 // Find phi corresponding to the end of the wire.
1997 double L_over_2 = u>0.0 ? wire->L/2.0:-wire->L/2.0;
1998 double a = -0.5*Ro*udir.Dot(xdir);
1999 double b = Ro*udir.Dot(ydir) + dz_dphi*udir.Dot(zdir);
2000 double c = udir.Dot(pos_diff) - L_over_2;
2001 double twoa=2.0*a;
2002 double sqroot=sqrt(b*b-4.0*a*c);
2003 double phi1 = (-b + sqroot)/(twoa);
2004 double phi2 = (-b - sqroot)/(twoa);
2005 phi = fabs(phi1)<fabs(phi2) ? phi1:phi2;
2006 u=L_over_2;
2007 }
2008 this->last_dist_along_wire = u;
2009
2010 // Use phi to calculate DOCA
2011 double d2 = U + phi*(T + phi*(S + phi*(R + phi*Q)));
2012 double d = sqrt(d2);
2013
2014 // Calculate distance along track ("s")
2015 double dz = dz_dphi*phi;
2016 double Rodphi = Ro*phi;
2017 double ds = sqrt(dz*dz + Rodphi*Rodphi);
2018 if(s)*s=step->s + (phi>0.0 ? ds:-ds);
4
←
Assuming 's' is null
→
5
←
Taking false branch
→
2019 if(debug_level>3){
6
←
Taking true branch
→
2020 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<2020<<" "<<"distance to rt: "<<*s<<" from step at "<<step->s<<" with ds="<<ds<<" d="<<d<<" dz="<<dz<<" Rodphi="<<Rodphi<<endl;
7
←
Dereference of null pointer (loaded from variable 's')
2021 _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc"
<<":"<<2021<<" "<<"phi="<<phi<<" U="<<U<<" u="<<u<<endl;
2022 }
2023
2024 // Remember phi and step so additional info on the point can be obtained
2025 this->last_phi = phi;
2026 this->last_swim_step = step;
2027 this->last_dz_dphi = dz_dphi;
2028
2029 return d; // WARNING: This could return nan!
2030}
2031
2032//------------------
2033// DistToRTBruteForce
2034//------------------
2035double DReferenceTrajectory::DistToRTBruteForce(const DCoordinateSystem *wire, const swim_step_t *step, double *s) const
2036{
2037 /// Calculate the distance of the given wire(in the lab
2038 /// reference frame) to the Reference Trajectory which the
2039 /// given swim step belongs to. This uses the momentum directions
2040 /// and positions of the swim step
2041 /// to define a curve and calculate the distance of the hit
2042 /// from it. The swim step should be the closest one to the wire.
2043 /// IMPORTANT: This calculates the distance using a "brute force"
2044 /// method of taking tiny swim steps to find the minimum distance.
2045 /// It is vey SLOW and you should be using DistToRT(...) instead.
2046 /// This is only here to provide an independent check of DistToRT(...).
2047
2048 const DVector3 &xdir = step->sdir;
2049 const DVector3 &ydir = step->tdir;
2050 const DVector3 &zdir = step->udir;
2051 const DVector3 &sdir = wire->sdir;
2052 const DVector3 &tdir = wire->tdir;
2053 DVector3 pos_diff = step->origin - wire->origin;
2054
2055 double Ro = step->Ro;
2056 double delta_z = step->mom.Dot(step->udir);
2057 double delta_phi = step->mom.Dot(step->tdir)/Ro;
2058 double dz_dphi = delta_z/delta_phi;
2059
2060 // Brute force
2061 double min_d2 = 1.0E6;
2062 double phi=M_PI3.14159265358979323846;
2063 for(int i=-2000; i<2000; i++){
2064 double myphi=(double)i*0.000005;
2065 DVector3 d = Ro*(cos(myphi)-1.0)*xdir
2066 + Ro*sin(myphi)*ydir
2067 + dz_dphi*myphi*zdir
2068 + pos_diff;
2069
2070 double d2 = pow(d.Dot(sdir),2.0) + pow(d.Dot(tdir),2.0);
2071 if(d2<min_d2){
2072 min_d2 = d2;
2073 phi = myphi;
2074 this->last_phi = myphi;
2075 }
2076 }
2077 double d2 = min_d2;
2078 double d = sqrt(d2);
2079 this->last_phi = phi;
2080 this->last_swim_step = step;
2081 this->last_dz_dphi = dz_dphi;
2082
2083 // Calculate distance along track ("s")
2084 double dz = dz_dphi*phi;
2085 double Rodphi = Ro*phi;
2086 double ds = sqrt(dz*dz + Rodphi*Rodphi);
2087 if(s)*s=step->s + (phi>0.0 ? ds:-ds);
2088
2089 return d;
2090}
2091
2092//------------------
2093// Straw_dx
2094//------------------
2095double DReferenceTrajectory::Straw_dx(const DCoordinateSystem *wire, double radius) const
2096{
2097 /// Find the distance traveled within the specified radius of the
2098 /// specified wire. This will give the "dx" component of a dE/dx
2099 /// measurement for cylindrical geometry as we have with straw tubes.
2100 ///
2101 /// At this point, the estimate is done using a simple linear
2102 /// extrapolation from the DOCA point in the direction of the momentum
2103 /// to the 2 points at which it itersects the given radius. Segments
2104 /// which extend past the end of the wire will be clipped to the end
2105 /// of the wire before calculating the total dx.
2106
2107 // First, find the DOCA point for this wire
2108 double s;
2109 double doca = DistToRT(wire, &s);
2110 if(!isfinite(doca))
2111 return 0.0;
2112
2113 // If doca is outside of the given radius, then we're done
2114 if(doca>=radius)return 0.0;
2115
2116 // Get the location and momentum direction of the DOCA point
2117 DVector3 pos, momdir;
2118 GetLastDOCAPoint(pos, momdir);
2119 if(momdir.Mag()!=0.0)momdir.SetMag(1.0);
2120
2121 // Get wire direction
2122 const DVector3 &udir = wire->udir;
2123
2124 // Calculate vectors used to form quadratic equation for "alpha"
2125 // the distance along the mometum direction from the DOCA point
2126 // to the intersection with a cylinder of the given radius.
2127 DVector3 A = udir.Cross(pos-wire->origin);
2128 DVector3 B = udir.Cross(momdir);
2129
2130 // If the magnitude of B is zero at this point, it means the momentum
2131 // direction is parallel to the wire. In this case, this method will
2132 // not work. Return NaN.
2133 if(B.Mag()<1.0E-10)return std::numeric_limits<double>::quiet_NaN();
2134
2135 double a = B.Mag();
2136 double b = A.Dot(B);
2137 double c = A.Mag() - radius;
2138 double d = sqrt(b*b - 4.0*a*c);
2139
2140 // The 2 roots should correspond to the 2 intersection points.
2141 double alpha1 = (-b + d)/(2.0*a);
2142 double alpha2 = (-b - d)/(2.0*a);
2143
2144 DVector3 int1 = pos + alpha1*momdir;
2145 DVector3 int2 = pos + alpha2*momdir;
2146
2147 // Check if point1 is past the end of the wire
2148 double q = udir.Dot(int1 - wire->origin);
2149 if(fabs(q) > wire->L/2.0){
2150 double gamma = udir.Dot(wire->origin - pos) + (q>0.0 ? +1.0:-1.0)*wire->L/2.0;
2151 gamma /= momdir.Dot(udir);
2152 int1 = pos + gamma*momdir;
2153 }
2154
2155 // Check if point2 is past the end of the wire
2156 q = udir.Dot(int2 - wire->origin);
2157 if(fabs(q) > wire->L/2.0){
2158 double gamma = udir.Dot(wire->origin - pos) + (q>0.0 ? +1.0:-1.0)*wire->L/2.0;
2159 gamma /= momdir.Dot(udir);
2160 int2 = pos + gamma*momdir;
2161 }
2162
2163 // Calculate distance
2164 DVector3 delta = int1 - int2;
2165
2166 return delta.Mag();
2167}
2168
2169//------------------
2170// GetLastDOCAPoint
2171//------------------
2172void DReferenceTrajectory::GetLastDOCAPoint(DVector3 &pos, DVector3 &mom) const
2173{
2174 /// Use values saved by the last call to one of the DistToRT functions
2175 /// to calculate the 3-D DOCA position in lab coordinates and momentum
2176 /// in GeV/c.
2177
2178 if(last_swim_step==NULL__null){
2179 if(Nswim_steps>0){
2180 last_swim_step = &swim_steps[0];
2181 last_phi = 0.0;
2182 }else{
2183 pos.SetXYZ(NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN());
2184 mom.SetXYZ(NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN());
2185 return;
2186 }
2187 }
2188
2189 // If last_phi is not finite, set it to 0 as a last resort
2190 if(!isfinite(last_phi))last_phi = 0.0;
2191
2192 const DVector3 &xdir = last_swim_step->sdir;
2193 const DVector3 &ydir = last_swim_step->tdir;
2194 const DVector3 &zdir = last_swim_step->udir;
2195
2196 double x = -(last_swim_step->Ro/2.0)*last_phi*last_phi;
2197 double y = last_swim_step->Ro*last_phi;
2198 double z = last_dz_dphi*last_phi;
2199
2200 pos = last_swim_step->origin + x*xdir + y*ydir + z*zdir;
2201 mom = last_swim_step->mom;
2202
2203 mom.Rotate(-last_phi, zdir);
2204}
2205
2206//------------------
2207// GetLastDOCAPoint
2208//------------------
2209DVector3 DReferenceTrajectory::GetLastDOCAPoint(void) const
2210{
2211 /// Use values saved by the last call to one of the DistToRT functions
2212 /// to calculate the 3-D DOCA position in lab coordinates. This is
2213 /// mainly intended for debugging.
2214 if(last_swim_step==NULL__null){
2215 if(Nswim_steps>0){
2216 last_swim_step = &swim_steps[0];
2217 last_phi = 0.0;
2218 }else{
2219 return DVector3(NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN(),NaNstd::numeric_limits<double>::quiet_NaN());
2220 }
2221 }
2222 const DVector3 &xdir = last_swim_step->sdir;
2223 const DVector3 &ydir = last_swim_step->tdir;
2224 const DVector3 &zdir = last_swim_step->udir;
2225 double Ro = last_swim_step->Ro;
2226 double delta_z = last_swim_step->mom.Dot(zdir);
2227 double delta_phi = last_swim_step->mom.Dot(ydir)/Ro;
2228 double dz_dphi = delta_z/delta_phi;
2229
2230 double x = -(Ro/2.0)*last_phi*last_phi;
2231 double y = Ro*last_phi;
2232 double z = dz_dphi*last_phi;
2233
2234 return last_swim_step->origin + x*xdir + y*ydir + z*zdir;
2235}
2236
2237//------------------
2238// dPdx
2239//------------------
2240double DReferenceTrajectory::dPdx_from_A_Z_rho(double ptot, double A, double Z, double density) const
2241{
2242 double I = (Z*12.0 + 7.0)*1.0E-9; // From Leo 2nd ed. pg 25.
2243 if (Z>=13) I=(9.76*Z+58.8*pow(Z,-0.19))*1.0e-9;
2244 double rhoZ_overA=density*Z/A;
2245 double KrhoZ_overA = 0.1535e-3*rhoZ_overA;
2246
2247 return dPdx(ptot, KrhoZ_overA,rhoZ_overA,log(I));
2248}
2249
2250//------------------
2251// dPdx
2252//------------------
2253double DReferenceTrajectory::dPdx(double ptot, double KrhoZ_overA,
2254 double rhoZ_overA,double LogI) const
2255{
2256 /// Calculate the momentum loss per unit distance traversed of the material with
2257 /// the given A, Z, and density. Value returned is in GeV/c per cm
2258 /// This follows the July 2008 PDG section 27.2 ppg 268-270.
2259 if(mass==0.0)return 0.0; // no ionization losses for neutrals
2260
2261 double gammabeta = ptot/mass;
2262 double gammabeta2=gammabeta*gammabeta;
2263 double gamma = sqrt(gammabeta2+1.);
2264 double beta = gammabeta/gamma;
2265 double beta2=beta*beta;
2266 double me = 0.511E-3;
2267 double m_ratio=me/mass;
2268 double two_me_gammabeta2=2.*me*gammabeta2;
2269
2270 double Tmax = two_me_gammabeta2/(1.0+2.0*gamma*m_ratio+m_ratio*m_ratio);
2271 //double K = 0.307075E-3; // GeV gm^-1 cm^2
2272 // Density effect
2273 double delta=0.;
2274 double X=log10(gammabeta);
2275 double X0,X1;
2276 double Cbar=2.*(LogI-log(28.816e-9*sqrt(rhoZ_overA)))+1.;
2277 if (rhoZ_overA>0.01){ // not a gas
2278 if (LogI<-1.6118){ // I<100
2279 if (Cbar<=3.681) X0=0.2;
2280 else X0=0.326*Cbar-1.;
2281 X1=2.;
2282 }
2283 else{
2284 if (Cbar<=5.215) X0=0.2;
2285 else X0=0.326*Cbar-1.5;
2286 X1=3.;
2287 }
2288 }
2289 else { // gases
2290 X1=4.;
2291 if (Cbar<=9.5) X0=1.6;
2292 else if (Cbar>9.5 && Cbar<=10.) X0=1.7;
2293 else if (Cbar>10 && Cbar<=10.5) X0=1.8;
2294 else if (Cbar>10.5 && Cbar<=11.) X0=1.9;
2295 else if (Cbar>11.0 && Cbar<=12.25) X0=2.;
2296 else if (Cbar>12.25 && Cbar<=13.804){
2297 X0=2.;
2298 X1=5.;
2299 }
2300 else {
2301 X0=0.326*Cbar-2.5;
2302 X1=5.;
2303 }
2304 }
2305 if (X>=X0 && X<X1)
2306 delta=4.606*X-Cbar+(Cbar-4.606*X0)*pow((X1-X)/(X1-X0),3.);
2307 else if (X>=X1)
2308 delta= 4.606*X-Cbar;
2309
2310 double dEdx = KrhoZ_overA/beta2*(log(two_me_gammabeta2*Tmax)
2311 -2.*LogI - 2.0*beta2 -delta);
2312
2313 double dP_dx = dEdx/beta;
2314
2315 //double g = 0.350/sqrt(-log(0.06));
2316 //dP_dx *= 1.0 + exp(-pow(ptot/g,2.0)); // empirical for really low momentum particles
2317
2318
2319 if(ploss_direction==kBackward)dP_dx = -dP_dx;
2320
2321 return dP_dx;
2322}
2323
2324//------------------
2325// Dump
2326//------------------
2327void DReferenceTrajectory::Dump(double zmin, double zmax)
2328{
2329 swim_step_t *step = swim_steps;
2330 for(int i=0; i<Nswim_steps; i++, step++){
2331 vector<pair<string,string> > item;
2332 double x = step->origin.X();
2333 double y = step->origin.Y();
2334 double z = step->origin.Z();
2335 if(z<zmin || z>zmax)continue;
2336
2337 double px = step->mom.X();
2338 double py = step->mom.Y();
2339 double pz = step->mom.Z();
2340
2341 cout<<i<<": ";
2342 cout<<"(x,y,z)=("<<x<<","<<y<<","<<z<<") ";
2343 cout<<"(px,py,pz)=("<<px<<","<<py<<","<<pz<<") ";
2344 cout<<"(Ro,s,t)=("<<step->Ro<<","<<step->s<<","<<step->t<<") ";
2345 cout<<endl;
2346 }
2347
2348}
2349
2350// Propagate the covariance matrix for {px,py,pz,x,y,z,t} along the step ds
2351jerror_t DReferenceTrajectory::PropagateCovariance(double ds,double q,
2352 double mass_sq,
2353 const DVector3 &mom,
2354 const DVector3 &pos,
2355 const DVector3 &B,
2356 DMatrixDSym &C) const{
2357 DMatrix J(7,7);
2358
2359 double one_over_p_sq=1./mom.Mag2();
2360 double one_over_p=sqrt(one_over_p_sq);
2361 double px=mom.X();
2362 double py=mom.Y();
2363 double pz=mom.Z();
2364 double Bx=B.x(),By=B.y(),Bz=B.z();
2365
2366 double ds_over_p=ds*one_over_p;
2367 double factor=0.003*q*ds_over_p;
2368 double temp=(Bz*py-Bx*pz)*one_over_p_sq;
2369 J(0,0)=1-factor*px*temp;
2370 J(0,1)=factor*(Bz-py*temp);
2371 J(0,2)=-factor*(By+pz*temp);
2372
2373 temp=(Bx*pz-Bz*px)*one_over_p_sq;
2374 J(1,0)=-factor*(Bz+px*temp);
2375 J(1,1)=1-factor*py*temp;
2376 J(1,2)=factor*(Bx-pz*temp);
2377
2378 temp=(By*px-Bx*py)*one_over_p_sq;
2379 J(2,0)=factor*(By-px*temp);
2380 J(2,1)=-factor*(Bx+py*temp);
2381 J(2,2)=1-factor*pz*temp;
2382
2383 J(3,3)=1.;
2384 double ds_over_p3=one_over_p_sq*ds_over_p;
2385 J(3,0)=ds_over_p*(1-px*px*one_over_p_sq);
2386 J(3,1)=-px*py*ds_over_p3;
2387 J(3,2)=-px*pz*ds_over_p3;
2388
2389 J(4,4)=1.;
2390 J(4,0)=J(3,1);
2391 J(4,1)=ds_over_p*(1-py*py*one_over_p_sq);
2392 J(4,2)=-py*pz*ds_over_p3;
2393
2394 J(5,5)=1.;
2395 J(5,0)=J(3,2);
2396 J(5,1)=J(4,2);
2397 J(5,2)=ds_over_p*(1-pz*pz*one_over_p_sq);
2398
2399 J(6,6)=1.;
2400
2401 double fac2=(-ds/SPEED_OF_LIGHT29.9792)*mass_sq*one_over_p_sq*one_over_p_sq
2402 /sqrt(1.+mass_sq*one_over_p_sq);
2403 J(6,0)=fac2*px;
2404 J(6,1)=fac2*py;
2405 J(6,2)=fac2*pz;
2406
2407 C=C.Similarity(J);
2408
2409 return NOERROR;
2410}
2411
2412// Find the position along a reference trajectory closest to a line.
2413// The error matrix for the line can also be input via a pointer. The error
2414// matrix is expected to be 7x7 with the order {Px,Py,Pz,X,Y,Z,T}.
2415// Outputs the kinematic data object (including the covariance) at this
2416// position, and the doca and the variance on the doca.
2417jerror_t DReferenceTrajectory::FindPOCAtoLine(const DVector3 &origin,
2418 const DVector3 &dir,
2419 const DMatrixDSym *covline,
2420 DKinematicData *track_kd,
2421 DVector3 &commonpos, double &doca, double &var_doca) const{
2422 const swim_step_t *swim_step=this->swim_steps;
2423 DMatrixDSym cov(7);
2424 if(track_kd!=NULL__null)
2425 cov=track_kd->errorMatrix();
2426 doca=1000.;
2427 double tflight=0.;
2428 double mass_sq=this->mass_sq;
2429 double q=this->q;
2430 double step_size=1.0,s=-step_size;
2431 DVector3 oldpos,oldmom;
2432 DVector3 point=origin;
2433
2434 // Find the magnitude of the direction vector
2435 double pscale=dir.Mag();
2436 // If the magnitude of the direction vector is zero, don't bother to propagate
2437 // along a line from the input origin...
2438 bool move_along_line=(pscale>0)?true:false;
2439
2440 // Propagate along the reference trajectory, comparing to the line at each
2441 // step
2442 for (int i=0;i<this->Nswim_steps-1; i++, swim_step++){
2443 DVector3 pos=swim_step->origin;
2444 DVector3 diff=pos-point;
2445 double new_doca=diff.Mag();
2446 if (new_doca>doca){
2447 if (i==1){ // backtrack to find the true doca
2448 tflight=0.;
2449
2450 swim_step=this->swim_steps;
2451 if(track_kd!=NULL__null)
2452 cov=track_kd->errorMatrix();
2453
2454 pos=swim_step->origin;
2455 DVector3 mom=swim_step->mom;
2456 DMagneticFieldStepper stepper(this->bfield, this->q, &pos, &mom);
2457
2458 int inew=0;
2459 while (inew<100){
2460 DVector3 B;
2461 double ds=stepper.Step(&pos,&B,-0.5);
2462 // Compute the revised estimate for the doca
2463 diff=pos-point;
2464 new_doca=diff.Mag();
2465
2466 if(new_doca > doca) break;
2467
2468 // Propagate the covariance matrix of the track along the trajectory
2469 if(track_kd!=NULL__null){
2470 this->PropagateCovariance(ds,q,mass_sq,mom,oldpos,B,cov);
2471 }
2472
2473 // Store the current positions, doca and adjust flight times
2474 oldpos=pos;
2475 doca=new_doca;
2476
2477 double one_over_p_sq=1./mom.Mag2();
2478 tflight+=ds*sqrt(1.+mass_sq*one_over_p_sq)/SPEED_OF_LIGHT29.9792;
2479
2480 // New momentum
2481 stepper.GetMomentum(mom);
2482
2483 oldmom=/*(-1.)*/mom;
2484 inew++;
2485
2486 // New point on line
2487 if (move_along_line){
2488 point-=(step_size/pscale)*dir;
2489 s-=step_size;
2490 }
2491 }
2492 }
2493 if(track_kd!=NULL__null)
2494 {
2495 track_kd->setErrorMatrix(cov);
2496 track_kd->setMomentum(oldmom);
2497 track_kd->setPosition(oldpos);
2498 track_kd->setTime(track_kd->time() + tflight);
2499 }
2500
2501 // Compute the variance on the doca
2502 diff=oldpos-point;
2503 double dx=diff.x();
2504 double dy=diff.y();
2505 double dz=diff.z();
2506
2507 if(track_kd==NULL__null)
2508 break;
2509 //calculate var_doca
2510 if (covline==NULL__null){
2511 var_doca=(dx*dx*(cov(kX,kX))+dy*dy*(cov(kY,kY))
2512 +dz*dz*(cov(kZ,kZ))+2.*dx*dy*(cov(kX,kY))
2513 +2.*dx*dz*(cov(kX,kZ))+2.*dy*dz*(cov(kY,kZ)))
2514 /(doca*doca);
2515 }
2516 else{
2517 DMatrixDSym cov2(*covline);
2518 if (move_along_line){
2519 double two_s=2.*s;
2520 double s_sq=s*s;
2521 cov2(kX,kX)+=two_s*cov2(kPx,kX)+s_sq*cov2(kPx,kPx);
2522 cov2(kY,kY)+=two_s*cov2(kPy,kY)+s_sq*cov2(kPy,kPy);
2523 cov2(kZ,kZ)+=two_s*cov2(kPz,kZ)+s_sq*cov2(kPz,kPz);
2524 }
2525 var_doca=(dx*dx*(cov(kX,kX)+cov2(kX,kX))
2526 +dy*dy*(cov(kY,kY)+cov2(kY,kY))
2527 +dz*dz*(cov(kZ,kZ)+cov2(kZ,kZ))
2528 +2.*dx*dy*(cov(kX,kY)+cov2(kX,kY))
2529 +2.*dx*dz*(cov(kX,kZ)+cov2(kX,kZ))
2530 +2.*dy*dz*(cov(kY,kZ)+cov2(kY,kZ)))
2531 /(doca*doca);
2532 }
2533 break;
2534 }
2535 // New point on line
2536 if (move_along_line){
2537 point+=(step_size/pscale)*dir;
2538 s+=step_size;
2539 }
2540
2541 // Propagate the covariance matrix of the track along the trajectory
2542 this->PropagateCovariance(this->swim_steps[i+1].s-swim_step->s,q,mass_sq,swim_step->mom,swim_step->origin,swim_step->B,cov);
2543
2544 // Store the current position and doca
2545 oldpos=pos;
2546 oldmom=swim_step->mom;
2547 tflight=swim_step->t;
2548 doca=new_doca;
2549 }
2550
2551 // "Vertex" is mid-point of line connecting the positions of closest
2552 // approach of the two tracks
2553 commonpos = 0.5*(oldpos + point);
2554
2555 return NOERROR;
2556}
2557
2558// Find the position along a reference trajectory closest to a given point.
2559// The error matrix for the point can also be input via a pointer. The error
2560// matrix is expected to be 7x7, with the order {Px,Py,Pz,X,Y,Z,T}.
2561// Outputs the kinematic data object (including the covariance) at this
2562// position,and the doca and the variance on the doca.
2563jerror_t DReferenceTrajectory::FindPOCAtoPoint(const DVector3 &point,
2564 const DMatrixDSym *covpoint,
2565 DKinematicData *track_kd,
2566 double &doca, double &var_doca) const{
2567 if (track_kd==NULL__null) return RESOURCE_UNAVAILABLE;
2568
2569 DVector3 dir, commonpos;
2570 return FindPOCAtoLine(point,dir,covpoint,track_kd,commonpos,doca,var_doca);
2571}
2572
2573// Find the mid-point of the line connecting the points of closest approach of the
2574// trajectories of two tracks. Return the positions, momenta, and error matrices
2575// at these points for the two tracks.
2576jerror_t DReferenceTrajectory::IntersectTracks(const DReferenceTrajectory *rt2, DKinematicData *track1_kd, DKinematicData *track2_kd, DVector3 &pos, double &doca, double &var_doca) const {
2577 const swim_step_t *swim_step1=this->swim_steps;
2578 const swim_step_t *swim_step2=rt2->swim_steps;
2579
2580 DMatrixDSym cov1(7), cov2(7);
2581
2582 if((track1_kd != NULL__null) && (track2_kd != NULL__null)){
2583 cov1=track1_kd->errorMatrix();
2584 cov2=track2_kd->errorMatrix();
2585 }
2586
2587 double q1=this->q;
2588 double q2=rt2->q;
2589 double mass_sq1=this->mass_sq;
2590 double mass_sq2=rt2->mass_sq;
2591
2592 // Initialize the doca and traverse both particles' trajectories
2593 doca=1000.;
2594 double tflight1=0.,tflight2=0.;
2595 for (int i=0;i<this->Nswim_steps-1&&i<rt2->Nswim_steps-1; i++, swim_step1++, swim_step2++){
2596 DVector3 pos1=swim_step1->origin;
2597 DVector3 pos2=swim_step2->origin;
2598 DVector3 diff=pos1-pos2;
2599 double new_doca=diff.Mag();
2600
2601 if (new_doca>doca){
2602 int prev_i=i-1;
2603 // positions and momenta of tracks at the center of the
2604 // bracketed region
2605 pos1=this->swim_steps[prev_i].origin;
2606 DVector3 mom1=this->swim_steps[prev_i].mom;
2607 pos2=rt2->swim_steps[prev_i].origin;
2608 DVector3 mom2=rt2->swim_steps[prev_i].mom;
2609
2610 // If we break out of the loop immediately, we have not bracketed the
2611 // doca yet...
2612 if (i==1) { // backtrack to find the true doca
2613 tflight1=tflight2=0.;
2614 if((track1_kd != NULL__null) && (track2_kd != NULL__null)){
2615 cov1=track1_kd->errorMatrix();
2616 cov2=track2_kd->errorMatrix();
2617 }
2618 // Initialize the steppers
2619 DMagneticFieldStepper stepper1(this->bfield, q1, &pos1, &mom1);
2620 DMagneticFieldStepper stepper2(this->bfield, q2, &pos2, &mom2);
2621
2622 // Do the backtracking...
2623 int inew=0;
2624 DVector3 oldpos1=pos1;
2625 DVector3 oldpos2=pos2;
2626 while (inew<20){
2627 if (pos1.z()<0. || pos2.z()<0. || pos1.z()>400. || pos2.z()>400.
2628 || pos1.Perp()>65. || pos2.Perp()>65.){
2629 break;
2630 }
2631 DVector3 B1,B2;
2632 double ds1=stepper1.Step(&pos1,&B1,-0.5);
2633 double ds2=stepper2.Step(&pos2,&B2,-0.5);
2634
2635 // Compute the revised estimate for the doca
2636 diff=pos1-pos2;
2637 new_doca=diff.Mag();
2638
2639 if(new_doca > doca){
2640 pos1=oldpos1;
2641 pos2=oldpos2;
2642 break;
2643 }
2644
2645 // Propagate the covariance matrices along the trajectories
2646 if((track1_kd != NULL__null) && (track2_kd != NULL__null)){
2647 this->PropagateCovariance(ds1,q1,mass_sq1,mom1,oldpos1,B1,cov1);
2648 rt2->PropagateCovariance(ds2,q2,mass_sq2,mom2,oldpos2,B2,cov2);
2649 }
2650
2651 // Store the current positions, doca and adjust flight times
2652 oldpos1=pos1;
2653 oldpos2=pos2;
2654 doca=new_doca;
2655
2656 double one_over_p1_sq=1./mom1.Mag2();
2657 tflight1+=ds1*sqrt(1.+mass_sq1*one_over_p1_sq)/SPEED_OF_LIGHT29.9792;
2658
2659 double one_over_p2_sq=1./mom2.Mag2();
2660 tflight2+=ds2*sqrt(1.+mass_sq2*one_over_p2_sq)/SPEED_OF_LIGHT29.9792;
2661
2662 // New momenta
2663 stepper1.GetMomentum(mom1);
2664 stepper2.GetMomentum(mom2);
2665 }
2666 }
2667
2668 // Use Brent's algorithm to find a better approximation for
2669 // the poca of the two tracks
2670 double ds=0.5;
2671 BrentsAlgorithm(pos1,mom1,pos2,mom2,ds,q2,doca);
2672
2673 // "Vertex" is mid-point of line connecting the positions of closest
2674 // approach of the two tracks
2675 pos=0.5*(pos1+pos2);
2676
2677 if((track1_kd != NULL__null) && (track2_kd != NULL__null)){
2678 // Adjust flight times
2679 double one_over_p1_sq=1./mom1.Mag2();
2680 tflight1+=ds*sqrt(1.+mass_sq1*one_over_p1_sq)/SPEED_OF_LIGHT29.9792;
2681
2682 double one_over_p2_sq=1./mom2.Mag2();
2683 tflight2+=ds*sqrt(1.+mass_sq2*one_over_p2_sq)/SPEED_OF_LIGHT29.9792;
2684
2685 track1_kd->setErrorMatrix(cov1);
2686 track1_kd->setMomentum(mom1);
2687 track1_kd->setPosition(pos1);
2688 track1_kd->setTime(track1_kd->time() + tflight1);
2689
2690 track2_kd->setErrorMatrix(cov2);
2691 track2_kd->setMomentum(mom2);
2692 track2_kd->setPosition(pos2);
2693 track2_kd->setTime(track2_kd->time() + tflight2);
2694
2695 // Compute the variance on the doca
2696 diff=pos1-pos2;
2697 double dx=diff.x();
2698 double dy=diff.y();
2699 double dz=diff.z();
2700 var_doca=(dx*dx*(cov1(kX,kX)+cov2(kX,kX))
2701 +dy*dy*(cov1(kY,kY)+cov2(kY,kY))
2702 +dz*dz*(cov1(kZ,kZ)+cov2(kZ,kZ))
2703 +2.*dx*dy*(cov1(kX,kY)+cov2(kX,kY))
2704 +2.*dx*dz*(cov1(kX,kZ)+cov2(kX,kZ))
2705 +2.*dy*dz*(cov1(kY,kZ)+cov2(kY,kZ)))
2706 /(doca*doca);
2707 }
2708 break;
2709 }
2710
2711 // Propagate the covariance matrices along the trajectories
2712 if((track1_kd != NULL__null) && (track2_kd != NULL__null)){
2713 this->PropagateCovariance(this->swim_steps[i+1].s-swim_step1->s,q1,mass_sq1,swim_step1->mom,swim_step1->origin,swim_step1->B,cov1);
2714 rt2->PropagateCovariance(rt2->swim_steps[i+1].s-swim_step2->s,q2,mass_sq2,swim_step2->mom,swim_step2->origin,swim_step2->B,cov2);
2715 }
2716
2717 // Store the current positions and doca
2718 tflight1=swim_step1->t;
2719 tflight2=swim_step2->t;
2720 doca=new_doca;
2721 }
2722
2723 return NOERROR;
2724}
2725
2726
2727// Routine for finding the minimum of a function bracketed between two values
2728// (see Numerical Recipes in C, pp. 404-405).
2729#define ITMAX20 20
2730#define CGOLD0.3819660 0.3819660
2731#define EPS21.e-4 1.e-4
2732#define ZEPS1.0e-10 1.0e-10
2733#define SHFT(a,b,c,d)(a)=(b);(b)=(c);(c)=(d); (a)=(b);(b)=(c);(c)=(d);
2734#define SIGN(a,b)((b)>=0.0?fabs(a):-fabs(a)) ((b)>=0.0?fabs(a):-fabs(a))
2735jerror_t DReferenceTrajectory::BrentsAlgorithm(DVector3 &pos1,DVector3 &mom1,
2736 DVector3 &pos2,DVector3 &mom2,
2737 double ds,double q2,
2738 double &doca) const{
2739 double d=0.,u=0.;
2740 double e=0.0; // will be distance moved on step before last
2741 double ax=0.;
2742 double bx=-ds;
2743 double cx=-2.*ds;
2744
2745 double a=(ax<cx?ax:cx);
2746 double b=(ax>cx?ax:cx);
2747 double x=bx,w=bx,v=bx;
2748
2749 // initialization
2750 double fw=doca;
2751 double fv=fw;
2752 double fx=fw;
2753 double u_old=x;
2754 DMagneticFieldStepper stepper1(this->bfield, this->q, &pos1, &mom1);
2755 DMagneticFieldStepper stepper2(this->bfield, q2, &pos2, &mom2);
2756
2757 // main loop
2758 for (unsigned int iter=1;iter<=ITMAX20;iter++){
2759 double xm=0.5*(a+b);
2760 double tol1=EPS21.e-4*fabs(x)+ZEPS1.0e-10;
2761 double tol2=2.0*tol1;
2762 if (fabs(x-xm)<=(tol2-0.5*(b-a))){
2763 doca=(pos1-pos2).Mag();
2764 ds=cx-x;
2765
2766 // New momenta
2767 stepper1.GetMomentum(mom1);
2768 stepper2.GetMomentum(mom2);
2769
2770 return NOERROR;
2771 }
2772 // trial parabolic fit
2773 if (fabs(e)>tol1){
2774 double x_minus_w=x-w;
2775 double x_minus_v=x-v;
2776 double r=x_minus_w*(fx-fv);
2777 double q=x_minus_v*(fx-fw);
2778 double p=x_minus_v*q-x_minus_w*r;
2779 q=2.0*(q-r);
2780 if (q>0.0) p=-p;
2781 q=fabs(q);
2782 double etemp=e;
2783 e=d;
2784 if (fabs(p)>=fabs(0.5*q*etemp) || p<=q*(a-x) || p>=q*(b-x))
2785 // fall back on the Golden Section technique
2786 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
2787 else{
2788 // parabolic step
2789 d=p/q;
2790 u=x+d;
2791 if (u-a<tol2 || b-u <tol2)
2792 d=SIGN(tol1,xm-x)((xm-x)>=0.0?fabs(tol1):-fabs(tol1));
2793 }
2794 } else{
2795 d=CGOLD0.3819660*(e=(x>=xm?a-x:b-x));
2796 }
2797 u=(fabs(d)>=tol1 ? x+d: x+SIGN(tol1,d)((d)>=0.0?fabs(tol1):-fabs(tol1)));
2798
2799 // Function evaluation
2800 double du=u_old-u;
2801 stepper1.Step(&pos1,NULL__null,du);
2802 stepper2.Step(&pos2,NULL__null,du);
2803 DVector3 diff=pos1-pos2;
2804 double fu=diff.Mag();
2805 u_old=u;
2806
2807 if (fu<=fx){
2808 if (u>=x) a=x; else b=x;
2809 SHFT(v,w,x,u)(v)=(w);(w)=(x);(x)=(u);;
2810 SHFT(fv,fw,fx,fu)(fv)=(fw);(fw)=(fx);(fx)=(fu);;
2811 }
2812 else {
2813 if (u<x) a=u; else b=u;
2814 if (fu<=fw || w==x){
2815 v=w;
2816 w=u;
2817 fv=fw;
2818 fw=fu;
2819 }
2820 else if (fu<=fv || v==x || v==w){
2821 v=u;
2822 fv=fu;
2823 }
2824 }
2825 }
2826
2827 // We only get here if there is a convergence issue...
2828 ds=cx-x;
2829 doca=(pos1-pos2).Mag();
2830 stepper1.GetMomentum(mom1);
2831 stepper2.GetMomentum(mom2);
2832
2833 return NOERROR;
2834}
2835
2836