2 * @file larcorealg/Geometry/LineClosestPoint.tcc
3 * @brief Utility for intersection of two 3D lines: template implementation.
4 * @see larcorealg/Geometry/LineClosestPoint.h
7 #ifndef LARCOREALG_GEOMETRY_LINECLOSESTPOINT_H
9 "LineClosestPoint.tcc should not be included directly: #include \"larcorealg/Geometry/LineClosestPoint.h\" instead."
12 // LArSoft and framework libraries
13 #include "cetlib/pow.h" // cet::square
16 #include "larcorealg/Geometry/geo_vectors_utils.h" // geo::vect::dot()
18 // C++ standard library
20 #include <cmath> // std::abs()
21 #include <tuple> // std::tie()
23 // -----------------------------------------------------------------------------
24 // --- implementation details
25 // -----------------------------------------------------------------------------
26 namespace geo::details {
28 template <typename Point, typename Vector>
29 Point LineClosestPointImpl(Point const& startA,
33 std::pair<double, double>* locOnLines /* = nullptr */
37 * The point on the first line ("A"):
39 * p1(t) = c1 + t w1 (c1 the starting point, w1 its direction)
41 * has the minimal distance from the other line (c2 + u w2, with the same
44 * t = [-(dc,w1)(w2,w2) + (dc,w2)(w1,w2)] / [ (w1,w2)^2 - (w1,w1)(w2,w2) ]
45 * u = [ (dc,w2)(w2,w2) - (dc,w1)(w1,w2)] / [ (w1,w2)^2 - (w1,w1)(w2,w2) ]
47 * (where (a,b) is a scalar product and dc = (c1 - c2) ).
48 * If w1 and w2 are unit vectors, t and u are in fact the distance of the
49 * point from the "start" of the respective lines in "standard" geometry
52 * If `locOnLines` is non-null, it is filled with { t, u }.
55 // aliases for quick notation
56 auto const& [c1, w1] = std::tie(startA, dirA);
57 auto const& [c2, w2] = std::tie(startB, dirB);
59 auto const dc = c2 - c1;
62 double const dcw1 = dot(dc, w1);
63 double const dcw2 = dot(dc, w2);
64 double const w1w2 = dot(w1, w2); // this is cos(angle), angle between lines
65 assert(std::abs(std::abs(w1w2) - 1.0) >= 1e-10); // prerequisite: not parallel
67 using geo::vect::mag2;
68 double const w1w1 = mag2(w1);
69 double const w2w2 = mag2(w2);
70 double const inv_den = 1.0 / (cet::square(w1w2) - (w1w1 * w2w2));
71 double const t = ((dcw2 * w1w2) - (dcw1 * w2w2)) * inv_den;
74 double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
80 } // geo::details::LineClosestPointImpl()
82 // ---------------------------------------------------------------------------
83 template <typename Point, typename UnitVector>
84 Point LineClosestPointWithUnitVectorsImpl(Point const& startA,
85 UnitVector const& dirA,
87 UnitVector const& dirB,
88 std::pair<double, double>* locOnLines /* = nullptr */
92 * The implementation is the same as in `LineClosestPoint()`,
93 * with some computation skipped while relying on the assumption of unit
97 using geo::vect::mag2;
98 assert(std::abs(mag2(dirA) - 1.0) < 1e-10); // prerequisite: dirA unit vector
99 assert(std::abs(mag2(dirB) - 1.0) < 1e-10); // prerequisite: dirB unit vector
101 // aliases for quick notation
102 Point const& c1 = startA;
103 UnitVector const& w1 = dirA;
105 Point const& c2 = startB;
106 UnitVector const& w2 = dirB;
108 auto const dc = c2 - c1;
110 using geo::vect::dot;
111 double const dcw1 = dot(dc, w1);
112 double const dcw2 = dot(dc, w2);
113 double const w1w2 = dot(w1, w2); // this is cos(angle), angle between lines
114 assert(std::abs(std::abs(w1w2) - 1.0) >= 1e-10); // prerequisite: not parallel
116 // we keep the generic math here, but freeze the modulus parameters
117 constexpr double const w1w1{1.0};
118 constexpr double const w2w2{1.0};
119 double const inv_den = 1.0 / (cet::square(w1w2) - (w1w1 * w2w2));
120 double const t = ((dcw2 * w1w2) - (dcw1 * w2w2)) * inv_den;
123 double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
124 *locOnLines = {t, u};
129 } // geo::details::LineClosestPointWithUnitVectorsImpl()
131 // ---------------------------------------------------------------------------
133 } // namespace geo::details
135 // -----------------------------------------------------------------------------
137 // -----------------------------------------------------------------------------
138 template <typename Point, typename Vector>
139 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsets(Point const& startA,
144 std::pair<double, double> locOnLines;
145 auto const point = details::LineClosestPointImpl(startA, dirA, startB, dirB, &locOnLines);
146 return {point, locOnLines.first, locOnLines.second};
147 } // geo::LineClosestPointAndOffsets()
149 // -----------------------------------------------------------------------------
150 template <typename Point, typename Vector>
151 Point geo::LineClosestPoint(Point const& startA,
156 return details::LineClosestPointImpl(startA, dirA, startB, dirB, nullptr);
157 } // geo::LineClosestPoint()
159 // -----------------------------------------------------------------------------
160 template <typename Point, typename UnitVector>
161 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsetsWithUnitVectors(
163 UnitVector const& dirA,
165 UnitVector const& dirB)
168 std::pair<double, double> locOnLines;
170 details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, &locOnLines);
171 return {point, locOnLines.first, locOnLines.second};
174 // -----------------------------------------------------------------------------
175 template <typename Point, typename UnitVector>
176 Point geo::LineClosestPointWithUnitVectors(Point const& startA,
177 UnitVector const& dirA,
179 UnitVector const& dirB)
181 return details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, nullptr);
184 // -----------------------------------------------------------------------------