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