2 * @file larcorealg/Geometry/LineClosestPoint.tcc
3 * @brief Utility for intersection of two 3D lines: template implementation.
4 * @author Gianluca Petrillo (petrillo@slac.stanford.edu)
5 * @see larcorealg/Geometry/LineClosestPoint.h
8 #ifndef LARCOREALG_GEOMETRY_LINECLOSESTPOINT_H
10 "LineClosestPoint.tcc should not be included directly: #include \"larcorealg/Geometry/LineClosestPoint.h\" instead."
13 // LArSoft and framework libraries
14 #include "cetlib/pow.h" // cet::square
17 #include "larcorealg/Geometry/geo_vectors_utils.h" // geo::vect::dot()
19 // C++ standard library
21 #include <cmath> // std::abs()
22 #include <tuple> // std::tie()
24 // -----------------------------------------------------------------------------
25 // --- implementation details
26 // -----------------------------------------------------------------------------
27 namespace geo::details {
29 template <typename Point, typename Vector>
30 Point LineClosestPointImpl(Point const& startA,
34 std::pair<double, double>* locOnLines /* = nullptr */
38 * The point on the first line ("A"):
40 * p1(t) = c1 + t w1 (c1 the starting point, w1 its direction)
42 * has the minimal distance from the other line (c2 + u w2, with the same
45 * t = [-(dc,w1)(w2,w2) + (dc,w2)(w1,w2)] / [ (w1,w2)^2 - (w1,w1)(w2,w2) ]
46 * u = [ (dc,w2)(w2,w2) - (dc,w1)(w1,w2)] / [ (w1,w2)^2 - (w1,w1)(w2,w2) ]
48 * (where (a,b) is a scalar product and dc = (c1 - c2) ).
49 * If w1 and w2 are unit vectors, t and u are in fact the distance of the
50 * point from the "start" of the respective lines in "standard" geometry
53 * If `locOnLines` is non-null, it is filled with { t, u }.
56 // aliases for quick notation
57 auto const& [c1, w1] = std::tie(startA, dirA);
58 auto const& [c2, w2] = std::tie(startB, dirB);
60 auto const dc = c2 - c1;
63 double const dcw1 = dot(dc, w1);
64 double const dcw2 = dot(dc, w2);
65 double const w1w2 = dot(w1, w2); // this is cos(angle), angle between lines
66 assert(std::abs(std::abs(w1w2) - 1.0) >= 1e-10); // prerequisite: not parallel
68 using geo::vect::mag2;
69 double const w1w1 = mag2(w1);
70 double const w2w2 = mag2(w2);
71 double const inv_den = 1.0 / (cet::square(w1w2) - (w1w1 * w2w2));
72 double const t = ((dcw2 * w1w2) - (dcw1 * w2w2)) * inv_den;
75 double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
81 } // geo::details::LineClosestPointImpl()
83 // ---------------------------------------------------------------------------
84 template <typename Point, typename UnitVector>
85 Point LineClosestPointWithUnitVectorsImpl(Point const& startA,
86 UnitVector const& dirA,
88 UnitVector const& dirB,
89 std::pair<double, double>* locOnLines /* = nullptr */
93 * The implementation is the same as in `LineClosestPoint()`,
94 * with some computation skipped while relying on the assumption of unit
98 using geo::vect::mag2;
99 assert(std::abs(mag2(dirA) - 1.0) < 1e-10); // prerequisite: dirA unit vector
100 assert(std::abs(mag2(dirB) - 1.0) < 1e-10); // prerequisite: dirB unit vector
102 // aliases for quick notation
103 Point const& c1 = startA;
104 UnitVector const& w1 = dirA;
106 Point const& c2 = startB;
107 UnitVector const& w2 = dirB;
109 auto const dc = c2 - c1;
111 using geo::vect::dot;
112 double const dcw1 = dot(dc, w1);
113 double const dcw2 = dot(dc, w2);
114 double const w1w2 = dot(w1, w2); // this is cos(angle), angle between lines
115 assert(std::abs(std::abs(w1w2) - 1.0) >= 1e-10); // prerequisite: not parallel
117 // we keep the generic math here, but freeze the modulus parameters
118 constexpr double const w1w1{1.0};
119 constexpr double const w2w2{1.0};
120 double const inv_den = 1.0 / (cet::square(w1w2) - (w1w1 * w2w2));
121 double const t = ((dcw2 * w1w2) - (dcw1 * w2w2)) * inv_den;
124 double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
125 *locOnLines = {t, u};
130 } // geo::details::LineClosestPointWithUnitVectorsImpl()
132 // ---------------------------------------------------------------------------
134 } // namespace geo::details
136 // -----------------------------------------------------------------------------
138 // -----------------------------------------------------------------------------
139 template <typename Point, typename Vector>
140 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsets(Point const& startA,
145 std::pair<double, double> locOnLines;
146 auto const point = details::LineClosestPointImpl(startA, dirA, startB, dirB, &locOnLines);
147 return {point, locOnLines.first, locOnLines.second};
148 } // geo::LineClosestPointAndOffsets()
150 // -----------------------------------------------------------------------------
151 template <typename Point, typename Vector>
152 Point geo::LineClosestPoint(Point const& startA,
157 return details::LineClosestPointImpl(startA, dirA, startB, dirB, nullptr);
158 } // geo::LineClosestPoint()
160 // -----------------------------------------------------------------------------
161 template <typename Point, typename UnitVector>
162 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsetsWithUnitVectors(
164 UnitVector const& dirA,
166 UnitVector const& dirB)
169 std::pair<double, double> locOnLines;
171 details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, &locOnLines);
172 return {point, locOnLines.first, locOnLines.second};
175 // -----------------------------------------------------------------------------
176 template <typename Point, typename UnitVector>
177 Point geo::LineClosestPointWithUnitVectors(Point const& startA,
178 UnitVector const& dirA,
180 UnitVector const& dirB)
182 return details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, nullptr);
185 // -----------------------------------------------------------------------------