LArSoft  v10_04_05
Liquid Argon Software toolkit - https://larsoft.org/
LineClosestPoint.tcc
Go to the documentation of this file.
1 /**
2  * @file larcorealg/Geometry/LineClosestPoint.tcc
3  * @brief Utility for intersection of two 3D lines: template implementation.
4  * @see larcorealg/Geometry/LineClosestPoint.h
5  */
6 
7 #ifndef LARCOREALG_GEOMETRY_LINECLOSESTPOINT_H
8 #error \
9  "LineClosestPoint.tcc should not be included directly: #include \"larcorealg/Geometry/LineClosestPoint.h\" instead."
10 #endif
11 
12 // LArSoft and framework libraries
13 #include "cetlib/pow.h" // cet::square
14 
15 // C/C++ libraries
16 #include "larcorealg/Geometry/geo_vectors_utils.h" // geo::vect::dot()
17 
18 // C++ standard library
19 #include <cassert>
20 #include <cmath> // std::abs()
21 #include <tuple> // std::tie()
22 
23 // -----------------------------------------------------------------------------
24 // --- implementation details
25 // -----------------------------------------------------------------------------
26 namespace geo::details {
27 
28  template <typename Point, typename Vector>
29  Point LineClosestPointImpl(Point const& startA,
30  Vector const& dirA,
31  Point const& startB,
32  Vector const& dirB,
33  std::pair<double, double>* locOnLines /* = nullptr */
34  )
35  {
36  /*
37  * The point on the first line ("A"):
38  *
39  * p1(t) = c1 + t w1 (c1 the starting point, w1 its direction)
40  *
41  * has the minimal distance from the other line (c2 + u w2, with the same
42  * notation) at
43  *
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) ]
46  *
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
50  * units.
51  *
52  * If `locOnLines` is non-null, it is filled with { t, u }.
53  */
54 
55  // aliases for quick notation
56  auto const& [c1, w1] = std::tie(startA, dirA);
57  auto const& [c2, w2] = std::tie(startB, dirB);
58 
59  auto const dc = c2 - c1;
60 
61  using geo::vect::dot;
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
66 
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;
72 
73  if (locOnLines) {
74  double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
75  *locOnLines = {t, u};
76  }
77 
78  return c1 + w1 * t;
79 
80  } // geo::details::LineClosestPointImpl()
81 
82  // ---------------------------------------------------------------------------
83  template <typename Point, typename UnitVector>
84  Point LineClosestPointWithUnitVectorsImpl(Point const& startA,
85  UnitVector const& dirA,
86  Point const& startB,
87  UnitVector const& dirB,
88  std::pair<double, double>* locOnLines /* = nullptr */
89  )
90  {
91  /*
92  * The implementation is the same as in `LineClosestPoint()`,
93  * with some computation skipped while relying on the assumption of unit
94  * vectors.
95  */
96 
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
100 
101  // aliases for quick notation
102  Point const& c1 = startA;
103  UnitVector const& w1 = dirA;
104 
105  Point const& c2 = startB;
106  UnitVector const& w2 = dirB;
107 
108  auto const dc = c2 - c1;
109 
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
115 
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;
121 
122  if (locOnLines) {
123  double const u = ((dcw2 * w1w1) - (dcw1 * w1w2)) * inv_den;
124  *locOnLines = {t, u};
125  }
126 
127  return c1 + w1 * t;
128 
129  } // geo::details::LineClosestPointWithUnitVectorsImpl()
130 
131  // ---------------------------------------------------------------------------
132 
133 } // namespace geo::details
134 
135 // -----------------------------------------------------------------------------
136 // --- interface
137 // -----------------------------------------------------------------------------
138 template <typename Point, typename Vector>
139 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsets(Point const& startA,
140  Vector const& dirA,
141  Point const& startB,
142  Vector const& dirB)
143 {
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()
148 
149 // -----------------------------------------------------------------------------
150 template <typename Point, typename Vector>
151 Point geo::LineClosestPoint(Point const& startA,
152  Vector const& dirA,
153  Point const& startB,
154  Vector const& dirB)
155 {
156  return details::LineClosestPointImpl(startA, dirA, startB, dirB, nullptr);
157 } // geo::LineClosestPoint()
158 
159 // -----------------------------------------------------------------------------
160 template <typename Point, typename UnitVector>
161 geo::IntersectionPointAndOffsets<Point> geo::LineClosestPointAndOffsetsWithUnitVectors(
162  Point const& startA,
163  UnitVector const& dirA,
164  Point const& startB,
165  UnitVector const& dirB)
166 {
167 
168  std::pair<double, double> locOnLines;
169  auto const point =
170  details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, &locOnLines);
171  return {point, locOnLines.first, locOnLines.second};
172 }
173 
174 // -----------------------------------------------------------------------------
175 template <typename Point, typename UnitVector>
176 Point geo::LineClosestPointWithUnitVectors(Point const& startA,
177  UnitVector const& dirA,
178  Point const& startB,
179  UnitVector const& dirB)
180 {
181  return details::LineClosestPointWithUnitVectorsImpl(startA, dirA, startB, dirB, nullptr);
182 }
183 
184 // -----------------------------------------------------------------------------