surfaces/base/Equations.h

0001 #ifndef VECGEOM_SURFACE_EQUATIONS_H_
0002 #define VECGEOM_SURFACE_EQUATIONS_H_
0003
0004 #include <VecGeom/base/Vector3D.h>
0005 #include <VecGeom/base/Math.h>
0006
0007 #include <VecCore/VecCore>
0008 #include <VecGeom/surfaces/base/CommonTypes.h>
0009
0010 namespace vgbrep {
0011
0012 using namespace vecCore::math;
0013
0014 template <typename Real_t>
0015 using Vector3D = vecgeom::Vector3D<Real_t>;
0016
0017 ///< Second order equation coefficients, as in: <x * x + 2*phalf * x + q = 0>
0018 template <typename Real_t>
0019 struct QuadraticCoef {
0020   Real_t phalf{0}; // we don't need p itself when solving the equation, hence we store just p/2
0021   Real_t q{0};     // we do need q
0022 };
0023
0024 ///< Fourth order equation coefficients, as in: <x^4 + a*x^3 + b*x^2 + c*x + d = 0>
0025 template <typename Real_t>
0026 struct QuarticCoef {
0027   Real_t a{0};
0028   Real_t b{0};
0029   Real_t c{0};
0030   Real_t d{0};
0031 };
0032
0033 /// @brief Fill equation for ray-cylinder intersections
0034 /// @param point Starting point in local coordinates
0035 /// @param dir Direction in local coordinates
0036 /// @param radius Cylinder radius
0037 /// @return coef Quadratic equation giving ray-cylinder intersections
0038 template <typename Real_t>
0039 VECCORE_ATT_HOST_DEVICE void CylinderEq(Vector3D<Real_t> const &point, Vector3D<Real_t> const &dir, Real_t radius,
0040                                         QuadraticCoef<Real_t> &coef)
0041 {
0042   // TODO: The cylyndrical and cone equations can be merged
0043   Real_t rsq    = point.Perp2();
0044   Real_t rdotn  = point.x() * dir.x() + point.y() * dir.y();
0045   Real_t invnsq = static_cast<Real_t>(1.) / vecgeom::NonZero(dir.Perp2());
0046
0047   coef.phalf = invnsq * rdotn;
0048   coef.q     = invnsq * (rsq - radius * radius);
0049 }
0050
0051 /// @brief Fill equation for ray-cone intersections
0052 /// @param point Starting point in local coordinates
0053 /// @param dir Direction in local coordinates
0054 /// @param radius Cone radius at Z = 0
0055 /// @param slope Cone surface slope
0056 /// @return coef Quadratic equation giving ray-cone intersections
0057 template <typename Real_t>
0058 VECCORE_ATT_HOST_DEVICE void ConeEq(Vector3D<Real_t> const &point, Vector3D<Real_t> const &dir, Real_t radius,
0059                                     Real_t slope, QuadraticCoef<Real_t> &coef)
0060 {
0061   Real_t rz     = radius + point[2] * slope;
0062   Real_t rsq    = point.Perp2();
0063   Real_t rdotn  = point.x() * dir.x() + point.y() * dir.y() - slope * rz * dir.z();
0064   Real_t invnsq = static_cast<Real_t>(1.) / vecgeom::NonZero(dir.Perp2() - slope * slope * dir.z() * dir.z());
0065
0066   coef.phalf = invnsq * rdotn;
0067   coef.q     = invnsq * (rsq - rz * rz);
0068 }
0069
0070 /// @brief Fill equation for ray-sphere intersections
0071 /// @param point Starting point in local coordinates
0072 /// @param dir Direction in local coordinates
0073 /// @param radius Sphere radius
0074 /// @return coef Quadratic equation giving ray-sphere intersections
0075 template <typename Real_t>
0076 VECCORE_ATT_HOST_DEVICE void SphereEq(Vector3D<Real_t> const &point, Vector3D<Real_t> const &dir, Real_t radius,
0077                                       QuadraticCoef<Real_t> &coef)
0078 {
0079   Real_t rsq = point.Mag2();
0080   coef.phalf = point.Dot(dir);
0081   coef.q     = rsq - radius * radius;
0082 }
0083
0084 template <typename Real_t>
0085 VECCORE_ATT_HOST_DEVICE void TorusEq(Vector3D<Real_t> const &pt, Vector3D<Real_t> const &dir, const Real_t radius_tor,
0086                                      const Real_t radius_tube, QuarticCoef<Real_t> &coef)
0087 {
0088   // to be taken from ROOT
0089   // Returns distance to the surface or the torus from a point, along
0090   // a direction. Point is close enough to the boundary so that the distance
0091   // to the torus is decreasing while moving along the given direction.
0092
0093   // Compute coeficients of the quartic
0094   Real_t radius_tor2  = radius_tor * radius_tor; // these could be precalculated
0095   Real_t radius_tube2 = radius_tube * radius_tube;
0096
0097   Real_t r0sq   = pt[0] * pt[0] + pt[1] * pt[1] + pt[2] * pt[2];
0098   Real_t rdotn  = pt[0] * dir[0] + pt[1] * dir[1] + pt[2] * dir[2];
0099   Real_t rsumsq = radius_tor2 + radius_tube2;
0100   coef.a        = 4. * rdotn;
0101   coef.b        = 2. * (r0sq + 2. * rdotn * rdotn - rsumsq + 2. * radius_tor2 * dir[2] * dir[2]);
0102   coef.c        = 4. * (r0sq * rdotn - rsumsq * rdotn + 2. * radius_tor2 * pt[2] * dir[2]);
0103   coef.d        = r0sq * r0sq - 2. * r0sq * rsumsq + 4. * radius_tor2 * pt[2] * pt[2] +
0104            (radius_tor2 - radius_tube2) * (radius_tor2 - radius_tube2);
0105 };
0106
0107 /// @brief Solver for quadratic equations
0108 /// @tparam Real_t Floating point type
0109 /// @param coef Quadratic equation coefficients
0110 /// @param roots Equation roots
0111 /// @param numroots Number of roots grater than -kTolerance
0112 template <typename Real_t>
0113 VECCORE_ATT_HOST_DEVICE void QuadraticSolver(QuadraticCoef<Real_t> const &coef, Real_t *roots, int &numroots)
0114 {
0115   numroots     = 0;
0116   Real_t delta = coef.phalf * coef.phalf - coef.q;
0117   if (delta < Real_t(0)) return;
0118   Real_t r1 = -coef.phalf - Sign(coef.phalf) * Sqrt(delta);
0119   Real_t r2 = coef.q / vecgeom::NonZero(r1);
0120   numroots += int(r1 > -vecgeom::kToleranceStrict<Real_t>) + int(r2 > -vecgeom::kToleranceStrict<Real_t>);
0121   roots[0] = Min(r1, r2);
0122   roots[1] = Max(r1, r2);
0123   if (numroots == 1) roots[0] = roots[1];
0124 }
0125
0126 template <typename Real_t>
0127 VECCORE_ATT_HOST_DEVICE unsigned int SolveCubic(Real_t a, Real_t b, Real_t c, Real_t *x)
0128 {
0129   // Find real solutions of the cubic equation : x^3 + a*x^2 + b*x + c = 0
0130   // Input: a,b,c
0131   // Output: x[3] real solutions
0132   // Returns number of real solutions (1 or 3)
0133   const Real_t ott     = 1. / 3.;
0134   const Real_t sq3     = Sqrt(3.);
0135   const Real_t inv6sq3 = 1. / (6. * sq3);
0136   unsigned int ireal   = 1;
0137   Real_t p             = b - a * a * ott;
0138   Real_t q             = c - a * b * ott + 2. * a * a * a * ott * ott * ott;
0139   Real_t delta         = 4 * p * p * p + 27. * q * q;
0140   Real_t t, u;
0141
0142   if (delta >= 0) {
0143     delta = Sqrt(delta);
0144     t     = (-3 * q * sq3 + delta) * inv6sq3;
0145     u     = (3 * q * sq3 + delta) * inv6sq3;
0146     x[0]  = CopySign(Real_t(1.), t) * Cbrt(Abs(t)) - CopySign(Real_t(1.), u) * Cbrt(Abs(u)) - a * ott;
0147   } else {
0148     delta = Sqrt(-delta);
0149     t     = -0.5 * q;
0150     u     = delta * inv6sq3;
0151     x[0]  = 2. * Pow(t * t + u * u, Real_t(0.5) * ott) * cos(ott * ATan2(u, t));
0152     x[0] -= a * ott;
0153   }
0154
0155   t     = x[0] * x[0] + a * x[0] + b;
0156   u     = a + x[0];
0157   delta = u * u - Real_t(4.) * t;
0158   if (delta >= 0) {
0159     ireal = 3;
0160     delta = Sqrt(delta);
0161     x[1]  = Real_t(0.5) * (-u - delta);
0162     x[2]  = Real_t(0.5) * (-u + delta);
0163   }
0164
0165   return ireal;
0166 }
0167
0168 template <typename Real_t, unsigned int i, unsigned int j>
0169 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void CmpAndSwap(Real_t *array)
0170 {
0171   if (array[i] > array[j]) {
0172     Real_t c = array[j];
0173     array[j] = array[i];
0174     array[i] = c;
0175   }
0176 }
0177
0178 // a special function to sort a 4 element array
0179 // sorting is done inplace and in increasing order
0180 // implementation comes from a sorting network
0181 template <typename Real_t>
0182 VECGEOM_FORCE_INLINE VECCORE_ATT_HOST_DEVICE void Sort4(Real_t *array)
0183 {
0184   CmpAndSwap<Real_t, 0, 2>(array);
0185   CmpAndSwap<Real_t, 1, 3>(array);
0186   CmpAndSwap<Real_t, 0, 1>(array);
0187   CmpAndSwap<Real_t, 2, 3>(array);
0188   CmpAndSwap<Real_t, 1, 2>(array);
0189 }
0190
0191 // solve quartic taken from ROOT/TGeo and adapted
0192 //_____________________________________________________________________________
0193
0194 template <typename Real_t>
0195 VECCORE_ATT_HOST_DEVICE void QuarticSolver(QuarticCoef<Real_t> const &coef, Real_t *roots, int &numroots)
0196 {
0197   // Find real solutions of the quartic equation : x^4 + a*x^3 + b*x^2 + c*x + d = 0
0198   // Input: coefficients a,b,c,d
0199   // Output: roots[4] - real solutions
0200   //         numroots number of real solutions (0 to 3)
0201   Real_t e = coef.b - 3. * coef.a * coef.a / 8.;
0202   Real_t f = coef.c + coef.a * coef.a * coef.a / 8. - 0.5 * coef.a * coef.b;
0203   Real_t g =
0204       coef.d - 3. * coef.a * coef.a * coef.a * coef.a / 256. + coef.a * coef.a * coef.b / 16. - coef.a * coef.c / 4.;
0205   Real_t xx[4] = {vecgeom::InfinityLength<Real_t>(), vecgeom::InfinityLength<Real_t>(),
0206                   vecgeom::InfinityLength<Real_t>(), vecgeom::InfinityLength<Real_t>()};
0207   Real_t delta;
0208   Real_t h = 0.;
0209   numroots = 0;
0210
0211   // special case when f is zero
0212   if (Abs(f) < 1E-6) {
0213     delta = e * e - 4. * g;
0214     if (delta < 0.) return;
0215     delta = Sqrt(delta);
0216     h     = 0.5 * (-e - delta);
0217     if (h >= 0) {
0218       h                 = Sqrt(h);
0219       roots[numroots++] = -h - 0.25 * coef.a;
0220       roots[numroots++] = h - 0.25 * coef.a;
0221     }
0222     h = 0.5 * (-e + delta);
0223     if (h >= 0) {
0224       h                 = Sqrt(h);
0225       roots[numroots++] = -h - 0.25 * coef.a;
0226       roots[numroots++] = h - 0.25 * coef.a;
0227     }
0228     Sort4(roots);
0229     return;
0230   }
0231
0232   if (Abs(g) < 1E-6) {
0233     roots[numroots++] = -0.25 * coef.a;
0234     // this actually wants to solve a second order equation
0235     // we should specialize if it happens often
0236     unsigned int ncubicroots = SolveCubic<Real_t>(0, e, f, xx);
0237     // this loop is not nice
0238     for (unsigned int i = 0; i < ncubicroots; i++)
0239       roots[numroots++] = xx[i] - 0.25 * coef.a;
0240     Sort4(roots); // could be Sort3
0241     return;
0242   }
0243
0244   numroots = SolveCubic<Real_t>(2. * e, e * e - 4. * g, -f * f, xx);
0245   if (numroots == 1) {
0246     if (xx[0] <= 0) return;
0247     h = Sqrt(xx[0]);
0248   } else {
0249     // 3 real solutions of the cubic
0250     for (unsigned int i = 0; i < 3; i++) {
0251       h = xx[i];
0252       if (h >= 0) break;
0253     }
0254     if (h <= 0) return;
0255     h = Sqrt(h);
0256   }
0257   Real_t j = 0.5 * (e + h * h - f / h);
0258   numroots = 0;
0259   delta    = h * h - 4. * j;
0260   if (delta >= 0) {
0261     delta             = Sqrt(delta);
0262     roots[numroots++] = 0.5 * (-h - delta) - 0.25 * coef.a;
0263     roots[numroots++] = 0.5 * (-h + delta) - 0.25 * coef.a;
0264   }
0265   delta = h * h - 4. * g / j;
0266   if (delta >= 0) {
0267     delta             = Sqrt(delta);
0268     roots[numroots++] = 0.5 * (h - delta) - 0.25 * coef.a;
0269     roots[numroots++] = 0.5 * (h + delta) - 0.25 * coef.a;
0270   }
0271   Sort4(roots);
0272   return;
0273 }
0274
0275 } // namespace vgbrep
0276 #endif