GeoAlgo.cxx
Go to the documentation of this file.
2 #include "larcorealg/GeoAlgo/GeoAlgoConstants.h" // for kINVALID_DOUBLE
3 #include "larcorealg/GeoAlgo/GeoAlgoException.h" // for GeoAlgoException
4 
5 #include <stddef.h>
6 
7 namespace geoalgo {
8 
9  // Ref. RTCD 5.3.2 p. 177
10  // Intersection of a HalfLine w/ AABox
11  std::vector<Point_t> GeoAlgo::Intersection(const AABox_t& box,
12  const HalfLine_t& line,
13  bool back) const
14  {
15  // Result container
16  std::vector<Point_t> result;
17  Point_t xs1(3); // Only 2 points max possible
18  Point_t xs2(3); // Create in advance for early termination checks
19  // One-time only initialization for unit vectors
20  static std::vector<Point_t> min_plane_n;
21  static std::vector<Point_t> max_plane_n;
22  if(!min_plane_n.size()) {
23  min_plane_n.reserve(3);
24  min_plane_n.push_back(Vector_t(-1,0,0));
25  min_plane_n.push_back(Vector_t(0,-1,0));
26  min_plane_n.push_back(Vector_t(0,0,-1));
27  max_plane_n.reserve(3);
28  max_plane_n.push_back(Vector_t(1,0,0));
29  max_plane_n.push_back(Vector_t(0,1,0));
30  max_plane_n.push_back(Vector_t(0,0,1));
31  }
32  // min/max points of the AABox
33  auto const& min_pt = box.Min();
34  auto const& max_pt = box.Max();
35  // start/dir of the line
36  auto const& start = line.Start();
37  auto dir = line.Dir();
38  if(back) dir *= -1;
39  // Inspect the case of parallel line
40  for(size_t i=0; i<min_pt.size(); ++i) {
41  if ( dir[i] == 0 &&
42  (start[i] <= min_pt[i] || max_pt[i] <= start[i]) )
43  return result;
44  }
45  // Look for xs w/ 3 planes
46  for(size_t i=0; i<3; ++i) {
47  auto const& normal = min_plane_n[i];
48  double s = (-1.) * ( normal * (start - min_pt) ) / (normal * dir);
49  if(s<0) continue;
50  auto xs = start + dir * s;
51  // Check if the found point is within the surface area of other 2 axis
52  bool on_surface=true;
53  for(size_t sur_axis=0; sur_axis<3; ++sur_axis) {
54  if(sur_axis==i) continue;
55  if(xs[sur_axis] < min_pt[sur_axis] || max_pt[sur_axis] < xs[sur_axis]) {
56  on_surface=false;
57  break;
58  }
59  }
60  if(on_surface && xs != xs1) {
61  // Directly assign to xs1 instead of making a copy
62  if(!(xs1.IsValid())) for(size_t j=0; j<3; ++j) xs1[j]=xs[j];
63  else {
64  // If xs2 is filled, no more point to search. Exit.
65  for(size_t j=0; j<3; ++j) xs2[j]=xs[j];
66  break;
67  }
68  }
69  }
70  // If xs2 is filled, simply return the result. Order the output via distance
71  if(xs2.IsValid()) {
72  result.reserve(2);
73  if(xs1._SqDist_(start) > xs2._SqDist_(start)) std::swap(xs1,xs2);
74  result.push_back(xs1);
75  result.push_back(xs2);
76  return result;
77  }
78  // Look for xs w/ 3 planes
79  for(size_t i=0; i<3; ++i) {
80  auto const& normal = max_plane_n[i];
81  double s = (-1.) * ( normal * (start - max_pt) ) / (normal * dir);
82  if(s<0) continue;
83  auto xs = start + dir * s;
84  bool on_surface=true;
85  for(size_t sur_axis=0; sur_axis<3; ++sur_axis) {
86  if(sur_axis==i) continue;
87  if(xs[sur_axis] < min_pt[sur_axis] || max_pt[sur_axis] < xs[sur_axis]) {
88  on_surface=false;
89  break;
90  }
91  }
92  if(on_surface && xs != xs1) {
93  if(!(xs1.IsValid())) for(size_t j=0; j<3; ++j) xs1[j]=xs[j];
94  else {
95  for(size_t j=0; j<3; ++j) xs2[j]=xs[j];
96  break;
97  }
98  }
99  }
100  if(!xs1.IsValid()) return result;
101  if(xs2.IsValid()) {
102  result.reserve(2);
103  if(xs1._SqDist_(start) > xs2._SqDist_(start)) std::swap(xs1,xs2);
104  result.push_back(xs1);
105  result.push_back(xs2);
106  return result;
107  }
108  result.push_back(xs1);
109  return result;
110  }
111 
112  // AABox_t & LineSegment_t intersection search. Make a use of AABox_t & HalfLine_t function
113  std::vector<Point_t> GeoAlgo::Intersection(const AABox_t& box,
114  const LineSegment_t& line) const
115  {
116  auto const& st = line.Start();
117  auto const& ed = line.End();
118  // Create a static HalfLine_t for this algorithm
119  static HalfLine_t hline(st,ed-st);
120  hline.Start(st[0],st[1],st[2]);
121  hline.Dir(ed[0]-st[0],ed[1]-st[1],ed[2]-st[2]);
122 
123  auto hline_result = Intersection(box,hline);
124 
125  if(!hline_result.size()) return hline_result;
126 
127  // For non-empty result, only keep ones that is within the line length
128  std::vector<Point_t> result;
129  auto length = st._SqDist_(ed);
130  for(auto const& pt : hline_result)
131  if(st._SqDist_(pt) < length) result.push_back(pt);
132  return result;
133  }
134 
135  // AABox_t & Trajectory_t intersection search. Make a use of AABox_t & HalfLine_t function
136  std::vector<Point_t> GeoAlgo::Intersection(const AABox_t& box,
137  const Trajectory_t& trj) const
138  {
139 
140  std::vector<Point_t> result;
141  if(trj.size() < 2) return result; // If only 1 point, return
142  // Check compat
143  trj.compat(box.Min());
144  // Call the onetime-only HalfLine constructor
145  static HalfLine_t hline(trj[0],trj[1]);
146  for(size_t i=0; i<trj.size()-1; ++i) {
147 
148  auto const& st = trj[i];
149  auto const& ed = trj[i+1];
150  hline.Start(st[0],st[1],st[2]);
151  hline.Dir(ed[0]-st[0],ed[1]-st[1],ed[2]-st[2]);
152 
153  auto hline_result = Intersection(box,hline);
154 
155  if(!hline_result.size()) continue;
156 
157  // Check if the length makes sense
158  auto length = st._SqDist_(ed);
159  for(auto const& pt : hline_result)
160  if(st._SqDist_(pt) < length) result.push_back(pt);
161 
162  }
163  return result;
164  }
165 
166  // LineSegment sub-segment of HalfLine inside an AABox w/o checks
168  {
169  // First find interection point of half-line and box
170  auto xs_v = Intersection(box, line);
171  if(xs_v.size()==2) return LineSegment_t(xs_v[0],xs_v[1]);
172 
173  // Build a new LineSegment
174  if(!(xs_v.size())) return LineSegment_t();
175 
176  // Only other possiblity is # = 1
177  return LineSegment_t(line.Start(),xs_v[0]);
178  }
179 
180  /// Get Trajectory inside box given some input trajectory -> now assumes trajectory cannot exit and re-enter box
181  Trajectory_t GeoAlgo::BoxOverlap(const AABox_t& box, const Trajectory_t& trj) const
182  {
183 
184  // if first & last points inside, then return full trajectory
185  if ( box.Contain(trj[0]) and box.Contain(trj.back()) )
186  return trj;
187 
188  return trj;
189  }
190 
191  // Ref. RTCD 5.1.8 p. 146
192  // Distance between two infinite lines
193  double GeoAlgo::_SqDist_(const Line_t& l1, const Line_t &l2, Point_t& L1, Point_t& L2) const
194  {
195 
196  // closest approach when segment connecting the two lines
197  // is perpendicular to both lines
198 
199  // L1 = P1 + s(Q1-P1)
200  // L1 = P2 + t(Q2-P2)
201  // L1(s) and L2(t) are the closest approach points
202  // d1 = Q1-P1
203  // d2 = Q2-P2
204  // v(s,t) = L1(s) - L2(t)
205  // require d1*v == 0 && d2*v == 0
206  Vector_t d1 = l1.Pt2()-l1.Pt1();
207  Vector_t d2 = l2.Pt2()-l2.Pt1();
208  Vector_t r = l1.Pt1()-l2.Pt1();
209 
210  double a = d1*d1;
211  double b = d1*d2;
212  double c = d1*r;
213  double e = d2*d2;
214  double f = d2*r;
215 
216  double d = a*e-b*b;
217 
218  // if d==0 the lines are parallel
219  // return Pt1 (doesn't matter) for line 1
220  // distance is distance between Pt1 of 1 line & second line
221  if (d == 0){
222  L1 = l1.Pt1();
223  L2 = _ClosestPt_(l1.Pt1(),l2);
224  return L1._SqDist_(L2);
225  }
226 
227  double s = (b*f-c*e)/d;
228  double t = (a*f-b*c)/d;
229 
230  // s & t represent the paramteric points on the lines
231  // for the closest approach point
232  // now find the Point_t object at those locations
233 
234  L1 = l1.Pt1() + ( l1.Pt2()-l1.Pt1() )*s;
235  L2 = l2.Pt1() + ( l2.Pt2()-l2.Pt1() )*t;
236 
237  // find distance between these points
238  double dist = L1._SqDist_(L2);
239 
240  return dist;
241  }
242 
243 
244  // Distance between two half-infinite lines
245  // use same function as for infinite lines
246  // but expect return points L1 and L2 to be
247  // "after" start point. Otherwise need
248  // to re-calculate using start point
249  // for one or both of the half-lines
250  double GeoAlgo::_SqDist_(const HalfLine_t& l1, const HalfLine_t &l2, Point_t& L1, Point_t& L2) const
251  {
252 
253  //Same as for _SqDist_ with infinite line but check whether s & t go out of bounds (i.e. negative)
254 
255  Vector_t d1 = l1.Dir();
256  Vector_t d2 = l2.Dir();
257  Vector_t r = l1.Start()-l2.Start();
258 
259  double a = d1*d1;
260  double b = d1*d2;
261  double c = d1*r;
262  double e = d2*d2;
263  double f = d2*r;
264 
265  double d = a*e-b*b;
266 
267  // Need to make sure d != 0
268  if ( d==0 ){
269  // lines are parallel
270  // check closest distance from one start point
271  // to other line. Order indifferent
272  // Choose l1 to have cloest point be Start point
273  L1 = l1.Start();
274  L2 = _ClosestPt_(L1,l2);
275  return L1._SqDist_(L2);
276  }
277 
278  double s = (b*f-c*e)/d;
279  double t = (a*f-b*c)/d;
280 
281  // if s or t < 0, out of bounds for half-line
282  if (s < 0) s = 0;
283  if (t < 0) t = 0;
284 
285  // s & t represent the paramteric points on the lines
286  // for the closest approach point
287  // now find the Point_t object at those locations
288 
289  L1 = l1.Start() + l1.Dir()*s;
290  L2 = l2.Start() + l2.Dir()*t;
291 
292  // find distance between these points
293  double dist = L1._SqDist_(L2);
294 
295  return dist;
296  }
297 
298 
299 
300  // Distance between two half-infinite lines
301  // use same function as for infinite lines
302  // but expect return points L1 and L2 to be
303  // "after" start point. Otherwise need
304  // to re-calculate using start point
305  // for one or both of the half-lines
306  double GeoAlgo::_SqDist_(const HalfLine_t& hline, const LineSegment_t &seg, Point_t& L1, Point_t& L2) const
307  {
308 
309  //Same as for _SqDist_ with infinite line but check whether s & t go out of bounds (i.e. negative)
310 
311  auto const d1 = hline.Dir();
312  auto const d2 = seg.End()-seg.Start();
313  auto const r = hline.Start()-seg.Start();
314 
315  double a = d1*d1;
316  double b = d1*d2;
317  double c = d1*r;
318  double e = d2*d2;
319  double f = d2*r;
320 
321  double d = a*e-b*b;
322 
323  // if parallel then d == 0
324  if (d == 0){
325  // distance is smallest quantity between:
326  // - distance from segment start to line
327  // - distance from segment end to line
328  double sDist = _SqDist_(seg.Start(),hline);
329  double eDist = _SqDist_(seg.End(),hline);
330  if ( sDist <= eDist ) {
331  // get closest point on line
332  L1 = _ClosestPt_(seg.Start(),hline);
333  L2 = seg.Start();
334  return sDist;
335  }
336  else{
337  L1 = _ClosestPt_(seg.End(),hline);
338  L2 = seg.End();
339  return eDist;
340  }
341  }// if parallel
342 
343  double s = (b*f-c*e)/d;
344 
345 
346  // now check if parameters are out of bounds
347  if ( s < 0 ){
348  s = 0; // closest point on half-line is start
349  // re-evaluate closest point on segment using line start point
350  L1 = hline.Start();
351  L2 = _ClosestPt_(L1,seg);
352  return L1._SqDist_(L2);
353  }
354 
355  // if closest point is not beyond half-line
356  // it could be due to an intersection between half-line
357  // and segment projection.
358  // check value of t
359  double t = (a*f-b*c)/d;
360  // if t > 0 && < 1 then the two lines intersect. We are good!
361  if ( (t < 1) and (t > 0) ){
362  L1 = hline.Start() + hline.Dir()*s;
363  L2 = seg.Start() + (seg.End()-seg.Start())*t;
364  return L1._SqDist_(L2);
365  }
366  // if out of bounds clamp
367  // then re-evaluate closest point on line
368  t = _Clamp_(t,0,1);
369  L2 = seg.Start() + (seg.End()-seg.Start())*t;
370  L1 = _ClosestPt_(L2,hline);
371  return L1._SqDist_(L2);
372  }
373 
374  // Ref. RTCD Ch 5.1 p. 130
375  double GeoAlgo::_SqDist_(const Point_t& pt, const Point_t& line_s, const Point_t& line_e) const
376  {
377  auto const ab = line_e - line_s;
378  auto const ac = pt - line_s;
379  auto const bc = pt - line_e;
380  auto e = ac * ab;
381  if( e <= 0. ) return ac.SqLength();
382  auto f = ab.SqLength();
383  if( e >= f ) return bc.SqLength();
384  return (ac.SqLength() - e * e / f);
385  }
386 
387  // Ref. RTCD Ch 5.1 p. 128-129
389  {
390 
391  auto const& ab = line.Dir();
392  // Project pt on line (ab), but deferring divide by ab * ab
393  auto t = ((pt - line.Start()) * ab);
394  // pt projects outside line, on the start side; clamp to start
395  if( t <= 0. ) return line.Start();
396  else {
397  auto denom = ab.SqLength();
398  // pt projects outside line, on the end side; clamp to end
399  if( t>= denom ) return line.End();
400  // pt projects inside the line. must deferred divide now
401  else return (line.Start() + ab * (t/denom));
402  }
403  }
404 
405  // Ref. RTCD Ch 5.1 p. 130
406  double GeoAlgo::_SqDist_(const Point_t& pt, const HalfLine_t& line) const
407  {
408  auto const& ab = line.Dir();
409  auto const ac = pt - line.Start();
410  auto const bc = ac - ab;
411 
412  auto e = ac * ab;
413  if( e <= 0. ) return (ac * ac);
414  auto f = ab.SqLength();
415  return (ac.SqLength() - e * e / f);
416  }
417 
418 
419  // Ref. RTCD Ch 5.1 p. 128-129
421  {
422  auto const& ab = line.Dir();
423  auto t = (pt - line.Start()) * ab;
424  if( t <= 0. ) return line.Start();
425  else {
426  auto denom = ab.Length();
427  return (line.Start() + ab * (t/denom));
428  }
429  }
430 
431  // Point & Infinite Line min Distance
432  double GeoAlgo::_SqDist_(const Line_t& line, const Point_t& pt) const
433  {
434  auto const ab = line.Pt2()-line.Pt1();
435  auto const ac = pt - line.Pt1();
436  auto const bc = ac - ab;
437 
438  auto e = ac * ab;
439  auto f = ab.SqLength();
440  return (ac.SqLength() - e * e / f);
441  }
442 
443  // Point & Infinite Line Closest Point
444  Point_t GeoAlgo::_ClosestPt_(const Line_t& line, const Point_t& pt) const
445  {
446  auto const& ab = line.Pt2()-line.Pt1();
447  auto t = (pt - line.Pt1()) * ab;
448  auto denom = ab.Length();
449  return (line.Pt1() + ab * (t/denom));
450  }
451 
452 
453  // Ref. RTCD Ch 5.1 p. 131-132 ... modified to consider distance to the box's wall
454  double GeoAlgo::_SqDist_(const Point_t& pt, const AABox_t& box) const
455  {
456  double dist = kINVALID_DOUBLE;
457 
458  // If a point is inside the box, simply compute the smallest perpendicular distance
459  if(box.Contain(pt)) {
460 
461  auto const& pt_min = box.Min();
462  auto const& pt_max = box.Max();
463  // (1) Compute the distance to the YZ wall
464  double dist_to_yz = pt[0] - pt_min[0];
465  if( dist_to_yz > (pt_max[0] - pt[0]) ) dist_to_yz = pt_max[0] - pt[0];
466 
467  // (2) Compute the distance to the XZ wall
468  double dist_to_zx = pt[1] - pt_min[1];
469  if( dist_to_zx > (pt_max[1] - pt[1]) ) dist_to_zx = pt_max[1] - pt[1];
470 
471  // (3) Compute the distance to the XY wall
472  double dist_to_xy = pt[2] - pt_min[2];
473  if( dist_to_xy > (pt_max[2] - pt[2]) ) dist_to_xy = pt_max[2] - pt[2];
474 
475  // (4) Compute the minimum of (3), (4), and (5)
476  dist = ( dist_to_yz < dist_to_zx ? dist_to_yz : dist_to_zx );
477  dist = ( dist < dist_to_xy ? dist : dist_to_xy );
478  dist *= dist;
479 
480  }
481 
482  else{
483  // This refers to Ref. RTCD 5.1.3.1
484  // re-set distance
485  dist = 0;
486  for(size_t i=0; i<pt.size(); ++i) {
487 
488  auto const& v_pt = pt[i];
489  auto const& v_max = box.Max()[i];
490  auto const& v_min = box.Min()[i];
491 
492  if(v_pt < v_min) dist += (v_min - v_pt) * (v_min - v_pt);
493  if(v_pt > v_max) dist += (v_pt - v_max) * (v_pt - v_max);
494  }
495  }
496  return dist;
497  }
498 
499  // Ref. RTCD Ch 5.1 p. 130-131 ... modified to consider a point on the surface
500  Point_t GeoAlgo::_ClosestPt_(const Point_t& pt, const AABox_t& box) const
501  {
502  // Result
503  auto res = pt;
504  // For each coordinate axis, if the point coordinate value is outside box,
505  // clamp it to the box, else keep it as is
506  for(size_t i=0; i<pt.size(); ++i) {
507  auto const& v_pt = pt[i];
508  auto const& v_min = box.Min()[i];
509  auto const& v_max = box.Max()[i];
510  res[i] = v_pt;
511  if( v_pt < v_min ) res[i] = v_min;
512  if( v_pt > v_max ) res[i] = v_max;
513  }
514  return res;
515  }
516 
517 
518 
519  // Distance between a Trajectory_t and a Point_t
520  // Loop over segments that make up the trajectory and keep track
521  // of shortest distance between any of them and the point
522  // The smallest such distance is the return
523  double GeoAlgo::SqDist(const Point_t& pt, const Trajectory_t& trj) const
524  {
525 
526  // Make sure trajectory object is properly defined
527  if (!trj.size())
528  throw GeoAlgoException("Trajectory object not properly set...");
529 
530  // Check dimensionality compatibility between point and trajectory
531  trj.compat(pt);
532 
533  // Now keep track of smallest distance and loop over traj segments
534  double distMin = kINVALID_DOUBLE;
535  for (size_t l=0; l < trj.size()-1; l++){
536  double distTmp = _SqDist_(pt,trj[l],trj[l+1]);
537  if (distTmp < distMin) { distMin = distTmp; }
538  }
539 
540  return distMin;
541  }
542 
543 
544 
545  // Distance between vector of Trajectories and a Point
546  // Loop over Trajectories and find the closest one
547  // then keep track of that closest one
548  double GeoAlgo::SqDist(const Point_t& pt, const std::vector<Trajectory_t> &trj, int &trackIdx) const
549  {
550 
551  // holder for shortest distance
552  double minDist = kINVALID_DOUBLE;
553 
554  // loop over trajectories
555  for (size_t t=0; t < trj.size(); t++){
556 
557  // trajectory & point dimensionality will be checked in next function
558  // now calculate distance w.r.t. this track
559  double tmpDist = SqDist(pt, trj[t]);
560  if (tmpDist < minDist){
561  minDist = tmpDist;
562  trackIdx = t;
563  }
564  }// for all tracks
565 
566  return minDist;
567  }
568 
569 
570 
571  // Closest point between a Trajectory and a Point
572  // Loop over segments that make up the trajectory and keep track
573  // of shortest distance between any of them and the point
574  // For the shortest distance find the point at which it is found
575  Point_t GeoAlgo::ClosestPt(const Point_t& pt, const Trajectory_t& trj, int &idx) const
576  {
577 
578  // Make sure trajectory object is properly defined
579  if (!trj.size())
580  throw GeoAlgoException("Trajectory object not properly set...");
581 
582  // Check dimensionality compatibility between point and trajectory
583  trj.compat(pt);
584 
585  // Now keep track of smallest distance and loop over traj segments
586  double distMin = kINVALID_DOUBLE;
587  // For that smallest distance, keep track of the segment for which it was found
588  for (size_t l=0; l < trj.size()-1; l++){
589  double distTmp = _SqDist_(pt,trj[l],trj[l+1]);
590  if (distTmp < distMin) { distMin = distTmp; idx = l; }
591  }
592 
593  // Now that we have the segment for the closest approach
594  // Use it to find the closest point on that segment
595  LineSegment_t segMin(trj[idx], trj[idx+1]);
596  return _ClosestPt_(pt,segMin);
597  }
598 
599 
600  // Closest point between a vector of trajectories and a point
601  // Loop over segments that make up the trajectory and keep track
602  // of shortest distance between any of them and the point
603  // For the shortest distance find the point at which it is found
604  Point_t GeoAlgo::ClosestPt(const Point_t& pt, const std::vector<Trajectory_t> &trj, int& TrackIdx) const
605  {
606 
607  // since finding the smallest distance is faster than finding the closest point
608  // first loop through tracks, and find the one that is closest to the point
609  // then finally find the closest point on that track
610 
611  for (size_t t=0; t < trj.size(); t++){
612 
613  // holder for smallest distance
614  double minDist = kINVALID_DOUBLE;
615 
616  // track & point dimensionality will be checked per-track by next function
617  // now calculate distance w.r.t. this track
618  double tmpDist = SqDist(pt, trj[t]);
619  if (tmpDist < minDist){
620  minDist = tmpDist;
621  TrackIdx = t;
622  }
623 
624  }// for all tracks
625 
626  // now we know which track is closest -> find the closest point to that track
627  return ClosestPt(pt, trj[TrackIdx]);
628  }
629 
630 
631  // Closest Approach between a segment and a Trajectory
632  // loop over segments in trajectory and find the one that
633  // is closest. Then find distance
634  double GeoAlgo::SqDist(const LineSegment_t& seg, const Trajectory_t& trj, Point_t& c1, Point_t& c2) const
635  {
636 
637  // Make sure trajectory object is properly defined
638  if (!trj.size())
639  throw GeoAlgoException("Trajectory object not properly set...");
640 
641  // Check dimensionality compatibility between point and trajectory
642  trj.compat(seg.Start());
643 
644  // keep track of c1 & c2
645  Point_t c1min;
646  Point_t c2min;
647  // Now keep track of smallest distance and loop over traj segments
648  double distMin = kMAX_DOUBLE;
649 
650  for (size_t l=0; l < trj.size()-1; l++){
651  LineSegment_t segTmp(trj[l], trj[l+1]);
652  double distTmp = _SqDist_(segTmp, seg, c1min, c2min);
653  if ( distTmp < distMin ){
654  c1 = c1min;
655  c2 = c2min;
656  distMin = distTmp;
657  }
658  }//for all segments in the track
659 
660  return distMin;
661  }
662 
663 
664  // Closest Approach between a Trajectory and a Trajectory
665  // loop over segments in trajectory1 and those in
666  // trahectory2 and find the best point
667  double GeoAlgo::SqDist(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& c1, Point_t& c2) const
668  {
669 
670  // Make sure trajectory object is properly defined
671  if ( !trj1.size() or !trj2.size() )
672  throw GeoAlgoException("Trajectory object not properly set...");
673 
674  // Check dimensionality compatibility between point and trajectory
675  trj1.compat(trj2[0]);
676 
677  // keep track of c1 & c2
678  Point_t c1min;
679  Point_t c2min;
680  // Now keep track of smallest distance and loop over traj segments
681  double distMin = kMAX_DOUBLE;
682 
683  for (size_t l1=0; l1 < trj1.size()-1; l1++){
684  for (size_t l2=0; l2 < trj2.size()-1; l2++){
685  LineSegment_t segTmp1(trj1[l1], trj1[l1+1]);
686  LineSegment_t segTmp2(trj2[l2], trj2[l2+1]);
687  double distTmp = _SqDist_(segTmp1, segTmp2, c1min, c2min);
688  if ( distTmp < distMin ){
689  c1 = c1min;
690  c2 = c2min;
691  distMin = distTmp;
692  }
693  }// for segments in trajectory 2
694  }//for all segments in trajectory 1
695 
696  return distMin;
697  }
698 
699  // Closest Approach between a HalfLine and a Trajectory
700  // loop over segments in trajectory and find the one that
701  // is closest. Then find distance
702  double GeoAlgo::SqDist(const HalfLine_t& hline, const Trajectory_t& trj, Point_t& c1, Point_t& c2) const
703  {
704 
705  // Make sure trajectory object is properly defined
706  if (!trj.size())
707  throw GeoAlgoException("Trajectory object not properly set...");
708 
709  // Check dimensionality compatibility between point and trajectory
710  trj.compat(hline.Start());
711 
712  // keep track of c1 & c2
713  Point_t c1min;
714  Point_t c2min;
715  // Now keep track of smallest distance and loop over traj segments
716  double distMin = kMAX_DOUBLE;
717 
718  for (size_t l=0; l < trj.size()-1; l++){
719  LineSegment_t segTmp(trj[l], trj[l+1]);
720  double distTmp = _SqDist_(hline, segTmp, c1min, c2min);
721  if ( distTmp < distMin ){
722  c1 = c1min;
723  c2 = c2min;
724  distMin = distTmp;
725  }
726  }//for all segments in the track
727 
728  return distMin;
729  }
730 
731 
732  // Closest Approach between a Segment and a vector of tracks
733  // keep track of points of closest approach both on nearest
734  // track as well as on segment
735  // keep track of which track has the point of closest approcah
736  double GeoAlgo::SqDist(const LineSegment& seg, const std::vector<Trajectory_t> &trj, Point_t& c1, Point_t& c2, int& trackIdx) const
737  {
738 
739  // holders to keep track of track with shortest distance
740  double minDist = kMAX_DOUBLE;
741  // holders for points of closest approach
742  Point_t c1min;
743  Point_t c2min;
744 
745  for (size_t t=0; t < trj.size(); t++){
746 
747  //need to loop over all tracks and find the one which is closest
748 
749  // dimensionality checks will be done in next function, per track.
750 
751  // now calculate closest approach
752  double tmpDist = SqDist(seg, trj[t], c1min, c2min);
753 
754  // is this the best yet?
755  if (tmpDist < minDist){
756  minDist = tmpDist;
757  c1 = c1min;
758  c2 = c2min;
759  trackIdx = t;
760  }
761 
762  }// for all tracks in vector
763 
764  return minDist;
765  }
766 
767  // Ref. RTCD Sec. 5.1.9 - pg. 148-150
768  double GeoAlgo::_SqDist_(const LineSegment_t& seg1, const LineSegment_t& seg2,
769  Point_t& c1, Point_t& c2) const
770  {
771 
772  double t1, t2;
773 
774  auto const& s1 = seg1.Start();
775  auto const& s2 = seg2.Start();
776  auto const& e1 = seg1.End();
777  auto const& e2 = seg2.End();
778 
779  auto d1 = e1 - s1;
780  auto d2 = e2 - s2;
781  auto r = s1 - s2;
782 
783  double a = d1.SqLength();
784  double e = d2.SqLength();
785  double f = d2 * r;
786 
787  // check if segment is too short
788  if ( (a <= 0) and (e <= 0) ){
789  //both segments are too short
790  t1 = t2 = 0.;
791  c1 = s1;
792  c2 = s2;
793  Vector_t distVector = c2 - c1;
794  return distVector.SqLength();
795  }
796  if (a <= 0){
797  //first segment degenerates into a point
798  t1 = 0.;
799  t2 = f/e;
800  t2 = _Clamp_(t2,0.,1.);
801  }
802  else{
803  double c = d1 * r;
804  if (e <= 0){
805  //second segment degenerates into a point
806  t2 = 0.;
807  t1 = _Clamp_(-c/a,0.,1.);
808  }
809  else{
810  // the general case...no degeneracies
811  double b = d1 * d2;
812  double denom = (a*e)-(b*b);
813 
814  if (denom != 0.)
815  t1 = _Clamp_((b*f-c*e)/denom, 0., 1.);
816  else
817  t1 = 0.;
818 
819  t2 = (b*t1+f)/e;
820 
821  if (t2 < 0.){
822  t2 = 0.;
823  t1 = _Clamp_(-c/a, 0., 1.);
824  }
825  else if (t2 > 1.){
826  t2 = 1.;
827  t1 = _Clamp_((b-c)/a, 0., 1.);
828  }
829 
830  }
831  }
832 
833  c1 = s1 + d1 * t1;
834  c2 = s2 + d2 * t2;
835 
836  Vector_t distVector = c2 - c1;
837  return distVector.SqLength();
838 
839  }
840 
841 
842  // Clamp function:
843  // if 1st argument out of bounds w.r.t. min & max
844  // return the boundary point
845  double GeoAlgo::_Clamp_(const double n, const double min, const double max) const
846  {
847  if (n < min) { return min; }
848  if (n > max) { return max; }
849  return n;
850  }
851 
852 
853  /// Common origin: Half Line & Half Line. Keep track of origin
854  double GeoAlgo::_commonOrigin_(const Line_t& lin1, const Line_t& lin2, Point_t& origin) const
855  {
856 
857  // Function Description:
858  // Given two HalfLine objects, project them backwards
859  // and find the point of closest approach on both
860  // lines.
861  // The half-point between these two is considered the
862  // candidate origin or vertex of the two lines
863  // Then make segments uniting this vertex and
864  // the start point of both half lines.
865  // Take the dot product of the segment uniting
866  // the vertex with the start of lin1, and the direction
867  // of lin1. Similarly for lin2.
868  // These dot-products will be close to 1 if the lines,
869  // traced backwards, indeed point to the reconstructed
870  // vertex.
871  // return the sum of these dot products, which
872  // is bound between -2 and +2.
873  // other values of this return (1, 0, -1, -2)
874  // will give insight on other possible topologies.
875 
876  // get directions of two lines
877  Vector_t dir1(lin1.Pt2()-lin1.Pt1());
878  Vector_t dir2(lin2.Pt2()-lin2.Pt1());
879  dir1.Normalize();
880  dir2.Normalize();
881 
882  // Closest approach points on the two lines
883  Point_t pt1(lin1.Pt1().size());
884  Point_t pt2(lin2.Pt1().size());
885 
886  //double IP = _SqDist_(lin1, lin2, pt1, pt2);
887  origin = (pt1+pt2)/2.;
888 
889  // If origin coincides with lin1 start
890  // -> vec1 should be in same direction of lin1
891  Vector_t vec1(dir1);
892  if (lin1.Pt1() != origin)
893  vec1 = lin1.Pt1()-origin;
894  vec1.Normalize();
895  // similarly for vec1
896  Vector_t vec2(dir2);
897  if (lin2.Pt1() != origin)
898  vec2 = lin2.Pt1()-origin;
899  vec2.Normalize();
900 
901  return vec1.Dot(dir1) + vec2.Dot(dir2);
902  }
903 
904 
905  /// Common origin: Half Line & Half Line. Keep track of origin
906  double GeoAlgo::_commonOrigin_(const HalfLine_t& lin1, const HalfLine_t& lin2, Point_t& origin, bool backwards) const
907  {
908 
909  //If backwards is false, call infinite line function, otherwise proceed
910  if (!backwards){
911  Line_t l1(lin1.Start(),lin1.Start()+lin1.Dir());
912  Line_t l2(lin2.Start(),lin2.Start()+lin2.Dir());
913  return _commonOrigin_(l1,l2,origin);
914  }
915 
916  // Function Description:
917  // Given two HalfLine objects, project them backwards
918  // and find the point of closest approach on both
919  // lines.
920  // The half-point between these two is considered the
921  // candidate origin or vertex of the two lines
922  // Then make segments uniting this vertex and
923  // the start point of both half lines.
924  // Take the dot product of the segment uniting
925  // the vertex with the start of lin1, and the direction
926  // of lin1. Similarly for lin2.
927  // These dot-products will be close to 1 if the lines,
928  // traced backwards, indeed point to the reconstructed
929  // vertex.
930  // return the sum of these dot products, which
931  // is bound between -2 and +2.
932  // other values of this return (1, 0, -1, -2)
933  // will give insight on other possible topologies.
934 
935  // Flip the HalfLines: want to project backwards
936  HalfLine_t lin1Back(lin1.Start(), lin1.Dir()*(-1));
937  HalfLine_t lin2Back(lin2.Start(), lin2.Dir()*(-1));
938  // Closest approach points on the two lines
939  Point_t pt1(lin1.Start().size());
940  Point_t pt2(lin2.Start().size());
941 
942  // double IP = _SqDist_(lin1Back, lin2Back, pt1, pt2); Unused variable
943  origin = (pt1+pt2)/2.;
944 
945  // If origin coincides with lin1 start
946  // -> vec1 should be in same direction of lin1
947  Vector_t vec1(lin1.Dir());
948  if (lin1.Start() != origin)
949  vec1 = lin1.Start()-origin;
950  vec1.Normalize();
951  // similarly for vec1
952  Vector_t vec2(lin2.Dir());
953  if (lin2.Start() != origin)
954  vec2 = lin2.Start()-origin;
955  vec2.Normalize();
956 
957  return vec1.Dot(lin1.Dir()) + vec2.Dot(lin2.Dir());
958  // std::cout << "dot is: " << dot << std::endl;
959  // if ( !((dot <= 2) && (dot >= -2)) )
960  // throw GeoAlgoException("commonOrigin failed. Sum of two dot-products must be bound by [-2,2]");
961 
962  // return dot;
963  }
964  /// Common origin: Line Segment & Half Line. Keep track of origin
965  double GeoAlgo::_commonOrigin_(const HalfLine_t& lin, const LineSegment_t& seg, Point_t& origin, bool backwards) const
966  {
967  // Make a Half-line out of the line-segment
968  // we want to project backwards to a common origin
969  // not limit ourselves to an origin that must be on the segment
970  HalfLine_t lin2(seg.Start(), seg.Dir());
971  return _commonOrigin_(lin, lin2, origin, backwards);
972  }
973  /// Common origin: Line Segment & Line Segment. Keep track of origin
974  double GeoAlgo::_commonOrigin_(const LineSegment_t& seg1, const LineSegment_t& seg2, Point_t& origin, bool backwards) const
975  {
976  // Make a Half-line out of the line-segments
977  // we want to project backwards to a common origin
978  // not limit ourselves to an origin that must be on the segment
979  HalfLine_t lin1(seg1.Start(), seg1.Dir());
980  HalfLine_t lin2(seg2.Start(), seg2.Dir());
981  return _commonOrigin_(lin1, lin2, origin, backwards);
982  }
983 
984  /// Common origin: Trajectory & Trajectory/ Keep track of origin
985  double GeoAlgo::_commonOrigin_(const Trajectory_t& trj1, const Trajectory_t& trj2, Point_t& origin, bool backwards) const
986  {
987  // Turn the trajectory into half-line that connect start -> end
988  HalfLine_t lin1(trj1.front(),trj1.back()-trj1.front());
989  // Turn the segment into half-line
990  HalfLine_t lin2(trj2.front(),trj2.back()-trj2.front());
991  return _commonOrigin_(lin1, lin2, origin, backwards);
992  }
993 
994  /// Common origin: Trajectory & Line Segment. Keep track of origin
995  double GeoAlgo::_commonOrigin_(const Trajectory_t& trj, const LineSegment_t& seg, Point_t& origin, bool backwards) const
996  {
997  // Turn the trajectory into half-line that connect start -> end
998  HalfLine_t lin1(trj.front(),trj.back()-trj.front());
999  // Turn the segment into half-line
1000  HalfLine_t lin2(seg.Start(), seg.Dir());
1001  return _commonOrigin_(lin1, lin2, origin, backwards);
1002  }
1003 
1004  /// Common origin: Trajectory & Half Line. Keep track of origin
1005  double GeoAlgo::_commonOrigin_(const Trajectory_t& trj, const HalfLine_t& lin, Point_t& origin, bool backwards) const
1006  {
1007  // Turn the trajectory into half-line that connect start -> end
1008  HalfLine_t lin2(trj.front(),trj.back()-trj.front());
1009  return _commonOrigin_(lin, lin2, origin, backwards);
1010  }
1011 
1012 
1013  /// Bounding Sphere problem
1014  /// Real-Time Collision Analysis 4.3.5 (Pg. 100) - WelzlSphere
1015  Sphere_t GeoAlgo::_boundingSphere_(const std::vector<Point_t>& pts) const
1016  {
1017 
1018  // Remove any duplicate points
1019  std::vector<Point_t> copyPts = {pts[0]};
1020  for(size_t p1=0; p1 < pts.size(); p1++){
1021  // if an identical point does not already exist in copyPts -> then add
1022  bool found = false;
1023  for (size_t p2 =0; p2 < copyPts.size(); p2++)
1024  if (pts[p1] == copyPts[p2]) { found = true; break; }
1025  if (!found)
1026  copyPts.push_back(pts[p1]);
1027  }
1028 
1029  // if 4 or less points call appropriate constructor
1030  if (copyPts.size() < 5)
1031  return Sphere_t(copyPts);
1032 
1033  size_t npoints = copyPts.size();
1034  std::vector<Point_t> points4 = {copyPts[npoints-1],copyPts[npoints-2],copyPts[npoints-3],copyPts[npoints-4]};
1035  copyPts.pop_back();
1036  copyPts.pop_back();
1037  copyPts.pop_back();
1038  copyPts.pop_back();
1039  Sphere_t tmpSphere = Sphere(points4);
1040  return _RemainingPoints_(copyPts,tmpSphere);
1041 
1042  // too many points to call simple constructor! find minimally bounding sphere
1043  // compute sphere for first 4 points
1044  //Sphere_t tmpSphere(copyPts[0],copyPts[1],copyPts[2],copyPts[3]);
1045  //std::vector<Point_t> sosPoints;// = {pts[0]};
1046  //sosPoints.clear();
1047  //return _WelzlSphere_(copyPts,copyPts.size(),sosPoints);
1048  }
1049 
1050  Sphere_t GeoAlgo::_RemainingPoints_(std::vector<Point_t>& remaining,
1051  const Sphere_t& thisSphere) const
1052  {
1053 
1054 
1055  // if no points lef -> done...return the current sphere
1056  if (remaining.size() == 0)
1057  return thisSphere;
1058 
1059  //std::cout << "Remaining points: " << remaining.size() << std::endl;
1060 
1061  auto const& lastPoint = remaining.back();
1062 
1063  // if this point is bounded by the already constructed sphere, continue
1064  if ( thisSphere.Contain(lastPoint) ){
1065  remaining.pop_back();
1066  return _RemainingPoints_(remaining,thisSphere);
1067  }
1068  // if not, need to adjust so that the new point is also bound
1069  // get distance from lastPoint and center
1070  double dist = lastPoint.Dist(thisSphere.Center());
1071  //std::cout << "point does not fit: " << lastPoint << std::endl;
1072  //std::cout << "distance: " << dist << std::endl;
1073  //std::cout << "center : " << thisSphere.Center() << std::endl;
1074  //std::cout << "radius : " << thisSphere.Radius() << std::endl;
1075  // the new center should be shifted in the direction
1076  // of the new point by half the difference between
1077  // the current radius and "dist"
1078  // direction in which to move:
1079  Vector_t dir = lastPoint-thisSphere.Center();
1080  dir.Normalize();
1081  // amount to move by
1082  double shift = (dist-thisSphere.Radius())/2.;
1083  if (shift < 0) { shift *= -1; }
1084  Point_t newCenter = thisSphere.Center() + dir*shift;
1085  double newRadius = thisSphere.Radius()+shift;
1086  //std::cout << "new center: " << newCenter << std::endl;
1087  //std::cout << "new radius: " << newRadius << std::endl;
1088  Sphere_t newsphere(newCenter,newRadius);
1089  //if (newsphere.Contain(lastPoint)) { std::cout << "new point contained!" << std::endl; }
1090  //else { std::cout << "WRONG!" << std::endl; }
1091  remaining.pop_back();
1092 
1093  return _RemainingPoints_(remaining,newsphere);
1094  }
1095 
1096  Sphere_t GeoAlgo::_WelzlSphere_(const std::vector<Point_t>& pts,
1097  int numPts,
1098  std::vector<Point_t> sosPts) const
1099  {
1100 
1101  if (numPts == 0)
1102  return Sphere_t(sosPts);
1103  // choose last point in the input set as the one to test (if it fits in current sphere or not)
1104  int index = numPts-1;
1105  // recursively compute the smallest bounding sphere of the remaining points
1106  Sphere_t smallestSphere = _WelzlSphere_(pts, numPts-1, sosPts);
1107  // if the selected point lies inside this sphere, it is indeed the smallest
1108  if ( smallestSphere.Contain(pts[index]) )
1109  return smallestSphere;
1110  // otherwise, update the set-of-support to additionally contain the new point
1111  sosPts.push_back(pts[index]);
1112  return _WelzlSphere_(pts, numPts-1, sosPts);
1113  }
1114 
1115 }
bool Contain(const Point_t &p) const
Judge if a point is contained within a sphere.
Definition: GeoSphere.cxx:357
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
const Point_t & Start() const
Start getter.
Definition: GeoHalfLine.cxx:25
static QCString result
double _Clamp_(const double n, const double min, const double max) const
Clamp function: checks if value out of bounds.
Definition: GeoAlgo.cxx:845
void compat(const Point_t &obj) const
Dimensionality check function w/ Trajectory.
double SqDist(const Line_t &line, const Point_t &pt) const
Definition: GeoAlgo.h:87
Class def header for a class GeoAlgoException.
Representation of a simple 3D line segment Defines a finite 3D straight line by having the start and ...
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 _SqDist_(const Vector &obj) const
Compute the squared-distance to another vector w/o dimension check.
Definition: GeoVector.cxx:108
string dir
static QStrList * l
Definition: config.cpp:1044
const Point_t & Pt2() const
Direction getter.
Definition: GeoLine.cxx:23
const Vector_t Dir() const
Direction getter.
const Point_t & Min() const
Minimum point getter.
Definition: GeoAABox.cxx:25
void Normalize()
Normalize itself.
Definition: GeoVector.cxx:89
double SqLength() const
Compute the squared length of the vector.
Definition: GeoVector.cxx:34
const double e
double Length() const
Compute the length of the vector.
Definition: GeoVector.cxx:40
LineSegment LineSegment_t
double Dot(const Vector &obj) const
Definition: GeoVector.cxx:51
bool Contain(const Point_t &pt) const
Test if a point is contained within the box.
Definition: GeoAABox.cxx:33
static const double kMAX_DOUBLE
void swap(Handle< T > &a, Handle< T > &b)
bool IsValid() const
Check if point is valid.
Definition: GeoVector.cxx:22
std::void_t< T > n
const double a
const Point_t & Pt1() const
Start getter.
Definition: GeoLine.cxx:22
const Point_t & End() const
End getter.
Representation of a 3D infinite line. Defines an infinite 3D line by having 2 points which completely...
Definition: GeoLine.h:27
double Radius() const
Radius getter.
Definition: GeoSphere.cxx:346
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)
const Vector_t & Dir() const
Direction getter.
Definition: GeoHalfLine.cxx:27
const Point_t & Center() const
Center getter.
Definition: GeoSphere.cxx:344
const Point_t & Start() const
Start getter.
Sphere_t _boundingSphere_(const std::vector< Point_t > &pts) const
Definition: GeoAlgo.cxx:1015
T min(sqlite3 *const db, std::string const &table_name, std::string const &column_name)
Definition: statistics.h:55
Representation of a 3D semi-infinite line. Defines a semi-infinite 3D line by having a start point (P...
Definition: GeoHalfLine.h:30
constexpr double dist(const TReal *x, const TReal *y, const unsigned int dimension)
Sphere_t _RemainingPoints_(std::vector< Point_t > &remaining, const Sphere_t &thisSphere) const
Definition: GeoAlgo.cxx:1050
Class def header for a class GeoAlgo.
void line(double t, double *p, double &x, double &y, double &z)
const Point_t & Max() const
Maximum point getter.
Definition: GeoAABox.cxx:26
Vector Vector_t
Point has same feature as Vector.
Definition: GeoVector.h:195
static bool * b
Definition: config.cpp:1043
LineSegment_t BoxOverlap(const AABox_t &box, const HalfLine_t &line) const
LineSegment sub-segment of HalfLine inside an AABox.
Definition: GeoAlgo.cxx:167
static const double kINVALID_DOUBLE
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
static QCString * s
Definition: config.cpp:1042
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
Sphere Sphere_t
Definition: GeoSphere.h:121
constexpr Point origin()
Returns a origin position with a point of the specified type.
Definition: geo_vectors.h:227
Point_t _ClosestPt_(const Point_t &pt, const LineSegment_t &line) const
Definition: GeoAlgo.cxx:388