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 & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:366
Point_t ClosestPt(const Point_t &pt, const Line_t &line) const
Definition: GeoAlgo.h:96
double commonOrigin(const Trajectory_t &trj, const LineSegment_t &seg, Point_t &origin, bool backwards=false) const
Common origin: Trajectory & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:360
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const LineSegment_t &seg, Point_t &c1, Point_t &c2, int &trackIdx) const
LineSegment & vector of Trajectories, keep track of points.
Definition: GeoAlgo.h:165
Point_t ClosestPt(const AABox_t &box, const Point_t &pt) const
Point & AABox closest point.
Definition: GeoAlgo.h:301
double SqDist(const HalfLine_t &line, const Point_t &pt) const
Point & HalfLine distance.
Definition: GeoAlgo.h:262
Point_t ClosestPt(const Line_t &line, const Point_t &pt) const
Definition: GeoAlgo.h:93
double _SqDist_(const Line_t &l1, const Line_t &l2, Point_t &L1, Point_t &L2) const
Line & Line distance w/o dimensionality check.
Definition: GeoAlgo.cxx:193
double SqDist(const AABox_t &box, const Point_t &pt)
Point & AABox distance.
Definition: GeoAlgo.h:295
double SqDist(const Point_t &pt, const LineSegment_t &line) const
Point & LineSegment_t distance.
Definition: GeoAlgo.h:243
Algorithm to compute various geometrical relation among geometrical objects. In particular functions ...
Definition: GeoAlgo.h:43
const Point_t & Start() const
Start getter.
Definition: GeoHalfLine.cxx:25
Point_t ClosestPt(const Point_t &pt, const std::vector< geoalgo::Trajectory_t > &trj) const
Point_t & Trajectory_t closest point - don&#39;t keep track of which track is closest.
Definition: GeoAlgo.h:232
Point_t ClosestPt(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt) const
Point_t & Trajectory_t closest point - don&#39;t keep track of which track is closest.
Definition: GeoAlgo.h:235
double SqDist(const LineSegment_t &line, const Point_t &pt) const
Point & LineSegment distance.
Definition: GeoAlgo.h:246
double SqDist(const LineSegment_t &seg, const HalfLine_t &hline, Point_t &L1, Point_t &L2) const
Definition: GeoAlgo.h:279
double SqDist(const LineSegment_t &seg1, const LineSegment_t &seg2) const
LineSegment & LineSegment, don&#39;t keep track of points.
Definition: GeoAlgo.h:129
double _Clamp_(const double n, const double min, const double max) const
Clamp function: checks if value out of bounds.
Definition: GeoAlgo.cxx:845
Point_t ClosestPt(const Point_t &pt, const LineSegment_t &line) const
Point & LineSegment closest point.
Definition: GeoAlgo.h:249
double commonOrigin(const HalfLine_t &lin, const Trajectory_t &trj, bool backwards=false) const
Common origin: Trajectory & Half Line. Do not keep track of origin.
Definition: GeoAlgo.h:351
double SqDist(const LineSegment_t &seg, const std::vector< geoalgo::Trajectory_t > &trj) const
LineSegment & vector of Trajectories, don&#39;t keep track of points.
Definition: GeoAlgo.h:171
void compat(const Point_t &obj) const
Dimensionality check function w/ Trajectory.
Class def header for a class HalfLine.
void _Swap_(double &tmin, double &tmax) const
Swap two points if min & max are inverted.
Definition: GeoAlgo.h:466
Point_t ClosestPt(const Point_t &pt, const HalfLine_t &line) const
Point & HalfLine closest point.
Definition: GeoAlgo.h:265
double SqDist(const HalfLine_t &l1, const HalfLine_t &l2, Point_t &L1, Point_t &L2) const
Definition: GeoAlgo.h:115
Point_t _ClosestPt_(const LineSegment_t &line, const Point_t &pt) const
Definition: GeoAlgo.h:404
double SqDist(const Trajectory_t &trj, const HalfLine_t &hline) const
HalfLine & Trajectory, don&#39;t keep track of points.
Definition: GeoAlgo.h:184
double SqDist(const Line_t &line, const Point_t &pt) const
Definition: GeoAlgo.h:87
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 & LineSegment_t distance - keep track of points.
Definition: GeoAlgo.h:126
double SqDist(const Trajectory_t &trj, const LineSegment_t &seg, Point_t &c1, Point_t &c2) const
LineSegment & Trajectory, keep track of points.
Definition: GeoAlgo.h:139
Representation of a 3D rectangular box which sides are aligned w/ coordinate axis. A representation of an Axis-Aligned-Boundary-Box, a simple & 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): .
Definition: GeoAABox.h:34
double commonOrigin(const HalfLine_t &lin, const LineSegment_t &seg, Point_t &origin, bool backwards=false) const
Common origin: Line Segment & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:324
double SqDist(const Point_t &pt, const std::vector< geoalgo::Trajectory_t > &trj) const
Point_t & Trajectory_t distance - don&#39;t keep track.
Definition: GeoAlgo.h:221
Class def header for a class Line.
void compat(const Vector &obj) const
Dimensional check for a compatibility.
Definition: GeoVector.cxx:97
double _SqDist_(const HalfLine_t &line, const Point_t &pt) const
Point & HalfLine distance w/o dimensionality check.
Definition: GeoAlgo.h:410
double commonOrigin(const Trajectory_t &trj, const HalfLine_t &lin, bool backwards=false) const
Common origin: Trajectory & Half Line. Do not keep track of origin.
Definition: GeoAlgo.h:345
double commonOrigin(const LineSegment_t &seg, const HalfLine_t &lin, bool backwards=false) const
Common origin: Line Segment & Half Line. Do not keep track of origin.
Definition: GeoAlgo.h:327
double SqDist(const Line_t &l1, const Line_t &l2) const
Definition: GeoAlgo.h:107
static QStrList * l
Definition: config.cpp:1044
double SqDist(const HalfLine_t &hline, const Trajectory_t &trj) const
HalfLine & Trajectory, don&#39;t keep track of points.
Definition: GeoAlgo.h:187
const Point_t & Min() const
Minimum point getter.
Definition: GeoAABox.cxx:25
Class def header for a class AABox.
Sphere_t boundingSphere(const std::vector< Point_t > &pts) const
Definition: GeoAlgo.h:373
Point_t _ClosestPt_(const Point_t &pt, const Line_t &line) const
Definition: GeoAlgo.h:421
Point_t ClosestPt(const HalfLine_t &line, const Point_t &pt) const
Point & HalfLine closest point.
Definition: GeoAlgo.h:268
double SqDist(const Point_t &pt, const AABox_t &box) const
Point & AABox distance.
Definition: GeoAlgo.h:292
Point_t ClosestPt(const Trajectory_t &trj, const Point_t &pt) const
Point_t & Trajectory_t closest point.
Definition: GeoAlgo.h:203
double commonOrigin(const Line_t &lin1, const Line_t &lin2, Point_t &origin) const
Common origin: Line Segment & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:312
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt, int &trackIdx) const
Point_t & Trajectory_t distance - keep track of which track.
Definition: GeoAlgo.h:218
Class def header for a class Point and Vector.
void swap(Handle< T > &a, Handle< T > &b)
double commonOrigin(const HalfLine_t &lin, const Trajectory_t &trj, Point_t &origin, bool backwards=false) const
Common origin: Trajectory & Half Line. Keep track of origin.
Definition: GeoAlgo.h:354
double commonOrigin(const LineSegment_t &seg, const Trajectory_t &trj, bool backwards=false) const
Common origin: Trajectory & Line Segment. Do not keep track of origin.
Definition: GeoAlgo.h:363
double SqDist(const Trajectory_t &trj, const LineSegment_t &seg) const
LineSegment & Trajectory, don&#39;t keep track of points.
Definition: GeoAlgo.h:142
std::void_t< T > n
Point_t ClosestPt(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt, int &trackIdx) const
Point_t & Trajectory_t closest point - keep track of which track is closest.
Definition: GeoAlgo.h:229
const Point_t & Pt1() const
Start getter.
Definition: GeoLine.cxx:22
const Point_t & End() const
End getter.
double commonOrigin(const Trajectory_t &trj1, const Trajectory_t &trj2, bool backwards=false) const
Common origin: Trajectory & Trajectory. Do not keep track of origin.
Definition: GeoAlgo.h:339
double SqDist(const Trajectory_t &trj, const Point_t &pt) const
Point_t & Trajectory_t distance.
Definition: GeoAlgo.h:197
Class def header for a class HalfLine.
double commonOrigin(const Line_t &lin1, const Line_t &lin2) const
Common origin: Line Segment & Line Segment. Do not keep track of origin.
Definition: GeoAlgo.h:309
p
Definition: test.py:223
std::vector< Point_t > Intersection(const HalfLine_t &line, const AABox_t &box, bool back=false) const
Intersection between a HalfLine and an AABox.
Definition: GeoAlgo.h:54
Representation of a 3D infinite line. Defines an infinite 3D line by having 2 points which completely...
Definition: GeoLine.h:27
Point_t ClosestPt(const Point_t &pt, const AABox_t &box) const
Point & AABox closest point.
Definition: GeoAlgo.h:298
Point_t ClosestPt(const Point_t &pt, const Trajectory_t &trj) const
Point_t & Trajectory_t closest point.
Definition: GeoAlgo.h:200
Sphere_t _WelzlSphere_(const std::vector< Point_t > &pts, int numPts, std::vector< Point_t > sosPts) const
Definition: GeoAlgo.cxx:1096
static int max(int a, int b)
double commonOrigin(const HalfLine_t &lin1, const HalfLine_t &lin2, Point_t &origin, bool backwards=false) const
Common origin: Half Line & Half Line. Keep track of origin.
Definition: GeoAlgo.h:336
double SqDist(const HalfLine_t &hline, const LineSegment_t &seg, Point_t &L1, Point_t &L2) const
Definition: GeoAlgo.h:276
double SqDist(const Line_t &l1, const Line_t &l2, Point_t &L1, Point_t &L2) const
Definition: GeoAlgo.h:104
double SqDist(const Point_t &pt, const Line_t &line) const
Definition: GeoAlgo.h:90
const Point_t & Start() const
Start getter.
Sphere_t _boundingSphere_(const std::vector< Point_t > &pts) const
Definition: GeoAlgo.cxx:1015
double commonOrigin(const Trajectory_t &trj, const LineSegment_t &seg, bool backwards=false) const
Common origin: Trajectory & Line Segment. Do not keep track of origin.
Definition: GeoAlgo.h:357
double _SqDist_(const Point_t &pt, const LineSegment_t &line) const
Point & LineSegment distance w/o dimensionality check.
Definition: GeoAlgo.h:385
Point_t ClosestPt(const Trajectory_t &trj, const Point_t &pt, int &idx) const
Point_t & Trajectory_t closest point. Keep track of index of segment.
Definition: GeoAlgo.h:208
double _SqDist_(const AABox_t &box, const Point_t &pt) const
Point & AABox distance w/o dimensionality check.
Definition: GeoAlgo.h:432
double SqDist(const LineSegment_t &seg, const Trajectory_t &trj) const
LineSegment & Trajectory, don&#39;t keep track of points.
Definition: GeoAlgo.h:145
T min(sqlite3 *const db, std::string const &table_name, std::string const &column_name)
Definition: statistics.h:55
Point_t ClosestPt(const LineSegment_t &line, const Point_t &pt) const
Point & LineSegment closest point.
Definition: GeoAlgo.h:252
Representation of a 3D semi-infinite line. Defines a semi-infinite 3D line by having a start point (P...
Definition: GeoHalfLine.h:30
Sphere_t _RemainingPoints_(std::vector< Point_t > &remaining, const Sphere_t &thisSphere) const
Definition: GeoAlgo.cxx:1050
double commonOrigin(const LineSegment_t &seg1, const LineSegment_t &seg2, bool backwards=false) const
Common origin: Line Segment & Line Segment. Do not keep track of origin.
Definition: GeoAlgo.h:315
void line(double t, double *p, double &x, double &y, double &z)
std::vector< Point_t > Intersection(const LineSegment_t &l, const AABox_t &box) const
Intersection between LineSegment and an AABox.
Definition: GeoAlgo.h:60
Point_t _ClosestPt_(const HalfLine_t &line, const Point_t &pt) const
Definition: GeoAlgo.h:415
double commonOrigin(const LineSegment_t &seg1, const LineSegment_t &seg2, Point_t &origin, bool backwards=false) const
Common origin: Line Segment & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:318
double SqDist(const LineSegment_t &seg, const HalfLine_t &hline) const
Definition: GeoAlgo.h:285
Class def header for a class Trajectory.
Class def header for a class LineSegment.
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const LineSegment_t &seg) const
LineSegment & vector of Trajectories, don&#39;t keep track of points.
Definition: GeoAlgo.h:168
void compat(const Point_t &p, const double r=0) const
3D point compatibility check
Definition: GeoSphere.cxx:363
double _SqDist_(const Point_t &pt, const Line_t &line) const
Definition: GeoAlgo.h:426
double SqDist(const HalfLine_t &l1, const HalfLine_t &l2) const
Definition: GeoAlgo.h:118
LineSegment_t BoxOverlap(const AABox_t &box, const HalfLine_t &line) const
LineSegment sub-segment of HalfLine inside an AABox.
Definition: GeoAlgo.cxx:167
double SqDist(const std::vector< geoalgo::Trajectory_t > &trj, const Point_t &pt) const
Point_t & Trajectory_t distance - don&#39;t keep track.
Definition: GeoAlgo.h:224
double SqDist(const Trajectory_t &trj1, const Trajectory_t &trj2) const
Trajectory & Trajectory, don&#39;t keep track of points.
Definition: GeoAlgo.h:155
LineSegment_t BoxOverlap(const HalfLine_t &line, const AABox_t &box) const
LineSegment sub-segment of HalfLine inside an AABox.
Definition: GeoAlgo.h:72
double commonOrigin(const LineSegment_t &seg, const HalfLine_t &lin, Point_t &origin, bool backwards=false) const
Common origin: Line Segment & Line Segment. Keep track of origin.
Definition: GeoAlgo.h:330
Point_t _ClosestPt_(const AABox_t &box, const Point_t &pt) const
Point & AABox closest point w/o dimensionality check.
Definition: GeoAlgo.h:438
double SqDist(const Point_t &pt, const HalfLine_t &line) const
Point & HalfLine distance.
Definition: GeoAlgo.h:259
double commonOrigin(const HalfLine_t &lin, const LineSegment_t &seg, bool backwards=false) const
Common origin: Line Segment & Half Line. Do not keep track of origin.
Definition: GeoAlgo.h:321
double _SqDist_(const LineSegment_t &line, const Point_t &pt) const
Point & LineSegment distance w/o dimensionality check.
Definition: GeoAlgo.h:392
std::vector< Point_t > Intersection(const AABox_t &box, const HalfLine_t &line, bool back=false) const
Intersection between a HalfLine and an AABox.
Definition: GeoAlgo.cxx:11
double SqDist(const Trajectory_t &trj, const HalfLine_t &hline, Point_t &c1, Point_t &c2) const
HalfLine & Trajectory, keep track of points.
Definition: GeoAlgo.h:181
double SqDist(const HalfLine_t &hline, const LineSegment_t &seg) const
Definition: GeoAlgo.h:282
double commonOrigin(const Trajectory_t &trj, const HalfLine_t &lin, Point_t &origin, bool backwards=false) const
Common origin: Trajectory & Half Line. Keep track of origin.
Definition: GeoAlgo.h:348
std::vector< Point_t > Intersection(const Trajectory_t &trj, const AABox_t &box) const
Intersection between Trajectory and an AABox.
Definition: GeoAlgo.h:66
double _commonOrigin_(const Line_t &lin1, const Line_t &lin2, Point_t &origin) const
Common origin: Line & Line. Keep track of origin.
Definition: GeoAlgo.cxx:854
double commonOrigin(const Trajectory_t &trj1, const Trajectory_t &trj2, Point_t &origin, bool backwards=false) const
Common origin: Trajectory & Trajectory. Keep track of origin.
Definition: GeoAlgo.h:342
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 -> now assumes trajectory cannot exit and re-en...
Definition: GeoAlgo.h:79
Point_t _ClosestPt_(const Point_t &pt, const LineSegment_t &line) const
Definition: GeoAlgo.cxx:388
double commonOrigin(const HalfLine_t &lin1, const HalfLine_t &lin2, bool backwards=false) const
Common origin: Half Line & Half Line. Do not keep track of origin.
Definition: GeoAlgo.h:333