SimpleHelixTrackModel.cc

Go to the documentation of this file.

#include "SimpleHelixTrackModel.h"

#include <iostream>
#include <string>
#include <cmath>

#include "marlin/Global.h"
#include <gear/BField.h>
#include <gearimpl/Vector3D.h>

#include "EVENT/LCIO.h"

#include "EVENT/TrackerHit.h"

#include "TMath.h"

using namespace std;
using namespace gear;

namespace marlintpc {


TMatrixD localHelixAnalyticalJacobian(double ds, double qbyp, Vector3D& t1, Vector3D& t2, Vector3D& bc) {
        TMatrixD ajac(5, 5);
        ajac.UnitMatrix();
        const double qp = -bc.r(); // -|B*c|
        const double q = qp * qbyp; // Q
        if (q == 0.) {
                // line
                ajac[3][2] = ds * sqrt(t1[0] * t1[0] + t1[1] * t1[1]);
                ;
                ajac[4][1] = ds;
        } else {
                // helix
                // at start
                const double cosl1 = sqrt(t1[0] * t1[0] + t1[1] * t1[1]);
                // at end
                const double cosl2 = sqrt(t2[0] * t2[0] + t2[1] * t2[1]);
                const double cosl2Inv = 1. / cosl2;
                // magnetic field direction
                Vector3D hn(bc.unit());
                // (signed) momentum
                const double pav = 1.0 / qbyp;
                //
                const double theta = q * ds;
                const double sint = sin(theta);
                const double cost = cos(theta);
                const double gamma = hn.dot(t2); // H*T
                Vector3D an1 = hn.cross(t1); // HxT0
                Vector3D an2 = hn.cross(t2); // HxT
                // U0, V0
                const double au1 = 1. / t1.trans();
                Vector3D u1(-au1 * t1[1], au1 * t1[0], 0.);
                Vector3D v1(-t1[2] * u1[1], t1[2] * u1[0], t1[0] * u1[1] - t1[1] * u1[0]);
                // U, V
                const double au2 = 1. / t2.trans();
                Vector3D u2(-au2 * t2[1], au2 * t2[0], 0.);
                Vector3D v2(-t2[2] * u2[1], t2[2] * u2[0], t2[0] * u2[1] - t2[1] * u2[0]);
                //
                const double anv = -hn.dot(u2); // N*V=-H*U
                const double anu = hn.dot(v2);  // N*U= H*V
                const double omcost = 1. - cost;
                const double tmsint = theta - sint;
                // M0-M
                Vector3D dx(-(gamma * tmsint * hn[0] + sint * t1[0] + omcost * an1[0]) / q,
                                -(gamma * tmsint * hn[1] + sint * t1[1] + omcost * an1[1]) / q,
                                -(gamma * tmsint * hn[2] + sint * t1[2] + omcost * an1[2]) / q);
                // HxU0
                Vector3D hu1 = hn.cross(u1);
                // HxV0
                Vector3D hv1 = hn.cross(v1);
                // some dot products
                const double u1u2 = u1.dot(u2), u1v2 = u1.dot(v2), v1u2 = v1.dot(u2), v1v2 = v1.dot(v2);
                const double hu1u2 = hu1.dot(u2), hu1v2 = hu1.dot(v2), hv1u2 = hv1.dot(u2), hv1v2 = hv1.dot(v2);
                const double hnu1 = hn.dot(u1), hnv1 = hn.dot(v1), hnu2 = hn.dot(u2), hnv2 = hn.dot(v2);
                const double t2u1 = t2.dot(u1), t2v1 = t2.dot(v1);
                const double t2dx = t2.dot(dx), u2dx = u2.dot(dx), v2dx = v2.dot(dx);
                const double an2u1 = an2.dot(u1), an2v1 = an2.dot(v1);
                // jacobian
                // 1/P
                ajac[0][0] = 1.;
                // Lambda
                ajac[1][0] = -qp * anv * t2dx;
                ajac[1][1] = cost * v1v2 + sint * hv1v2 + omcost * hnv1 * hnv2 + anv * (-sint * t2v1 + omcost * an2v1 - gamma * tmsint * hnv1);
                ajac[1][2] = cosl1
                                * (cost * u1v2 + sint * hu1v2 + omcost * hnu1 * hnv2 + anv * (-sint * t2u1 + omcost * an2u1 - gamma * tmsint * hnu1));
                ajac[1][3] = -q * anv * t2u1;
                ajac[1][4] = -q * anv * t2v1;
                // Phi
                ajac[2][0] = -qp * anu * t2dx * cosl2Inv;
                ajac[2][1] = cosl2Inv
                                * (cost * v1u2 + sint * hv1u2 + omcost * hnv1 * hnu2 + anu * (-sint * t2v1 + omcost * an2v1 - gamma * tmsint * hnv1));
                ajac[2][2] = cosl2Inv * cosl1
                                * (cost * u1u2 + sint * hu1u2 + omcost * hnu1 * hnu2 + anu * (-sint * t2u1 + omcost * an2u1 - gamma * tmsint * hnu1));
                ajac[2][3] = -q * anu * t2u1 * cosl2Inv;
                ajac[2][4] = -q * anu * t2v1 * cosl2Inv;
                // Xt
                ajac[3][0] = pav * u2dx;
                ajac[3][1] = (sint * v1u2 + omcost * hv1u2 + tmsint * hnu2 * hnv1) / q;
                ajac[3][2] = (sint * u1u2 + omcost * hu1u2 + tmsint * hnu2 * hnu1) * cosl1 / q;
                ajac[3][3] = u1u2;
                ajac[3][4] = v1u2;
                // Yt
                ajac[4][0] = pav * v2dx;
                ajac[4][1] = (sint * v1v2 + omcost * hv1v2 + tmsint * hnv2 * hnv1) / q;
                ajac[4][2] = (sint * u1v2 + omcost * hu1v2 + tmsint * hnv2 * hnu1) * cosl1 / q;
                ajac[4][3] = u1v2;
                ajac[4][4] = v1v2;
        }
        return ajac;
}

simpleHelix::simpleHelix(const double rinv, const double phi0, const double dca, const double dzds, const double z0,
                const double* refPoint, const double Bzc) :
                _xref(refPoint[0]), _yref(refPoint[1]), _zref(refPoint[2]), _bzc(Bzc), _rinv(rinv), _phi0(phi0), _dca(dca), _dzds(dzds), _z0(
                                z0), _xRelCenter(-(1. - _dca * _rinv) * sin(_phi0)), _yRelCenter((1. - _dca * _rinv) * cos(_phi0)), _eps(1.0e-6) {
}

simpleHelix::simpleHelix(const double* parameters, const double* refPoint, const double Bzc) :
                _xref(refPoint[0]), _yref(refPoint[1]), _zref(refPoint[2]), _bzc(Bzc), _rinv(parameters[0]), _phi0(parameters[1]), _dca(
                                parameters[2]), _dzds(parameters[3]), _z0(parameters[4]), _xRelCenter(-(1. - _dca * _rinv) * sin(_phi0)), _yRelCenter(
                                (1. - _dca * _rinv) * cos(_phi0)), _eps(1.0e-6) {
}

void simpleHelix::dump() const {
        std::cout << " simpleHelix " << _rinv << ", " << _phi0 << ", " << _dca << ", " << _dzds << ", " << _z0 << std::endl;
        std::cout << "   rel. center " << _xRelCenter << ", " << _yRelCenter << std::endl;
        std::cout << "   ref. point  " << _xref << ", " << _yref << ", " << _zref << std::endl;
}


double simpleHelix::getArcLengthXY(const double* position) const {
        const double xLoc = position[0] - _xref;
        const double yLoc = position[1] - _yref;
        // line (approximation)
        if ((xLoc * xLoc + yLoc * yLoc) * _rinv * _rinv < _eps * _eps) return cos(_phi0) * xLoc + sin(_phi0) * yLoc;
        // helix
        const double dx = xLoc * _rinv - _xRelCenter;
        const double dy = yLoc * _rinv - _yRelCenter;
        double dphi = atan2(dx, -dy) - _phi0;
        if (abs(dphi) > M_PI) dphi -= (dphi > 0.) ? 2.0 * M_PI : -2.0 * M_PI;
        return dphi / _rinv;
}

void simpleHelix::getZSDirection(double* direction) const {
        const double tanl = _dzds;
        const double cosl = 1. / sqrt(1. + tanl * tanl);
        direction[0] = cosl;
        direction[1] = cosl * tanl;
}


void simpleHelix::getPosAtArcLength(const double sArc, double* position) const {
        if (abs(_rinv * sArc) <= _eps) {
                // line (approximation)
                position[0] = _xref + _dca * sin(_phi0) + sArc * cos(_phi0);
                position[1] = _yref - _dca * cos(_phi0) + sArc * sin(_phi0);
        } else {
                // helix
                const double phi = _phi0 + sArc * _rinv;
                position[0] = _xref + (_xRelCenter + sin(phi)) / _rinv;
                position[1] = _yref + (_yRelCenter - cos(phi)) / _rinv;
        }
        position[2] = _zref + _z0 + sArc * _dzds;
}


bool simpleHelix::getExpectedPlanePos(const double* point, const double measPhi, double& xyPos, double& zPos, double& sArc,
                double& phi, double& lambda) const {
        const double xLoc = point[0] - _xref;
        const double yLoc = point[1] - _yref;
        const double zLoc = point[2] - _zref;
        if ((xLoc * xLoc + yLoc * yLoc) * _rinv * _rinv < _eps * _eps) {
                // line (approximation)
                if (measPhi == _phi0) return false;
                // intersection exists
                const double cosb = sin(measPhi - _phi0);
                const double sinb = cos(measPhi - _phi0);
                sArc = (sin(measPhi) * xLoc - cos(measPhi) * yLoc - _dca * sinb) / cosb;
                xyPos = (sin(_phi0) * xLoc - cos(_phi0) * yLoc - _dca) / cosb;
                phi = _phi0;
        } else {
                // helix
                const double dx = _xRelCenter - xLoc * _rinv;
                const double dy = _yRelCenter - yLoc * _rinv;
                const double ex = cos(measPhi);
                const double ey = sin(measPhi);
                const double A = ex * dx + ey * dy;
                const double B = dx * dx + dy * dy - 1.0;
                // C = ex * dy - ey * dx
                if (A * A < B) return false;
                // intersection exists
                const double cosb = A * sqrt(1.0 - B / (A * A));
                // sinb = C
                xyPos = (A - cosb) / _rinv;
                const double dxSec = (xLoc + ex * xyPos) * _rinv - _xRelCenter;
                const double dySec = (yLoc + ey * xyPos) * _rinv - _yRelCenter;
                phi = atan2(dxSec, -dySec);
                double dphi = phi - _phi0;
                if (abs(dphi) > M_PI) dphi -= dphi > 0. ? 2.0 * M_PI : -2.0 * M_PI;
                sArc = dphi / _rinv;
        }
        zPos = _z0 + sArc * _dzds - zLoc;
        lambda = atan(_dzds);
        return true;
}


TMatrixD simpleHelix::analyticalHelixJacobian(const double phi1, const double ds) const {
        // magnetic field * c (in Z direction)
        Vector3D bFieldc(0., 0., _bzc);
        // track direction vectors T0, T
        const double cosl = 1. / sqrt(1. + _dzds * _dzds);
        const double sinl = cosl * _dzds;
        const double phi2 = phi1 + ds * cosl * _rinv;
        Vector3D dir1(cosl * cos(phi1), cosl * sin(phi1), sinl);
        Vector3D dir2(cosl * cos(phi2), cosl * sin(phi2), sinl);
        // q/p
        const double qbyp = (_rinv != 0.) ? -cosl * _rinv / _bzc : 0.;
        return localHelixAnalyticalJacobian(ds, qbyp, dir1, dir2, bFieldc);
}


TMatrixD simpleHelix::simplifiedHelixJacobian(const double phi1, const double ds) const {
        const double cosl = 1. / sqrt(1. + _dzds * _dzds);
        TMatrixD ajac(5, 5);
        ajac.UnitMatrix();
        ajac[2][0] = -_bzc * ds;
        ajac[3][0] = -0.5 * _bzc * ds * ds * cosl;
        ajac[3][2] = ds * cosl;
        ajac[4][1] = ds;
        return ajac;
}


TMatrixD simpleHelix::curvilinearToPerigeeJacobian() const {
        // define local curvilinear coordinate system
        // U = Z x T / |Z x T|, V = T x U
        const double cosPhi = cos(_phi0);
        const double sinPhi = sin(_phi0);
        const double tanLambda = _dzds;
        const double cosLambda = 1. / sqrt(1. + tanLambda * tanLambda);
        const double sinLambda = cosLambda * _dzds;
        Vector3D T(cosPhi * cosLambda, sinPhi * cosLambda, sinLambda);
        Vector3D U(-sinPhi, cosPhi, 0.);
        Vector3D V(-cosPhi * sinLambda, -sinPhi * sinLambda, cosLambda);
        // helper vectors for local (perigee) system
        // J = -U , K = Z, I = J x K
        Vector3D J(sinPhi, -cosPhi, 0.);
        Vector3D K(0., 0., 1.);
        Vector3D I = J.cross(K);
        // set the right variables for helix parametrisation by wittek/strandlie
        // H is direction of magnetic field, N is H x T/a, a = |H x T|
        Vector3D H(0., 0., 1.); // magnetic field direction
        Vector3D aN = H.cross(T); // a*N
        // formal helpers for later evaluation of jacobian transformation from curvilinear track parameters to perigee frame
        const double ui = U.dot(I), anv = aN.dot(V), ti = T.dot(I);
        const double vi = V.dot(I), anu = aN.dot(U), vk = V.dot(K);

        TMatrixD cl2perjacobian(5, 5);
        cl2perjacobian.UnitMatrix();
        // differences to unit matrix:
        const double Q = _rinv * cosLambda; // -Bz*q/p
        cl2perjacobian[0][0] = -_bzc / cosLambda;
        cl2perjacobian[0][1] = Q * tanLambda / cosLambda;
        cl2perjacobian[0][3] = -Q * Q * tanLambda * ui * anv / cosLambda / ti;
        cl2perjacobian[0][4] = -Q * Q * tanLambda * vi * anv / cosLambda / ti;
        cl2perjacobian[1][1] = -1.;
        cl2perjacobian[1][3] = Q * ui * anv / ti;
        cl2perjacobian[1][4] = Q * vi * anv / ti;
        cl2perjacobian[2][3] = -Q * ui * anu / cosLambda / ti;
        cl2perjacobian[2][4] = -Q * vi * anu / cosLambda / ti;
        cl2perjacobian[3][3] = vk / ti;
        cl2perjacobian[4][4] = -1. / ti;

        return cl2perjacobian;
}


TMatrixD simpleHelix::perigeeToILDJacobian() const {
        TMatrixD per2ILDjacobian(5, 5);
        per2ILDjacobian.UnitMatrix();
        // differences to unit matrix:
        per2ILDjacobian[0][0] = -1.;
        per2ILDjacobian[1][1] = -(1. + _dzds * _dzds);
        per2ILDjacobian[3][3] = -1.;

        return per2ILDjacobian;
}


TMatrixD simpleHelix::perigeeToLCIOJacobian() const {
        TMatrixD per2LCIOjacobian(5, 5);
        // differences to zero matrix:
        per2LCIOjacobian[0][3] = -1.;
        per2LCIOjacobian[1][2] = 1.;
        per2LCIOjacobian[2][0] = -1.;
        per2LCIOjacobian[3][4] = 1.;
        per2LCIOjacobian[4][1] = -(1. + _dzds * _dzds);

        return per2LCIOjacobian;
}


TMatrixD simpleHelix::helixToLCIOJacobian() const {
        TMatrixD hlx2LCIOjacobian(5, 5);
        // differences to zero matrix:
        hlx2LCIOjacobian[0][2] = -1.;
        hlx2LCIOjacobian[1][1] = 1.;
        hlx2LCIOjacobian[2][0] = -1.;
        hlx2LCIOjacobian[3][4] = 1.;
        hlx2LCIOjacobian[4][3] = 1.;

        return hlx2LCIOjacobian;
}


void simpleHelix::moveTo(const double newRefX, const double newRefY, const double newRefZ, double* newPar) {
        const double dx = _xref - newRefX;
        const double dy = _yref - newRefY;
        const double dz = _zref - newRefZ;
        newPar[0] = _rinv;
        newPar[1] = _phi0;
        newPar[2] = _dca;
        newPar[3] = _dzds;
        newPar[4] = _z0 + dz;

        const double cosphi = cos(_phi0);
        const double sinphi = sin(_phi0);
        const double u = 1. - _rinv * _dca;
        const double dp = dx * sinphi - dy * cosphi + _dca;
        const double dl = dx * cosphi + dy * sinphi;
        const double sa = 2. * dp - _rinv * (dp * dp + dl * dl);
        const double sb = -_rinv * dx + u * sinphi;
        const double sc = _rinv * dy + u * cosphi;
        const double sd = sqrt(1. - _rinv * sa);
        // transformed parameters
        if ((dx * dx + dy * dy) * _rinv * _rinv < _eps * _eps) {
                newPar[2] = dp;
                newPar[4] -= dl * _dzds;
        } else {
                newPar[1] = atan2(sb, sc);
                newPar[2] = sa / (1. + sd);
                double dphi = newPar[1] - _phi0;
                if (abs(dphi) > M_PI) dphi -= (dphi > 0.) ? 2.0 * M_PI : -2.0 * M_PI;
                newPar[4] += dphi / _rinv * _dzds;
        }
}


void simpleHelix::getStateAt(const double posX, const double posY, const double posZ, TMatrixDSym& refCov, TVectorD& newPar,
                TMatrixDSym& newCov) {
        const double dx = _xref - posX;
        const double dy = _yref - posY;
        const double dz = _zref - posZ;

        const double cosphi = cos(_phi0);
        const double sinphi = sin(_phi0);
        const double u = 1. - _rinv * _dca;
        const double dp = dx * sinphi - dy * cosphi + _dca;
        const double dl = dx * cosphi + dy * sinphi;
        const double sa = 2. * dp - _rinv * (dp * dp + dl * dl);
        const double sb = -_rinv * dx + u * sinphi;
        const double sc = _rinv * dy + u * cosphi;
        const double sd = sqrt(1. - _rinv * sa);
        // transformed parameters
        double sArc;
        if ((dx * dx + dy * dy) * _rinv * _rinv < _eps * _eps) {
                newPar[0] = _phi0;
                newPar[1] = dp;
                newPar[2] = _dzds;
                sArc = -dl;
                newPar[3] = _z0 + sArc * _dzds + dz;
        } else {
                newPar[0] = _rinv;
                newPar[1] = atan2(sb, sc);
                newPar[2] = sa / (1. + sd);
                newPar[3] = _dzds;
                double dphi = newPar[1] - _phi0;
                if (abs(dphi) > M_PI) dphi -= (dphi > 0.) ? 2.0 * M_PI : -2.0 * M_PI;
                sArc = dphi / _rinv;
                newPar[4] = _z0 + sArc * _dzds + dz;
        }
        // define jacobian
        int nPar = (_rinv == 0.) ? 4 : 5;
        TMatrixD aJac(nPar, nPar);
        aJac.UnitMatrix();
        if ((dx * dx + dy * dy) * _rinv * _rinv < _eps * _eps) {
                aJac[1][0] = -sArc;
                aJac[3][2] = sArc;
        } else {
                const double fc = 1. / (sb * sb + sc * sc);
                const double fl = 0.5 * sa / (sd * (1. + sd) * (1. + sd));
                const double fm = -_rinv * fl + 1. / (sd * (1. + sd));
                aJac[1][0] = -fc * dl;
                aJac[1][1] = fc * u * (1. - _rinv * dp);
                aJac[1][2] = -fc * _rinv * _rinv * dl;
                aJac[2][0] = -fm * (dp * dp + dl * dl) + fl * sa;
                aJac[2][1] = 2. * fm * u * dl;
                aJac[2][2] = 2. * fm * (1. - _rinv * dp);
                aJac[4][0] = _dzds * (aJac[1][0] - sArc) / _rinv;
                aJac[4][1] = _dzds * (aJac[1][1] - 1.) / _rinv;
                aJac[4][2] = _dzds * aJac[1][2] / _rinv;
                aJac[4][3] = sArc;
        }
        // transformed covariance
        newCov = refCov;
        newCov.Similarity(aJac);
}


localHelix::localHelix(const double* parameters, const double* refPoint, const double bScale) :
                _bfac(bScale * 0.0002998), _offset(3), _rotation(3, 3), _rotInv(3, 3) {
        // track direction at PCA
        const double cosPhi = cos(parameters[1]);
        const double sinPhi = sin(parameters[1]);
        const double cosLambda = 1. / sqrt(1. + parameters[3] * parameters[3]);
        _trackDir = cosLambda * Vector3D(cosPhi, sinPhi, parameters[3]);
        // position at PCA
        double pca[] = { refPoint[0] + sinPhi * parameters[2], refPoint[1] - cosPhi * parameters[2], refPoint[2] + parameters[4] };
        _offset = TVectorD(3, pca);
        // Bfield at PCA
        _bFieldc = getBFieldc(pca);
        // Q/P (= const)
        _qbyp = (_bFieldc.z() != 0.) ? -cosLambda * parameters[0] / _bFieldc.z() : 0.;
        // local parameters
        calcLocal();
}


void localHelix::calcLocal() {
        // calculate rotation
        Vector3D h = (_bFieldc.r() != 0.) ? _bFieldc.unit() : Vector3D(0., 0., 1.); // in field (or Z) direction
        Vector3D t = _trackDir; // in track direction
        Vector3D n = h.cross(t).unit(); // in bending plane perpendicular to track direction
        Vector3D p = n.cross(h); // in bending plane parallel to track direction
        // local system is (p,n,h)
        for (int i = 0; i < 3; ++i) {
                _rotation[0][i] = p[i];
                _rotation[1][i] = n[i];
                _rotation[2][i] = h[i];
                _localRef[i] = 0.;
        }
        _rotInv = _rotation;
        _rotInv.Invert();
        // calculate local track parameters
        const double gamma = h.dot(t);
        const double alpha = sqrt(1. - gamma * gamma);
        _cosLambda = alpha;
        _localPar[0] = _qbyp * _bFieldc.r() / (-alpha); // rho
        _localPar[1] = 0.; // phi0
        _localPar[2] = 0.; // rho
        _localPar[3] = gamma / alpha; // dzds
        _localPar[4] = 0.; // z0
}


void localHelix::calcGlobal() {
        // local direction
        TVectorD localDir(3);
        localDir[0] = cos(_localPar[1]) * _cosLambda;
        localDir[1] = sin(_localPar[1]) * _cosLambda;
        localDir[2] = _localPar[3] * _cosLambda;
        // global direction
        TVectorD globalDir(_rotInv * localDir);
        for (int i = 0; i < 3; ++i)
                _trackDir[i] = globalDir[i];
        // global offset
        _offset += _rotInv * TVectorD(3, _localRef);
}

void localHelix::dump() const {
        cout << " localHelix, Q/P = " << _qbyp << endl;
        cout << "   Bfield*c  " << _bFieldc.x() << ", " << _bFieldc.y() << ", " << _bFieldc.z() << endl;
        cout << "   direction " << _trackDir.x() << ", " << _trackDir.y() << ", " << _trackDir.z() << endl;
        cout << "   offset    " << _offset[0] << ", " << _offset[1] << ", " << _offset[2] << ", " << endl;
        cout << "   rotation " << endl;
        _rotation.Print();
}


TMatrixD localHelix::propagateTo(const double* point, const double mstep) {
        TMatrixD aJac(5, 5);
        if (_qbyp == 0.) {
                // line
                // current helix
                simpleHelix startHelix(_localPar, _localRef, _bFieldc.r());
                // position relative to offset
                double relPos[] = { point[0] - _offset[0], point[1] - _offset[1], point[2] - _offset[2] };
                // 3D arc length to point
                double deltaW = startHelix.getArcLengthXY(relPos) / _cosLambda;
                aJac = startHelix.analyticalHelixJacobian(0., deltaW);
                // adjust offset
                for (int i = 0; i < 3; ++i)
                        _offset[i] += deltaW * _trackDir[i];
        } else {
                //helix
                const double maxStep = abs(mstep);
                aJac.UnitMatrix();
                double sArc, newPos[3], step;
                TVectorD localPos(3);
                do {
                        // current helix, (global) track direction, field
                        simpleHelix startHelix(_localPar, _localRef, _bFieldc.r());
                        Vector3D tStart = _trackDir;
                        Vector3D bStart = _bFieldc;
                        // arc length to point
                        localPos = _rotation * (TVectorD(3, point) - _offset);
                        sArc = startHelix.getArcLengthXY(localPos.GetMatrixArray());
                        // step size in 3D, limited to _maxStep
                        step = abs(sArc / _cosLambda) > maxStep ? (sArc > 0. ? maxStep : -maxStep) : sArc / _cosLambda;
                        // new position (on helix) after 'step' for new magnetic field
                        startHelix.getPosAtArcLength(step * _cosLambda, newPos);
                        TVectorD newGlobalPos(_rotInv * TVectorD(3, newPos) + _offset);
                        // new field
                        Vector3D newBc = getBFieldc(newGlobalPos.GetMatrixArray());
                        // average field
                        _bFieldc = 0.5 * (_bFieldc + newBc);
                        // adjust local parameters
                        calcLocal();
                        // propgate using helix with average field
                        simpleHelix avgHelix(_localPar, _localRef, _bFieldc.r());
                        // redo arc length to point
                        localPos = _rotation * (TVectorD(3, point) - _offset);
                        sArc = startHelix.getArcLengthXY(localPos.GetMatrixArray());
                        step = abs(sArc / _cosLambda) > maxStep ? (sArc > 0. ? maxStep : -maxStep) : sArc / _cosLambda;
                        // new position (on helix) after 'step' as new reference point
                        startHelix.getPosAtArcLength(step * _cosLambda, _localRef);
                        // adjust track parameters at new (reference) position
                        _localPar[1] += step * _localPar[0];
                        // adjust global parameters
                        calcGlobal();
                        // update jacobian
                        aJac = localHelixAnalyticalJacobian(step, _qbyp, tStart, _trackDir, bStart) * aJac;
                        // new field
                        _bFieldc = newBc;
                        // adjust local parameters
                        calcLocal();
                } while (abs(sArc / _cosLambda) > maxStep);
        }
        return aJac;
}


simpleHelix localHelix::getSimpleHelix() const {
        double globalPar[5];
        // calculate global track parameters
        const double cosLambda = sqrt(1. - _trackDir[2] * _trackDir[2]);
        globalPar[0] = _qbyp * _bFieldc.z() / (-cosLambda); // rho
        globalPar[1] = atan2(_trackDir[1], _trackDir[0]); // phi0
        globalPar[2] = 0.; // dca
        globalPar[3] = _trackDir[2] / cosLambda; // dzds = tanLambda
        globalPar[4] = 0.; // z0
        return simpleHelix(globalPar, _offset.GetMatrixArray(), _bFieldc.z());
}


Vector3D localHelix::getBFieldc(const double* pos) const {
        Vector3D bfield = marlin::Global::GEAR->getBField().at(Vector3D(pos));
        return _bfac * bfield;
}


simpleFitXY::simpleFitXY(bool flag, double xr, double yr) :
                _curved(flag), _npar((flag) ? 3 : 2), _xRef(xr), _yRef(yr), _parameters(_npar), _covariance(_npar) {
        _numPoints = 0;
        _sx = _sy = _sxx = _sxy = _syy = _sw = 0.;
        _sr = _sxr = _syr = _srr = 0.;
}


void simpleFitXY::addPoint(double x, double y, double w) {
        _numPoints++;
        double xl = x - _xRef;
        double yl = y - _yRef;
        _sw += w;
        _sx += w * xl;
        _sy += w * yl;
        _sxx += w * xl * xl;
        _sxy += w * xl * yl;
        _syy += w * yl * yl;
        if (_curved) {
                double r2 = xl * xl + yl * yl;
                _sr += w * r2;
                _sxr += w * r2 * xl;
                _syr += w * r2 * yl;
                _srr += w * r2 * r2;
        }
}


void simpleFitXY::addTrack(EVENT::Track const * seedTrack) {
        EVENT::TrackerHitVec inputHitVec = seedTrack->getTrackerHits();
        for (unsigned int iHit = 0; iHit < inputHitVec.size(); iHit++) {
                this->simpleFitXY::addPoint(inputHitVec[iHit]->getPosition()[0], inputHitVec[iHit]->getPosition()[1], 1);
        }

}


int simpleFitXY::fit(double& Chi2, int& nPoints) {
        // averages
        double ax = _sx / _sw;
        double ay = _sy / _sw;
        double ar = _sr / _sw;
        double axx = _sxx / _sw;
        double ayy = _syy / _sw;
        double axy = _sxy / _sw;
        double axr = _sxr / _sw;
        double ayr = _syr / _sw;
        double arr = _srr / _sw;
        // variances
        double cxx = axx - ax * ax;
        double cyy = ayy - ay * ay;
        double cxy = axy - ax * ay;
        double cxr = axr - ax * ar;
        double cyr = ayr - ay * ar;
        double crr = arr - ar * ar;

        double q1, q2;
        if (_curved) {
                q1 = crr * cxy - cxr * cyr;
                q2 = crr * (cxx - cyy) - cxr * cxr + cyr * cyr;
        } else {
                q1 = cxy;
                q2 = cxx - cyy;
        }
        double phi = 0.5 * atan2(2. * q1, q2);
        double sinphi = sin(phi);
        double cosphi = cos(phi);
        // compare phi with initial track direction
        if (cosphi * (ax + _xRef) + sinphi * (ay + _yRef) < 0.) {
                // reverse direction
                phi -= (phi > 0.) ? M_PI : -M_PI;
                cosphi = -cosphi;
                sinphi = -sinphi;
        }
        if (_curved) {
                double kappa = (sinphi * cxr - cosphi * cyr) / crr;
                double delta = -kappa * ar + sinphi * ax - cosphi * ay;
                // track parameters
                double rho = -2. * kappa / sqrt(1. - 4. * delta * kappa);
                double d = 2. * delta / (1. + sqrt(1. - 4. * delta * kappa));
                _parameters[0] = rho;
                _parameters[1] = phi;
                _parameters[2] = d;
                // chi2
                double u = 1. - rho * d;
                Chi2 = _sw * u * u * (sinphi * sinphi * cxx - 2. * sinphi * cosphi * cxy + cosphi * cosphi * cyy - kappa * kappa * crr);
                //cout << " xyfit " << Chi2 << " " << _numPoints << " " <<_npar << ": " << rho << " " << phi << " " << d << endl;
                // calculate covariance matrix
                double sa = sinphi * _sx - cosphi * _sy;
                double sb = cosphi * _sx + sinphi * _sy;
                double sc = (sinphi * sinphi - cosphi * cosphi) * _sxy + sinphi * cosphi * (_sxx - _syy);
                double sd = sinphi * _sxr - cosphi * _syr;
                double saa = sinphi * sinphi * _sxx - 2. * sinphi * cosphi * _sxy + cosphi * cosphi * _syy;
                _covariance[0][0] = 0.25 * _srr - d * (sd - d * (saa + 0.5 * _sr - d * (sa - 0.25 * d * _sw)));
                _covariance[0][1] = u * (0.5 * (cosphi * _sxr + sinphi * _syr) - d * (sc - 0.5 * d * sb));
                _covariance[1][0] = _covariance[0][1];
                _covariance[1][1] = u * u * (cosphi * cosphi * _sxx + 2. * cosphi * sinphi * _sxy + sinphi * sinphi * _syy);
                _covariance[0][2] = rho * (-0.5 * sd + d * saa) - 0.5 * u * _sr + 0.5 * d * ((3 * u - 1.) * sa - u * d * _sw);
                _covariance[2][0] = _covariance[0][2];
                _covariance[1][2] = -u * (rho * sc + u * sb);
                _covariance[2][1] = _covariance[1][2];
                _covariance[2][2] = rho * (rho * saa + 2 * u * sa) + u * u * _sw;
        } else {
                // track parameters
                double d = sinphi * ax - cosphi * ay;
                _parameters[0] = phi;
                _parameters[1] = d;
                // chi2
                Chi2 = _sw * (sinphi * sinphi * cxx - 2. * sinphi * cosphi * cxy + cosphi * cosphi * cyy);
                //cout << " xyfit " << chi2 << " " << _numPoints-_npar << ": " << phi << " " << d << endl;
                // calculate covariance matrix
                _covariance[0][0] = cosphi * cosphi * _sxx + 2. * cosphi * sinphi * _sxy + sinphi * sinphi * _syy;
                _covariance[0][1] = -(cosphi * _sx + sinphi * _sy);
                _covariance[1][0] = _covariance[0][1];
                _covariance[1][1] = _sw;
        }
        _covariance.Invert();
        nPoints = _numPoints;
        return _npar;
}


TVectorD simpleFitXY::getPar() const {
        return _parameters;
}


TMatrixDSym simpleFitXY::getCov() const {
        return _covariance;
}

simpleFitZS::simpleFitZS() :
                _npar(2), _parameters(_npar), _covariance(_npar) {
        _numPoints = 0;
        _sx = _sy = _sxx = _sxy = _syy = _sw = 0.;
}


void simpleFitZS::addPoint(double x, double y, double w) {
        _numPoints++;
        // x is S, y is Z
        _sw += w;
        _sx += w * x;
        _sy += w * y;
        _sxx += w * x * x;
        _sxy += w * x * y;
        _syy += w * y * y;
}


int simpleFitZS::fit(double& Chi2, int& nPoints) {
        // linear equation system A*x = b
        //cout << " zsfit " << endl;
        TVectorD bVec(2);
        bVec[0] = _sxy;
        bVec[1] = _sy;
        TMatrixDSym aMat(2);
        aMat[0][0] = _sxx;
        aMat[0][1] = aMat[1][0] = _sx;
        aMat[1][1] = _sw;
        // solve
        aMat.Invert();
        _parameters = aMat * bVec;
        _covariance = aMat;
        // chi2
        Chi2 = _parameters[0] * _parameters[0] * _sxx + _parameters[1] * _parameters[1] * _sw + _syy
                        + 2. * _parameters[0] * _parameters[1] * _sx - 2. * _parameters[0] * _sxy - 2. * _parameters[1] * _sy;
        //cout << " zsfit " << Chi2 << " " << _numPoints-_npar << ": " << _parameters[0] << " " << _parameters[1] << endl;
        nPoints = _numPoints;
        return _npar;
}


TVectorD simpleFitZS::getPar() const {
        return _parameters;
}


TMatrixDSym simpleFitZS::getCov() const {
        return _covariance;
}

} // close namespace brackets