trf++/src/trfxyp
diff -N PropXYXYRK.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ PropXYXYRK.cpp 29 Jul 2011 00:09:17 -0000 1.1
@@ -0,0 +1,938 @@
+// PropXYXYRK.cpp
+
+#include "trfxyp/PropXYXYRK.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 "trfxyp/SurfXYPlane.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::SurfXYPlane;
+using trf::PropXYXYRK;
+//using trf::MagFieldCacher;
+using static_la::array;
+
+//**********************************************************************
+// Free functions.
+//**********************************************************************
+
+namespace {
+
+ // Assign track parameter indices.
+ enum {
+ IV = SurfXYPlane::IV,
+ IZ = SurfXYPlane::IZ,
+ IDVDU = SurfXYPlane::IDVDU,
+ IDZDU = SurfXYPlane::IDZDU,
+ IQP = SurfXYPlane::IQP
+ };
+
+ typedef array<double,25> TrackPar;
+
+ // Creator.
+
+ ObjPtr create(const ObjData& data) {
+ assert( data.get_object_type() == "PropXYXYRK" );
+ 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<PropXYXYRK,LocalSharedDeletePolicy>
+ pobj(new PropXYXYRK(type, precision, scale1, scale2, derivprec));
+ pobj->set_direction(data);
+ return pobj;
+ }
+
+ // Equations of motion in terms of track parameters v, z, dvdu, dzdu,
+ // curvature (q/p), and path length and the magnetic field.
+
+ TrackPar motion(const TrackPar& par, double u, double sinphi, double cosphi,
+ double crv, double sign_du,
+ const AbstractMagneticField* bfield) {
+
+ TrackPar result(par.get_size());
+ bool deriv = par.get_size() > 5;
+
+ // Extract track parameters.
+
+ double v = par(0);
+ double z = par(1);
+ double vv = par(2);
+ double zv = par(3);
+ double s = par(4);
+ double x = u*cosphi - v*sinphi;
+ double y = u*sinphi + v*cosphi;
+
+ // Declare derivative matrix.
+
+ double dvdv0;
+ double dvdz0;
+ double dvdvv0;
+ double dvdzv0;
+ double dvdcrv;
+
+ double dzdv0;
+ double dzdz0;
+ double dzdvv0;
+ double dzdzv0;
+ double dzdcrv;
+
+ double dvvdv0;
+ double dvvdz0;
+ double dvvdvv0;
+ double dvvdzv0;
+ double dvvdcrv;
+
+ double dzvdv0;
+ double dzvdz0;
+ double dzvdvv0;
+ double dzvdzv0;
+ double dzvdcrv;
+
+ if(deriv) {
+
+ // Extract derivative matrix.
+
+ dvdv0 = par(5);
+ dvdz0 = par(6);
+ dvdvv0 = par(7);
+ dvdzv0 = par(8);
+ dvdcrv = par(9);
+
+ dzdv0 = par(10);
+ dzdz0 = par(11);
+ dzdvv0 = par(12);
+ dzdzv0 = par(13);
+ dzdcrv = par(14);
+
+ dvvdv0 = par(15);
+ dvvdz0 = par(16);
+ dvvdvv0 = par(17);
+ dvvdzv0 = par(18);
+ dvvdcrv = par(19);
+
+ dzvdv0 = par(20);
+ dzvdz0 = par(21);
+ dzvdvv0 = par(22);
+ dzvdzv0 = par(23);
+ dzvdcrv = par(24);
+ }
+ double dmax = 1.e30;
+ if(!(vv < dmax && vv > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(zv < dmax && zv > -dmax))
+ throw runtime_error("Floating point exception");
+ double dsdu2 = 1. + vv*vv + zv*zv;
+ double dsdu = sign_du * sqrt(dsdu2);
+
+ // Get the rotated Cartesian components of magnetic field & derivatives.
+
+ double bu;
+ double bv;
+ double bz;
+ double dbudv;
+ double dbudz;
+ double dbvdv;
+ double dbvdz;
+ double dbzdv;
+ double dbzdz;
+ SpacePointVector bf;
+ if(deriv) {
+ SpacePointTensor t;
+ bf = bfield->field(CartesianPoint(x, y, z), &t);
+ double dbxdx = t.t_x_x();
+ double dbxdy = t.t_x_y();
+ double dbxdz = t.t_x_z();
+ double dbydx = t.t_y_x();
+ double dbydy = t.t_y_y();
+ double dbydz = t.t_y_z();
+ double dbzdx = t.t_z_x();
+ double dbzdy = t.t_z_y();
+ dbzdz = t.t_z_z();
+ dbudv = (dbydy - dbxdx)*cosphi*sinphi
+ - dbydx*sinphi*sinphi + dbxdy*cosphi*cosphi;
+ dbudz = dbxdz*cosphi + dbydz*sinphi;
+ dbvdv = dbxdx*sinphi*sinphi + dbydy*cosphi*cosphi
+ - (dbydx + dbxdy)*cosphi*sinphi;
+ dbvdz = -dbxdz*sinphi + dbydz*cosphi;
+ dbzdv = -dbzdx*sinphi + dbzdy*cosphi;
+ }
+ else
+ bf = bfield->field(CartesianPoint(x, y, z));
+ double bx = bf.v_x();
+ double by = bf.v_y();
+ bz = bf.v_z();
+ bu = bx*cosphi + by*sinphi;
+ bv = -bx*sinphi + by*cosphi;
+
+ // dv/du.
+
+ result(0) = vv;
+
+ // dz/du.
+
+ result(1) = zv;
+
+ // dvv/du.
+
+ double dvvdu_nocrv = BFAC * dsdu * (zv*bu + vv*zv*bv - (1. + vv*vv)*bz);
+ result(2) = dvvdu_nocrv * crv;
+
+ // dzv/du.
+
+ double dzvdu_nocrv = BFAC * dsdu * (-vv*bu + (1. + zv*zv)*bv - vv*zv*bz);
+ result(3) = dzvdu_nocrv * crv;
+
+ // ds/dz.
+
+ result(4) = dsdu;
+
+ if(deriv) {
+
+ // d(dv/d v0)/du.
+
+ result(5) = dvvdv0;
+
+ // d(dv/d z0)/du.
+
+ result(6) = dvvdz0;
+
+ // d(dv/d vv0)/du.
+
+ result(7) = dvvdvv0;
+
+ // d(dv/d zv0)/du.
+
+ result(8) = dvvdzv0;
+
+ // d(dv/d crv)/du.
+
+ result(9) = dvvdcrv;
+
+ // d(dz/d v0)/du.
+
+ result(10) = dzvdv0;
+
+ // d(dz/d z0)/du.
+
+ result(11) = dzvdz0;
+
+ // d(dz/d vv0)/du.
+
+ result(12) = dzvdvv0;
+
+ // d(dz/d zv0)/du.
+
+ result(13) = dzvdzv0;
+
+ // d(dz/d crv)/du.
+
+ result(14) = dzvdcrv;
+
+ // d(d vv/d v0)/du.
+
+ result(15) = result(2) * (vv*dvvdv0 + zv*dzvdv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (dvvdv0*zv*bv + vv*dzvdv0*bv + vv*zv*(dbvdv*dvdv0 + dbvdz*dzdv0)
+ -2.*vv*dvvdv0*bz - (1. + vv*vv)*(dbzdv*dvdv0 + dbzdz*dzdv0)
+ +dzvdv0*bu + zv*(dbudv*dvdv0 + dbudz*dzdv0));
+
+ // d(d vv/d z0)/du.
+
+ result(16) = result(2) * (vv*dvvdz0 + zv*dzvdz0) / dsdu2
+ + BFAC * crv * dsdu *
+ (dvvdz0*zv*bv + vv*dzvdz0*bv + vv*zv*(dbvdv*dvdz0 + dbvdz*dzdz0)
+ -2.*vv*dvvdz0*bz - (1. + vv*vv)*(dbzdv*dvdz0 + dbzdz*dzdz0)
+ +dzvdz0*bu + zv*(dbudv*dvdz0 + dbudz*dzdz0));
+
+ // d(d vv/d vv0)/du.
+
+ result(17) = result(2) * (vv*dvvdvv0 + zv*dzvdvv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (dvvdvv0*zv*bv + vv*dzvdvv0*bv + vv*zv*(dbvdv*dvdvv0 + dbvdz*dzdvv0)
+ -2.*vv*dvvdvv0*bz - (1. + vv*vv)*(dbzdv*dvdvv0 + dbzdz*dzdvv0)
+ +dzvdvv0*bu + zv*(dbudv*dvdvv0 + dbudz*dzdvv0));
+
+ // d(d vv/d zv0)/du.
+
+ result(18) = result(2) * (vv*dvvdzv0 + zv*dzvdzv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (dvvdzv0*zv*bv + vv*dzvdzv0*bv + vv*zv*(dbvdv*dvdzv0 + dbvdz*dzdzv0)
+ -2.*vv*dvvdzv0*bz - (1. + vv*vv)*(dbzdv*dvdzv0 + dbzdz*dzdzv0)
+ +dzvdzv0*bu + zv*(dbudv*dvdzv0 + dbudz*dzdzv0));
+
+ // d(d vv/d crv)/du.
+
+ result(19) = dvvdu_nocrv * (1. + crv*(vv*dvvdcrv + zv*dzvdcrv) / dsdu2)
+ + BFAC * crv * dsdu *
+ (dvvdcrv*zv*bv + vv*dzvdcrv*bv + vv*zv*(dbvdv*dvdcrv + dbvdz*dzdcrv)
+ -2.*vv*dvvdcrv*bz - (1. + vv*vv)*(dbzdv*dvdcrv + dbzdz*dzdcrv)
+ +dzvdcrv*bu + zv*(dbudv*dvdcrv + dbudz*dzdcrv));
+
+
+ // d(d zv/d v0)/dr
+
+ result(20) = result(3) * (vv*dvvdv0 + zv*dzvdv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (2.*zv*dzvdv0*bv + (1. + zv*zv)*(dbvdv*dvdv0 + dbvdz*dzdv0)
+ -dvvdv0*zv*bz - vv*dzvdv0*bz - vv*zv*(dbzdv*dvdv0 + dbzdz*dzdv0)
+ -dvvdv0*bu - vv*(dbudv*dvdv0 + dbudz*dzdv0));
+
+ // d(d zv/d z0)/dr
+
+ result(21) = result(3) * (vv*dvvdz0 + zv*dzvdz0) / dsdu2
+ + BFAC * crv * dsdu *
+ (2.*zv*dzvdz0*bv + (1. + zv*zv)*(dbvdv*dvdz0 + dbvdz*dzdz0)
+ -dvvdz0*zv*bz - vv*dzvdz0*bz - vv*zv*(dbzdv*dvdz0 + dbzdz*dzdz0)
+ -dvvdz0*bu - vv*(dbudv*dvdz0 + dbudz*dzdz0));
+
+ // d(d zv/d vv0)/dr
+
+ result(22) = result(3) * (vv*dvvdvv0 + zv*dzvdvv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (2.*zv*dzvdvv0*bv + (1. + zv*zv)*(dbvdv*dvdvv0 + dbvdz*dzdvv0)
+ -dvvdvv0*zv*bz - vv*dzvdvv0*bz - vv*zv*(dbzdv*dvdvv0 + dbzdz*dzdvv0)
+ -dvvdvv0*bu - vv*(dbudv*dvdvv0 + dbudz*dzdvv0));
+
+ // d(d zv/d zv0/dr
+
+ result(23) = result(3) * (vv*dvvdzv0 + zv*dzvdzv0) / dsdu2
+ + BFAC * crv * dsdu *
+ (2.*zv*dzvdzv0*bv + (1. + zv*zv)*(dbvdv*dvdzv0 + dbvdz*dzdzv0)
+ -dvvdzv0*zv*bz - vv*dzvdzv0*bz - vv*zv*(dbzdv*dvdzv0 + dbzdz*dzdzv0)
+ -dvvdzv0*bu - vv*(dbudv*dvdzv0 + dbudz*dzdzv0));
+
+ // d(d zv/d crv)/dr
+
+ result(24) = dzvdu_nocrv * (1. + crv*(vv*dvvdcrv + zv*dzvdcrv) / dsdu2)
+ + BFAC * crv * dsdu *
+ (2.*zv*dzvdcrv*bv + (1. + zv*zv)*(dbvdv*dvdcrv + dbvdz*dzdcrv)
+ -dvvdcrv*zv*bz - vv*dzvdcrv*bz - vv*zv*(dbzdv*dvdcrv + dbzdz*dzdcrv)
+ -dvvdcrv*bu - vv*(dbudv*dvdcrv + dbudz*dzdcrv));
+ }
+ return result;
+ }
+
+ // Function to evolve track paramters using a single fourth order
+ // Runge-Kutta step from u1 to u2.
+ void rk4(TrackPar& par, double& u, double h, double sinphi, double cosphi,
+ double crv, double sign_du, 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, u, sinphi, cosphi, crv, sign_du, bfield);
+ k2 = h * motion(par + 0.5*k1, u + 0.5*h, sinphi, cosphi, crv, sign_du,
+ bfield);
+ k3 = h * motion(par + 0.5*k2, u + 0.5*h, sinphi, cosphi, crv, sign_du,
+ bfield);
+ k4 = h * motion(par + k3, u + h, sinphi, cosphi, crv, sign_du, bfield);
+ par += (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4);
+ u += 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& u, double h, double sinphi, double cosphi,
+ double crv, double sign_du, 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 u1 = u;
+ TrackPar k1(par.get_size());
+ rk4(par1, u1, h, sinphi, cosphi, crv, sign_du, bfield, k1, false);
+
+ // In short hop situations, quit here.
+
+ if(abs(hmin) <= 0.01) {
+ par = par1;
+ u = u1;
+ break;
+ }
+
+ // Try two steps using hh.
+
+ double u2 = u;
+ double hh = 0.5 * h;
+ k1 = 0.5 * k1;
+ rk4(par2, u2, hh, sinphi, cosphi, crv, sign_du, bfield, k1, true);
+ rk4(par2, u2, hh, sinphi, cosphi, crv, sign_du, 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;
+ u = u1;
+ 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& u, double h, double sinphi, double cosphi,
+ double crv, double sign_du, const AbstractMagneticField* bfield,
+ double precision, double derivprec) {
+ assert(h != 0);
+ double htry = h;
+ double hnext;
+ double u2 = u + h;
+ while(h > 0 && u < u2 || h < 0 && u > u2) {
+ double hmax = abs(u2 - u);
+ if (hmax < 1e-6)
+ break;
+ if(abs(htry) > hmax)
+ htry = u2 - u;
+ hnext = rkv(par, u, htry, sinphi, cosphi, crv, sign_du, bfield,
+ precision, derivprec);
+ assert(hnext * htry > 0);
+ assert(u + hnext != u);
+ htry = hnext;
+ }
+ }
+}
+
+//************************************************************************
+// PropXYXYRK member functions.
+//************************************************************************
+
+// output stream
+
+void PropXYXYRK::ostr(ostream& stream) const {
+ stream << begin_object;
+ stream << "XY 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.
+
+PropXYXYRK::PropXYXYRK(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 << "PropXYXYRK: Error fetching field map global object "
+ << type << endl;
+ abort();
+ }
+ }
+ */
+ //_bfield = new MagFieldCacher(_bfield_map);
+}
+
+//************************************************************************
+
+// Destructor.
+
+PropXYXYRK::~PropXYXYRK() {
+}
+
+//************************************************************************
+
+// Clone.
+
+Propagator* PropXYXYRK::new_propagator() const {
+ return new PropXYXYRK(_type, _precision, _scale1, _scale2, _derivprec);
+}
+
+//**********************************************************************
+
+// Return the creator.
+
+ObjCreator PropXYXYRK::get_creator() {
+ return create;
+}
+
+//**********************************************************************
+
+// Write the object data.
+
+ObjData PropXYXYRK::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 xy-plane are:
+// v z dv/du dz/du curvature=q/p
+//
+// If pder is nonzero, return the derivative matrix there.
+
+PropStat PropXYXYRK::
+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 an xy plane.
+ if ( srf1.get_pure_type( ) != SurfXYPlane::get_static_type() )
+ return pstat;
+ const SurfXYPlane& sxy1 = (const SurfXYPlane&) srf1;
+
+ // Check destination is an xy plane.
+ assert( srf.get_pure_type() == SurfXYPlane::get_static_type() );
+ if ( srf.get_pure_type( ) != SurfXYPlane::get_static_type() )
+ return pstat;
+ const SurfXYPlane& sxy2 = (const SurfXYPlane&) srf;
+
+ // Check that original track has defined direction.
+ if(!trv.is_forward() && !trv.is_backward())
+ return pstat;
+
+ // 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 originating and destination plane parameters.
+ int iu = SurfXYPlane::DISTNORM;
+ int iphi = SurfXYPlane::NORMPHI;
+ double u0 = sxy1.get_parameter(iu);
+ double phi0 = sxy1.get_parameter(iphi);
+ double u2 = sxy2.get_parameter(iu);
+ double phi2 = sxy2.get_parameter(iphi);
+
+ // Fetch the starting track vector.
+ TrackVector vec = trv.get_vector();
+ double v0 = vec(IV); // v
+ double z1 = vec(IZ); // z
+ double vv0 = vec(IDVDU); // dv/du
+ double zv0 = vec(IDZDU); // dv/du
+ double crv = vec(IQP); // q/p
+
+ // Transform the initial VTrack onto a surface that is parallel
+ // to the destination surface.
+ TrackDerivative deriv1;
+ if(pder)
+ deriv1.fill(0.);
+ double u1;
+ double v1;
+ double vv1;
+ double zv1;
+ double sign_du = 0.;
+ if(trv.is_forward())
+ sign_du = 1.;
+ else if(trv.is_backward())
+ sign_du = -1.;
+ else
+ return pstat;
+ if(phi0 != phi2) {
+ double sindphi = sin(phi2-phi0);
+ double cosdphi = cos(phi2-phi0);
+ u1 = u0*cosdphi + v0*sindphi;
+ v1 = -u0*sindphi + v0*cosdphi;
+ double du1du0 = vv0*sindphi + cosdphi;
+ vv1 = (vv0*cosdphi - sindphi)/du1du0;
+ zv1 = zv0/du1du0;
+ if(du1du0 < 0)
+ sign_du = -sign_du;
+ else if (du1du0 == 0)
+ return pstat;
+ if(pder) {
+
+ // Get components of the magnetic field.
+
+ double cosphi2 = cos(phi2);
+ double sinphi2 = sin(phi2);
+ double x = u1 * cosphi2 - v1 * sinphi2;
+ double y = u1 * sinphi2 + v1 * cosphi2;
+ SpacePointVector bf = _bfield->field(CartesianPoint(x, y, z1));
+ double bx = bf.v_x();
+ double by = bf.v_y();
+ double bz = bf.v_z();
+ double bu = bx * cosphi2 + by * sinphi2;
+ double bv = -bx * cosphi2 + by * cosphi2;
+
+ double dsdu1 = sqrt(1. + vv1*vv1 + zv1*zv1);
+
+ deriv1(IV,IV) = cosdphi - sindphi*vv1;
+ deriv1(IV,IZ) = 0.;
+ deriv1(IV,IDVDU) = 0.;
+ deriv1(IV,IDZDU) = 0.;
+ deriv1(IZ,IV) = -sindphi*zv1;
+ deriv1(IZ,IZ) = 1.;
+ deriv1(IZ,IDVDU) = 0.;
+ deriv1(IZ,IDZDU) = 0.;
+ deriv1(IZ,IQP) = 0.;
+ deriv1(IDVDU,IV) = -sign_du * sindphi * BFAC * crv * dsdu1 *
+ (zv1*bu + vv1*zv1*bv - (1. + vv1*vv1)*bz);
+ deriv1(IDVDU,IZ) = 0.;
+ deriv1(IDVDU,IDVDU) = 1. / (du1du0*du1du0);
+ deriv1(IDVDU,IDZDU) = 0.;
+ deriv1(IDVDU,IQP) = 0.;
+ deriv1(IDZDU,IV) = -sign_du * sindphi * BFAC * crv * dsdu1 *
+ (-vv1*bu + (1. + zv1*zv1)*bv - vv1*zv1*bz);
+ deriv1(IDZDU,IZ) = 0.;
+ deriv1(IDZDU,IDVDU) = -zv0 * sindphi / (du1du0*du1du0);
+ deriv1(IDZDU,IDZDU) = 1. / du1du0;
+ deriv1(IDZDU,IQP) = 0.;
+ deriv1(IQP,IV) = 0.;
+ deriv1(IQP,IZ) = 0.;
+ deriv1(IQP,IDVDU) = 0.;
+ deriv1(IQP,IDZDU) = 0.;
+ deriv1(IQP,IQP) = 1.;
+ }
+ }
+ else {
+ u1 = u0;
+ v1 = v0;
+ vv1 = vv0;
+ zv1 = zv0;
+ if(pder) {
+ deriv1(IV,IV) = 1.;
+ deriv1(IZ,IZ) = 1.;
+ deriv1(IDVDU,IDVDU) = 1.;
+ deriv1(IDZDU,IDZDU) = 1.;
+ deriv1(IQP,IQP) = 1.;
+ }
+ }
+ bool forward = sign_du > 0.;
+ bool backward = sign_du < 0.;
+ assert(forward || backward);
+ assert(!(forward && backward));
+
+ // Check the consistency of the initial and final u positions, the track
+ // direction and the propagation direction. Four of the eight possible
+ // forward/backward combinations are illegal.
+
+ if(u2 == u1) {
+ if(move) {
+ cerr << "PropXYXYRK: Invalid move" << endl;
+ return pstat;
+ }
+ }
+ else if(u2 > u1) {
+ if(rdir == Propagator::FORWARD && backward ||
+ rdir == Propagator::BACKWARD && forward) {
+ cerr << "PropXYXYRK: Wrong direction" << endl;
+ return pstat;
+ }
+ }
+ else { // u2 < u1
+ if(rdir == Propagator::FORWARD && forward ||
+ rdir == Propagator::BACKWARD && backward) {
+ cerr << "PropXYXYRK: 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.
+
+ TrackDerivative deriv2;
+ int size = 5;
+ if(pder)
+ size = 25;
+ TrackPar par(size);
+ par.fill(0.);
+ par(0) = v1; // v
+ par(1) = z1; // z
+ par(2) = vv1; // dv/du
+ par(3) = zv1; // dz/du
+ par(4) = 0.; // Path length.
+ if(pder) {
+ par(5) = 1.; // d v / d v0
+ par(11) = 1.; // d z / d z0
+ par(17) = 1.; // d vv / d vv0
+ par(23) = 1.; // d zv / d zv0
+ }
+
+ // Propagate parameter vector.
+
+ double u = u1;
+ double h = u2 - u1;
+ if(abs(h) < 1.e-10)
+ u = u2;
+ else
+ rka(par, u, h, sin(phi2), cos(phi2), crv, sign_du, _bfield,
+ _precision, _derivprec);
+
+ // Calculate final parameters and put them back into vec.
+
+ double dmax = 1.e30;
+ double v2 = par(0);
+ double z2 = par(1);
+ double vv2 = par(2);
+ double zv2 = par(3);
+ double s = par(4);
+ vec(IV) = v2;
+ vec(IZ) = z2;
+ vec(IDVDU) = vv2;
+ vec(IDZDU) = zv2;
+ vec(IQP) = crv;
+ if(!(v2 < dmax && v2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(z2 < dmax && z2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(vv2 < dmax && vv2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(zv2 < dmax && zv2 > -dmax))
+ throw runtime_error("Floating point exception");
+ if(!(s < dmax && s > -dmax))
+ throw runtime_error("Floating point exception");
+ if(pder) {
+
+ // Calculate final derivative matrix.
+
+ deriv2(0,0) = par(5); // dv / d v0
+ deriv2(0,1) = par(6); // dv / d z0
+ deriv2(0,2) = par(7); // dv / d vv0
+ deriv2(0,3) = par(8); // dv / d zv0
+ deriv2(0,4) = par(9); // dv / d crv
+ deriv2(1,0) = par(10); // dz / d v0
+ deriv2(1,1) = par(11); // dz / d z0
+ deriv2(1,2) = par(12); // dz / d vv0
+ deriv2(1,3) = par(13); // dz / d zv0
+ deriv2(1,4) = par(14); // dz / d crv
+ deriv2(2,0) = par(15); // d vv / d v0
+ deriv2(2,1) = par(16); // d vv / d z0
+ deriv2(2,2) = par(17); // d vv / d vv0
+ deriv2(2,3) = par(18); // d vv / d zv0
+ deriv2(2,4) = par(19); // d vv / d crv
+ deriv2(3,0) = par(20); // d zv / d v0
+ deriv2(3,1) = par(21); // d zv / d z0
+ deriv2(3,2) = par(22); // d zv / d vv0
+ deriv2(3,3) = par(23); // d zv / d zv0
+ deriv2(3,4) = par(24); // d zv / d crv
+ deriv2(4,0) = 0.; // d crv / d v0
+ deriv2(4,1) = 0.; // d crv / d z0
+ deriv2(4,2) = 0.; // d crv / d vv0
+ deriv2(4,3) = 0.; // d crv / d zv0
+ deriv2(4,4) = 1.; // d crv / d crv
+ for(int i=0; i<4; ++i) {
+ for(int j=0; j<5; ++j) {
+ double x = deriv2(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(forward)
+ trv.set_forward();
+ else if(backward)
+ trv.set_backward();
+
+ // Set the return status.
+ pstat.set_path_distance(s);
+
+ // Update derivative matrix.
+ if(pder)
+ *pder = deriv2 * deriv1;
+ }
+ catch(runtime_error x) {}
+ return pstat;
+}
+
+//**********************************************************************
+
+// return Bz at the origin.
+double PropXYXYRK::get_bfield() const
+{
+ SpacePointVector bf = _bfield->field(CartesianPoint(0., 0., 0.));
+ return bf.v_z();
+}
+
+//**********************************************************************
