TrackingPlane.cxx

Go to the documentation of this file.

#include "TrackingPlane.h"

namespace recob {
  namespace tracking {

    // formulas for 5->6 and 6->5 transformations
    //
    // par6d[0] = planePos.X() + par5d[0]*cosalpha;
    // par6d[1] = planePos.Y() + par5d[0]*sinalpha*sinbeta + par5d[1]*cosbeta;
    // par6d[2] = planePos.Z() - par5d[0]*sinalpha*cosbeta + par5d[1]*sinbeta;
    // par6d[3] = (par5d[2]*cosalpha + 1.*sinalpha)/(par5d[4]*std::sqrt(1. + par5d[2]*par5d[2] + par5d[3]*par5d[3]));
    // par6d[4] = (par5d[2]*sinalpha*sinbeta + par5d[3]*cosbeta - cosalpha*sinbeta)/(par5d[4]*std::sqrt(1. + par5d[2]*par5d[2] + par5d[3]*par5d[3]));
    // par6d[5] = (-par5d[2]*sinalpha*cosbeta + par5d[3]*sinbeta + cosalpha*cosbeta)/(par5d[4]*std::sqrt(1. + par5d[2]*par5d[2] + par5d[3]*par5d[3]));
    //
    // par5d[0] = (par6d[0]-planePos.X())*cosalpha + (par6d[1]-planePos.Y())*sinalpha*sinbeta - (par6d[2]-planePos.Z())*sinalpha*cosbeta;
    // par5d[1] = (par6d[1]-planePos.Y())*cosbeta + (par6d[2]-planePos.Z())*sinbeta;
    // par5d[2] = (par6d[3]*cosalpha + par6d[4]*sinalpha*sinbeta - par6d[5]*sinalpha*cosbeta)/(par6d[3]*sinalpha - par6d[4]*cosalpha*sinbeta + par6d[5]*cosalpha*cosbeta);
    // par5d[3] = (par6d[4]*cosbeta + par6d[5]*sinbeta)/(par6d[3]*sinalpha - par6d[4]*cosalpha*sinbeta + par6d[5]*cosalpha*cosbeta);
    // par5d[4] = 1./sqrt(par6d[3]*par6d[3]+par6d[4]*par6d[4]+par6d[5]*par6d[5]);

    SVector6 Plane::Local5DToGlobal6DParameters(const SVector5& par5d, const Point_t& planePos,const Vector_t& /* planeDir */, const TrigCache& trigCache, bool trackAlongPlaneDir) const {
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      const double denom = (trackAlongPlaneDir ? par5d[4]*std::sqrt(1. + par5d[2]*par5d[2] + par5d[3]*par5d[3]) : -par5d[4]*std::sqrt(1. + par5d[2]*par5d[2] + par5d[3]*par5d[3]) );
      SVector6 par6d;
      par6d[0] = planePos.X() + par5d[0]*cosalpha;
      par6d[1] = planePos.Y() + par5d[0]*sinalpha*sinbeta + par5d[1]*cosbeta;
      par6d[2] = planePos.Z() - par5d[0]*sinalpha*cosbeta + par5d[1]*sinbeta;
      par6d[3] = (par5d[2]*cosalpha + 1.*sinalpha)/denom;
      par6d[4] = (par5d[2]*sinalpha*sinbeta + par5d[3]*cosbeta - cosalpha*sinbeta)/denom;
      par6d[5] = (-par5d[2]*sinalpha*cosbeta + par5d[3]*sinbeta + cosalpha*cosbeta)/denom;
      return par6d;
    }

    SVector5 Plane::Global6DToLocal5DParameters(const SVector6& par6d, const Point_t& planePos,const Vector_t& /* planeDir */, const TrigCache& trigCache) const {
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      const double pu = par6d[3]*cosalpha + par6d[4]*sinalpha*sinbeta - par6d[5]*sinalpha*cosbeta;
      const double pv = par6d[4]*cosbeta + par6d[5]*sinbeta;
      const double pw = par6d[3]*sinalpha - par6d[4]*cosalpha*sinbeta + par6d[5]*cosalpha*cosbeta;
      const double pval = sqrt(par6d[3]*par6d[3]+par6d[4]*par6d[4]+par6d[5]*par6d[5]);
      SVector5 par5d;
      par5d[0] = (par6d[0]-planePos.X())*cosalpha + (par6d[1]-planePos.Y())*sinalpha*sinbeta - (par6d[2]-planePos.Z())*sinalpha*cosbeta;
      par5d[1] = (par6d[1]-planePos.Y())*cosbeta + (par6d[2]-planePos.Z())*sinbeta;
      par5d[2] = pu/pw;
      par5d[3] = pv/pw;
      par5d[4] = (pval>0 ? 1./pval : 1.);
      return par5d;
    }

    SMatrix65 Plane::Local5DToGlobal6DJacobian(bool hasMomentum, const Vector_t& trackMomOrDir, const Vector_t& planeDir, const TrigCache& trigCache) const {
      bool trackAlongPlaneDir = trackMomOrDir.Dot(planeDir)>0;
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      const double pu = trackMomOrDir.X()*cosalpha + trackMomOrDir.Y()*sinalpha*sinbeta - trackMomOrDir.Z()*sinalpha*cosbeta;
      const double pv = trackMomOrDir.Y()*cosbeta + trackMomOrDir.Z()*sinbeta;
      const double pw = trackMomOrDir.X()*sinalpha - trackMomOrDir.Y()*cosalpha*sinbeta + trackMomOrDir.Z()*cosalpha*cosbeta;
      //local parameters 2,3,4
      const double l2 = pu/pw;
      const double l3 = pv/pw;
      const double p2 = trackMomOrDir.X()*trackMomOrDir.X() + trackMomOrDir.Y()*trackMomOrDir.Y() + trackMomOrDir.Z()*trackMomOrDir.Z();
      const double l4 = (hasMomentum ? 1./sqrt(p2) : 1.);
      const double den23 = ( trackAlongPlaneDir ? l4*(l2*l2+l3*l3+1.)*sqrt(l2*l2+l3*l3+1.) : -l4*(l2*l2+l3*l3+1.)*sqrt(l2*l2+l3*l3+1.) );
      const double den4 = l4*l4*sqrt(l2*l2+l3*l3+1.);
      SMatrix65 j;
      //
      j(0,0) = cosalpha;
      j(0,1) = 0.;
      j(0,2) = 0.;
      j(0,3) = 0.;
      j(0,4) = 0.;
      //
      j(1,0) = sinalpha*sinbeta;
      j(1,1) = cosbeta;
      j(1,2) = 0.;
      j(1,3) = 0.;
      j(1,4) = 0.;
      //
      j(2,0) = -sinalpha*cosbeta;
      j(2,1) = sinbeta;
      j(2,2) = 0.;
      j(2,3) = 0.;
      j(2,4) = 0.;
      //
      j(3,0) = 0.;
      j(3,1) = 0.;
      j(3,2) = (cosalpha*(l3*l3+1.) - sinalpha*l2)/den23;
      j(3,3) = -l3*(l2*cosalpha + sinalpha)/den23;
      j(3,4) = (hasMomentum ? -(l2*cosalpha + sinalpha)/den4 : 0.);
      //
      j(4,0) = 0.;
      j(4,1) = 0.;
      j(4,2) = (cosalpha*sinbeta*l2 - cosbeta*l2*l3 + sinalpha*sinbeta*(l3*l3+1.))/den23;
      j(4,3) = (sinbeta*l3*(cosalpha-sinalpha*l2) + cosbeta*(l2*l2+1.))/den23;
      j(4,4) = (hasMomentum ? (cosalpha*sinbeta - cosbeta*l3 - sinalpha*sinbeta*l2)/den4 : 0.);
      //
      j(5,0) = 0.;
      j(5,1) = 0.;
      j(5,2) = -(cosalpha*cosbeta*l2 + cosbeta*sinalpha*l3*l3 + cosbeta*sinalpha + sinbeta*l2*l3)/den23;
      j(5,3) = (-cosalpha*cosbeta*l3 + cosbeta*sinalpha*l2*l3 + sinbeta*l2*l2 + sinbeta)/den23;
      j(5,4) = (hasMomentum ? (-cosalpha*cosbeta + cosbeta*sinalpha*l2 - sinbeta*l3)/den4 : 0.);
      //
      return j;
    }

    SMatrix56 Plane::Global6DToLocal5DJacobian(bool hasMomentum, const Vector_t& trackMomOrDir, const Vector_t& /* planeDir */, const TrigCache& trigCache) const {
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      const double den23 = (cosalpha*(cosbeta*trackMomOrDir.Z() - sinbeta*trackMomOrDir.Y()) + sinalpha*trackMomOrDir.X())*(cosalpha*(cosbeta*trackMomOrDir.Z() - sinbeta*trackMomOrDir.Y()) + sinalpha*trackMomOrDir.X());
      const double den4 = sqrt(trackMomOrDir.X()*trackMomOrDir.X()+trackMomOrDir.Y()*trackMomOrDir.Y()+trackMomOrDir.Z()*trackMomOrDir.Z())*(trackMomOrDir.X()*trackMomOrDir.X()+trackMomOrDir.Y()*trackMomOrDir.Y()+trackMomOrDir.Z()*trackMomOrDir.Z());
      SMatrix56 j;
      //
      j(0,0) = cosalpha;
      j(0,1) = sinalpha*sinbeta ;
      j(0,2) = -sinalpha*cosbeta;
      j(0,3) = 0.;
      j(0,4) = 0.;
      j(0,5) = 0.;
      //
      j(1,0) = 0.;
      j(1,1) = cosbeta;
      j(1,2) = sinbeta;
      j(1,3) = 0.;
      j(1,4) = 0.;
      j(1,5) = 0.;
      //
      j(2,0) = 0.;
      j(2,1) = 0.;
      j(2,2) = 0.;
      j(2,3) = ((cosalpha*cosalpha + sinalpha*sinalpha)*(cosbeta*trackMomOrDir.Z() - sinbeta*trackMomOrDir.Y()))/den23;
      j(2,4) = (sinbeta*trackMomOrDir.X()*(cosalpha*cosalpha + sinalpha*sinalpha))/den23;
      j(2,5) = -(cosbeta*trackMomOrDir.X()*(cosalpha*cosalpha + sinalpha*sinalpha))/den23;
      //
      j(3,0) = 0.;
      j(3,1) = 0.;
      j(3,2) = 0.;
      j(3,3) = -(sinalpha*(cosbeta*trackMomOrDir.Y() + sinbeta*trackMomOrDir.Z()))/den23;
      j(3,4) = (cosalpha*trackMomOrDir.Z()*(cosbeta*cosbeta + sinbeta*sinbeta) + cosbeta*sinalpha*trackMomOrDir.X())/den23;
      j(3,5) = (-cosalpha*cosbeta*cosbeta*trackMomOrDir.Y() - cosalpha*sinbeta*sinbeta*trackMomOrDir.Y() + sinalpha*sinbeta*trackMomOrDir.X())/den23;
      //
      j(4,0) = 0.;
      j(4,1) = 0.;
      j(4,2) = 0.;
      j(4,3) = (hasMomentum ? -trackMomOrDir.X()/den4 : 0.);
      j(4,4) = (hasMomentum ? -trackMomOrDir.Y()/den4 : 0.);
      j(4,5) = (hasMomentum ? -trackMomOrDir.Z()/den4 : 0.);
      //
      return j;
    }

    Rotation_t Plane::Global3DToLocal3DRotation(const Vector_t& /* planeDir */, const TrigCache& trigCache) const {
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      return {
        cosalpha /* xx */,  sinalpha * sinbeta /* xy */, -sinalpha * cosbeta /* xz */,
        0.0      /* yx */,  cosbeta            /* yy */,  sinbeta            /* yz */,
        sinalpha /* zx */, -cosalpha * sinbeta /* zy */,  cosalpha * cosbeta /* zz */
      };
    }

    Rotation_t Plane::Local3DToGlobal3DRotation(const Vector_t& /* planeDir */, const TrigCache& trigCache) const {
      const double& sinalpha = trigCache.fSinA;
      const double& cosalpha = trigCache.fCosA;
      const double& sinbeta  = trigCache.fSinB;
      const double& cosbeta  = trigCache.fCosB;
      return {
        cosalpha            /* xx */, 0.      /* xy */,  sinalpha           /* xz */,
        sinalpha * sinbeta  /* yx */, cosbeta /* yy */, -cosalpha * sinbeta /* yz */,
        -sinalpha * cosbeta /* zx */, sinbeta /* zy */,  cosalpha * cosbeta /* zz */
      };
    }

  }
}