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