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);
109 const Vector3d& tDir,
const Vector3d& uDir,
const Vector3d& vDir,
110 const Vector3d& nDir,
const Vector3d& aPos) :
111 sarc(sArc), pred(aPred), tdir(tDir), udir(uDir), vdir(vDir), ndir(nDir), pos(
113 global2meas << uDir.transpose(), vDir.transpose(), nDir.transpose();
149 const double cosTheta =
tdir[2];
151 const double cosPhi =
tdir[0] / sinTheta;
152 const double sinPhi =
tdir[1] / sinTheta;
153 Eigen::Matrix<double, 2, 3> uvDir;
154 uvDir << -sinPhi, cosPhi, 0., -cosPhi * cosTheta, -sinPhi * cosTheta, sinTheta;
167 double aDzds,
double aZ0) :
168 rinv(aRinv), phi0(aPhi0), dca(aDca), dzds(aDzds), z0(aZ0), cosPhi0(
169 cos(phi0)), sinPhi0(sin(phi0)), xRelCenter(
170 -(1. - dca * rinv) * sinPhi0), yRelCenter(
171 (1. - dca * rinv) * cosPhi0) {
184 double arg = (0.5 *
rinv * (aRadius * aRadius +
dca *
dca) -
dca)
185 / (aRadius * (1.0 -
rinv *
dca));
186 return asin(arg) +
phi0;
196 double arg = (0.5 *
rinv * (aRadius * aRadius +
dca *
dca) -
dca)
197 / (aRadius * (1.0 -
rinv *
dca));
198 if (fabs(arg) >= 1.) {
199 std::cout <<
" bad arc " << aRadius <<
" " <<
rinv <<
" " <<
dca
205 return sqrt(aRadius * aRadius -
dca *
dca);
207 double sxy = asin(aRadius *
rinv * sqrt(1.0 - arg * arg)) /
rinv;
210 sxy = M_PI / fabs(
rinv) - sxy;
226 double dphi = atan2(dx, -dy) -
phi0;
229 else if (dphi < -M_PI)
243 double& newDca,
double& newZ0)
const {
249 const double u = 1. -
rinv *
dca;
252 const double sa = 2. * dp -
rinv * (dp * dp + dl * dl);
255 const double sd = sqrt(1. -
rinv * sa);
262 newPhi0 = atan2(sb, sc);
263 newDca = sa / (1. + sd);
264 double dphi = newPhi0 -
phi0;
267 else if (dphi < -M_PI)
271 newZ0 += sArc *
dzds;
281 const Eigen::Vector3d& uDir,
const Eigen::Vector3d& vDir)
const {
283 Vector3d nDir = uDir.cross(vDir).normalized();
285 const double cosLambda = 1. / sqrt(1. +
dzds *
dzds);
286 const double sinLambda =
dzds * cosLambda;
288 Vector3d dist, pos, tDir;
297 double sArc3D = -dist.dot(nDir) / tDir.dot(nDir);
298 sArc2D = sArc3D * cosLambda;
300 pos = pca + sArc3D * tDir;
306 unsigned int nIter = 0;
310 const double dPhi = sArc2D *
rinv;
311 const double cosPhi = cos(
phi0 + dPhi);
312 const double sinPhi = sin(
phi0 + dPhi);
313 tDir << cosLambda * cosPhi, cosLambda * sinPhi, sinLambda;
320 const double sCorr3D = -dist.dot(nDir) / tDir.dot(nDir);
321 if (fabs(sCorr3D) > 0.00001) {
323 sArc2D += sCorr3D * cosLambda;
331 Vector2d pred(dist.dot(uDir), dist.dot(vDir));
349 const unsigned int aLayer,
const int aDim,
const double thickness,
350 Eigen::Vector3d& aCenter, Eigen::Vector2d& aResolution,
351 Eigen::Vector2d& aPrecision, Eigen::Matrix3d& measTrafo,
352 Eigen::Matrix3d& alignTrafo) :
353 name(aName), layer(aLayer), measDim(aDim), xbyx0(thickness), center(
354 aCenter), resolution(aResolution), precision(aPrecision), global2meas(
355 measTrafo), global2align(alignTrafo) {
366 IOFormat CleanFmt(4, 0,
", ",
"\n",
"[",
"]");
368 <<
xbyx0 <<
" X0, @ " <<
center.transpose().format(CleanFmt)
369 <<
", res " <<
resolution.transpose().format(CleanFmt) <<
", udir "
370 <<
udir.transpose().format(CleanFmt) <<
", vdir "
371 <<
vdir.transpose().format(CleanFmt) << std::endl;
430 Eigen::Vector3d& position, Eigen::Vector3d& direction)
const {
432 Vector3d dist = position;
434 Matrix3d drdm = Matrix3d::Identity()
435 - (direction *
ndir.transpose()) / (direction.transpose() *
ndir);
437 Matrix<double, 3, 6> dmdg = Matrix<double, 3, 6>::Zero();
439 dmdg(0, 4) = -dist(2);
440 dmdg(0, 5) = dist(1);
442 dmdg(1, 3) = dist(2);
443 dmdg(1, 5) = -dist(0);
445 dmdg(2, 3) = -dist(1);
446 dmdg(2, 4) = dist(0);
465 Eigen::Vector3d& position, Eigen::Vector3d& direction)
const {
469 const double uSlope = tLoc[0] / tLoc[2];
470 const double vSlope = tLoc[1] / tLoc[2];
473 const double uPos = dist[0];
474 const double vPos = dist[1];
477 Matrix<double, 2, 6> drldg;
478 drldg << 1.0, 0.0, -uSlope, vPos * uSlope, -uPos * uSlope, vPos, 0.0, 1.0, -vSlope, vPos
479 * vSlope, -uPos * vSlope, -uPos;
482 return local2meas.block<2, 2>(0, 0) * drldg;
493 Eigen::Vector3d& offset, Eigen::Matrix3d& rotation)
const {
495 Matrix<double, 6, 6> glo2loc = Matrix<double, 6, 6>::Zero();
497 leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
498 glo2loc.block<3, 3>(0, 0) = rotation;
499 glo2loc.block<3, 3>(0, 3) = -rotation * leverArms;
500 glo2loc.block<3, 3>(3, 3) = rotation;
512 Eigen::Vector3d& offset, Eigen::Matrix3d& rotation)
const {
514 Matrix<double, 6, 6> loc2glo = Matrix<double, 6, 6>::Zero();
516 leverArms << 0., offset[2], -offset[1], -offset[2], 0., offset[0], offset[1], -offset[0], 0.;
517 loc2glo.block<3, 3>(0, 0) = rotation.transpose();
518 loc2glo.block<3, 3>(0, 3) = leverArms * rotation.transpose();
519 loc2glo.block<3, 3>(3, 3) = rotation.transpose();
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.
Definitions for exampleUtil(ities).
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