trf++/src/trfzp
diff -N PropZZRK.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ PropZZRK.cpp 29 Jul 2011 00:08:23 -0000 1.1
@@ -0,0 +1,817 @@
+// PropZZRK.cpp
+
+#include "trfzp/PropZZRK.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 "trfzp/SurfZPlane.h"
+#include "trfbase/VTrack.h"
+#include "trfbase/ETrack.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::fmod2;
+using trf::Surface;
+using trf::TrackVector;
+using trf::TrackDerivative;
+using trf::VTrack;
+using trf::ETrack;
+using trf::PropStat;
+using trf::Propagator;
+using trf::SurfZPlane;
+using trf::PropZZRK;
+//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() == "PropZZRK" );
+ 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<PropZZRK,LocalSharedDeletePolicy>
+ pobj(new PropZZRK(type, precision, scale1, scale2, derivprec));
+ pobj->set_direction(data);
+ return pobj;
+ }
+
+ // Equations of motion in terms of track parameters x, y, dxdz, dydz,
+ // curvature (q/p), and path length and the magnetic field.
+
+ TrackPar motion(const TrackPar& par, double z, double crv, double sign_dz,
+ const AbstractMagneticField* bfield) {
+
+ TrackPar result(par.get_size());
+ bool deriv = par.get_size() > 5;
+
+ // Extract track parameters.
+
+ double x = par(0);
+ double y = par(1);
+ double xv = par(2);
+ double yv = par(3);
+ double s = par(4);
+
+ // Declare derivative matrix.
+
+ double dxdx0;
+ double dxdy0;
+ double dxdxv0;
+ double dxdyv0;
+ double dxdcrv;
+
+ double dydx0;
+ double dydy0;
+ double dydxv0;
+ double dydyv0;
+ double dydcrv;
+
+ double dxvdx0;
+ double dxvdy0;
+ double dxvdxv0;
+ double dxvdyv0;
+ double dxvdcrv;
+
+ double dyvdx0;
+ double dyvdy0;
+ double dyvdxv0;
+ double dyvdyv0;
+ double dyvdcrv;
+
+ if(deriv) {
+
+ // Extract derivative matrix.
+
+ dxdx0 = par(5);
+ dxdy0 = par(6);
+ dxdxv0 = par(7);
+ dxdyv0 = par(8);
+ dxdcrv = par(9);
+
+ dydx0 = par(10);
+ dydy0 = par(11);
+ dydxv0 = par(12);
+ dydyv0 = par(13);
+ dydcrv = par(14);
+
+ dxvdx0 = par(15);
+ dxvdy0 = par(16);
+ dxvdxv0 = par(17);
+ dxvdyv0 = par(18);
+ dxvdcrv = par(19);
+
+ dyvdx0 = par(20);
+ dyvdy0 = par(21);
+ dyvdxv0 = par(22);
+ dyvdyv0 = par(23);
+ dyvdcrv = par(24);
+ }
+ double dmax = 1.e30;
+ if(!(xv < dmax && xv > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(yv < dmax && yv > -dmax))
+ throw runtime_error("Floating point exception");
+ double dsdz2 = 1. + xv*xv + yv*yv;
+ double dsdz = sign_dz * sqrt(dsdz2);
+
+ // Get Cartesian components of magnetic field & derivatives.
+
+ double bx;
+ double by;
+ double bz;
+ double dbxdx;
+ double dbxdy;
+ double dbydx;
+ double dbydy;
+ double dbzdx;
+ double dbzdy;
+ SpacePointVector bf;
+ if(deriv) {
+ SpacePointTensor t;
+ bf = bfield->field(CartesianPoint(x, y, z), &t);
+ dbxdx = t.t_x_x();
+ dbxdy = t.t_x_y();
+ dbydx = t.t_y_x();
+ dbydy = t.t_y_y();
+ dbzdx = t.t_z_x();
+ dbzdy = t.t_z_y();
+ }
+ else
+ bf = bfield->field(CartesianPoint(x, y, z));
+ bx = bf.v_x();
+ by = bf.v_y();
+ bz = bf.v_z();
+
+ // dx/dz.
+
+ result(0) = xv;
+
+ // dy/dz.
+
+ result(1) = yv;
+
+ // dxv/dz.
+
+ double dxvdz_nocrv = BFAC * dsdz * (xv*yv*bx - (1. + xv*xv)*by + yv*bz);
+ result(2) = dxvdz_nocrv * crv;
+
+ // dyv/dz.
+
+ double dyvdz_nocrv = BFAC * dsdz * ((1. + yv*yv)*bx - xv*yv*by - xv*bz);
+ result(3) = dyvdz_nocrv * crv;
+
+ // ds/dz.
+
+ result(4) = dsdz;
+
+ if(deriv) {
+
+ // d(dx/d x0)/dz.
+
+ result(5) = dxvdx0;
+
+ // d(dx/d y0)/dz.
+
+ result(6) = dxvdy0;
+
+ // d(dx/d xv0)/dz.
+
+ result(7) = dxvdxv0;
+
+ // d(dx/d yv0)/dz.
+
+ result(8) = dxvdyv0;
+
+ // d(dx/d crv)/dz.
+
+ result(9) = dxvdcrv;
+
+ // d(dy/d x0)/dz.
+
+ result(10) = dyvdx0;
+
+ // d(dy/d y0)/dz.
+
+ result(11) = dyvdy0;
+
+ // d(dy/d xv0)/dz.
+
+ result(12) = dyvdxv0;
+
+ // d(dy/d yv0)/dz.
+
+ result(13) = dyvdyv0;
+
+ // d(dy/d crv)/dz.
+
+ result(14) = dyvdcrv;
+
+ // d(d xv/d x0)/dz.
+
+ result(15) = result(2) * (xv*dxvdx0 + yv*dyvdx0) / dsdz2
+ + BFAC * crv * dsdz *
+ (dxvdx0*yv*bx + xv*dyvdx0*bx + xv*yv*(dbxdx*dxdx0 + dbxdy*dydx0)
+ -2.*xv*dxvdx0*by - (1. + xv*xv)*(dbydx*dxdx0 + dbydy*dydx0)
+ +dyvdx0*bz + yv*(dbzdx*dxdx0 + dbzdy*dydx0));
+
+ // d(d xv/d y0)/dz.
+
+ result(16) = result(2) * (xv*dxvdy0 + yv*dyvdy0) / dsdz2
+ + BFAC * crv * dsdz *
+ (dxvdy0*yv*bx + xv*dyvdy0*bx + xv*yv*(dbxdx*dxdy0 + dbxdy*dydy0)
+ -2.*xv*dxvdy0*by - (1. + xv*xv)*(dbydx*dxdy0 + dbydy*dydy0)
+ +dyvdy0*bz + yv*(dbzdx*dxdy0 + dbzdy*dydy0));
+
+ // d(d xv/d xv0)/dz.
+
+ result(17) = result(2) * (xv*dxvdxv0 + yv*dyvdxv0) / dsdz2
+ + BFAC * crv * dsdz *
+ (dxvdxv0*yv*bx + xv*dyvdxv0*bx + xv*yv*(dbxdx*dxdxv0 + dbxdy*dydxv0)
+ -2.*xv*dxvdxv0*by - (1. + xv*xv)*(dbydx*dxdxv0 + dbydy*dydxv0)
+ +dyvdxv0*bz + yv*(dbzdx*dxdxv0 + dbzdy*dydxv0));
+
+ // d(d xv/d yv0)/dz.
+
+ result(18) = result(2) * (xv*dxvdyv0 + yv*dyvdyv0) / dsdz2
+ + BFAC * crv * dsdz *
+ (dxvdyv0*yv*bx + xv*dyvdyv0*bx + xv*yv*(dbxdx*dxdyv0 + dbxdy*dydyv0)
+ -2.*xv*dxvdyv0*by - (1. + xv*xv)*(dbydx*dxdyv0 + dbydy*dydyv0)
+ +dyvdyv0*bz + yv*(dbzdx*dxdyv0 + dbzdy*dydyv0));
+
+ // d(d xv/d crv)/dz.
+
+ result(19) = dxvdz_nocrv * (1. + crv*(xv*dxvdcrv + yv*dyvdcrv) / dsdz2)
+ + BFAC * crv * dsdz *
+ (dxvdcrv*yv*bx + xv*dyvdcrv*bx + xv*yv*(dbxdx*dxdcrv + dbxdy*dydcrv)
+ -2.*xv*dxvdcrv*by - (1. + xv*xv)*(dbydx*dxdcrv + dbydy*dydcrv)
+ +dyvdcrv*bz + yv*(dbzdx*dxdcrv + dbzdy*dydcrv));
+
+
+ // d(d yv/d x0)/dr
+
+ result(20) = result(3) * (xv*dxvdx0 + yv*dyvdx0) / dsdz2
+ + BFAC * crv * dsdz *
+ (2.*yv*dyvdx0*bx + (1. + yv*yv)*(dbxdx*dxdx0 + dbxdy*dydx0)
+ -dxvdx0*yv*by - xv*dyvdx0*by - xv*yv*(dbydx*dxdx0 + dbydy*dydx0)
+ -dxvdx0*bz - xv*(dbzdx*dxdx0 + dbzdy*dydx0));
+
+ // d(d yv/d y0)/dr
+
+ result(21) = result(3) * (xv*dxvdy0 + yv*dyvdy0) / dsdz2
+ + BFAC * crv * dsdz *
+ (2.*yv*dyvdy0*bx + (1. + yv*yv)*(dbxdx*dxdy0 + dbxdy*dydy0)
+ -dxvdy0*yv*by - xv*dyvdy0*by - xv*yv*(dbydx*dxdy0 + dbydy*dydy0)
+ -dxvdy0*bz - xv*(dbzdx*dxdy0 + dbzdy*dydy0));
+
+ // d(d yv/d xv0)/dr
+
+ result(22) = result(3) * (xv*dxvdxv0 + yv*dyvdxv0) / dsdz2
+ + BFAC * crv * dsdz *
+ (2.*yv*dyvdxv0*bx + (1. + yv*yv)*(dbxdx*dxdxv0 + dbxdy*dydxv0)
+ -dxvdxv0*yv*by - xv*dyvdxv0*by - xv*yv*(dbydx*dxdxv0 + dbydy*dydxv0)
+ -dxvdxv0*bz - xv*(dbzdx*dxdxv0 + dbzdy*dydxv0));
+
+ // d(d yv/d yv0/dr
+
+ result(23) = result(3) * (xv*dxvdyv0 + yv*dyvdyv0) / dsdz2
+ + BFAC * crv * dsdz *
+ (2.*yv*dyvdyv0*bx + (1. + yv*yv)*(dbxdx*dxdyv0 + dbxdy*dydyv0)
+ -dxvdyv0*yv*by - xv*dyvdyv0*by - xv*yv*(dbydx*dxdyv0 + dbydy*dydyv0)
+ -dxvdyv0*bz - xv*(dbzdx*dxdyv0 + dbzdy*dydyv0));
+
+ // d(d yv/d crv)/dr
+
+ result(24) = dyvdz_nocrv * (1. + crv*(xv*dxvdcrv + yv*dyvdcrv) / dsdz2)
+ + BFAC * crv * dsdz *
+ (2.*yv*dyvdcrv*bx + (1. + yv*yv)*(dbxdx*dxdcrv + dbxdy*dydcrv)
+ -dxvdcrv*yv*by - xv*dyvdcrv*by - xv*yv*(dbydx*dxdcrv + dbydy*dydcrv)
+ -dxvdcrv*bz - xv*(dbzdx*dxdcrv + dbzdy*dydcrv));
+ }
+ return result;
+ }
+
+ // Function to evolve track paramters using a single fourth order
+ // Runge-Kutta step from z1 to z2.
+ void rk4(TrackPar& par, double& z, double h, double crv, double sign_dz,
+ const AbstractMagneticField* bfield, TrackPar& k1, bool reuse) {
+ int size = par.get_size();
+ TrackPar k2(size);
+ TrackPar k3(size);
+ TrackPar k4(size);
+ if(!reuse)
+ k1 = h * motion(par, z, crv, sign_dz, bfield);
+ k2 = h * motion(par + 0.5*k1, z + 0.5*h, crv, sign_dz, bfield);
+ k3 = h * motion(par + 0.5*k2, z + 0.5*h, crv, sign_dz, bfield);
+ k4 = h * motion(par + k3, z + h, crv, sign_dz, bfield);
+ par += (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4);
+ z += h;
+ double dmax = 1.e30;
+ for(int i=0; i<par.get_size(); ++i) {
+ double x = par(i);
+ if(!(x < dmax && x > -dmax))
+ throw runtime_error("Floating point exception");
+ }
+ }
+
+ // 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.);
+ 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 z coordinate 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& z, double h, double crv, double sign_dz,
+ 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 = 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 z1 = z;
+ TrackPar k1(par.get_size());
+ rk4(par1, z1, h, crv, sign_dz, bfield, k1, false);
+
+ // In short hop situations, quit here.
+
+ if(abs(hmin) <= 0.01) {
+ par = par1;
+ z = z1;
+ break;
+ }
+
+ // Try two steps using hh.
+
+ double z2 = z;
+ double hh = 0.5 * h;
+ k1 = 0.5 * k1;
+ rk4(par2, z2, hh, crv, sign_dz, bfield, k1, true);
+ rk4(par2, z2, hh, crv, sign_dz, bfield, k1, false);
+
+ // Calculate difference and get the next step size.
+
+ double eps = pardiff(par1, par2, derivscale);
+ if(eps <= maxdiff || abs(h) <= abs(hmin)) {
+
+ // Check for catastrophic loss of accuracy.
+
+ // if(eps > 100000.*maxdiff)
+ // throw runtime_error("Catastrophic loss of accuracy");
+
+ // 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;
+ z = z1;
+ break;
+ }
+ else {
+
+ // Not accurate enough. Shrink the step size and try again.
+ // Don't let the step get too small.
+
+ hnext = 0.8 * h * pow(maxdiff/eps, 0.25);
+ 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& z, double h, double crv, double sign_dz,
+ const AbstractMagneticField* bfield, double precision,
+ double derivprec) {
+ assert(h != 0);
+ double htry = h;
+ double hnext;
+ double z2 = z + h;
+ while(h > 0 && z < z2 || h < 0 && z > z2) {
+ double hmax = abs(z2 - z);
+ if(abs(htry) > hmax)
+ htry = z2 - z;
+ hnext = rkv(par, z, htry, crv, sign_dz, bfield, precision, derivprec);
+ assert(hnext * htry > 0);
+ assert(z + hnext != z);
+ htry = hnext;
+ }
+ }
+}
+
+//************************************************************************
+// PropZZRK member functions.
+//************************************************************************
+
+// output stream
+
+void PropZZRK::ostr(ostream& stream) const {
+ stream << begin_object;
+ stream << "Z Plane 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.
+
+PropZZRK::PropZZRK(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 << "PropZZRK: Error fetching field map global object "
+ << type << endl;
+ abort();
+ }
+ }
+ */
+ // _bfield = new MagFieldCacher(_bfield_map);
+}
+
+//************************************************************************
+
+// Destructor.
+
+PropZZRK::~PropZZRK() {
+}
+
+//************************************************************************
+
+// Clone.
+
+Propagator* PropZZRK::new_propagator() const {
+ return new PropZZRK(_type, _precision, _scale1, _scale2, _derivprec);
+}
+
+//**********************************************************************
+
+// Return the creator.
+
+ObjCreator PropZZRK::get_creator() {
+ return create;
+}
+
+//**********************************************************************
+
+// Write the object data.
+
+ObjData PropZZRK::write_data() const {
+ ObjData data( get_type_name() );
+ data.add_string( "type", _type );
+ data.add_double( "precision", _precision );
+ data.add_double( "scale1", _scale1 );
+ data.add_double( "scale2", _scale2 );
+ data.add_double( "derivprec", _derivprec );
+ return data;
+}
+
+//************************************************************************
+
+// Propagate a track without error in the specified direction.
+//
+// The track parameters for a z-plane are:
+// x y dx/dz dy/dz curvature=q/p
+//
+// If pder is nonzero, return the derivative matrix there.
+
+PropStat PropZZRK::
+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 z plane.
+ if ( srf1.get_pure_type( ) != SurfZPlane::get_static_type() )
+ return pstat;
+ const SurfZPlane& sz1 = (const SurfZPlane&) srf1;
+
+ // Check destination is a z plane.
+ assert( srf.get_pure_type() == SurfZPlane::get_static_type() );
+ if ( srf.get_pure_type( ) != SurfZPlane::get_static_type() )
+ return pstat;
+ const SurfZPlane& sz2 = (const SurfZPlane&) 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 z coordinates.
+ int iz = SurfZPlane::ZPOS;
+ double z1 = sz1.get_parameter(iz);
+ double z2 = sz2.get_parameter(iz);
+
+ // Fetch the starting track vector.
+ TrackVector vec = trv.get_vector();
+ double x1 = vec(SurfZPlane::IX); // x
+ double y1 = vec(SurfZPlane::IY); // y
+ double xv1 = vec(SurfZPlane::IDXDZ); // dx/dz
+ double yv1 = vec(SurfZPlane::IDYDZ); // dy/dz
+ double crv = vec(SurfZPlane::IQP); // q/p
+ double sign_dz = 0.;
+ if(trv.is_forward())
+ sign_dz = 1.;
+ else if(trv.is_backward())
+ sign_dz = -1.;
+ if(sign_dz == 0.)
+ return pstat;
+
+ // Check the consistency of the initial and final z positions, the track
+ // direction and the propagation direction. Four of the eight possible
+ // forward/backward combinations are illegal.
+
+ if(z2 == z1) {
+ if(move) {
+ cerr << "PropZZRK: Invalid move" << endl;
+ return pstat;
+ }
+ }
+ else if(z2 > z1) {
+ if(rdir == Propagator::FORWARD && trv.is_backward() ||
+ rdir == Propagator::BACKWARD && trv.is_forward()) {
+ cerr << "PropZZRK: Wrong direction" << endl;
+ return pstat;
+ }
+ }
+ else { // z2 < z1
+ if(rdir == Propagator::FORWARD && trv.is_forward() ||
+ rdir == Propagator::BACKWARD && trv.is_backward()) {
+ cerr << "PropZZRK: 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) = x1; // x
+ par(1) = y1; // y
+ par(2) = xv1; // dx/dz
+ par(3) = yv1; // dy/dz
+ par(4) = 0.; // Path length.
+ if(pder) {
+ par(5) = 1.; // d x / d x0
+ par(11) = 1.; // d y / d y0
+ par(17) = 1.; // d xv / d xv0
+ par(23) = 1.; // d yv / d yv0
+ }
+
+ // Propagate parameter vector.
+
+ double z = z1;
+ double h = z2 - z1;
+ if(abs(h) < 1.e-10)
+ z = z2;
+ else
+ rka(par, z, h, crv, sign_dz, _bfield, _precision, _derivprec);
+
+ // Calculate final parameters and put them back into vec.
+
+ double dmax = 1.e30;
+ double x2 = par(0);
+ double y2 = par(1);
+ double xv2 = par(2);
+ double yv2 = par(3);
+ double s = par(4);
+ vec(SurfZPlane::IX) = x2;
+ vec(SurfZPlane::IY) = y2;
+ vec(SurfZPlane::IDXDZ) = xv2;
+ vec(SurfZPlane::IDYDZ) = yv2;
+ vec(SurfZPlane::IQP) = crv;
+ if(!(x2 < dmax && x2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(y2 < dmax && y2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(xv2 < dmax && xv2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(yv2 < dmax && yv2 > -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); // dx / d x0
+ deriv(0,1) = par(6); // dx / d y0
+ deriv(0,2) = par(7); // dx / d xv0
+ deriv(0,3) = par(8); // dx / d yv0
+ deriv(0,4) = par(9); // dx / d crv
+ deriv(1,0) = par(10); // dy / d x0
+ deriv(1,1) = par(11); // dy / d y0
+ deriv(1,2) = par(12); // dy / d xv0
+ deriv(1,3) = par(13); // dy / d yv0
+ deriv(1,4) = par(14); // dy / d crv
+ deriv(2,0) = par(15); // d xv / d x0
+ deriv(2,1) = par(16); // d xv / d y0
+ deriv(2,2) = par(17); // d xv / d xv0
+ deriv(2,3) = par(18); // d xv / d yv0
+ deriv(2,4) = par(19); // d xv / d crv
+ deriv(3,0) = par(20); // d yv / d x0
+ deriv(3,1) = par(21); // d yv / d y0
+ deriv(3,2) = par(22); // d yv / d xv0
+ deriv(3,3) = par(23); // d yv / d yv0
+ deriv(3,4) = par(24); // d yv / d crv
+ deriv(4,0) = 0.; // d crv / d x0
+ deriv(4,1) = 0.; // d crv / d y0
+ deriv(4,2) = 0.; // d crv / d xv0
+ deriv(4,3) = 0.; // d crv / d yv0
+ deriv(4,4) = 1.; // d crv / d crv
+ for(int i=0; i<4; ++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
+ bool forward = trv.is_forward();
+ bool backward = trv.is_backward();
+ assert(forward || backward);
+ assert(!(forward && backward));
+ trv.set_surface( SurfacePtr(srf.new_pure_surface()) );
+ trv.set_vector(vec);
+ if(forward)
+ trv.set_forward();
+ else if(backward)
+ trv.set_backward();
+
+ // Set the return status.
+ pstat.set_path_distance(s);
+ }
+ catch(runtime_error x) {}
+ return pstat;
+}
+
+//**********************************************************************
+
+// return Bz at the origin.
+double PropZZRK::get_bfield() const
+{
+ SpacePointVector bf = _bfield->field(CartesianPoint(0., 0., 0.));
+ return bf.v_z();
+}
+
+//**********************************************************************