trf++/src/trfdca
diff -N PropParamRK.cpp
--- /dev/null 1 Jan 1970 00:00:00 -0000
+++ PropParamRK.cpp 8 Aug 2011 22:16:42 -0000 1.1
@@ -0,0 +1,540 @@
+// PropParamRK.cpp
+
+#include <cmath>
+#include <stdexcept>
+#include <cassert>
+#include "trfdca/PropParamRK.h"
+#include "trfutil/TRFMath.h"
+#include "mag_field/AbstractMagneticField.hpp"
+
+using trf::PropParamRK;
+using std::abs;
+using std::max;
+using std::sin;
+using std::cos;
+using std::sqrt;
+using std::runtime_error;
+
+typedef PropParamRK::TrackPar TrackPar;
+
+// Equations of motion in terms of track parameters x, y, z,
+// phid, tan(lambda), curvature (q/p) and the magnetic field.
+
+TrackPar PropParamRK::motion(const PropParamRK::TrackPar& par, double crv,
+ 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 z = par(2);
+ double phid = par(3);
+ double tlam = par(4);
+
+ double cphid = cos(phid);
+ double sphid = sin(phid);
+ double clam = 1./sqrt(1. + tlam*tlam); // cos(lambda).
+ double slam = tlam * clam; // sin(lambda).
+
+ // Declare derivative matrix.
+
+ double dxdx0;
+ double dxdy0;
+ double dxdz0;
+ double dxdphid0;
+ double dxdtlam0;
+ double dxdcrv;
+
+ double dydx0;
+ double dydy0;
+ double dydz0;
+ double dydphid0;
+ double dydtlam0;
+ double dydcrv;
+
+ double dzdx0;
+ double dzdy0;
+ double dzdz0;
+ double dzdphid0;
+ double dzdtlam0;
+ double dzdcrv;
+
+ double dphiddx0;
+ double dphiddy0;
+ double dphiddz0;
+ double dphiddphid0;
+ double dphiddtlam0;
+ double dphiddcrv;
+
+ double dtlamdx0;
+ double dtlamdy0;
+ double dtlamdz0;
+ double dtlamdphid0;
+ double dtlamdtlam0;
+ double dtlamdcrv;
+
+ if(deriv) {
+
+ // Extract derivative matrix.
+
+ dxdx0 = par(5);
+ dxdy0 = par(6);
+ dxdz0 = par(7);
+ dxdphid0 = par(8);
+ dxdtlam0 = par(9);
+ dxdcrv = par(10);
+
+ dydx0 = par(11);
+ dydy0 = par(12);
+ dydz0 = par(13);
+ dydphid0 = par(14);
+ dydtlam0 = par(15);
+ dydcrv = par(16);
+
+ dzdx0 = par(17);
+ dzdy0 = par(18);
+ dzdz0 = par(19);
+ dzdphid0 = par(20);
+ dzdtlam0 = par(21);
+ dzdcrv = par(22);
+
+ dphiddx0 = par(23);
+ dphiddy0 = par(24);
+ dphiddz0 = par(25);
+ dphiddphid0 = par(26);
+ dphiddtlam0 = par(27);
+ dphiddcrv = par(28);
+
+ dtlamdx0 = par(29);
+ dtlamdy0 = par(30);
+ dtlamdz0 = par(31);
+ dtlamdphid0 = par(32);
+ dtlamdtlam0 = par(33);
+ dtlamdcrv = par(34);
+ }
+
+ // Get Cartesian components of magnetic field & derivatives.
+
+ double bx;
+ double by;
+ double bz;
+ double dbxdx;
+ double dbxdy;
+ double dbxdz;
+ double dbydx;
+ double dbydy;
+ double dbydz;
+ double dbzdx;
+ double dbzdy;
+ double dbzdz;
+ SpacePointVector bf;
+ if(deriv) {
+ SpacePointTensor t;
+ bf = bfield->field(CartesianPoint(x, y, z), &t);
+ dbxdx = t.t_x_x();
+ dbxdy = t.t_x_y();
+ dbxdz = t.t_x_z();
+ dbydx = t.t_y_x();
+ dbydy = t.t_y_y();
+ dbydz = t.t_y_z();
+ dbzdx = t.t_z_x();
+ dbzdy = t.t_z_y();
+ dbzdz = t.t_z_z();
+ }
+ else
+ bf = bfield->field(CartesianPoint(x, y, z));
+ bx = bf.v_x();
+ by = bf.v_y();
+ bz = bf.v_z();
+
+ // Calculate derivatives of magnetic field with respect in initial
+ // parameters.
+
+ double dbxdx0;
+ double dbydx0;
+ double dbzdx0;
+ double dbxdy0;
+ double dbydy0;
+ double dbzdy0;
+ double dbxdz0;
+ double dbydz0;
+ double dbzdz0;
+ double dbxdphid0;
+ double dbydphid0;
+ double dbzdphid0;
+ double dbxdtlam0;
+ double dbydtlam0;
+ double dbzdtlam0;
+ double dbxdcrv;
+ double dbydcrv;
+ double dbzdcrv;
+ if(deriv) {
+ dbxdx0 = dbxdx*dxdx0 + dbxdy*dydx0 + dbxdz*dzdx0;
+ dbydx0 = dbydx*dxdx0 + dbydy*dydx0 + dbydz*dzdx0;
+ dbzdx0 = dbzdx*dxdx0 + dbzdy*dydx0 + dbzdz*dzdx0;
+ dbxdy0 = dbxdx*dxdy0 + dbxdy*dydy0 + dbxdz*dzdy0;
+ dbydy0 = dbydx*dxdy0 + dbydy*dydy0 + dbydz*dzdy0;
+ dbzdy0 = dbzdx*dxdy0 + dbzdy*dydy0 + dbzdz*dzdy0;
+ dbxdz0 = dbxdx*dxdz0 + dbxdy*dydz0 + dbxdz*dzdz0;
+ dbydz0 = dbydx*dxdz0 + dbydy*dydz0 + dbydz*dzdz0;
+ dbzdz0 = dbzdx*dxdz0 + dbzdy*dydz0 + dbzdz*dzdz0;
+ dbxdphid0 = dbxdx*dxdphid0 + dbxdy*dydphid0 + dbxdz*dzdphid0;
+ dbydphid0 = dbydx*dxdphid0 + dbydy*dydphid0 + dbydz*dzdphid0;
+ dbzdphid0 = dbzdx*dxdphid0 + dbzdy*dydphid0 + dbzdz*dzdphid0;
+ dbxdtlam0 = dbxdx*dxdtlam0 + dbxdy*dydtlam0 + dbxdz*dzdtlam0;
+ dbydtlam0 = dbydx*dxdtlam0 + dbydy*dydtlam0 + dbydz*dzdtlam0;
+ dbzdtlam0 = dbzdx*dxdtlam0 + dbzdy*dydtlam0 + dbzdz*dzdtlam0;
+ dbxdcrv = dbxdx*dxdcrv + dbxdy*dydcrv + dbxdz*dzdcrv;
+ dbydcrv = dbydx*dxdcrv + dbydy*dydcrv + dbydz*dzdcrv;
+ dbzdcrv = dbzdx*dxdcrv + dbzdy*dydcrv + dbzdz*dzdcrv;
+ }
+
+ // dx/ds.
+
+ result(0) = cphid * clam;
+
+ // dy/ds.
+
+ result(1) = sphid * clam;
+
+ // dz/ds.
+
+ result(2) = slam;
+
+ // d(phid)/ds.
+
+ double dphidds_nocrv = BFAC * (cphid*tlam*bx + sphid*tlam*by - bz);
+ result(3) = crv * dphidds_nocrv;
+
+ // d(tan(lambda))/ds.
+
+ double dtlamds_nocrv = BFAC / (clam*clam) * (-sphid*bx + cphid*by);
+ result(4) = crv * dtlamds_nocrv;
+
+ if(deriv) {
+
+ // d(dx/d x0)/ds.
+
+ result(5) = -clam * (sphid * dphiddx0 + cphid * slam * clam * dtlamdx0);
+
+ // d(dx/d y0)/ds.
+
+ result(6) = -clam * (sphid * dphiddy0 + cphid * slam * clam * dtlamdy0);
+
+ // d(dx/d z0)/ds.
+
+ result(7) = -clam * (sphid * dphiddz0 + cphid * slam * clam * dtlamdz0);
+
+ // d(dx/d phid0)/ds.
+
+ result(8) =
+ -clam * (sphid * dphiddphid0 + cphid * slam * clam * dtlamdphid0);
+
+ // d(dx/d tlam0)/ds.
+
+ result(9) =
+ -clam * (sphid * dphiddtlam0 + cphid * slam * clam * dtlamdtlam0);
+
+ // d(dx/d crv)/ds.
+
+ result(10) =
+ -clam * (sphid * dphiddcrv + cphid * slam * clam * dtlamdcrv);
+
+ // d(dy/d x0)/ds.
+
+ result(11) = clam * (cphid * dphiddx0 - sphid * slam * clam * dtlamdx0);
+
+ // d(dy/d y0)/ds.
+
+ result(12) = clam * (cphid * dphiddy0 - sphid * slam * clam * dtlamdy0);
+
+ // d(dy/d z0)/ds.
+
+ result(13) = clam * (cphid * dphiddz0 - sphid * slam * clam * dtlamdz0);
+
+ // d(dy/d phid0)/ds.
+
+ result(14) =
+ clam * (cphid * dphiddphid0 - sphid * slam * clam * dtlamdphid0);
+
+ // d(dy/d tlam0)/ds.
+
+ result(15) =
+ clam * (cphid * dphiddtlam0 - sphid * slam * clam * dtlamdtlam0);
+
+ // d(dy/d crv)/ds.
+
+ result(16) =
+ clam * (cphid * dphiddcrv - sphid * slam * clam * dtlamdcrv);
+
+ // d(dz/d x0)/ds.
+
+ result(17) = clam*clam*clam * dtlamdx0;
+
+ // d(dz/d y0)/ds.
+
+ result(18) = clam*clam*clam * dtlamdy0;
+
+ // d(dz/d z0)/ds.
+
+ result(19) = clam*clam*clam * dtlamdz0;
+
+ // d(dz/d phid0)/ds.
+
+ result(20) = clam*clam*clam * dtlamdphid0;
+
+ // d(dz/d tlam0)/ds.
+
+ result(21) = clam*clam*clam * dtlamdtlam0;
+
+ // d(dz/d crv)/ds.
+
+ result(22) = clam*clam*clam * dtlamdcrv;
+
+ // d(d phid/d x0)/ds.
+
+ result(23) = BFAC * crv *
+ (- sphid*tlam*bx*dphiddx0 + cphid*bx*dtlamdx0 + cphid*tlam*dbxdx0
+ + cphid*tlam*by*dphiddx0 + sphid*by*dtlamdx0 + sphid*tlam*dbydx0
+ - dbzdx0);
+
+ // d(d phid/d y0)/ds.
+
+ result(24) = BFAC * crv *
+ (- sphid*tlam*bx*dphiddy0 + cphid*bx*dtlamdy0 + cphid*tlam*dbxdy0
+ + cphid*tlam*by*dphiddy0 + sphid*by*dtlamdy0 + sphid*tlam*dbydy0
+ - dbzdy0);
+
+ // d(d phid/d z0)/ds.
+
+ result(25) = BFAC * crv *
+ (- sphid*tlam*bx*dphiddz0 + cphid*bx*dtlamdz0 + cphid*tlam*dbxdz0
+ + cphid*tlam*by*dphiddz0 + sphid*by*dtlamdz0 + sphid*tlam*dbydz0
+ - dbzdz0);
+
+ // d(d phid/d phid0)/ds.
+
+ result(26) = BFAC * crv *
+ (- sphid*tlam*bx*dphiddphid0 + cphid*bx*dtlamdphid0
+ + cphid*tlam*dbxdphid0
+ + cphid*tlam*by*dphiddphid0 + sphid*by*dtlamdphid0
+ + sphid*tlam*dbydphid0
+ - dbzdphid0);
+
+ // d(d phid/d tlam0)/ds.
+
+ result(27) = BFAC * crv *
+ (- sphid*tlam*bx*dphiddtlam0 + cphid*bx*dtlamdtlam0
+ + cphid*tlam*dbxdtlam0
+ + cphid*tlam*by*dphiddtlam0 + sphid*by*dtlamdtlam0
+ + sphid*tlam*dbydtlam0
+ - dbzdtlam0);
+
+ // d(d phid/d crv)/ds.
+
+ result(28) = dphidds_nocrv + BFAC * crv *
+ (- sphid*tlam*bx*dphiddcrv + cphid*bx*dtlamdcrv + cphid*tlam*dbxdcrv
+ + cphid*tlam*by*dphiddcrv + sphid*by*dtlamdcrv + sphid*tlam*dbydcrv
+ - dbzdcrv);
+
+ // d(d tlam/d x0)/ds.
+
+ result(29) = BFAC * crv *
+ (2.*tlam*dtlamdx0 * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddx0 - sphid*dbxdx0
+ -sphid*by*dphiddx0 + cphid*dbydx0));
+
+ // d(d tlam/d y0)/ds.
+
+ result(30) = BFAC * crv *
+ (2.*tlam*dtlamdy0 * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddy0 - sphid*dbxdy0
+ -sphid*by*dphiddy0 + cphid*dbydy0));
+
+ // d(d tlam/d z0)/ds.
+
+ result(31) = BFAC * crv *
+ (2.*tlam*dtlamdz0 * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddz0 - sphid*dbxdz0
+ -sphid*by*dphiddz0 + cphid*dbydz0));
+
+ // d(d tlam/d phid0)/ds.
+
+ result(32) = BFAC * crv *
+ (2.*tlam*dtlamdphid0 * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddphid0 - sphid*dbxdphid0
+ -sphid*by*dphiddphid0 + cphid*dbydphid0));
+
+ // d(d tlam/d tlam0)/ds.
+
+ result(33) = BFAC * crv *
+ (2.*tlam*dtlamdtlam0 * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddtlam0 - sphid*dbxdtlam0
+ -sphid*by*dphiddtlam0 + cphid*dbydtlam0));
+
+ // d(d tlam/d crv)/ds.
+
+ result(34) = dtlamds_nocrv + BFAC * crv *
+ (2.*tlam*dtlamdcrv * (-sphid*bx + cphid*by)
+ + 1. / (clam*clam) * (-cphid*bx*dphiddcrv - sphid*dbxdcrv
+ -sphid*by*dphiddcrv + cphid*dbydcrv));
+ }
+ return result;
+}
+
+// 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 PropParamRK::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 paramters using a single fourth order
+// Runge-Kutta step from s1 to s2.
+void PropParamRK::rk4(TrackPar& par, double& s, double h, double crv,
+ 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, crv, bfield);
+ k2 = h * motion(par + 0.5*k1, crv, bfield);
+ k3 = h * motion(par + 0.5*k2, crv, bfield);
+ k4 = h * motion(par + k3, crv, bfield);
+ par += (1./6.)*(k1 + 2.*k2 + 2.*k3 + k4);
+ s += 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 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 distance is updated to reflect the actual step size. The estimated
+// next step size is returned as the value of the function.
+
+double PropParamRK::rkv(TrackPar& par, double& s, 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 = 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 s1 = s;
+ TrackPar k1(par.get_size());
+ rk4(par1, s1, h, crv, bfield, k1, false);
+
+ // In short hop situations, quit here.
+
+ if(abs(hmin) <= 0.01) {
+ par = par1;
+ s = s1;
+ break;
+ }
+
+ // Try two steps using hh.
+
+ double s2 = s;
+ double hh = 0.5 * h;
+ k1 = 0.5 * k1;
+ rk4(par2, s2, hh, crv, bfield, k1, true);
+ rk4(par2, s2, hh, crv, 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;
+ s = s1;
+ 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;
+}
+
+void PropParamRK::rka(TrackPar& par, double& s, double h, double crv,
+ const AbstractMagneticField* bfield, double precision,
+ double derivprec) {
+ assert(h != 0);
+ double htry = h;
+ double hnext;
+ double s2 = s + h;
+ while(h > 0 && s < s2 || h < 0 && s > s2) {
+ if(abs(htry) > abs(s2 - s))
+ htry = s2 - s;
+ hnext = rkv(par, s, htry, crv, bfield, precision, derivprec);
+ assert(hnext * htry > 0);
+ assert(s + hnext != s);
+ htry = hnext;
+ }
+}
