trf++/src/trfcyl
diff -N PropCylRK.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ PropCylRK.cpp 29 Jul 2011 00:07:56 -0000 1.1
@@ -0,0 +1,929 @@
+// PropCylRK.cpp
+
+#include "trfcyl/PropCylRK.h"
+#include <iostream>
+#include <cassert>
+#include <fstream>
+#include <stdexcept>
+#include "objstream/ObjData.hpp"
+#include "objstream/ObjTable.hpp"
+#include "trfutil/trfstream.h"
+#include "trfbase/PropStat.h"
+#include "trfutil/TRFMath.h"
+#include "trfcyl/SurfCylinder.h"
+#include "trfbase/VTrack.h"
+#include "trfbase/ETrack.h"
+#include "trfcyl/SurfCylinderParameters.h"
+#include "mag_field/ConstantMagneticField.hpp"
+#include "mag_field/MCField.hpp"
+//#include "mag_field/ToscaField.hpp"
+#include "static_la/array.h"
+//#include "trfbase/MagFieldCacher.h"
+
+using std::cerr;
+using std::endl;
+using std::ifstream;
+using std::runtime_error;
+//using mgf::MagFieldPtr;
+
+#ifndef DEFECT_CMATH_NOT_STD
+using std::sqrt;
+using std::cos;
+using std::sin;
+using std::atan2;
+using std::asin;
+using std::abs;
+using std::max;
+using std::pow;
+#endif
+
+using trf::PI;
+using trf::TWOPI;
+using trf::PI2;
+using trf::BFAC;
+using trf::fmod1;
+using trf::fmod2;
+using trf::Surface;
+using trf::TrackVector;
+using trf::TrackDerivative;
+using trf::VTrack;
+using trf::ETrack;
+using trf::PropStat;
+using trf::Propagator;
+using trf::SurfCylinder;
+using trf::PropCylField;
+using trf::PropCylRK;
+//using trf::MagFieldCacher;
+using static_la::array;
+
+//**********************************************************************
+// Free functions.
+//**********************************************************************
+
+namespace {
+
+ typedef array<double,25> TrackPar;
+
+ // Creator.
+
+ ObjPtr create(const ObjData& data) {
+ assert( data.get_object_type() == "PropCylRK" );
+ string type;
+ if(data.has("type"))
+ type = data.get_string("type");
+ double precision=1.e-7;
+ if(data.has("precision"))
+ precision=data.get_double("precision");
+ double derivprec=precision;
+ if(data.has("derivprec"))
+ derivprec=data.get_double("derivprec");
+ double scale1 = 1.;
+ if(data.has("scale1"))
+ scale1 = data.get_double("scale1");
+ double scale2 = 1.;
+ if(data.has("scale2"))
+ scale1 = data.get_double("scale2");
+ Ptr<PropCylRK,LocalSharedDeletePolicy>
+ pobj(new PropCylRK(type, precision, scale1, scale2, derivprec));
+ pobj->set_direction(data);
+ return pobj;
+ }
+
+ // Equations of motion in terms of track parameters phi, z, alpha,
+ // tan(lambda), curvature (q/p), and path length and the
+ // magnetic field.
+
+ TrackPar motion(const TrackPar& par, double r, double crv,
+ const AbstractMagneticField* bfield, double& calf) {
+
+ TrackPar result(par.get_size());
+ if(r == 0.) {
+ calf = 0.;
+ return result;
+ }
+ bool deriv = par.get_size() > 5;
+
+ // Extract track parameters.
+
+ double phi = par(0);
+ double z = par(1);
+ double alf = par(2);
+ double tlm = par(3);
+ double s = par(4);
+
+ // Check if angles are going crazy.
+
+ if(abs(tlm) > 1.e10 || abs(alf) > 1.e10) {
+ calf = 0.;
+ return result;
+ }
+
+ // Declare derivative matrix.
+
+ double dphidphi0;
+ double dphidz0;
+ double dphidalf0;
+ double dphidtlm0;
+ double dphidcrv;
+
+ double dzdphi0;
+ double dzdz0;
+ double dzdalf0;
+ double dzdtlm0;
+ double dzdcrv;
+
+ double dalfdphi0;
+ double dalfdz0;
+ double dalfdalf0;
+ double dalfdtlm0;
+ double dalfdcrv;
+
+ double dtlmdphi0;
+ double dtlmdz0;
+ double dtlmdalf0;
+ double dtlmdtlm0;
+ double dtlmdcrv;
+
+ if(deriv) {
+
+ // Extract derivative matrix.
+
+ dphidphi0 = par(5);
+ dphidz0 = par(6);
+ dphidalf0 = par(7);
+ dphidtlm0 = par(8);
+ dphidcrv = par(9);
+
+ dzdphi0 = par(10);
+ dzdz0 = par(11);
+ dzdalf0 = par(12);
+ dzdtlm0 = par(13);
+ dzdcrv = par(14);
+
+ dalfdphi0 = par(15);
+ dalfdz0 = par(16);
+ dalfdalf0 = par(17);
+ dalfdtlm0 = par(18);
+ dalfdcrv = par(19);
+
+ dtlmdphi0 = par(20);
+ dtlmdz0 = par(21);
+ dtlmdalf0 = par(22);
+ dtlmdtlm0 = par(23);
+ dtlmdcrv = par(24);
+ }
+
+ double salf = sin(alf);
+ calf = cos(alf);
+ if(calf == 0)
+ throw runtime_error("Divide by zero");
+ double talf = salf/calf;
+ double secalf = 1./calf;
+ double seclm = sqrt(1. + tlm*tlm);
+ double slm = tlm / seclm;
+
+ // Get cylindrical components of magnetic field & derivatives.
+
+ double bz;
+ double br;
+ double bphi;
+ double dbrdz;
+ double dbrdphi;
+ double dbzdz;
+ double dbzdphi;
+ double dbphidz;
+ double dbphidphi;
+ if(deriv) {
+ SpacePointTensor t;
+ SpacePointVector bf = bfield->field(CylindricalPoint(r, phi, z), &t);
+ br = bf.v_rxy();
+ bphi = bf.v_phi();
+ bz = bf.v_z();
+ dbrdz = t.t_rxy_z();
+ dbrdphi = r * t.t_rxy_phi() + bphi;
+ dbphidz = t.t_phi_z();
+ dbphidphi = r * t.t_phi_phi() - br;
+ dbzdz = t.t_z_z();
+ dbzdphi = r * t.t_z_phi();
+ }
+ else {
+ SpacePointVector bf = bfield->field(CylindricalPoint(r, phi, z));
+ br = bf.v_rxy();
+ bphi = bf.v_phi();
+ bz = bf.v_z();
+ }
+
+ // d(phi)/dr.
+
+ result(0) = talf / r;
+
+ // dz/dr.
+
+ result(1) = secalf * tlm;
+
+ // d(alpha)/dr.
+
+ result(2) =
+ BFAC * crv * seclm * (tlm*br + talf*tlm*bphi - secalf*bz) - talf/r;
+
+ // d(tan(lambda))/dr.
+
+ result(3) = BFAC * crv * seclm * seclm * seclm * (-talf*br + bphi);
+
+ // ds/dr.
+
+ result(4) = secalf * seclm;
+
+ if(deriv) {
+
+ // d(d phi/d phi0)/dr.
+
+ result(5) = secalf * secalf / r * dalfdphi0;
+
+ // d(d phi/d z0)/dr.
+
+ result(6) = secalf * secalf / r * dalfdz0;
+
+ // d(d phi/d alpha0)/dr.
+
+ result(7) = secalf * secalf / r * dalfdalf0;
+
+ // d(d phi/d tan(lambda0))/dr.
+
+ result(8) = secalf * secalf / r * dalfdtlm0;
+
+ // d(d phi/d crv)/dr.
+
+ result(9) = secalf * secalf / r * dalfdcrv;
+
+ // d(d z/d phi0)/dr.
+
+ result(10) = secalf * ( talf * tlm * dalfdphi0 + dtlmdphi0 );
+
+ // d(d z/d z0)/dr.
+
+ result(11) = secalf * ( talf * tlm * dalfdz0 + dtlmdz0 );
+
+ // d(d z/d alpha0)/dr.
+
+ result(12) = secalf * ( talf * tlm * dalfdalf0 + dtlmdalf0 );
+
+ // d(d z/d tan(lambda0))/dr.
+
+ result(13) = secalf * ( talf * tlm * dalfdtlm0 + dtlmdtlm0 );
+
+ // d(d z/d crv)/dr.
+
+ result(14) = secalf * ( talf * tlm * dalfdcrv + dtlmdcrv );
+
+ // d(d alpha/d phi0)/dr.
+
+ result(15) = BFAC * crv *
+ ( slm * dtlmdphi0 * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + seclm * (br*dtlmdphi0 + tlm*dbrdphi*dphidphi0 + tlm*dbrdz*dzdphi0
+ + secalf*secalf*tlm*bphi*dalfdphi0 + talf*bphi*dtlmdphi0
+ + talf*tlm*dbphidphi*dphidphi0 + talf*tlm*dbphidz*dzdphi0
+ - secalf*talf*bz*dalfdphi0
+ - secalf*dbzdphi*dphidphi0 - secalf*dbzdz*dzdphi0))
+ - secalf * secalf / r * dalfdphi0;
+
+ // d(d alpha/d z0)/dr.
+
+ result(16) = BFAC * crv *
+ ( slm * dtlmdz0 * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + seclm * (br*dtlmdz0 + tlm*dbrdphi*dphidz0 + tlm*dbrdz*dzdz0
+ + secalf*secalf*tlm*bphi*dalfdz0 + talf*bphi*dtlmdz0
+ + talf*tlm*dbphidphi*dphidz0 + talf*tlm*dbphidz*dzdz0
+ - secalf*talf*bz*dalfdz0
+ - secalf*dbzdphi*dphidz0 - secalf*dbzdz*dzdz0))
+ - secalf * secalf / r * dalfdz0;
+
+ // d(d alpha/d alpha0)/dr.
+
+ result(17) = BFAC * crv *
+ ( slm * dtlmdalf0 * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + seclm * (br*dtlmdalf0 + tlm*dbrdphi*dphidalf0 + tlm*dbrdz*dzdalf0
+ + secalf*secalf*tlm*bphi*dalfdalf0 + talf*bphi*dtlmdalf0
+ + talf*tlm*dbphidphi*dphidalf0 + talf*tlm*dbphidz*dzdalf0
+ - secalf*talf*bz*dalfdalf0
+ - secalf*dbzdphi*dphidalf0 - secalf*dbzdz*dzdalf0))
+ - secalf * secalf / r * dalfdalf0;
+
+ // d(d alpha/d tan(lambda0))/dr.
+
+ result(18) = BFAC * crv *
+ ( slm * dtlmdtlm0 * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + seclm * (br*dtlmdtlm0 + tlm*dbrdphi*dphidtlm0 + tlm*dbrdz*dzdtlm0
+ + secalf*secalf*tlm*bphi*dalfdtlm0 + talf*bphi*dtlmdtlm0
+ + talf*tlm*dbphidphi*dphidtlm0 + talf*tlm*dbphidz*dzdtlm0
+ - secalf*talf*bz*dalfdtlm0
+ - secalf*dbzdphi*dphidtlm0 - secalf*dbzdz*dzdtlm0))
+ - secalf * secalf / r * dalfdtlm0;
+
+ // d(d alpha/d crv)/dr.
+
+ result(19) =
+ BFAC * seclm * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + BFAC * crv *
+ ( slm * dtlmdcrv * (tlm*br + talf*tlm*bphi - secalf*bz)
+ + seclm * (br*dtlmdcrv + tlm*dbrdphi*dphidcrv + tlm*dbrdz*dzdcrv
+ + secalf*secalf*tlm*bphi*dalfdcrv + talf*bphi*dtlmdcrv
+ + talf*tlm*dbphidphi*dphidcrv + talf*tlm*dbphidz*dzdcrv
+ - secalf*talf*bz*dalfdcrv
+ - secalf*dbzdphi*dphidcrv - secalf*dbzdz*dzdcrv))
+ - secalf * secalf / r * dalfdcrv;
+
+ // d(d tan(lambda)/d phi0)/dr
+
+ result(20) = BFAC * crv * seclm*seclm *
+ (3 * slm * dtlmdphi0 * (-talf*br + bphi)
+ + seclm * (- secalf*secalf*br*dalfdphi0
+ - talf*dbrdphi*dphidphi0 - talf*dbrdz*dzdphi0
+ + dbphidphi*dphidphi0 + dbphidz*dzdphi0));
+
+ // d(d tan(lambda)/d z0)/dr
+
+ result(21) = BFAC * crv * seclm*seclm *
+ (3 * slm * dtlmdz0 * (-talf*br + bphi)
+ + seclm * (- secalf*secalf * br * dalfdz0
+ - talf*dbrdphi*dphidz0 - talf*dbrdz*dzdz0
+ + dbphidphi*dphidz0 + dbphidz*dzdz0));
+
+ // d(d tan(lambda)/d alpha0)/dr
+
+ result(22) = BFAC * crv * seclm*seclm *
+ (3 * slm * dtlmdalf0 * (-talf*br + bphi)
+ + seclm * (- secalf*secalf * br * dalfdalf0
+ - talf*dbrdphi*dphidalf0 - talf*dbrdz*dzdalf0
+ + dbphidphi*dphidalf0 + dbphidz*dzdalf0));
+
+ // d(d tan(lambda)/d tan(lambda0))/dr
+
+ result(23) = BFAC * crv * seclm*seclm *
+ (3 * slm * dtlmdtlm0 * (-talf*br + bphi)
+ + seclm * (- secalf*secalf * br * dalfdtlm0
+ - talf*dbrdphi*dphidtlm0 - talf*dbrdz*dzdtlm0
+ + dbphidphi*dphidtlm0 + dbphidz*dzdtlm0));
+
+ // d(d tan(lambda)/d crv)/dr
+
+ result(24) = BFAC * seclm * seclm * seclm * (-talf*br + bphi)
+ + BFAC * crv * seclm*seclm *
+ (3 * slm * dtlmdcrv * (-talf*br + bphi)
+ + seclm * (- secalf*secalf * br * dalfdcrv
+ - talf*dbrdphi*dphidcrv - talf*dbrdz*dzdcrv
+ + dbphidphi*dphidcrv + dbphidz*dzdcrv));
+ }
+ return result;
+ }
+
+ // Function to evolve track paramters using a single fourth order
+ // Runge-Kutta step from r1 to r2.
+ bool rk4(TrackPar& par, double& r, double h, double crv,
+ const AbstractMagneticField* bfield, TrackPar& k1, double& calf1,
+ bool reuse) {
+ int size = par.get_size();
+ TrackPar k2(size);
+ TrackPar k3(size);
+ TrackPar k4(size);
+ double calf2;
+ if(!reuse)
+ k1 = h * motion(par, r, crv, bfield, calf1);
+ if(calf1 == 0.)
+ return false;
+ k2 = h * motion(par + 0.5*k1, r + 0.5*h, crv, bfield, calf2);
+ if(calf1*calf2 <= 0.)
+ return false;
+ k3 = h * motion(par + 0.5*k2, r + 0.5*h, crv, bfield, calf2);
+ if(calf1*calf2 <= 0.)
+ return false;
+ k4 = h * motion(par + k3, r + h, crv, bfield, calf2);
+ if(calf1*calf2 <= 0.)
+ return false;
+ par += (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4);
+ r += h;
+ double dmax = 1.e30;
+ for(int i=0; i<par.get_size(); ++i) {
+ double x = par(i);
+ if(!(x < dmax && x > -dmax))
+ return false;
+ }
+ return true;
+ }
+
+ // Function to calculate the relative difference between two sets
+ // of track parameters. The result is scaled so that a difference
+ // of less than one is "good."
+ double pardiff(const TrackPar& par1, const TrackPar& par2,
+ double derivscale) {
+ int n = par1.get_size();
+ assert(par2.get_size() == n);
+ double epsmax = 0.;
+ for(int i=0; i<n; ++i) {
+ double p1 = par1(i);
+ double p2 = par2(i);
+ double eps = abs(p2-p1)/max(max(abs(p1), abs(p2)), 10.);
+
+ // Scale down derivative differences.
+
+ if(i >= 5)
+ eps *= derivscale;
+ if(eps > epsmax)
+ epsmax = eps;
+ }
+ return epsmax;
+ }
+
+ // Function to evolve track parameters using fourth order Runge-Kutta
+ // with a variable step size. The starting step size is as specified,
+ // which may be reduced until the error is estimated to be small enough.
+ // The radius is updated to reflect the actual step size. The estimated
+ // next step size is returned as the value of the function.
+
+ double rkv(TrackPar& par, double& r, double h, double crv,
+ const AbstractMagneticField* bfield, double precision,
+ double derivprec) {
+
+ double derivscale = precision / derivprec;
+
+ // Calculate the minimum allowed step, which is the lesser of
+ // either 1 cm or the initial step.
+
+ double hmin = 0.02 * r * abs(h) / h;
+ if(abs(hmin) > abs(h))
+ hmin = h;
+ double hnext = h;
+ for(;;) {
+
+ // Reduce the target precision for short steps to control the
+ // accumulation of roundoff error.
+
+ double maxdiff = precision * sqrt(0.1*abs(h));
+ TrackPar par1 = par;
+ TrackPar par2 = par;
+
+ // Try step using h.
+
+ double r1 = r;
+ TrackPar k1(par.get_size());
+ double calf;
+ bool ok1 = rk4(par1, r1, h, crv, bfield, k1, calf, false);
+
+ // In short hop situations, quit here.
+
+ if(abs(hmin) <= 0.01) {
+ if(!ok1)
+ throw runtime_error("RK4 error");
+ par = par1;
+ r = r1;
+ break;
+ }
+
+ // Try two steps using hh.
+
+ double r2 = r;
+ double hh = 0.5 * h;
+ k1 = 0.5 * k1;
+ bool ok2 = rk4(par2, r2, hh, crv, bfield, k1, calf, true);
+ ok2 = ok2 && rk4(par2, r2, hh, crv, bfield, k1, calf, false);
+
+ // Calculate difference and get the next step size.
+
+ double eps = pardiff(par1, par2, derivscale);
+ if((ok1 && ok2 && eps <= maxdiff) || abs(h) <= abs(hmin)) {
+
+ // Check for catastrophic loss of accuracy.
+
+ // if(eps > 1000.*maxdiff)
+ // throw runtime_error("Catastrophic loss of accuracy");
+
+ // Make sure that we didn't cross a tan(alpha) pole in this
+ // propagation.
+
+ if(!(ok1 && ok2))
+ throw runtime_error("RK4 error");
+
+ // Current step is accurate enough. Adjust the step size.
+ // and return current propagation.
+
+ if(eps != 0) {
+ hnext = 0.8 * h * pow(maxdiff/eps, 0.25);
+ if(abs(hnext) > 4. * abs(h))
+ hnext = 4. * h;
+ }
+ else
+ hnext = 4. * h;
+ par = (16./15.)*par2 - (1./15.)*par1;
+ r = r1;
+ break;
+ }
+ else {
+
+ // Not accurate enough. Shrink the step size and try again.
+ // Don't let the step get too small.
+
+ if(!(ok1 && ok2))
+ hnext = 0.;
+ else
+ hnext = 0.8 * h * pow(maxdiff/eps, 0.25);
+ if(abs(hnext) < 0.25 * abs(h))
+ hnext = 0.25 * h;
+ if(abs(hnext) < abs(hmin))
+ hnext = hmin;
+ h = hnext;
+ }
+ }
+
+ // Final check on minimum step size.
+
+ if(abs(hnext) < abs(hmin))
+ hnext = hmin;
+ return hnext;
+ }
+
+ // This routine uses adaptive step size control to make the specified
+ // step using one or several steps.
+
+ void rka(TrackPar& par, double& r, double h, double crv,
+ const AbstractMagneticField* bfield, double precision,
+ double derivprec) {
+ assert(h != 0);
+ double htry = h;
+ double hnext;
+ double r2 = r + h;
+ while(h > 0 && r < r2 || h < 0 && r > r2) {
+ double hmax = abs(r2 - r);
+ if(abs(htry) > hmax)
+ htry = r2 - r;
+ hnext = rkv(par, r, htry, crv, bfield, precision, derivprec);
+ assert(hnext * htry > 0);
+ assert(r + hnext != r);
+ htry = hnext;
+ }
+ }
+}
+
+//************************************************************************
+// PropCylRK member functions.
+//************************************************************************
+
+// output stream
+
+void PropCylRK::ostr(ostream& stream) const {
+ stream << begin_object;
+ stream << "Cylinder RK propagation with magnetic field type " << _type
+ << "," << new_line
+ << "precision = " << _precision
+ << ", scale factor 1 = " << _scale1
+ << ", scale factor 2 = " << _scale2
+ << ", derivprec = " << _derivprec;
+ stream << end_object;
+}
+
+//************************************************************************
+
+// Constructor.
+
+PropCylRK::PropCylRK(const string& type, double precision, double scale1,
+ double scale2, double derivprec) :
+ _type(type),
+ _scale1(scale1),
+ _scale2(scale2),
+ _precision(precision),
+ _derivprec(derivprec)
+{
+ assert(precision > 0. && precision <= 0.01);
+ assert(derivprec > 0. && derivprec <= 0.1);
+ if(type.size() == 0 || type == "constant")
+ _bfield = new ConstantMagneticField(-2.*scale1);
+
+ // MCField.
+/*
+ else if(type == "mc")
+ _bfield_map = new ObjMagField(new MCField(scale1, scale2));
+ else if(type == "tosca")
+ _bfield_map = new ObjMagField(new ToscaField(scale1, scale2));
+ else if(type == "D0") {
+ MagFieldMgr* mgr = MagFieldMgr::get_instance();
+ _bfield_map = new ObjMagField(mgr->get_field());
+ }
+ else {
+ ObjTable::Status status = ObjTable::get_object(type, _bfield_map);
+ if(status != ObjTable::OK) {
+ cerr << "PropCylRK: Error fetching field map global object "
+ << type << endl;
+ abort();
+ }
+ }
+ */
+ // _bfield = new MagFieldCacher(_bfield_map);
+}
+
+//************************************************************************
+
+// Destructor.
+
+PropCylRK::~PropCylRK() {
+}
+
+//************************************************************************
+
+// Clone.
+
+Propagator* PropCylRK::new_propagator() const {
+ return new PropCylRK(_type, _precision, _scale1, _scale2, _derivprec);
+}
+
+//**********************************************************************
+
+// Return the creator.
+
+ObjCreator PropCylRK::get_creator() {
+ return create;
+}
+
+//**********************************************************************
+
+// Write the object data.
+
+ObjData PropCylRK::write_data() const {
+ ObjData data( get_type_name() );
+ data.add_string( "type", _type );
+ data.add_double( "precision", _precision );
+ data.add_double( "derivprec", _derivprec );
+ data.add_double( "scale1", _scale1 );
+ data.add_double( "scale2", _scale2 );
+ return data;
+}
+
+//************************************************************************
+
+// Propagate a track without error in the specified direction.
+//
+// The track parameters for a cylinder are:
+// phi z alpha tan(lambda) curvature
+//
+// If pder is nonzero, return the derivative matrix there.
+
+PropStat PropCylRK::
+vec_dir_prop(VTrack& trv, const Surface& srf, PropDir dir,
+ TrackDerivative* pder) const {
+
+ // construct return status
+ PropStat pstat;
+
+ // fetch the originating surface.
+ const Surface& srf1 = *trv.get_surface();
+
+ // Check origin is a cylinder.
+ if ( srf1.get_pure_type( ) != SurfCylinder::get_static_type() )
+ return pstat;
+ const SurfCylinder& scy1 = (const SurfCylinder&) srf1;
+
+ // Check destination is a cylinder.
+ assert( srf.get_pure_type() == SurfCylinder::get_static_type() );
+ if ( srf.get_pure_type( ) != SurfCylinder::get_static_type() )
+ return pstat;
+ const SurfCylinder& scy2 = (const SurfCylinder&) srf;
+
+ // Separate the move status from the direction.
+ PropDirected::PropDir rdir(dir);
+ bool move = Propagator::reduce_direction(rdir);
+
+ // This flag indicates the new surface is only a short hop away.
+ bool short_hop = false;
+
+ // If surfaces are the same and move is not set, then we leave the
+ // track where it is.
+ if ( srf.pure_equal(srf1) && !move ) {
+ if ( pder != 0 ) {
+ TrackDerivative& deriv = *pder;
+ deriv.fill( 0.0 );
+ deriv(0,0) = 1.0;
+ deriv(1,1) = 1.0;
+ deriv(2,2) = 1.0;
+ deriv(3,3) = 1.0;
+ deriv(4,4) = 1.0;
+ }
+ pstat.set_same();
+ return pstat;
+ }
+
+ // Fetch the radii.
+ int irad = SurfCylinder::RADIUS;
+ double r1 = scy1.get_parameter(irad);
+ double r2 = scy2.get_parameter(irad);
+
+ // Fetch the starting track vector.
+ TrackVector vec = trv.get_vector();
+ double phi1 = vec(IPHI); // phi position
+ double z1 = vec(IZ); // z
+ double alf1 = vec(IALF); // phi_dir - phi_pos
+ double tlam1 = vec(ITLM); // tan(lambda)
+ double qpt1 = vec(IQPT); // q/pT
+
+ // Check alpha range.
+ alf1 = fmod2( alf1, TWOPI );
+ assert( fabs(alf1) <= PI );
+ if ( trv.is_forward() ) assert( fabs(alf1) <= PI2 );
+ else assert( fabs(alf1) > PI2 );
+
+ // Check the consistency of the initial and final radii, the track
+ // direction and the propagation direction. Four of the eight possible
+ // forward/backward combinations are illegal. Unlike PropCyl, this
+ // propagator can not be used to propagate accross the DCA. Propagation
+ // is always possible for the NEAREST motion, unless when r1 == r2.
+
+ if(r2 == r1) {
+ if(move) {
+ cerr << "PropCylRK: Attempt to cross DCA" << endl;
+ return pstat;
+ }
+ }
+ else if(r2 > r1) {
+ if(rdir == Propagator::FORWARD && trv.is_backward() ||
+ rdir == Propagator::BACKWARD && trv.is_forward()) {
+ cerr << "PropCylRK: Wrong direction" << endl;
+ return pstat;
+ }
+ }
+ else { // r2 < r1
+ if(rdir == Propagator::FORWARD && trv.is_forward() ||
+ rdir == Propagator::BACKWARD && trv.is_backward()) {
+ cerr << "PropCylRK: Wrong direction" << endl;
+ return pstat;
+ }
+ }
+
+ // We have all non-trivial mathematical operations in a try block
+ // so we can catch floating point exceptions.
+
+ try {
+
+ // Fill initial parameter vector.
+
+ int size = 5;
+ if(pder)
+ size = 25;
+ TrackPar par(size);
+ par.fill(0.);
+ par(0) = phi1; // phi
+ par(1) = z1; // z
+ par(2) = alf1; // Alpha
+ par(3) = tlam1; // tan(lambda)
+ par(4) = 0.; // Path length.
+ double seclam1 = sqrt(1. + tlam1*tlam1);
+ double coslam1 = 1./seclam1;
+ double sinlam1 = tlam1 * coslam1;
+ double crv = qpt1 * coslam1;
+ if(pder) {
+ par(5) = 1.; // d phi / d phi0
+ par(11) = 1.; // d z / d z0
+ par(17) = 1.; // d alpha / d alpha0
+ par(23) = 1.; // d tan(lambda) / d tan(lambda0)
+ }
+
+ // Propagate parameter vector.
+
+ double r = r1;
+ double h = r2 - r1;
+ if(r + h <= 1.e-6)
+ throw runtime_error("Propagate to zero radius");
+ if(abs(h) < 1.e-10)
+ r = r2;
+ else
+ rka(par, r, h, crv, _bfield, _precision, _derivprec);
+
+ // Calculate final parameters and put them back into vec.
+
+ double dmax = 1.e30;
+ double phi2 = par(0);
+ double z2 = par(1);
+ double alf2 = par(2);
+ double tlam2 = par(3);
+ double s = par(4);
+ double seclam2 = sqrt(1. + tlam2*tlam2);
+ double sinlam2 = tlam2 / seclam2;
+ double qpt2 = crv * seclam2;
+ vec(IPHI) = fmod1(phi2, TWOPI);
+ vec(IZ) = z2;
+ vec(IALF) = fmod2(alf2, TWOPI);
+ vec(ITLM) = tlam2;
+ vec(IQPT) = qpt2;
+
+ // For axial tracks, zero z and tan(lambda).
+
+ if(trv.is_axial()) {
+ vec(IZ) = 0.;
+ vec(ITLM) = 0.;
+ }
+
+ // It is an error for the track to change direction.
+
+ if ( trv.is_forward() && fabs(vec(IALF)) > PI2 ||
+ trv.is_backward() && fabs(vec(IALF)) < PI2 )
+ throw runtime_error("Track changed direction");
+ if(!(phi2 < dmax && phi2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(z2 < dmax && z2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(alf2 < dmax && alf2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(tlam2 < dmax && tlam2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(s < dmax && s > -dmax))
+ throw runtime_error("Floating point exception");
+ if(pder) {
+
+ // Calculate final derivative matrix.
+
+ TrackDerivative& deriv = *pder;
+ deriv(0,0) = par(5); // d phi / d phi0
+ deriv(0,1) = par(6); // d phi / d z0
+ deriv(0,2) = par(7); // d phi / d alpha0
+ deriv(0,3) = par(8) - crv * sinlam1 * coslam1 * par(9);
+ // d phi / d tan(lambda0)
+ deriv(0,4) = coslam1 * par(9); // d phi / d qpt0
+ deriv(1,0) = par(10); // d z / d phi0
+ deriv(1,1) = par(11); // d z / d z0
+ deriv(1,2) = par(12); // d z / d alpha0
+ deriv(1,3) = par(13) - crv * sinlam1 * coslam1 * par(14);
+ // d z / d tan(lambda0)
+ deriv(1,4) = coslam1 * par(14); // d z / d qpt0
+ deriv(2,0) = par(15); // d alpha / d phi0
+ deriv(2,1) = par(16); // d alpha / d z0
+ deriv(2,2) = par(17); // d alpha / d alpha0
+ deriv(2,3) = par(18) - crv * sinlam1 * coslam1 * par(19);
+ // d alpha / d tan(lambda0)
+ deriv(2,4) = coslam1 * par(19); // d alpha / d qpt0
+ deriv(3,0) = par(20); // d tan(lambda) / d phi0
+ deriv(3,1) = par(21); // d tan(lambda) / d z0
+ deriv(3,2) = par(22); // d tan(lambda) / d alpha0
+ deriv(3,3) = par(23) - crv * sinlam1 * coslam1 * par(24);
+ // d tan(lambda) / d tan(lambda0)
+ deriv(3,4) = coslam1 * par(24); // d tan(lambda) / d qpt0
+ deriv(4,0) = crv * sinlam2 * par(20); // d qpt / d phi0
+ deriv(4,1) = crv * sinlam2 * par(21); // d qpt / d z0
+ deriv(4,2) = crv * sinlam2 * par(22); // d qpt / d alpha0
+ deriv(4,3) = crv * ( sinlam2 * par(23)
+ - sinlam1 * coslam1 * (seclam2 + crv * sinlam2 * par(24)));
+ // d qpt / d tlm0
+ deriv(4,4) = coslam1 * (seclam2 + crv * sinlam2 * par(24));
+ // d qpt / d qpt0
+ // For axial tracks, zero all derivatives of or with respect to z or
+ // tan(lambda), that are not already zero. This will force the error
+ // matrix to have zero errors for z and tan(lambda).
+
+ if(trv.is_axial()) {
+ deriv(0,1) = 0.; // d phi / d z0
+ deriv(0,3) = 0.; // d phi / d tan(lambda0)
+ deriv(1,0) = 0.; // d z / d phi0
+ deriv(1,1) = 0.; // d z / d z0
+ deriv(1,2) = 0.; // d z / d alpha0
+ deriv(1,3) = 0.; // d z / d tan(lambda0)
+ deriv(1,4) = 0.; // d z / d qpt0
+ deriv(2,1) = 0.; // d alpha / d z0
+ deriv(2,3) = 0.; // d alpha / d tan(lambda0)
+ deriv(3,0) = 0.; // d tan(lambda) / d phi0
+ deriv(3,1) = 0.; // d tan(lambda) / d z0
+ deriv(3,2) = 0.; // d tan(lambda) / d alpha0
+ deriv(3,3) = 0.; // d tan(lambda) / d tan(lambda0)
+ deriv(3,4) = 0.; // d tan(lambda) / d qpt0
+ deriv(4,1) = 0.; // d qpt / d z0
+ deriv(4,3) = 0.; // d qpt / d tlm0
+ }
+
+ // Check for impending overflow.
+
+ for(int i=0; i<5; ++i) {
+ for(int j=0; j<5; ++j) {
+ double x = deriv(i,j);
+ if(!(x < dmax && x > -dmax))
+ throw runtime_error("Floating point exception");
+ }
+ }
+ }
+
+ // Update trv
+ trv.set_surface( SurfacePtr(srf.new_pure_surface()) );
+ trv.set_vector(vec);
+ if ( fabs(alf2) <= PI2 ) trv.set_forward();
+ else trv.set_backward();
+
+ // Set the return status.
+ pstat.set_path_distance(s);
+ }
+ catch(runtime_error x) {}
+ return pstat;
+}
+
+//**********************************************************************
+
+// return Bz at the origin.
+double PropCylRK::get_bfield() const
+{
+ SpacePointVector bf = _bfield->field(CartesianPoint(0., 0., 0.));
+ return bf.v_z();
+}
+
+//**********************************************************************
