36typedef Eigen::Matrix<double, 5, 5>
Matrix5d;
45 return 0.015 * fabs(qbyp) * sqrt(xbyx0);
61 jac(1, 0) = -bfac * ds * cosl;
62 jac(3, 0) = -0.5 * bfac * ds * ds * cosl;
70 static double unrm2 = 0.0;
71 static bool cached =
false;
75 x = 2.0 *
static_cast<double>(rand())
76 /
static_cast<double>(RAND_MAX) - 1;
77 y = 2.0 *
static_cast<double>(rand())
78 /
static_cast<double>(RAND_MAX) - 1;
80 }
while (r == 0.0 || r > 1.0);
82 double d = sqrt(-2.0 * log(r) / r);
95 return static_cast<double>(rand()) /
static_cast<double>(RAND_MAX);
111 const Vector3d &tDir,
const Vector3d &uDir,
const Vector3d &vDir,
112 const Vector3d &nDir,
const Vector3d &aPos) :
113 sarc(sArc), pred(aPred), tdir(tDir), udir(uDir), vdir(vDir), ndir(nDir), pos(
115 global2meas << uDir.transpose(), vDir.transpose(), nDir.transpose();
151 const double cosTheta =
tdir[2];
153 const double cosPhi =
tdir[0] / sinTheta;
154 const double sinPhi =
tdir[1] / sinTheta;
155 Eigen::Matrix<double, 2, 3> uvDir;
156 uvDir << -sinPhi, cosPhi, 0., -cosPhi * cosTheta, -sinPhi * cosTheta, sinTheta;
171 double aDzds,
double aZ0) :
172 rinv(aRinv), phi0(aPhi0), dca(aDca), dzds(aDzds), z0(aZ0), cosPhi0(
173 cos(phi0)), sinPhi0(sin(phi0)), xRelCenter(
174 -(1. - dca * rinv) * sinPhi0), yRelCenter(
175 (1. - dca * rinv) * cosPhi0) {
188 double arg = (0.5 *
rinv * (aRadius * aRadius +
dca *
dca) -
dca)
189 / (aRadius * (1.0 -
rinv *
dca));
190 return asin(arg) +
phi0;
200 double arg = (0.5 *
rinv * (aRadius * aRadius +
dca *
dca) -
dca)
201 / (aRadius * (1.0 -
rinv *
dca));
202 if (fabs(arg) >= 1.) {
203 std::cout <<
" bad arc " << aRadius <<
" " <<
rinv <<
" " <<
dca
209 return sqrt(aRadius * aRadius -
dca *
dca);
211 double sxy = asin(aRadius *
rinv * sqrt(1.0 - arg * arg)) /
rinv;
214 sxy = M_PI / fabs(
rinv) - sxy;
230 double dphi = atan2(dx, -dy) -
phi0;
233 else if (dphi < -M_PI)
247 double &newDca,
double &newZ0)
const {
253 const double u = 1. -
rinv *
dca;
256 const double sa = 2. * dp -
rinv * (dp * dp + dl * dl);
259 const double sd = sqrt(1. -
rinv * sa);
266 newPhi0 = atan2(sb, sc);
267 newDca = sa / (1. + sd);
268 double dphi = newPhi0 -
phi0;
271 else if (dphi < -M_PI)
275 newZ0 += sArc *
dzds;
285 const Eigen::Vector3d &uDir,
const Eigen::Vector3d &vDir)
const {
287 Vector3d nDir = uDir.cross(vDir).normalized();
289 const double cosLambda = 1. / sqrt(1. +
dzds *
dzds);
290 const double sinLambda =
dzds * cosLambda;
292 Vector3d dist, pos, tDir;
301 double sArc3D = -dist.dot(nDir) / tDir.dot(nDir);
302 sArc2D = sArc3D * cosLambda;
304 pos = pca + sArc3D * tDir;
310 unsigned int nIter = 0;
314 const double dPhi = sArc2D *
rinv;
315 const double cosPhi = cos(
phi0 + dPhi);
316 const double sinPhi = sin(
phi0 + dPhi);
317 tDir << cosLambda * cosPhi, cosLambda * sinPhi, sinLambda;
324 const double sCorr3D = -dist.dot(nDir) / tDir.dot(nDir);
325 if (fabs(sCorr3D) > 0.00001) {
327 sArc2D += sCorr3D * cosLambda;
335 Vector2d pred(dist.dot(uDir), dist.dot(vDir));
354 const unsigned int aLayer,
const int aDim,
const double thickness,
355 Eigen::Vector3d &aCenter, Eigen::Vector2d &aResolution,
356 Eigen::Vector2d &aPrecision, Eigen::Matrix3d &measTrafo,
357 Eigen::Matrix3d &alignTrafo) :
358 name(aName), layer(aLayer), measDim(aDim), xbyx0(thickness), center(
359 aCenter), resolution(aResolution), precision(aPrecision), global2meas(
360 measTrafo), global2align(alignTrafo) {
371 IOFormat CleanFmt(4, 0,
", ",
"\n",
"[",
"]");
373 <<
xbyx0 <<
" X0, @ " <<
center.transpose().format(CleanFmt)
374 <<
", res " <<
resolution.transpose().format(CleanFmt) <<
", udir "
375 <<
udir.transpose().format(CleanFmt) <<
", vdir "
376 <<
vdir.transpose().format(CleanFmt) << std::endl;
435 Eigen::Vector3d &position, Eigen::Vector3d &direction)
const {
437 Vector3d dist = position;
439 Matrix3d drdm = Matrix3d::Identity()
440 - (direction *
ndir.transpose()) / (direction.transpose() *
ndir);
442 Matrix<double, 3, 6> dmdg = Matrix<double, 3, 6>::Zero();
444 dmdg(0, 4) = -dist(2);
445 dmdg(0, 5) = dist(1);
447 dmdg(1, 3) = dist(2);
448 dmdg(1, 5) = -dist(0);
450 dmdg(2, 3) = -dist(1);
451 dmdg(2, 4) = dist(0);
470 Eigen::Vector3d &position, Eigen::Vector3d &direction)
const {
474 const double uSlope = tLoc[0] / tLoc[2];
475 const double vSlope = tLoc[1] / tLoc[2];
478 const double uPos = dist[0];
479 const double vPos = dist[1];
482 Matrix<double, 2, 6> drldg;
483 drldg << 1.0, 0.0, -uSlope, vPos * uSlope, -uPos * uSlope, vPos, 0.0, 1.0, -vSlope, vPos
484 * vSlope, -uPos * vSlope, -uPos;
487 return local2meas.block<2, 2>(0, 0) * drldg;
498 Eigen::Vector3d &offset, Eigen::Matrix3d &rotation)
const {
500 Matrix<double, 6, 6> glo2loc = Matrix<double, 6, 6>::Zero();
502 leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
503 glo2loc.block<3, 3>(0, 0) = rotation;
504 glo2loc.block<3, 3>(0, 3) = -rotation * leverArms;
505 glo2loc.block<3, 3>(3, 3) = rotation;
517 Eigen::Vector3d &offset, Eigen::Matrix3d &rotation)
const {
519 Matrix<double, 6, 6> loc2glo = Matrix<double, 6, 6>::Zero();
521 leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
522 loc2glo.block<3, 3>(0, 0) = rotation.transpose();
523 loc2glo.block<3, 3>(0, 3) = leverArms * rotation.transpose();
524 loc2glo.block<3, 3>(3, 3) = rotation.transpose();
Definitions for GBL utilities.
double xbyx0
normalized material thickness
Eigen::Vector3d getCenter() const
Get center.
GblHelixPrediction intersectWithHelix(GblSimpleHelix hlx) const
Intersect with helix.
Eigen::Vector3d ndir
normal to measurement plane
Eigen::Vector3d center
center
double getRadiationLength() const
Get radiation length.
void print() const
Print GblDetectorLayer.
unsigned int measDim
measurement dimension (1 or 2)
Eigen::Matrix< double, 2, 6 > getRigidBodyDerLocal(Eigen::Vector3d &position, Eigen::Vector3d &direction) const
Get rigid body derivatives in local (alignment) frame (rotated in measurement plane).
unsigned int getLayerID() const
Get layer ID.
Eigen::Matrix3d getMeasSystemDirs() const
Get directions of measurement system.
Eigen::Matrix< double, 6, 6 > getTrafoGlobalToLocal(Eigen::Vector3d &offset, Eigen::Matrix3d &rotation) const
Get transformation for rigid body derivatives from global to local (alignment) system.
Eigen::Matrix< double, 3, 6 > getRigidBodyDerGlobal(Eigen::Vector3d &position, Eigen::Vector3d &direction) const
Get rigid body derivatives in global frame.
Eigen::Matrix3d global2meas
transformation into measurement system
unsigned int layer
layer ID
GblDetectorLayer(const std::string aName, const unsigned int aLayer, const int aDim, const double thickness, Eigen::Vector3d &aCenter, Eigen::Vector2d &aResolution, Eigen::Vector2d &aPrecision, Eigen::Matrix3d &measTrafo, Eigen::Matrix3d &alignTrafo)
Create a detector layer.
Eigen::Matrix3d getAlignSystemDirs() const
Get directions of alignment system.
virtual ~GblDetectorLayer()
Eigen::Vector2d precision
measurements precision
Eigen::Vector2d getResolution() const
Get resolution.
Eigen::Matrix< double, 6, 6 > getTrafoLocalToGlobal(Eigen::Vector3d &offset, Eigen::Matrix3d &rotation) const
Get transformation for rigid body derivatives from local (alignment) to global system.
Eigen::Vector2d resolution
measurements resolution
Eigen::Vector2d getPrecision() const
Get precision.
Eigen::Matrix3d global2align
transformation into (local) alignment system
Eigen::Matrix< double, 2, 3 > getCurvilinearDirs() const
Get curvilinear directions (U,V)
const Eigen::Vector3d pos
position at prediction
double getArcLength() const
Get arc-length.
const Eigen::Vector2d & getMeasPred() const
Get (measurement) prediction.
const double sarc
arc-length at prediction
double getCosIncidence() const
Get cosine of incidence.
Eigen::Matrix3d global2meas
transformation into measurement system
const Eigen::Vector3d & getPosition() const
Get position.
const Eigen::Vector2d pred
prediction for measurement (u,v)
const Eigen::Vector3d tdir
track direction at prediction
GblHelixPrediction(double sArc, const Eigen::Vector2d &aPred, const Eigen::Vector3d &tDir, const Eigen::Vector3d &uDir, const Eigen::Vector3d &vDir, const Eigen::Vector3d &nDir, const Eigen::Vector3d &aPos)
Create helix prediction.
const Eigen::Vector3d & getDirection() const
Get position.
const Eigen::Vector3d ndir
normal to measurement plane
virtual ~GblHelixPrediction()
double getArcLengthR(double aRadius) const
Get (2D) arc length for given radius (to ref. point)
void moveToXY(double xPos, double yPos, double &newPhi0, double &newDca, double &newZ0) const
Move to new reference point (X,Y)
virtual ~GblSimpleHelix()
GblHelixPrediction getPrediction(const Eigen::Vector3d &refPos, const Eigen::Vector3d &uDir, const Eigen::Vector3d &vDir) const
Get prediction.
GblSimpleHelix(double aRinv, double aPhi0, double aDca, double aDzds, double aZ0)
Create simple helix.
const double rinv
curvature (1/Radius)
double getPhi(double aRadius) const
Get phi (of point on circle) for given radius (to ref. point)
const double sinPhi0
sin(phi0)
double getArcLengthXY(double xPos, double yPos) const
Get (2D) arc length for given point.
const double dca
distance to origin in XY plane at PCA
const double z0
offset in ZS plane
const double cosPhi0
cos(phi0)
const double xRelCenter
X position of circle center / R.
const double dzds
slope in ZS plane (dZ/dS)
const double phi0
azimuth at PCA (point of closest approach to origin in XY plane, defines arc-length S=0)
const double yRelCenter
Y position of circle center / R.
Namespace for the general broken lines package.
double unrm()
unit normal distribution, Box-Muller method, polar form
double gblMultipleScatteringError(double qbyp, double xbyx0)
Multiple scattering error.
double unif()
uniform distribution [0..1]
Matrix5d gblSimpleJacobian(double ds, double cosl, double bfac)
Simple jacobian.
Eigen::Matrix< double, 5, 5 > Matrix5d