1 package org.lcsim.recon.tracking.trfdca;
2
3
4 import org.lcsim.recon.tracking.trfutil.Assert;
5 import org.lcsim.recon.tracking.trfutil.TRFMath;
6 import org.lcsim.recon.tracking.trfbase.PropDirected;
7 import org.lcsim.recon.tracking.trfbase.PropStat;
8 import org.lcsim.recon.tracking.trfbase.Surface;
9 import org.lcsim.recon.tracking.trfbase.VTrack;
10 import org.lcsim.recon.tracking.trfbase.TrackDerivative;
11 import org.lcsim.recon.tracking.trfbase.TrackVector;
12
13 import org.lcsim.recon.tracking.trfcyl.SurfCylinder;
14
15 import org.lcsim.recon.tracking.trfbase.Propagator;
16 import org.lcsim.recon.tracking.trfbase.PropDirected;
17 import org.lcsim.recon.tracking.trfbase.PropDir;
18 /**
19 * Propagates tracks from a DCA surface to a Cylinder.
20 *<p>
21 * Propagation will fail if either the origin is not a DCA surface,
22 * or the destination is not a Cylinder.
23 *<p>
24 * The default direction for propagation is forward.
25 *<p>
26 * Propagation to a cylinder at the radius of the DCA will succeed
27 * and the track parameters are valid but the errors are not valid.
28 *
29 *
30 *@author Norman A. Graf
31 *@version 1.0
32 *
33 */
34 public class PropDCACyl extends PropDirected
35 {
36
37 // static variables
38 // Assign track parameter indices
39
40
41 public static final int IRSIGNED = SurfDCA.IRSIGNED;
42 public static final int IZ_DCA = SurfDCA.IZ;
43 public static final int IPHID = SurfDCA.IPHID;
44 public static final int ITLM_DCA = SurfDCA.ITLM;
45 public static final int IQPT_DCA = SurfDCA.IQPT;
46
47 public static final int IPHI = SurfCylinder.IPHI;
48 public static final int IZ_CYL = SurfCylinder.IZ;
49 public static final int IALF = SurfCylinder.IALF;
50 public static final int ITLM_CYL = SurfCylinder.ITLM;
51 public static final int IQPT_CYL = SurfCylinder.IQPT;
52
53 // Attributes ***************************************************
54
55
56
57 // bfield * BFAC
58 private double _bfac;
59
60
61 // Methods ******************************************************
62
63 // static methods
64
65 /**
66 *Return a String representation of the class' type name.
67 *Included for completeness with the C++ version.
68 *
69 * @return A String representation of the class' type name.
70 */
71 public static String typeName()
72 { return "PropDCACyl";
73 }
74
75 /**
76 *Return a String representation of the class' type name.
77 *Included for completeness with the C++ version.
78 *
79 * @return A String representation of the class' type name.
80 */
81 public static String staticType()
82 { return typeName();
83 }
84
85
86
87 /**
88 *Construct an instance from a constant solenoidal magnetic field in Tesla.
89 *
90 * @param bfield The magnetic field strength in Tesla.
91 */
92 public PropDCACyl(double bfield)
93 {
94 super(PropDir.FORWARD);
95 _bfac=bfield * TRFMath.BFAC;
96 }
97
98 /**
99 *Clone an instance.
100 *
101 * @return A Clone of this instance.
102 */
103 public Propagator newPropagator()
104 {
105 return new PropDCACyl( bField() );
106 }
107
108 /**
109 *Return a String representation of the class' type name.
110 *Included for completeness with the C++ version.
111 *
112 * @return A String representation of the class' type name.
113 */
114 public String type()
115 { return staticType();
116 }
117
118 /**
119 *Propagate a track without error in the specified direction.
120 *and return the derivative matrix in deriv.
121 *
122 * The track parameters for a cylinder are:
123 * phi z alpha tan(lambda) curvature
124 *
125 * @param trv The VTrack to propagate.
126 * @param srf The Surface to which to propagate.
127 * @param dir The direction in which to propagate.
128 * @param deriv The track derivatives to update at the surface srf.
129 * @return The propagation status.
130 */
131 public PropStat vecDirProp( VTrack trv, Surface srf,
132 PropDir dir, TrackDerivative deriv )
133 {
134 return dcaCylPropagate( _bfac, trv, srf, dir, deriv );
135 }
136
137 /**
138 *Propagate a track without error in the specified direction.
139 *
140 * The track parameters for a cylinder are:
141 * phi z alpha tan(lambda) curvature
142 *
143 * @param trv The VTrack to propagate.
144 * @param srf The Surface to which to propagate.
145 * @param dir The direction in which to propagate.
146 * @return The propagation status.
147 */
148 public PropStat vecDirProp( VTrack trv, Surface srf,
149 PropDir dir )
150 {
151 TrackDerivative deriv =null;
152 return vecDirProp(trv, srf, dir, deriv);
153
154 }
155
156 // propagate a track with error in the specified direction
157 // PropStat err_dir_prop( ETrack& tre, Surface& srf,
158 // PropDir dir ) ;
159
160 //
161
162 /**
163 *Return the strength of the magnetic field in Tesla.
164 *
165 * @return The strength of the magnetic field in Tesla.
166 */
167 public double bField()
168 {
169 return _bfac/TRFMath.BFAC;
170 }
171
172
173 /**
174 *output stream
175 * @return A String representation of this instance.
176 */
177 public String toString()
178 {
179 return "DCA propagation to a Cylinder with constant "
180 + bField() + " Tesla field";
181 }
182
183
184
185 /**
186 * Propagate from dca to cylinder.
187 * Why is this public?
188 *
189 * @param _bfac The numerical factor (including the field)
190 * @param trv The VTrack to propagate.
191 * @param srf The cylindrical surface to which to propagate.
192 * @param dir The direction in which to propagate.
193 * @param deriv The track derivatives to update at the surface srf.
194 * @return The propagation status.
195 */
196 public PropStat dcaCylPropagate( double _bfac,
197 VTrack trv,
198 Surface srf,
199 PropDir dir,
200 TrackDerivative deriv )
201 {
202
203 // construct return status
204 PropStat pstat = new PropStat();
205
206 // fetch the originating Surface and check it is a DCA surface
207 Surface srf0 = trv.surface();
208 Assert.assertTrue( srf0.pureType().equals(SurfDCA.staticType() ));
209 if ( ! srf0.pureType( ).equals(SurfDCA.staticType()) )
210 return pstat;
211 SurfDCA srf_dca = ( SurfDCA ) srf0;
212
213 // Check that dca surface has zero tilt.
214
215 boolean tilted = srf_dca.dXdZ() != 0 || srf_dca.dYdZ() != 0;
216 Assert.assertTrue(!tilted);
217 if(tilted) return pstat;
218
219 // check that the destination surface is a Cylinder
220 Assert.assertTrue( srf.pureType().equals(SurfCylinder.staticType()) );
221 if ( !srf.pureType( ).equals(SurfCylinder.staticType()) )
222 return pstat;
223 SurfCylinder srf_cyl = ( SurfCylinder ) srf;
224 //SurfCylinder srf_cyl = new SurfCylinder( (SurfCylinder) srf);
225
226 // fetch the originating TrackVector
227 TrackVector vec_dca = trv.vector();
228 double r1p = Math.abs(vec_dca.get(IRSIGNED)); // r
229 double z1 = vec_dca.get(IZ_DCA); // z
230 double phid1p= vec_dca.get(IPHID); // phi_direction
231 double tlam1 = vec_dca.get(ITLM_DCA); // tan(lambda)
232 double qpt1 = vec_dca.get(IQPT_DCA); // q/pT
233
234 double xv = srf_dca.x();
235 double yv = srf_dca.y();
236
237 // calculate alf1 and phi1
238 double sign_alf1p;
239 double sign_r1p=0.;
240
241 if ( !TRFMath.isZero( vec_dca.get(IRSIGNED) ) )
242 {
243 sign_alf1p = vec_dca.get(IRSIGNED)/Math.abs(vec_dca.get(IRSIGNED));
244 sign_r1p = sign_alf1p;
245 }
246 else
247 {
248 sign_alf1p = 0.0;
249 }
250
251 if( (!TRFMath.isZero(xv) || !TRFMath.isZero(yv)) && TRFMath.isZero(sign_alf1p) ) sign_alf1p=1;
252
253 double alf1p = sign_alf1p * TRFMath.PI2; // alpha
254 double phi1p = phid1p - alf1p; // phi_position
255 phi1p = TRFMath.fmod2( phi1p, TRFMath.TWOPI );
256
257 // calculate salf1, calf1 and crv1
258 double salf1p=0.;
259 salf1p = alf1p>0. ? 1.:( alf1p < 0. ? -1. : 0.) ;
260
261 double cosphi1p=Math.cos(phi1p);
262 double sinphi1p=Math.sin(phi1p);
263
264 double x1=r1p*cosphi1p+xv;
265 double y1=r1p*sinphi1p+yv;
266
267 double phi1 = Math.atan2(y1,x1); // phi position in (xv,yv) coord system
268 double r1 = Math.sqrt(x1*x1+y1*y1); // r1 in (xv,yv) coord system
269 double alf1 = alf1p + phi1p - phi1; // alf1 in (xv,yv) coord system
270 if( TRFMath.isZero(x1) && TRFMath.isZero(y1) )
271 {
272 phi1 = phid1p;
273 alf1 = 0.;
274 }
275 if( sign_r1p == 0 && !TRFMath.isZero(r1) )
276 sign_r1p=1;
277
278 // calculate salf1, calf1 and crv1
279 double salf1 = Math.sin( alf1 );
280 double calf1 = Math.cos( alf1 );
281
282 if( TRFMath.isZero(alf1) )
283 {
284 salf1=0.;
285 calf1=1.;
286 }
287 if( TRFMath.isEqual(Math.abs(alf1),TRFMath.PI2) )
288 {
289 salf1= alf1 > 0 ? 1. : -1. ;
290 calf1=0.;
291 }
292
293 double crv1 = _bfac * qpt1;
294 double sign_crv = 1.;
295
296 // fetch r2 of the destination Cylinder
297 double r2 = srf_cyl.parameter(SurfCylinder.RADIUS);
298
299 // calculate tlam2, qpt2 and crv2 of the destination Cylinder
300 double tlam2 = tlam1;
301 double qpt2 = qpt1;
302 double crv2 = crv1;
303
304 // calculate salf2 of the destination Cylinder
305 double salf2 = r1/r2*salf1 + 0.5*crv1/r2*(r2*r2-r1*r1);
306
307 // If salf2 is close to 1 or -1, set it to that value.
308 double diff = Math.abs( Math.abs(salf2) - 1.0 );
309 if ( diff < 1.e-10 ) salf2 = salf2>0 ? 1.0 : -1.0;
310 // if salf2 > 1, track does not cross cylinder
311 if ( Math.abs(salf2) > 1.0 ) return pstat;
312
313 // there are two possibilities for alf2
314 double alf21 = Math.asin( salf2 );
315 double alf22 = alf21>0 ? Math.PI-alf21 : -Math.PI-alf21;
316 double calf21 = Math.cos( alf21 );
317 double calf22 = Math.cos( alf22 );
318 //double phi21 = phi1 + atan2( -sign_crv*calf21, r2*crv2-salf2 );
319 //double phi22 = phi1 + atan2( -sign_crv*calf22, r2*crv2-salf2 );
320 // double cnst = salf1-r1*crv1 > 0 ? TRFMath.PI2 : -TRFMath.PI2;
321 // cnst = (r1 == 0.) ? 0. : cnst;
322
323 double cnst = Math.atan2(salf1-r1*crv1,calf1);
324 if( TRFMath.isEqual(Math.abs(alf1),TRFMath.PI2) )
325 {
326 cnst = salf1-r1*crv1 > 0 ? TRFMath.PI2 : -TRFMath.PI2;
327 cnst = (r1==0.) ? 0. : cnst;
328 }
329
330 double phi21 = phi1 + cnst - Math.atan2( salf2-r2*crv2, calf21 );
331 double phi22 = phi1 + cnst - Math.atan2( salf2-r2*crv2, calf22 );
332
333 if( TRFMath.isZero(crv1) )
334 {
335 phi21 = phi1 + cnst - alf21;
336 phi22 = phi1 + cnst - alf22;
337 }
338
339 if ( TRFMath.isZero(calf21) )
340 {
341 phi21 = phi1;
342 phi22 = phi1;
343 }
344
345 // construct an sT object for each solution
346 ST_DCACyl sto1 = new ST_DCACyl(r1,phi1,alf1,crv1,r2,phi21,alf21);
347 ST_DCACyl sto2 = new ST_DCACyl(r1,phi1,alf1,crv1,r2,phi22,alf22);
348 // check the two solutions are nonzero and have opposite sign
349 // or at least one is nonzero
350
351 // choose the correct solution
352 boolean use_first_solution;
353
354 if( dir.equals(PropDir.NEAREST))
355 use_first_solution = Math.abs(sto2.st()) > Math.abs(sto1.st());
356
357 else if( dir.equals(PropDir.FORWARD))
358 use_first_solution = sto1.st() > 0.0;
359
360 else if( dir.equals(PropDir.BACKWARD))
361 use_first_solution = sto1.st() < 0.0;
362
363 else
364 {
365 use_first_solution = false;
366 System.out.println( "PropCyl._vec_propagate: Unknown direction.");
367 System.exit(1);
368 }
369
370 // assign phi2, alf2 and sto2 for the chosen solution
371 double phi2, alf2;
372 ST_DCACyl sto = new ST_DCACyl();
373 double calf2;
374 if ( use_first_solution )
375 {
376 sto = sto1;
377 phi2 = phi21;
378 alf2 = alf21;
379 calf2 = calf21;
380 }
381 else
382 {
383 sto = sto2;
384 phi2 = phi22;
385 alf2 = alf22;
386 calf2 = calf22;
387 }
388
389 // check alpha range
390 Assert.assertTrue( Math.abs(alf2) <= Math.PI );
391
392 // fetch sT
393 double st = sto.st();
394 double s = st*Math.sqrt(1+tlam1*tlam1);
395
396 // calculate z2 of the destination Cylinder
397 double z2 = z1 + st * tlam1;
398
399 // construct the destination TrackVector
400 TrackVector vec_cyl = new TrackVector();
401 /*
402 #ifdef TRF_PHIRANGE
403 vec_cyl(IPHI) = fmod1(phi2, TWOPI);
404 #else
405 vec_cyl(IPHI) = phi2;
406 #endif
407 */
408 vec_cyl.set(IPHI , phi2);
409 vec_cyl.set(IZ_CYL , z2);
410 vec_cyl.set(IALF , alf2);
411 vec_cyl.set(ITLM_CYL , tlam2);
412 vec_cyl.set(IQPT_CYL , qpt2);
413 /*
414 // For axial tracks, zero z and tan(lambda).
415
416 if(trv.is_axial()) {
417 vec_cyl(SurfDCA::IZ) = 0.;
418 vec_cyl(SurfDCA::ITLM) = 0.;
419 }
420 */
421
422 // set the surface of trv to the destination Cylinder
423 trv.setSurface( srf_cyl.newPureSurface() );
424
425 // set the vector of trv to the destination TrackVector (Cyl. coord.)
426 trv.setVector(vec_cyl);
427
428 // set the direction of trv
429 if ( Math.abs(alf2) <= TRFMath.PI2 ) trv.setForward();
430 else trv.setBackward();
431
432 // set the return status
433 pstat.setPathDistance(s);
434
435 // exit now if user did not ask for error matrix.
436 if ( deriv == null ) return pstat;
437
438
439 // calculate derivatives
440
441 //double dalf2_dr1 = (salf1-r1*crv1)/(r2*calf2);
442 //double dalf2_dr1 = (1.0-r1*crv1*salf1)/(r2*calf2);
443 // commented out by SK ( DCA to DCA(xv,yv) change)
444 double salf12 = sign_r1p*salf1;
445 if( sign_r1p == 0 && salf1 == 0. )
446 salf12 = 1;
447 double dalf2_dr1_sign = (salf12-crv1*r1*sign_r1p)/r2/calf2;
448 double dalf2_dr1 = (salf1-crv1*r1)/r2/calf2;
449 double dalf2_dalf1_or = 1/r2*calf1/calf2; // over r1
450 double dalf2_dalf1 = r1*dalf2_dalf1_or;
451 double dalf2_dcrv1 = (r2*r2-r1*r1)/(2.0*r2*calf2);
452
453 //double dphi2_dr1 = -sign_crv*
454 // (r2*crv2*salf2-1.0)*(r1*crv1-salf1)/
455 // (r2*calf2*(1.0 + r2*r2*crv2*crv2 - 2.0*r2*crv2*salf2));
456 // commented out on 10.16.2001 by SK ( DCA to DCA(xv,yv) change)
457 // double dphi2_dr1 = -sign_crv *
458 // (r2*crv2*salf2-1.0)*(sign_alf1*r1*crv1-1.)/
459 // (r2*calf2*(1.0 + r2*r2*crv2*crv2 - 2.0*r2*crv2*salf2));
460 //
461 // double dphi2_dcrv1 = -sign_crv*
462 // ((1.0-r2*crv2*salf2)*(r2*r2-r1*r1)-2.0*r2*r2*calf2*calf2)/
463 // (2.0*r2*calf2*(1.0 + r2*r2*crv2*crv2 - 2.0*r2*crv2*salf2));
464
465 double dphi2_dphi1=1.;
466 double dphi2_dr1_sign = -crv1*calf1*sign_r1p/(1.-2.*r1*crv1*salf1+r1*r1*crv1*crv1)
467 - dalf2_dr1_sign*(1-salf2*crv2*r2)/(1.-2.*r2*crv2*salf2+r2*r2*crv2*crv2);
468
469 double dphi2_dr1 = -crv1*calf1/(1.-2.*r1*crv1*salf1+r1*r1*crv1*crv1)
470 - dalf2_dr1*(1-salf2*crv2*r2)/(1.-2.*r2*crv2*salf2+r2*r2*crv2*crv2);
471 double dphi2_dalf1_m1_or = (-salf1*crv1-r1*crv1*crv1+2.*crv1*salf1)/(1.-2.*r1*crv1*salf1+r1*r1*crv1*crv1)
472 - dalf2_dalf1_or*(1-salf2*crv2*r2)/(1.-2.*r2*crv2*salf2+r2*r2*crv2*crv2); // minus 1
473 double dphi2_dalf1 = dphi2_dalf1_m1_or*r1 + 1;
474 double dphi2_dcrv1 = -r1*calf1/(1.-2.*r1*crv1*salf1+r1*r1*crv1*crv1)
475 -(dalf2_dcrv1*(1.-r2*crv2*salf2) - r2*calf2)/(1.-2.*r2*crv2*salf2+r2*r2*crv2*crv2);
476
477
478 //double dst_dr1 = sto.d_st_dr1(dphi2_dr1, dalf2_dr1 );
479 // double dst_dr1 = sto.d_st_dr1(sign_alf1*dphi2_dr1,sign_alf1*dalf2_dr1 );
480 // double dst_crv1 = sto.d_st_dcrv1(dphi2_dcrv1, dalf2_dcrv1);
481 double dst_dr1_sign = sto.d_st_dr1_sign(sign_r1p,dphi2_dr1_sign,dalf2_dr1_sign);
482 double dst_dr1 = sto.d_st_dr1(dphi2_dr1,dalf2_dr1);
483 double dst_dcrv1 = sto.d_st_dcrv1(dphi2_dcrv1, dalf2_dcrv1);
484 double dst_dalf1_or = sto.d_st_dalf1_or(r1,dphi2_dalf1_m1_or, dalf2_dalf1_or);
485
486
487 // build the derivative matrix
488 // derivatives to prime coordinates
489
490 double dphi1_dphi1p_tr = r1p*Math.cos(phi1p-phi1); // times r1
491 double dphi1_dr1p_tr = sign_r1p*Math.sin(phi1p-phi1); // times r1
492
493 double dalf1_dphi1p_tr = r1 - dphi1_dphi1p_tr;
494 double dalf1_dr1p_tr = -dphi1_dr1p_tr;
495
496 double dr1_dr1p_sign = Math.cos(phi1p-phi1);
497 double dr1_dphi1p = r1p*Math.sin(phi1-phi1p);
498
499
500 double dphi2_dphi1p = dphi2_dr1*dr1_dphi1p +dphi2_dalf1 - dphi1_dphi1p_tr*dphi2_dalf1_m1_or;
501 double dphi2_dr1p = dphi2_dr1_sign*dr1_dr1p_sign - dphi2_dalf1_m1_or*dphi1_dr1p_tr;
502
503 double dalf2_dphi1p = dalf2_dr1*dr1_dphi1p + dalf2_dalf1_or*dalf1_dphi1p_tr;
504 double dalf2_dr1p = dalf2_dr1_sign*dr1_dr1p_sign + dalf2_dalf1_or*dalf1_dr1p_tr;
505
506 double dst_dphi1p = dst_dr1*dr1_dphi1p + dst_dalf1_or*dalf1_dphi1p_tr;
507 double dst_dr1p = dst_dr1_sign*dr1_dr1p_sign + dst_dalf1_or*dalf1_dr1p_tr;
508
509 //deriv(IPHI, IRSIGNED, sign_alf1 * dphi2_dr1);
510 deriv.set(IPHI, IRSIGNED, dphi2_dr1p);
511 deriv.set(IPHI, IZ_DCA, 0.0);
512 deriv.set(IPHI, IPHID, dphi2_dphi1p);
513 deriv.set(IPHI, ITLM_DCA, 0.0);
514 deriv.set(IPHI, IQPT_DCA, _bfac * dphi2_dcrv1);
515
516 deriv.set(IZ_CYL, IRSIGNED, tlam1 * dst_dr1p);
517 deriv.set(IZ_CYL, IZ_DCA, 1.0);
518 deriv.set(IZ_CYL, IPHID, tlam1 * dst_dphi1p);
519 deriv.set(IZ_CYL, ITLM_DCA, st);
520 deriv.set(IZ_CYL, IQPT_DCA, tlam1 * _bfac * dst_dcrv1);
521
522 //deriv.set(IALF, IRSIGNED, sign_alf1 * dalf2_dr1);
523 deriv.set(IALF, IRSIGNED, dalf2_dr1p);
524 deriv.set(IALF, IZ_DCA, 0.0);
525 deriv.set(IALF, IPHID, dalf2_dphi1p);
526 deriv.set(IALF, ITLM_DCA, 0.0);
527 deriv.set(IALF, IQPT_DCA, _bfac * dalf2_dcrv1);
528
529 deriv.set(ITLM_CYL, IRSIGNED, 0.0);
530 deriv.set(ITLM_CYL, IZ_DCA, 0.0);
531 deriv.set(ITLM_CYL, IPHID, 0.0);
532 deriv.set(ITLM_CYL, ITLM_DCA, 1.0);
533 deriv.set(ITLM_CYL, IQPT_DCA, 0.0);
534
535 deriv.set(IQPT_CYL, IRSIGNED, 0.0);
536 deriv.set(IQPT_CYL, IZ_DCA, 0.0);
537 deriv.set(IQPT_CYL, IPHID, 0.0);
538 deriv.set(IQPT_CYL, ITLM_DCA, 0.0);
539 deriv.set(IQPT_CYL, IQPT_DCA, 1.0);
540
541
542 /*
543 // For axial tracks, zero all derivatives of or with respect to z or
544 // tan(lambda), that are not already zero. This will force the error
545 // matrix to have zero errors for z and tan(lambda).
546
547 if(trv.is_axial()) {
548 deriv.set(IZ_CYL, IRSIGNED, 0.);
549 deriv.set(IZ_CYL, IZ_DCA, 0.);
550 deriv.set(IZ_CYL, IPHID, 0.);
551 deriv.set(IZ_CYL, ITLM_DCA, 0.);
552 deriv.set(IZ_CYL, IQPT_DCA, 0.);
553
554 deriv.set(ITLM_CYL, ITLM_DCA, 0.);
555 }
556 */
557 return pstat;
558
559 }
560
561
562 // Private class STCalc.
563 //
564 // An STCalc_ object calculates sT (the signed transverse path length)
565 // and its derivatives w.r.t. alf1 and crv1. It is constructed from
566 // the starting (r1, phi1, alf1, crv1) and final track parameters
567 // (r2, phi2, alf2) assuming these are consistent. Methods are
568 // provided to retrieve sT and the two derivatives.
569
570 class ST_DCACyl
571 {
572
573 private boolean _big_crv;
574 private double _st;
575 // double _dst_dalf2;
576 private double _dst_dr1;
577 private double _dst_dcrv1;
578 private double _dst_dphi2;
579 double _dst_dphi2_or;
580 private double _crv1; // was public? need to look into this
581 // constructor
582 public ST_DCACyl()
583 {
584 }
585 public ST_DCACyl(double r1, double phi1, double alf1, double crv1,
586 double r2, double phi2, double alf2)
587 {
588
589 _crv1 = crv1;
590 Assert.assertTrue( r1 >= 0.0 );
591 Assert.assertTrue( r2 >= 0.0 );
592 double rmax = r1+r2;
593
594 // Calculate the change in xy direction
595 double phi_dir_diff = TRFMath.fmod2(phi2+alf2-phi1-alf1,TRFMath.TWOPI);
596 Assert.assertTrue( Math.abs(phi_dir_diff) <= Math.PI );
597
598 // Evaluate whether |C| is "big"
599 _big_crv = rmax*Math.abs(crv1) > 0.001;
600
601 // If the curvature is big we can use
602 // sT = (phi_dir2 - phi_dir1)/crv1
603 if ( _big_crv )
604 {
605 Assert.assertTrue( crv1 != 0.0 );
606 _st = phi_dir_diff/crv1;
607 }
608
609 // Otherwise, we calculate the straight-line distance
610 // between the points and use an approximate correction
611 // for the (small) curvature.
612 else
613 {
614
615 // evaluate the distance
616 double d = Math.sqrt( r1*r1 + r2*r2 - 2.0*r1*r2*Math.cos(phi2-phi1) );
617 double arg = 0.5*d*crv1;
618 double arg2 = arg*arg;
619 double st_minus_d = d*arg2*( 1.0/6.0 + 3.0/40.0*arg2 );
620 _st = d + st_minus_d;
621
622 // evaluate the sign
623 // We define a metric xsign = abs( (dphid-d*C)/(d*C) ).
624 // Because sT*C = dphid and d = abs(sT):
625 // xsign = 0 for sT > 0
626 // xsign = 2 for sT < 0
627 // Numerical roundoff will smear these predictions.
628 double sign = 0.0;
629 if ( crv1*_st != 0. )
630 {
631 double xsign = Math.abs( (phi_dir_diff - _st*crv1) / (_st*crv1) );
632 if ( xsign < 0.5 ) sign = 1.0;
633 if ( xsign > 1.5 && xsign < 3.0 ) sign = -1.0;
634 }
635 // If the above is indeterminate, assume zero curvature.
636 // In this case abs(alpha) decreases monotonically
637 // with sT. Track passing through origin has alpha = 0 on one
638 // side and alpha = +/-pi on the other. If both points are on
639 // the same side, we use dr/ds > 0 for |alpha|<pi/2.
640 if ( sign == 0. )
641 {
642 sign = 1.0;
643 if ( Math.abs(alf2) > Math.abs(alf1) ) sign = -1.0;
644 if ( Math.abs(alf2) == Math.abs(alf1) )
645 {
646 if ( Math.abs(alf2) < TRFMath.PI2 )
647 {
648 if ( r2 < r1 ) sign = -1.0;
649 }
650 else
651 {
652 if ( r2 > r1 ) sign = -1.0;
653 }
654 }
655 }
656
657 // Correct _st using the above sign.
658 Assert.assertTrue( Math.abs(sign) == 1.0 );
659 _st = sign*_st;
660
661 // save derivatives
662 // _dst_dalf2 = (_st/d)*2.0*(r1-r2*cos(phi2-phi1));
663 //_dst_dphi2 = (_st/d)*2.0*r1*r2*sin(phi2-phi1);
664 // _dst_dcrv1 = sign*d*d*arg*( 1.0/6.0 + 3.0/20.0*arg2);
665 // double root = (1.0 + 0.5*arg*arg + 3.0/8.0*arg*arg*arg*arg );
666 // _dst_dphi2 = sign*(r1*r2*Math.sin(phi2-phi1))*root/d;
667 // _dst_dr1 = sign*(r1-r2*Math.cos(phi2-phi1))*root/d;
668
669 if ( TRFMath.isZero(d) )
670 {
671 _dst_dcrv1 = 0.0;
672 _dst_dphi2 = sign*r1;
673 _dst_dphi2_or = sign;
674 _dst_dr1 = 0.0;
675 }
676 else
677 {
678 _dst_dcrv1 = sign*d*d*arg*( 1.0/6.0 + 3.0/20.0*arg2);
679 double root = (1.0 + 0.5*arg*arg + 3.0/8.0*arg*arg*arg*arg );
680 _dst_dphi2_or = sign*(r2*Math.sin(phi2-phi1))*root/d;
681 _dst_dphi2 = _dst_dphi2_or*r1;
682 _dst_dr1 = sign*(r1-r2*Math.cos(phi2-phi1))*root/d;
683 }
684 }
685
686 }
687 public double st()
688 {
689 return _st;
690 }
691
692 public double d_st_dr1_sign(double sign_alf1,double dphi2_dr1_sign,double dalf2_dr1_sign)
693 {
694 if ( _big_crv ) return ( dphi2_dr1_sign + dalf2_dr1_sign ) / _crv1;
695 else return _dst_dphi2 * dphi2_dr1_sign + _dst_dr1*sign_alf1;
696 }
697
698 public double d_st_dr1( double d_phi2_dr1, double d_alf2_dr1 )
699 {
700 if ( _big_crv ) return ( d_phi2_dr1 + d_alf2_dr1 ) / _crv1;
701 else return _dst_dphi2 * d_phi2_dr1 + _dst_dr1;
702 }
703 public double d_st_dcrv1( double d_phi2_dcrv1, double d_alf2_dcrv1 )
704 {
705 if ( _big_crv ) return ( d_phi2_dcrv1 + d_alf2_dcrv1 - _st ) / _crv1;
706 else return _dst_dphi2 * d_phi2_dcrv1 + _dst_dcrv1;
707 }
708
709 public double d_st_dalf1_or(double r1,double dphi2_dalf1_m1_or, double dalf2_dalf1_or)
710 {
711 if ( _big_crv ) return ( dphi2_dalf1_m1_or + dalf2_dalf1_or) / _crv1;
712 else return _dst_dphi2_or * (dphi2_dalf1_m1_or*r1+1);
713 }
714
715 }
716
717 }
718
719