| File: | libraries/TRACKING/DReferenceTrajectory.cc |
| Location: | line 2764, column 7 |
| Description: | Value stored to 'ds' is never read |
| 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> |
| 13 | using 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 | |
| 26 | struct StepStruct {DReferenceTrajectory::swim_step_t steps[256];}; |
| 27 | |
| 28 | //--------------------------------- |
| 29 | // DReferenceTrajectory (Constructor) |
| 30 | //--------------------------------- |
| 31 | DReferenceTrajectory::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 | //--------------------------------- |
| 92 | DReferenceTrajectory::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 | //--------------------------------- |
| 138 | DReferenceTrajectory& 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 | //--------------------------------- |
| 200 | DReferenceTrajectory::~DReferenceTrajectory() |
| 201 | { |
| 202 | if(own_swim_steps){ |
| 203 | delete[] swim_steps; |
| 204 | } |
| 205 | } |
| 206 | |
| 207 | //--------------------------------- |
| 208 | // CopyWithShift |
| 209 | //--------------------------------- |
| 210 | void 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 | //--------------------------------- |
| 223 | void 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 | //--------------------------------- |
| 243 | void 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. |
| 305 | void 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 | //--------------------------------- |
| 492 | void 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. |
| 774 | jerror_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 | //--------------------------------- |
| 866 | jerror_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 | } |
| 870 | jerror_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 | //--------------------------------- |
| 1049 | int 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 | //--------------------------------- |
| 1144 | double 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 | //--------------------------------- |
| 1174 | double 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 | //--------------------------------- |
| 1418 | DReferenceTrajectory::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 | //--------------------------------- |
| 1508 | DReferenceTrajectory::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 | //--------------------------------- |
| 1560 | DReferenceTrajectory::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 | //--------------------------------- |
| 1639 | double 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 | //--------------------------------- |
| 1653 | double 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 | //------------------ |
| 1667 | double 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){ |
| 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)){ |
| 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){ |
| 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); |
| 2019 | if(debug_level>3){ |
| 2020 | _DBG_std::cerr<<"libraries/TRACKING/DReferenceTrajectory.cc" <<":"<<2020<<" "<<"distance to rt: "<< step->s + (phi>0.0 ? ds:-ds) <<" from step at "<<step->s<<" with ds="<<ds<<" d="<<d<<" dz="<<dz<<" Rodphi="<<Rodphi<<endl; |
| 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 | //------------------ |
| 2035 | double 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 | //------------------ |
| 2095 | double 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 | //------------------ |
| 2172 | void 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 | //------------------ |
| 2209 | DVector3 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 | //------------------ |
| 2240 | double 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 | //------------------ |
| 2253 | double 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 | //------------------ |
| 2327 | void 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 |
| 2351 | jerror_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. |
| 2417 | jerror_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. |
| 2563 | jerror_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. |
| 2576 | jerror_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)) |
| 2735 | jerror_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; |
Value stored to 'ds' is never read | |
| 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 | doca=(pos1-pos2).Mag(); |
| 2829 | stepper1.GetMomentum(mom1); |
| 2830 | stepper2.GetMomentum(mom2); |
| 2831 | |
| 2832 | return NOERROR; |
| 2833 | } |
| 2834 | |
| 2835 |