All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
larcorealg/larcorealg/GeoAlgo/GeoAlgo.h
Go to the documentation of this file.
1 /**
2  * \file GeoAlgo.h
3  *
4  * \ingroup GeoAlgo
5  *
6  * \brief Class def header for a class GeoAlgo
7  *
8  * @author kazuhiro
9  */
10 
11 /** \addtogroup GeoAlgo
12 
13  @{*/
14 #ifndef BASICTOOL_GEOALGO_H
15 #define BASICTOOL_GEOALGO_H
16 
24 
25 #include <vector>
26 
27 namespace geoalgo {
28 
29  /**
30  \class GeoAlgo
31  @brief Algorithm to compute various geometrical relation among geometrical objects.
32  In particular functions to inspect following relations are implemented: \n
33  0) Distance between geometrical objects \n
34  1) Closest point of approach \n
35  2) Intersection points \n
36  3) Containment/Overlap of objects \n
37  4) Common Origin functions \n
38  5) Bounding Sphere functions \n
39 
40  Most functions are taken from the reference Real-Time-Collision-Detection (RTCD):
41  Ref: http://realtimecollisiondetection.net
42  */
43  class GeoAlgo{
44 
45  public:
46 
47  //
48  // Intersections
49  //
50 
51  /// Intersection between a HalfLine and an AABox
52  std::vector<Point_t> Intersection(const AABox_t& box, const HalfLine_t& line, bool back=false) const;
53  /// Intersection between a HalfLine and an AABox
54  std::vector<Point_t> Intersection(const HalfLine_t& line, const AABox_t& box, bool back=false) const
55  { return Intersection(box, line, back); }
56 
57  /// Intersection between LineSegment and an AABox
58  std::vector<Point_t> Intersection(const AABox_t& box, const LineSegment_t& l) const;
59  /// Intersection between LineSegment and an AABox
60  std::vector<Point_t> Intersection(const LineSegment_t& l, const AABox_t& box) const
61  { return Intersection(box,l); }
62 
63  /// Intersection between Trajectory and an AABox
64  std::vector<Point_t> Intersection(const AABox_t& box, const Trajectory_t& trj) const;
65  /// Intersection between Trajectory and an AABox
66  std::vector<Point_t> Intersection(const Trajectory_t& trj, const AABox_t& box) const
67  { return Intersection(box,trj); }
68 
69  /// LineSegment sub-segment of HalfLine inside an AABox
70  LineSegment_t BoxOverlap(const AABox_t& box, const HalfLine_t& line) const;
71  /// LineSegment sub-segment of HalfLine inside an AABox
72  LineSegment_t BoxOverlap(const HalfLine_t& line, const AABox_t& box) const
73  { return BoxOverlap(box, line); }
74 
75 
76  /// Get Trajectory inside box given some input trajectory -> now assumes trajectory cannot exit and re-enter box
77  Trajectory_t BoxOverlap(const AABox_t& box, const Trajectory_t& trj) const;
78  /// Get Trajectory inside box given some input trajectory -> now assumes trajectory cannot exit and re-enter box
79  Trajectory_t BoxOverlap(const Trajectory_t& trj, const AABox_t& box) const
80  { return BoxOverlap(box, trj); }
81 
82 
83  //************************************************
84  //CLOSEST APPROACH BETWEEN POINT AND INFINITE LINE
85  //************************************************
86  // min distance between point and infinite line
87  double SqDist(const Line_t& line, const Point_t& pt) const
88  { pt.compat(line.Pt1()); return _SqDist_(line,pt); }
89  // min distance between point and infinite line
90  double SqDist(const Point_t& pt, const Line_t& line) const
91  { return SqDist(line,pt); }
92  // closest point (on infinite line) from point
93  Point_t ClosestPt(const Line_t& line, const Point_t& pt) const
94  { pt.compat(line.Pt1()); return _ClosestPt_(pt,line); }
95  // closest point (on infinite line) from point
96  Point_t ClosestPt(const Point_t& pt, const Line_t& line) const
97  { return _ClosestPt_(pt,line); }
98 
99 
100  //*******************************************
101  //CLOSEST APPROACH BETWEEN TWO INFINITE LINES
102  //*******************************************
103  // Closest approach between two infinite line segments - keep track of closest approach points
104  double SqDist(const Line_t& l1, const Line_t& l2, Point_t& L1, Point_t& L2) const
105  { l1.Pt1().compat(l2.Pt1()); return _SqDist_(l1, l2, L1, L2); }
106  // Closest approach between two infinite line segments - don't keep track of closest approach points
107  double SqDist(const Line_t& l1, const Line_t& l2) const
108  { Point_t L1; Point_t L2; return SqDist(l1, l2, L1, L2); }
109 
110 
111  //************************************************
112  //CLOSEST APPROACH BETWEEN TWO HALF-INFINITE LINES
113  //************************************************
114  // Closest approach between two infinite line segments - keep track of closest approach points
115  double SqDist(const HalfLine_t& l1, const HalfLine_t& l2, Point_t& L1, Point_t& L2) const
116  { l1.Start().compat(l2.Start()); return _SqDist_(l1, l2, L1, L2); }
117  // Closest approach between two infinite line segments - don't keep track of closest approach points
118  double SqDist(const HalfLine_t& l1, const HalfLine_t& l2) const
119  { Point_t L1; Point_t L2; return SqDist(l1, l2, L1, L2); }
120 
121 
122  //******************************************
123  //CLOSEST APPROACH BETWEEN TWO LINE SEGMENTS
124  //******************************************
125  /// LineSegment_t & LineSegment_t distance - keep track of points
126  double SqDist(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& c1, Point_t& c2) const
127  { seg1.Start().compat(seg2.Start()); return _SqDist_(seg1, seg2, c1, c2); }
128  /// LineSegment & LineSegment, don't keep track of points
129  double SqDist(const LineSegment_t& seg1, const LineSegment_t& seg2) const
130  { Point_t c1; Point_t c2; return SqDist(seg1, seg2, c1, c2); }
131 
132 
133  //******************************************
134  //CLOSEST APPROACH BETWEEN SEGMENT AND TRACK
135  //******************************************
136  /// LineSegment & Trajectory, keep track of points
137  double SqDist(const LineSegment_t& seg, const Trajectory_t& trj, Point_t& c1, Point_t& c2) const;
138  /// LineSegment & Trajectory, keep track of points
139  double SqDist(const Trajectory_t& trj, const LineSegment_t& seg, Point_t& c1, Point_t& c2) const
140  { return SqDist(seg, trj, c1, c2); }
141  /// LineSegment & Trajectory, don't keep track of points
142  double SqDist(const Trajectory_t& trj, const LineSegment_t& seg) const
143  { Point_t c1; Point_t c2; return SqDist(seg, trj, c1, c2); }
144  /// LineSegment & Trajectory, don't keep track of points
145  double SqDist(const LineSegment_t& seg, const Trajectory_t& trj) const
146  { Point_t c1; Point_t c2; return SqDist(seg, trj, c1, c2); }
147 
148 
149  //****************************************
150  //CLOSEST APPROACH BETWEEN TRACK AND TRACK
151  //****************************************
152  /// Trajectory & Trajectory, keep track of points
153  double SqDist(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& c1, Point_t& c2) const;
154  /// Trajectory & Trajectory, don't keep track of points
155  double SqDist(const Trajectory_t& trj1, const Trajectory_t& trj2) const
156  { Point_t c1; Point_t c2; return SqDist(trj1, trj2, c1, c2); }
157 
158 
159  //*****************************************************
160  //CLOSEST APPROACH BETWEEN SEGMENT AND VECTOR OF TRACKS
161  //*****************************************************
162  /// LineSegment & vector of Trajectories, keep track of points
163  double SqDist(const LineSegment_t& seg, const std::vector<geoalgo::Trajectory_t> &trj, Point_t& c1, Point_t& c2, int& trackIdx) const;
164  /// LineSegment & vector of Trajectories, keep track of points
165  double SqDist(const std::vector<geoalgo::Trajectory_t> &trj, const LineSegment_t& seg, Point_t& c1, Point_t& c2, int& trackIdx) const
166  { return SqDist(seg, trj, c2, c1, trackIdx); }
167  /// LineSegment & vector of Trajectories, don't keep track of points
168  double SqDist(const std::vector<geoalgo::Trajectory_t> &trj, const LineSegment_t& seg) const
169  { Point_t c1; Point_t c2; int trackIdx; return SqDist(seg, trj, c1, c2, trackIdx); }
170  /// LineSegment & vector of Trajectories, don't keep track of points
171  double SqDist(const LineSegment_t& seg, const std::vector<geoalgo::Trajectory_t> &trj) const
172  { Point_t c1; Point_t c2; int trackIdx; return SqDist(seg, trj, c1, c2, trackIdx); }
173 
174 
175  //*******************************************
176  //CLOSEST APPROACH BETWEEN HALFLINE AND TRACK
177  //*******************************************
178  /// HalfLine & Trajectory, keep track of points
179  double SqDist(const HalfLine_t& hline, const Trajectory_t& trj, Point_t& c1, Point_t& c2) const;
180  /// HalfLine & Trajectory, keep track of points
181  double SqDist(const Trajectory_t& trj, const HalfLine_t& hline, Point_t& c1, Point_t& c2) const
182  { return SqDist(hline, trj, c2, c1); }
183  /// HalfLine & Trajectory, don't keep track of points
184  double SqDist(const Trajectory_t& trj, const HalfLine_t& hline) const
185  { Point_t c1; Point_t c2; return SqDist(hline, trj, c1, c2); }
186  /// HalfLine & Trajectory, don't keep track of points
187  double SqDist(const HalfLine_t& hline, const Trajectory_t& trj) const
188  { Point_t c1; Point_t c2; return SqDist(hline, trj, c1, c2); }
189 
190 
191  //****************************************
192  //CLOSEST APPROACH BETWEEN POINT AND TRACK
193  //****************************************
194  /// Point_t & Trajectory_t distance
195  double SqDist(const Point_t& pt, const Trajectory_t& trj) const;
196  /// Point_t & Trajectory_t distance
197  double SqDist(const Trajectory_t& trj, const Point_t& pt) const
198  { return SqDist(pt,trj); }
199  /// Point_t & Trajectory_t closest point
200  Point_t ClosestPt(const Point_t& pt, const Trajectory_t& trj) const
201  { int idx=0; return ClosestPt(pt,trj,idx); }
202  /// Point_t & Trajectory_t closest point
203  Point_t ClosestPt(const Trajectory_t& trj, const Point_t& pt) const
204  { int idx=0; return ClosestPt(pt,trj,idx); }
205  /// Point_t & Trajectory_t closest point. Keep track of index of segment
206  Point_t ClosestPt(const Point_t& pt, const Trajectory_t& trj, int& idx) const;
207  /// Point_t & Trajectory_t closest point. Keep track of index of segment
208  Point_t ClosestPt(const Trajectory_t& trj, const Point_t& pt, int& idx) const
209  { return ClosestPt(pt,trj,idx); }
210 
211 
212  //***************************************************
213  //CLOSEST APPROACH BETWEEN POINT AND VECTOR OF TRACKS
214  //***************************************************
215  /// Point_t & Trajectory_t distance - keep track of which track
216  double SqDist(const Point_t& pt, const std::vector<geoalgo::Trajectory_t> &trj, int& trackIdx) const;
217  /// Point_t & Trajectory_t distance - keep track of which track
218  double SqDist(const std::vector<geoalgo::Trajectory_t> &trj, const Point_t& pt, int& trackIdx) const
219  { return SqDist(pt,trj, trackIdx); }
220  /// Point_t & Trajectory_t distance - don't keep track
221  double SqDist(const Point_t& pt, const std::vector<geoalgo::Trajectory_t> &trj) const
222  { int trackIdx; return SqDist(pt, trj, trackIdx); }
223  /// Point_t & Trajectory_t distance - don't keep track
224  double SqDist(const std::vector<geoalgo::Trajectory_t> &trj, const Point_t& pt) const
225  { int trackIdx; return SqDist(pt, trj, trackIdx); }
226  /// Point_t & Trajectory_t closest point - keep track of which track is closest
227  Point_t ClosestPt(const Point_t& pt, const std::vector<geoalgo::Trajectory_t> &trj, int &trackIdx) const;
228  /// Point_t & Trajectory_t closest point - keep track of which track is closest
229  Point_t ClosestPt(const std::vector<geoalgo::Trajectory_t> &trj, const Point_t& pt, int &trackIdx) const
230  { return ClosestPt(pt, trj, trackIdx); }
231  /// Point_t & Trajectory_t closest point - don't keep track of which track is closest
232  Point_t ClosestPt(const Point_t& pt, const std::vector<geoalgo::Trajectory_t> &trj) const
233  { int trackIdx; return ClosestPt(pt, trj, trackIdx); }
234  /// Point_t & Trajectory_t closest point - don't keep track of which track is closest
235  Point_t ClosestPt(const std::vector<geoalgo::Trajectory_t> &trj, const Point_t& pt) const
236  { int trackIdx; return ClosestPt(pt, trj, trackIdx); }
237 
238 
239  //***********************************************
240  //CLOSEST APPROACH BETWEEN POINT AND LINE SEGMENT
241  //***********************************************
242  /// Point & LineSegment_t distance
243  double SqDist(const Point_t& pt, const LineSegment_t& line) const
244  { pt.compat(line.Start()); return _SqDist_(pt,line); }
245  /// Point & LineSegment distance
246  double SqDist(const LineSegment_t& line, const Point_t& pt) const
247  { return SqDist(pt,line); }
248  /// Point & LineSegment closest point
249  Point_t ClosestPt(const Point_t& pt, const LineSegment_t& line) const
250  { pt.compat(line.Start()); return _ClosestPt_(pt,line); }
251  /// Point & LineSegment closest point
252  Point_t ClosestPt(const LineSegment_t& line, const Point_t& pt) const
253  { return ClosestPt(pt,line); }
254 
255  //********************************************
256  //CLOSEST APPROACH BETWEEN POINT AND HALF LINE
257  //********************************************
258  /// Point & HalfLine distance
259  double SqDist(const Point_t& pt, const HalfLine_t& line) const
260  { pt.compat(line.Start()); return _SqDist_(pt,line); }
261  /// Point & HalfLine distance
262  double SqDist(const HalfLine_t& line, const Point_t& pt) const
263  { return SqDist(pt,line); }
264  /// Point & HalfLine closest point
265  Point_t ClosestPt(const Point_t& pt, const HalfLine_t& line) const
266  { pt.compat(line.Start()); return _ClosestPt_(pt,line); }
267  /// Point & HalfLine closest point
268  Point_t ClosestPt(const HalfLine_t& line, const Point_t& pt) const
269  { return ClosestPt(pt,line); }
270 
271 
272  //***************************************************
273  //CLOSEST APPROACH BETWEEN HALF LINE AND LINE SEGMENT
274  //***************************************************
275  // half-line and line-segment. keep track of closest approach points
276  double SqDist(const HalfLine_t& hline, const LineSegment_t& seg, Point_t& L1, Point_t& L2) const
277  { hline.Start().compat(seg.Start()); return _SqDist_(hline, seg, L1, L2); }
278  // half-line and line-segment. keep track of closest approach points
279  double SqDist(const LineSegment_t& seg, const HalfLine_t& hline, Point_t& L1, Point_t& L2) const
280  { return SqDist(hline,seg, L2, L1); }
281  // half-line and line-segment. Do not keep track of closest approach points
282  double SqDist(const HalfLine_t& hline, const LineSegment_t& seg) const
283  { Point_t L1; Point_t L2; return SqDist(hline, seg, L1, L2); }
284  // half-line and line-segment. Do not keep track of closest approach points
285  double SqDist(const LineSegment_t& seg, const HalfLine_t& hline) const
286  { return SqDist(hline,seg); }
287 
288  //***************************************************
289  //CLOSEST APPROACH BETWEEN POINT AND AXIS ALIGNED BOX
290  //***************************************************
291  /// Point & AABox distance
292  double SqDist(const Point_t& pt, const AABox_t& box) const
293  { pt.compat(box.Min()); return _SqDist_(pt,box); }
294  /// Point & AABox distance
295  double SqDist(const AABox_t& box, const Point_t& pt)
296  { return SqDist(pt,box); }
297  /// Point & AABox closest point
298  Point_t ClosestPt(const Point_t& pt, const AABox_t& box) const
299  { pt.compat(box.Min()); return _ClosestPt_(pt,box); }
300  /// Point & AABox closest point
301  Point_t ClosestPt(const AABox_t& box, const Point_t& pt) const
302  { return ClosestPt(pt,box); }
303 
304 
305  //***************************************************************************
306  //COMMON ORIGIN ALGORITHMS: DETERMINE IF TWO GEO-OBJECTS HAVE A COMMON ORIGIN
307  //***************************************************************************
308  /// Common origin: Line Segment & Line Segment. Do not keep track of origin
309  double commonOrigin(const Line_t& lin1, const Line_t& lin2) const
310  { Point_t origin(lin1.Pt1().size()); return commonOrigin(lin1,lin2, origin); }
311  /// Common origin: Line Segment & Line Segment. Keep track of origin
312  double commonOrigin(const Line_t& lin1, const Line_t& lin2, Point_t& origin) const
313  { lin1.Pt1().compat(lin2.Pt1()); return _commonOrigin_(lin1, lin2, origin); }
314  /// Common origin: Line Segment & Line Segment. Do not keep track of origin
315  double commonOrigin(const LineSegment_t& seg1, const LineSegment_t& seg2, bool backwards=false) const
316  { Point_t origin(seg1.Start().size()); return commonOrigin(seg1,seg2, origin, backwards); }
317  /// Common origin: Line Segment & Line Segment. Keep track of origin
318  double commonOrigin(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& origin, bool backwards=false) const
319  { seg1.Start().compat(seg2.Start()); return _commonOrigin_(seg1, seg2, origin, backwards); }
320  /// Common origin: Line Segment & Half Line. Do not keep track of origin
321  double commonOrigin(const HalfLine_t& lin, const LineSegment_t& seg, bool backwards=false) const
322  { Point_t origin(lin.Start().size()); return commonOrigin(lin, seg, origin, backwards); }
323  /// Common origin: Line Segment & Line Segment. Keep track of origin
324  double commonOrigin(const HalfLine_t& lin, const LineSegment_t& seg, Point_t& origin, bool backwards=false) const
325  { lin.Start().compat(seg.Start()); return _commonOrigin_(lin, seg, origin, backwards); }
326  /// Common origin: Line Segment & Half Line. Do not keep track of origin
327  double commonOrigin(const LineSegment_t& seg, const HalfLine_t& lin, bool backwards=false) const
328  { Point_t origin(lin.Start().size()); return commonOrigin(lin, seg, origin, backwards); }
329  /// Common origin: Line Segment & Line Segment. Keep track of origin
330  double commonOrigin(const LineSegment_t& seg, const HalfLine_t& lin, Point_t& origin, bool backwards=false) const
331  { lin.Start().compat(seg.Start()); return _commonOrigin_(lin, seg, origin, backwards); }
332  /// Common origin: Half Line & Half Line. Do not keep track of origin
333  double commonOrigin(const HalfLine_t& lin1, const HalfLine_t& lin2, bool backwards=false) const
334  { Point_t origin(lin1.Start().size()); return commonOrigin(lin1,lin2, origin, backwards); }
335  /// Common origin: Half Line & Half Line. Keep track of origin
336  double commonOrigin(const HalfLine_t& lin1, const HalfLine_t& lin2, Point_t& origin, bool backwards=false) const
337  { lin1.Start().compat(lin2.Start()); return _commonOrigin_(lin1, lin2, origin, backwards); }
338  /// Common origin: Trajectory & Trajectory. Do not keep track of origin
339  double commonOrigin(const Trajectory_t& trj1, const Trajectory_t& trj2, bool backwards=false) const
340  { Point_t origin(trj1.front().size()); return commonOrigin(trj1,trj2, origin, backwards); }
341  /// Common origin: Trajectory & Trajectory. Keep track of origin
342  double commonOrigin(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& origin, bool backwards=false) const
343  { trj1.front().compat(trj2.front()); return _commonOrigin_(trj1, trj2, origin, backwards); }
344  /// Common origin: Trajectory & Half Line. Do not keep track of origin
345  double commonOrigin(const Trajectory_t& trj, const HalfLine_t& lin, bool backwards=false) const
346  { Point_t origin(trj.front().size()); return commonOrigin(trj,lin, origin, backwards); }
347  /// Common origin: Trajectory & Half Line. Keep track of origin
348  double commonOrigin(const Trajectory_t& trj, const HalfLine_t& lin, Point_t& origin, bool backwards=false) const
349  { trj.front().compat(lin.Start()); return _commonOrigin_(trj, lin, origin, backwards); }
350  /// Common origin: Trajectory & Half Line. Do not keep track of origin
351  double commonOrigin(const HalfLine_t& lin, const Trajectory_t& trj, bool backwards=false) const
352  { Point_t origin(trj.front().size()); return commonOrigin(trj,lin, origin, backwards); }
353  /// Common origin: Trajectory & Half Line. Keep track of origin
354  double commonOrigin(const HalfLine_t& lin, const Trajectory_t& trj, Point_t& origin, bool backwards=false) const
355  { trj.front().compat(lin.Start()); return _commonOrigin_(trj, lin, origin, backwards); }
356  /// Common origin: Trajectory & Line Segment. Do not keep track of origin
357  double commonOrigin(const Trajectory_t& trj, const LineSegment_t& seg, bool backwards=false) const
358  { Point_t origin(trj.front().size()); return commonOrigin(trj, seg, origin, backwards); }
359  /// Common origin: Trajectory & Line Segment. Keep track of origin
360  double commonOrigin(const Trajectory_t& trj, const LineSegment_t& seg, Point_t& origin, bool backwards=false) const
361  { trj.front().compat(seg.Start()); return _commonOrigin_(trj, seg, origin, backwards); }
362  /// Common origin: Trajectory & Line Segment. Do not keep track of origin
363  double commonOrigin(const LineSegment_t& seg, const Trajectory_t& trj, bool backwards=false) const
364  { Point_t origin(trj.front().size()); return commonOrigin(trj, seg, origin, backwards); }
365  /// Common origin: Trajectory & Line Segment. Keep track of origin
366  double commonOrigin(const LineSegment_t& seg, const Trajectory_t& trj, Point_t& origin, bool backwards=false) const
367  { trj.front().compat(seg.Start()); return _commonOrigin_(trj, seg, origin, backwards); }
368 
369  //************************************************************************
370  //BOUNDING SPHERE ALGORITHM: RETURN SMALLEST SPHERE THAT BOUNDS ALL POINTS
371  //************************************************************************
372  // Bounding Sphere problem given a vector of 3D points
373  Sphere_t boundingSphere(const std::vector<Point_t>& pts) const
374  { for(auto &p : pts) { pts.front().compat(p); } return _boundingSphere_(pts); }
375 
376  protected:
377 
378  /// Line & Line distance w/o dimensionality check
379  double _SqDist_(const Line_t& l1, const Line_t& l2, Point_t& L1, Point_t& L2) const;
380 
381  /// HalfLine & HalfLine distance w/o dimensionality check
382  double _SqDist_(const HalfLine_t& l1, const HalfLine_t& l2, Point_t& L1, Point_t& L2) const;
383 
384  /// Point & LineSegment distance w/o dimensionality check
385  double _SqDist_(const Point_t& pt, const LineSegment_t& line) const
386  { return _SqDist_(pt,line.Start(),line.End()); }
387 
388  /// Point & LineSegment distance w/o dimensionality check
389  double _SqDist_(const Point_t& pt, const Point_t& line_s, const Point_t& line_e) const;
390 
391  /// Point & LineSegment distance w/o dimensionality check
392  double _SqDist_(const LineSegment_t& line, const Point_t&pt) const
393  { return _SqDist_(pt,line); }
394 
395  /// HalfLine & LineSegment distance w/o dimensionality check
396  double _SqDist_(const HalfLine_t& hline, const LineSegment_t& seg, Point_t& L1, Point_t& L2) const;
397 
398  /// LineSegment & LineSegment distance w/o dimensionality check
399  double _SqDist_(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& c1, Point_t& c2) const;
400 
401  // Point & LineSegment closest point w/o dimensionality check
402  Point_t _ClosestPt_(const Point_t& pt, const LineSegment_t& line) const;
403  // Point & LineSegment closest point w/o dimensionality check
404  Point_t _ClosestPt_(const LineSegment_t& line, const Point_t& pt) const
405  { return _ClosestPt_(pt,line); }
406 
407  /// Point & HalfLine distance w/o dimensionality check
408  double _SqDist_(const Point_t& pt, const HalfLine_t& line) const;
409  /// Point & HalfLine distance w/o dimensionality check
410  double _SqDist_(const HalfLine_t& line, const Point_t& pt) const
411  { return _SqDist_(pt,line); }
412  // Point & HalfLine closest point w/o dimensionality check
413  Point_t _ClosestPt_(const Point_t& pt, const HalfLine_t& line) const;
414  // Point & HalfLine closest point w/o dimensionality check
415  Point_t _ClosestPt_(const HalfLine_t& line, const Point_t& pt) const
416  { return _ClosestPt_(pt,line); }
417 
418  // Point & InfLine closest point w/o dimensionality check
419  Point_t _ClosestPt_(const Line_t& line, const Point_t& pt) const;
420  // Point & InfLine closest point w/o dimensionality check
421  Point_t _ClosestPt_(const Point_t& pt, const Line_t& line) const
422  { return _ClosestPt_(line,pt); }
423  // Point & InfLine distance w/o dimensionality check
424  double _SqDist_(const Line_t& line, const Point_t& pt) const;
425  // Point & InfLine distance w/o dimensionality check
426  double _SqDist_(const Point_t& pt, const Line_t& line) const
427  { return _SqDist_(line,pt); }
428 
429  /// Point & AABox distance w/o dimensionality check
430  double _SqDist_(const Point_t& pt, const AABox_t& box) const;
431  /// Point & AABox distance w/o dimensionality check
432  double _SqDist_(const AABox_t& box, const Point_t& pt) const
433  { return _SqDist_(pt,box); }
434 
435  /// Point & AABox closest point w/o dimensionality check
436  Point_t _ClosestPt_(const Point_t& pt, const AABox_t& box) const;
437  /// Point & AABox closest point w/o dimensionality check
438  Point_t _ClosestPt_(const AABox_t& box, const Point_t& pt) const
439  { return _ClosestPt_(pt,box); }
440 
441  /// Common origin: Line & Line. Keep track of origin
442  double _commonOrigin_(const Line_t& lin1, const Line_t& lin2, Point_t& origin) const;
443  /// Common origin: Half Line & Half Line. Keep track of origin
444  double _commonOrigin_(const HalfLine_t& lin1, const HalfLine_t& lin2, Point_t& origin, bool backwards) const;
445  /// Common origin: Line Segment & Half Line. Keep track of origin
446  double _commonOrigin_(const HalfLine_t& lin, const LineSegment_t& seg, Point_t& origin, bool backwards) const;
447  /// Common origin: Line Segment & Line Segment. Keep track of origin
448  double _commonOrigin_(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& origin, bool backwards) const;
449  /// Common origin: Trajectory & Trajectory. Keep track of origin
450  double _commonOrigin_(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& origin, bool backwards) const;
451  /// Common origin: Trajectory & Line Segment. Keep track of origin
452  double _commonOrigin_(const Trajectory_t& trj, const LineSegment_t& seg, Point_t& origin, bool backwards) const;
453  /// Common origin: Trajectory & Half Line. Keep track of origin
454  double _commonOrigin_(const Trajectory_t& trj, const HalfLine_t& lin, Point_t& origin, bool backwards) const;
455 
456 
457  // Bounding Sphere given a vector of points
458  Sphere_t _boundingSphere_(const std::vector<Point_t>& pts) const;
459  Sphere_t _RemainingPoints_(std::vector<Point_t>& remaining, const Sphere_t& thisSphere)const;
460  Sphere_t _WelzlSphere_(const std::vector<Point_t>& pts, int numPts, std::vector<Point_t> sosPts) const;
461 
462  /// Clamp function: checks if value out of bounds
463  double _Clamp_(const double n, const double min, const double max) const;
464 
465  /// Swap two points if min & max are inverted
466  inline void _Swap_(double& tmin, double& tmax) const
467  { if(tmin > tmax) std::swap(tmin,tmax); }
468 
469  };
470 }
471 
472 #endif
473 /** @} */ // end of doxygen group
double commonOrigin(const LineSegment_t &seg, const Trajectory_t &trj, Point_t &origin, bool backwards=false) const
Common origin: Trajectory &amp; Line Segment. Keep track of origin.
Point_t ClosestPt(const Point_t &pt, const Line_t &line) const
double commonOrigin(const Trajectory_t &trj, const LineSegment_t &seg, Point_t &origin, bool backwards=false) const
Common origin: Trajectory &amp; Line Segment. Keep track of origin.
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const LineSegment_t &seg, Point_t &c1, Point_t &c2, int &trackIdx) const
LineSegment &amp; vector of Trajectories, keep track of points.
Point_t ClosestPt(const AABox_t &box, const Point_t &pt) const
Point &amp; AABox closest point.
double SqDist(const HalfLine_t &line, const Point_t &pt) const
Point &amp; HalfLine distance.
Point_t ClosestPt(const Line_t &line, const Point_t &pt) const
double _SqDist_(const Line_t &l1, const Line_t &l2, Point_t &L1, Point_t &L2) const
Line &amp; Line distance w/o dimensionality check.
double SqDist(const AABox_t &box, const Point_t &pt)
Point &amp; AABox distance.
double SqDist(const Point_t &pt, const LineSegment_t &line) const
Point &amp; LineSegment_t distance.
Algorithm to compute various geometrical relation among geometrical objects. In particular functions ...
const Point_t & Start() const
Start getter.
Point_t ClosestPt(const Point_t &pt, const std::vector< geoalgo::Trajectory_t > &trj) const
Point_t &amp; Trajectory_t closest point - don&#39;t keep track of which track is closest.
Point_t ClosestPt(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt) const
Point_t &amp; Trajectory_t closest point - don&#39;t keep track of which track is closest.
double SqDist(const LineSegment_t &line, const Point_t &pt) const
Point &amp; LineSegment distance.
double SqDist(const LineSegment_t &seg, const HalfLine_t &hline, Point_t &L1, Point_t &L2) const
double SqDist(const LineSegment_t &seg1, const LineSegment_t &seg2) const
LineSegment &amp; LineSegment, don&#39;t keep track of points.
double _Clamp_(const double n, const double min, const double max) const
Clamp function: checks if value out of bounds.
Point_t ClosestPt(const Point_t &pt, const LineSegment_t &line) const
Point &amp; LineSegment closest point.
double commonOrigin(const HalfLine_t &lin, const Trajectory_t &trj, bool backwards=false) const
Common origin: Trajectory &amp; Half Line. Do not keep track of origin.
double SqDist(const LineSegment_t &seg, const std::vector< geoalgo::Trajectory_t > &trj) const
LineSegment &amp; vector of Trajectories, don&#39;t keep track of points.
void compat(const Point_t &obj) const
Dimensionality check function w/ Trajectory.
void _Swap_(double &tmin, double &tmax) const
Swap two points if min &amp; max are inverted.
Point_t ClosestPt(const Point_t &pt, const HalfLine_t &line) const
Point &amp; HalfLine closest point.
double SqDist(const HalfLine_t &l1, const HalfLine_t &l2, Point_t &L1, Point_t &L2) const
Point_t _ClosestPt_(const LineSegment_t &line, const Point_t &pt) const
double SqDist(const Trajectory_t &trj, const HalfLine_t &hline) const
HalfLine &amp; Trajectory, don&#39;t keep track of points.
double SqDist(const Line_t &line, const Point_t &pt) const
pdgs p
Definition: selectors.fcl:22
Representation of a simple 3D line segment Defines a finite 3D straight line by having the start and ...
double SqDist(const LineSegment_t &seg1, const LineSegment_t &seg2, Point_t &c1, Point_t &c2) const
LineSegment_t &amp; LineSegment_t distance - keep track of points.
double SqDist(const Trajectory_t &trj, const LineSegment_t &seg, Point_t &c1, Point_t &c2) const
LineSegment &amp; Trajectory, keep track of points.
Representation of a 3D rectangular box which sides are aligned w/ coordinate axis. A representation of an Axis-Aligned-Boundary-Box, a simple &amp; popular representation of 3D boundary box for collision detection. The concept was taken from the reference, Real-Time-Collision-Detection (RTCD), and in particular Ch. 4.2 (page 77): .
double commonOrigin(const HalfLine_t &lin, const LineSegment_t &seg, Point_t &origin, bool backwards=false) const
Common origin: Line Segment &amp; Line Segment. Keep track of origin.
double SqDist(const Point_t &pt, const std::vector< geoalgo::Trajectory_t > &trj) const
Point_t &amp; Trajectory_t distance - don&#39;t keep track.
void compat(const Vector &obj) const
Dimensional check for a compatibility.
double _SqDist_(const HalfLine_t &line, const Point_t &pt) const
Point &amp; HalfLine distance w/o dimensionality check.
double commonOrigin(const Trajectory_t &trj, const HalfLine_t &lin, bool backwards=false) const
Common origin: Trajectory &amp; Half Line. Do not keep track of origin.
double commonOrigin(const LineSegment_t &seg, const HalfLine_t &lin, bool backwards=false) const
Common origin: Line Segment &amp; Half Line. Do not keep track of origin.
double SqDist(const Line_t &l1, const Line_t &l2) const
double SqDist(const HalfLine_t &hline, const Trajectory_t &trj) const
HalfLine &amp; Trajectory, don&#39;t keep track of points.
const Point_t & Min() const
Minimum point getter.
Sphere_t boundingSphere(const std::vector< Point_t > &pts) const
Point_t _ClosestPt_(const Point_t &pt, const Line_t &line) const
Point_t ClosestPt(const HalfLine_t &line, const Point_t &pt) const
Point &amp; HalfLine closest point.
double SqDist(const Point_t &pt, const AABox_t &box) const
Point &amp; AABox distance.
Point_t ClosestPt(const Trajectory_t &trj, const Point_t &pt) const
Point_t &amp; Trajectory_t closest point.
double commonOrigin(const Line_t &lin1, const Line_t &lin2, Point_t &origin) const
Common origin: Line Segment &amp; Line Segment. Keep track of origin.
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt, int &trackIdx) const
Point_t &amp; Trajectory_t distance - keep track of which track.
double commonOrigin(const HalfLine_t &lin, const Trajectory_t &trj, Point_t &origin, bool backwards=false) const
Common origin: Trajectory &amp; Half Line. Keep track of origin.
double commonOrigin(const LineSegment_t &seg, const Trajectory_t &trj, bool backwards=false) const
Common origin: Trajectory &amp; Line Segment. Do not keep track of origin.
double SqDist(const Trajectory_t &trj, const LineSegment_t &seg) const
LineSegment &amp; Trajectory, don&#39;t keep track of points.
Point_t ClosestPt(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt, int &trackIdx) const
Point_t &amp; Trajectory_t closest point - keep track of which track is closest.
const Point_t & Pt1() const
Start getter.
const Point_t & End() const
End getter.
double commonOrigin(const Trajectory_t &trj1, const Trajectory_t &trj2, bool backwards=false) const
Common origin: Trajectory &amp; Trajectory. Do not keep track of origin.
double SqDist(const Trajectory_t &trj, const Point_t &pt) const
Point_t &amp; Trajectory_t distance.
double commonOrigin(const Line_t &lin1, const Line_t &lin2) const
Common origin: Line Segment &amp; Line Segment. Do not keep track of origin.
std::vector< Point_t > Intersection(const HalfLine_t &line, const AABox_t &box, bool back=false) const
Intersection between a HalfLine and an AABox.
Representation of a 3D infinite line. Defines an infinite 3D line by having 2 points which completely...
Point_t ClosestPt(const Point_t &pt, const AABox_t &box) const
Point &amp; AABox closest point.
Point_t ClosestPt(const Point_t &pt, const Trajectory_t &trj) const
Point_t &amp; Trajectory_t closest point.
Sphere_t _WelzlSphere_(const std::vector< Point_t > &pts, int numPts, std::vector< Point_t > sosPts) const
double commonOrigin(const HalfLine_t &lin1, const HalfLine_t &lin2, Point_t &origin, bool backwards=false) const
Common origin: Half Line &amp; Half Line. Keep track of origin.
double SqDist(const HalfLine_t &hline, const LineSegment_t &seg, Point_t &L1, Point_t &L2) const
double SqDist(const Line_t &l1, const Line_t &l2, Point_t &L1, Point_t &L2) const
double SqDist(const Point_t &pt, const Line_t &line) const
const Point_t & Start() const
Start getter.
Sphere_t _boundingSphere_(const std::vector< Point_t > &pts) const
double commonOrigin(const Trajectory_t &trj, const LineSegment_t &seg, bool backwards=false) const
Common origin: Trajectory &amp; Line Segment. Do not keep track of origin.
double _SqDist_(const Point_t &pt, const LineSegment_t &line) const
Point &amp; LineSegment distance w/o dimensionality check.
Point_t ClosestPt(const Trajectory_t &trj, const Point_t &pt, int &idx) const
Point_t &amp; Trajectory_t closest point. Keep track of index of segment.
double _SqDist_(const AABox_t &box, const Point_t &pt) const
Point &amp; AABox distance w/o dimensionality check.
double SqDist(const LineSegment_t &seg, const Trajectory_t &trj) const
LineSegment &amp; Trajectory, don&#39;t keep track of points.
Point_t ClosestPt(const LineSegment_t &line, const Point_t &pt) const
Point &amp; LineSegment closest point.
Representation of a 3D semi-infinite line. Defines a semi-infinite 3D line by having a start point (P...
Sphere_t _RemainingPoints_(std::vector< Point_t > &remaining, const Sphere_t &thisSphere) const
double commonOrigin(const LineSegment_t &seg1, const LineSegment_t &seg2, bool backwards=false) const
Common origin: Line Segment &amp; Line Segment. Do not keep track of origin.
std::vector< Point_t > Intersection(const LineSegment_t &l, const AABox_t &box) const
Intersection between LineSegment and an AABox.
Point_t _ClosestPt_(const HalfLine_t &line, const Point_t &pt) const
double commonOrigin(const LineSegment_t &seg1, const LineSegment_t &seg2, Point_t &origin, bool backwards=false) const
Common origin: Line Segment &amp; Line Segment. Keep track of origin.
double SqDist(const LineSegment_t &seg, const HalfLine_t &hline) const
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const LineSegment_t &seg) const
LineSegment &amp; vector of Trajectories, don&#39;t keep track of points.
void compat(const Point_t &p, const double r=0) const
3D point compatibility check
double _SqDist_(const Point_t &pt, const Line_t &line) const
double SqDist(const HalfLine_t &l1, const HalfLine_t &l2) const
LineSegment_t BoxOverlap(const AABox_t &box, const HalfLine_t &line) const
LineSegment sub-segment of HalfLine inside an AABox.
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt) const
Point_t &amp; Trajectory_t distance - don&#39;t keep track.
double SqDist(const Trajectory_t &trj1, const Trajectory_t &trj2) const
Trajectory &amp; Trajectory, don&#39;t keep track of points.
LineSegment_t BoxOverlap(const HalfLine_t &line, const AABox_t &box) const
LineSegment sub-segment of HalfLine inside an AABox.
double commonOrigin(const LineSegment_t &seg, const HalfLine_t &lin, Point_t &origin, bool backwards=false) const
Common origin: Line Segment &amp; Line Segment. Keep track of origin.
Point_t _ClosestPt_(const AABox_t &box, const Point_t &pt) const
Point &amp; AABox closest point w/o dimensionality check.
double SqDist(const Point_t &pt, const HalfLine_t &line) const
Point &amp; HalfLine distance.
double commonOrigin(const HalfLine_t &lin, const LineSegment_t &seg, bool backwards=false) const
Common origin: Line Segment &amp; Half Line. Do not keep track of origin.
double _SqDist_(const LineSegment_t &line, const Point_t &pt) const
Point &amp; LineSegment distance w/o dimensionality check.
std::vector< Point_t > Intersection(const AABox_t &box, const HalfLine_t &line, bool back=false) const
Intersection between a HalfLine and an AABox.
double SqDist(const Trajectory_t &trj, const HalfLine_t &hline, Point_t &c1, Point_t &c2) const
HalfLine &amp; Trajectory, keep track of points.
double SqDist(const HalfLine_t &hline, const LineSegment_t &seg) const
double commonOrigin(const Trajectory_t &trj, const HalfLine_t &lin, Point_t &origin, bool backwards=false) const
Common origin: Trajectory &amp; Half Line. Keep track of origin.
std::vector< Point_t > Intersection(const Trajectory_t &trj, const AABox_t &box) const
Intersection between Trajectory and an AABox.
double _commonOrigin_(const Line_t &lin1, const Line_t &lin2, Point_t &origin) const
Common origin: Line &amp; Line. Keep track of origin.
double commonOrigin(const Trajectory_t &trj1, const Trajectory_t &trj2, Point_t &origin, bool backwards=false) const
Common origin: Trajectory &amp; Trajectory. Keep track of origin.
constexpr Point origin()
Returns a origin position with a point of the specified type.
Definition: geo_vectors.h:227
Trajectory_t BoxOverlap(const Trajectory_t &trj, const AABox_t &box) const
Get Trajectory inside box given some input trajectory -&gt; now assumes trajectory cannot exit and re-en...
Point_t _ClosestPt_(const Point_t &pt, const LineSegment_t &line) const
double commonOrigin(const HalfLine_t &lin1, const HalfLine_t &lin2, bool backwards=false) const
Common origin: Half Line &amp; Half Line. Do not keep track of origin.