Fréchet View  1.6.0
A Tool for Exploring Fréchet Distance Algorithms
fs_path.cpp
Go to the documentation of this file.
1 
2 #include <poly_path.h>
3 #include <fs_path.h>
4 #include <QtGlobal>
5 #include <grid.h>
6 #include <iostream>
7 
8 using namespace frechet;
9 using namespace reach;
10 
11 
12 const double FSPath::PRECISION = 1e-9;
13 
14 
15 inline bool is_int(double x) {
16  return x==((int)x);
17 }
18 
20 : LR(0,0), BR(0,0),
21  fs(), fix(),
22  wrapTop(false), wrapRight(false),
23  path{Curve(),Curve()}
24 { }
25 
27 : LR(afs->n,afs->m),
28  BR(afs->n,afs->m),
29  fs(afs),
30  fix(),
31  wrapTop(afs->wrapTop()),
32  wrapRight(afs->wrapRight()),
33  path{Curve(),Curve()}
34 { }
35 
36 Curve FSPath::getPath(int i) const {
37  Q_ASSERT(i>=0);
38  if (i<2)
39  return path[i];
40  else
41  return Curve();
42 }
43 
44 double FSPath::mapToSegment(double a, Point p1, Point p2) {
45  double x = (a-p1.x()) / (p2.x()-p1.x());
46  return (1-x)*p1.y() + x*p2.y();
47 }
48 
49 void FSPath::append(std::vector<double>& map, double x)
50 {
51  if (map.empty() || map.back() != x)
52  map.push_back(x);
53 }
54 
55 void FSPath::mapFromP(Segment pseg, Curve result[2]) const
56 {
57  result[0].clear();
58  result[1].clear();
59 
60  int i,j;
61  i = binSearchX(path[0],pseg.first);
62  Q_ASSERT(i>=0 || i==(-path[0].size()-1));
63 
64  if (i < 0) {
65  // both in second part
66  i = binSearchX(path[1],pseg.first);
67  Q_ASSERT(i >= 0);
68  j = binSearchX(path[1],pseg.second);
69  Q_ASSERT(j >= 0);
70  copy(result[1], path[1], i,j+1);
71  }
72  else {
73  j = binSearchX(path[0],pseg.second);
74  Q_ASSERT(j>=0 || j==(-path[0].size()-1));
75  if (j >= 0) {
76  // both in first part
77  copy(result[0],path[0], i,j+1);
78  }
79  else {
80  // first part + second part
81  j = binSearchX(path[1],pseg.second);
82  Q_ASSERT(j >= 0);
83 
84  copy(result[0],path[0],i,path[0].size());
85  copy(result[1],path[1],0,j+1);
86  }
87  }
88 }
89 
90 void FSPath::mapFromQ(Segment pseg, Curve result[2]) const
91 {
92  result[0].clear();
93  result[1].clear();
94 
95  int i,j;
96  i = binSearchY(path[1],pseg.first);
97  Q_ASSERT(i>=0 || i==(-path[1].size()-1));
98 
99  if (i < 0) {
100  // both in first part
101  i = binSearchY(path[0],pseg.first);
102  Q_ASSERT(i >= 0);
103  j = binSearchY(path[0],pseg.second);
104  Q_ASSERT(j >= 0);
105  copy(result[0],path[0], i,j+1);
106  }
107  else {
108  j = binSearchY(path[1],pseg.second);
109  Q_ASSERT(j>=0 || j==(-path[1].size()-1));
110  if (j >= 0) {
111  // both in second part
112  copy(result[1],path[1], i,j+1);
113  }
114  else {
115  // second part + first part
116  j = binSearchY(path[0],pseg.second);
117  Q_ASSERT(j >= 0);
118 
119  copy(result[0],path[1], i,path[1].size());
120  copy(result[1],path[0], 0,j+1);
121  }
122  }
123 }
124 
125 void FSPath::mapToP(Curve path[2]) const
126 {
127  Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
128  || fmod(path[0].last().x(),fs->n-1) == fmod(path[1].first().x(),fs->n-1));
129 
130  const Curve& P = fs->P;
131  for(int i=0; i < 2; ++i)
132  for(int j=0; j < path[i].size(); ++j)
133  path[i][j] = frechet::Grid::mapToPoint(P,path[i][j].x());
134 
135  Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
136  || path[0].last() == path[1].first());
137 
138  path[0] += path[1];
139  path[1].clear();
140 }
141 
142 void FSPath::mapToQ(Curve path[2]) const {
143  Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
144  || fmod(path[0].last().y(),fs->m-1)==fmod(path[1].first().y(),fs->m-1));
145 
146  const Curve& Q = fs->Q;
147  for(int i=0; i < 2; ++i)
148  for(int j=0; j < path[i].size(); ++j)
149  path[i][j] = frechet::Grid::mapToPoint(Q,path[i][j].y());
150 
151  Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
152  || path[0].last()==path[1].first());
153 
154  path[0] += path[1];
155  path[1].clear();
156 }
157 
158 int FSPath::binSearchX(const Curve& curve, int x) {
159  int lo=0;
160  int hi=curve.size();
161  while(lo < hi) {
162  int mid = (lo+hi)/2;
163  double mx = curve[mid].x();
164  if (x==mx)
165  return mid;
166  if (x > mx)
167  lo = mid+1;
168  else
169  hi = mid;
170  }
171  return -lo-1;
172 }
173 
174 int FSPath::binSearchY(const Curve& curve, int y) {
175  int lo=0;
176  int hi=curve.size();
177  while(lo < hi) {
178  int mid = (lo+hi)/2;
179  double my = curve[mid].y();
180  if (y==my)
181  return mid;
182  if (y > my)
183  lo = mid+1;
184  else
185  hi = mid;
186  }
187  return -lo-1;
188 }
189 
190 void FSPath::copy(Curve& dest, const Curve& source, int i, int j)
191 {
192  while(i < j && i < source.size())
193  dest.push_back(source[i++]);
194 }
195 
196 
197 
198 int FSPath::next_hor(int i) {
199  if (++i==BR.n-1)
200  return wrapRight ? 0 : -1;
201  else
202  return i;
203 }
204 
205 int FSPath::next_vert(int j) {
206  if (++j==LR.m-1)
207  return wrapTop ? 0 : -1;
208  else
209  return j;
210 }
211 
212 
214  for(int i=0; i < LR.n; ++i)
215  for(int j=0; j < LR.m; ++j)
216  {
217  LR.at(i,j).clear();
218  BR.at(i,j).clear();
219  }
220 }
221 
224  fix.clear();
225  path[0].clear();
226  path[1].clear();
227 }
228 
229 bool FSPath::propagateHorizontalEdge(Point a, double bx, double prec)
230 {
231  int j = floor(a.y());
232  if (a.y() > j)
233  return false;
234  // Startpunkt liegt nicht auf einer horizontalen Kante
235 
236  // start at start.x()
237  int i0 = floor(a.x());
238  double x = a.x()-i0;
239 
240  const Interval* F = &fs->cell(i0,j).B;
241  if (!F->contains(x,prec))
242  return false;
243 
244  BR.at(i0,j) = {x,F->upper()};
245  if (!std::isnan(bx) && (i0+F->upper() > bx+prec))
246  LR.at(i0,j).upper() = bx-i0+prec;
247 
248  if (F->upper() < 1.0)
249  return true;
250 
251  int i = i0+1;
252  int i1 = (int)floor(bx); // % (BR.n-1);
253  if (i > i1) return true;
254 
255  for( ; i>=0 && i < i1; ++i)
256  {
257 // if (i==i1) {
258  // ? BR.at(i,j).upper() = (bx-i1);
259 // break;
260 // }
261  int imod = i % (BR.n-1);
262  F = &fs->cell(imod,j).B;
263  if (F->lower() > 0.0) break;
264 
265  BR.at(imod,j) = *F;
266  if (!std::isnan(bx) && (i0+F->upper() > bx+prec))
267  BR.at(imod,j).upper() = bx-i0+prec;
268 
269  if (F->upper() < 1.0) break;
270  }
271 
272  return true;
273 }
274 
275 bool FSPath::propagateVerticalEdge(Point a, double by, double prec)
276 {
277  int i = (int)a.x();
278  if (a.x() > i)
279  return false;
280  // Startpunkt liegt nicht auf einer vertikalen Kante
281 
282  // start at start.y()
283  int j0 = (int)a.y();
284  double y = a.y()-j0;
285 
286  const Interval* F = &fs->cell(i,j0).L;
287  if (!F->contains(y,prec))
288  return false; // Start liegt nicht im Freespace
289 
290  LR.at(i,j0) = {y,F->upper()};
291  if (!std::isnan(by) && (j0+F->upper() > by+prec))
292  LR.at(i,j0).upper() = by-j0+prec;
293 
294  if (F->upper() < 1.0)
295  return true;
296 
297  int j = j0+1;
298  int j1 = (int)floor(by);// % (LR.m-1);
299  if (j > j1) return true;
300 
301  for( ; j >= 0 && j < j1; ++j)
302  {
303 // if (j==j1) {
304 // // ? LR.at(i,j).upper() = (by-j);
305 // break;
306 // }
307  int jmod = j % (LR.m-1);
308  F = &fs->cell(i,jmod).L;
309  if (F->lower() > 0.0) break;
310 
311  LR.at(i,jmod) = *F;
312  if (!std::isnan(by) && (jmod+F->upper() > by+prec))
313  LR.at(i,j0).upper() = by-jmod+prec;
314 
315  if (F->upper() < 1.0) break;
316  }
317 
318  return true;
319 }
320 
321 void FSPath::propagateInner(int i, int j, double bx, double by)
322 {
323  if (j >= LR.m-1) {
324  Q_ASSERT(wrapTop);
325  j -= LR.m-1;
326  }
327 
328  // Innere Zelle: rechte und obere Kante (i+1,j+1) berechnen
329  const Interval &LR = this->LR.at(i, j);
330  const Interval &BR = this->BR.at(i, j);
331 
332  const Interval &RF = fs->cell(i + 1, j).L;
333  const Interval &TF = fs->cell(i, j + 1).B;
334 
335  Interval &RR = this->LR.at(i + 1, j);
336  Interval &TR = this->BR.at(i, j + 1);
337 
338  Interval ignore;
339  propagate(RF,TF, LR,BR,
340  std::isnan(bx) ? RR:ignore,
341  std::isnan(by) ? TR:ignore);
342 
343  static const double TOLERANCE = PRECISION;
344  // clip to upper bounds
345  // (slight tolerance improves propagation, actually)
346  if (!std::isnan(bx) && (TR.upper() > bx-i+TOLERANCE)) {
347  TR.upper() = bx-i+TOLERANCE;
348  if (TR.empty())
349  TR.clear();
350  }
351 
352  if (!std::isnan(by) && (RR.upper() > by-j+TOLERANCE)) {
353  RR.upper() = by-j+TOLERANCE;
354  if (RR.empty())
355  RR.clear();
356  }
357 
358  // wrap-around: (n-1) == 0 (unless cut-off)
359  if (i==this->BR.n-2 && /*(startPoint().x() > 0.0) &&*/ wrapRight && std::isnan(bx))
360  this->LR.at(0,j) += this->LR.at(i+1,j);
361 
362  // wrap-around: (m-1) == 0 (
363  if (j==this->LR.m-2 && /*(startPoint().y() > 0.0) &&*/ wrapTop && std::isnan(by))
364  this->BR.at(i,0) += this->BR.at(i,j+1);
365 }
366 
367 void FSPath::propagateBottom(int i, int j, double bx, double by)
368 {
369  if (j >= LR.m-1) {
370  Q_ASSERT(wrapTop);
371  j -= LR.m-1;
372  }
373 
374  // Innere Zelle: rechte und obere Kante (i+1,j+1) berechnen
375  const Interval &BR = this->BR.at(i, j);
376  const Interval &TF = fs->cell(i, j + 1).B;
377  Interval &TR = this->BR.at(i, j + 1);
378 
379  Interval ignore;
380  propagate(ignore,TF, ignore,BR,
381  ignore,
382  std::isnan(by) ? TR:ignore);
383 
384  static const double TOLERANCE = PRECISION;
385  // clip to upper bounds
386  // (slight tolerance improves propagation, actually)
387  if (!std::isnan(bx) && (TR.upper() > bx-i+TOLERANCE)) {
388  TR.upper() = bx-i+TOLERANCE;
389  if (TR.empty())
390  TR.clear();
391  }
392 
393  // wrap-around: (m-1) == 0 (
394  if (j==this->LR.m-2 && /*(startPoint().y() > 0.0) &&*/ wrapTop && std::isnan(by))
395  this->BR.at(i,0) += this->BR.at(i,j+1);
396 }
397 
399 {
400  Interval RR(std::max(LR.lower(),RF.lower()),
401  RF.upper());
402  if (!RR.valid() || RR.empty()) RR.clear();
403  return RR;
404 }
405 
406 void FSPath::propagate(const Interval& RF, const Interval& TF,
407  const Interval& LR, const Interval& BR,
408  Interval& RR, Interval& TR)
409 {
410  if (!LR && !BR) {
411  // kein Pfad "nonaccessible"
412  //RR.clear();
413  //TR.clear();
414  }
415  else if (LR && BR) {
416  RR = RF;
417  TR = TF;
418  }
419  else if (LR) { // ! BR && LR
420  TR = TF;
421  RR = opposite(LR,RF);
422  }
423  else { // !LR && BR
424  RR = RF;
425  TR = opposite(BR,TF);
426  }
427 }
428 
429 void FSPath::calculateReachability(Point start, Point end, bool allow_wrap, double prec)
430 {
431  Q_ASSERT(start.x() >= 0.0 && start.x() < fs->n-1);
432  Q_ASSERT(start.y() >= 0.0 && start.y() < fs->m-1);
433 
434  // calculate reachable intervals from start point
435  // assume start point is on an edge (right?)
436  Q_ASSERT(is_int(start.x()) || is_int(start.y()));
437 
438  Q_ASSERT(BR.n==fs->n);
439  Q_ASSERT(BR.m==fs->m);
440 
441  // Reachability an den Rändern
442  bool hor_edge = propagateHorizontalEdge(start,end.x(),prec);
443  bool vert_edge = propagateVerticalEdge(start,end.y(),prec);
444  if (! hor_edge && ! vert_edge) {
445  /* Der Startpunkt liegt nicht im Free-Space, oder nicht auf einer Kante.
446 
447  Ein Startpunkt im Inneren wäre möglich, ist aber nicht vorgesehen.
448  Wir müssten den Abstand aus P,Q ermitteln, dann die R,T Intervalle.
449  */
450  return;
451  }
452 
453  // Spalte für Spalte propagieren
454  int i0 = floor(start.x());
455  int j0 = floor(start.y());
456  // TODO mit Obergrenze
457  int i1 = ((int)floor(end.x())) % (LR.n-1);
458 
459  int i=i0;
460  if (i==i1 && allow_wrap) {
461  propagateColumn(i,j0, NAN, end.y(),allow_wrap);
462  i = next_hor(i);
463  }
464  for( ; i >= 0 && i != i1; i = next_hor(i)) {
465  double bx = NAN;//(next_hor(i)==i1 && is_int(end.x())) ? end.x() : NAN;
466  propagateColumn(i,j0, bx, end.y(),allow_wrap);
467  }
468  //if (i==i0 && (start.x() > i0))
469  if (! is_int(end.x()))
470  propagateColumn(i1,j0, end.x(), end.y(),allow_wrap);
471 
472  // Konsistenzcheck
473 /* for(i=0; i < BR.n; ++i)
474  for(j=0; j < LR.m; ++j)
475  {
476  //Q_ASSERT(L.at(i,j).contains(0) == B.at(i,j).contains(0)); // wenn 0 \in L, dann auch \in B
477  //Q_ASSERT(!L.at(i,j).valid() || !L.at(i,j).empty());
478  //Q_ASSERT(!B.at(i,j).valid() || !B.at(i,j).empty());
479  }
480 */
481 
482 }
483 
484 void FSPath::update(Point start, double epsilon)
485 {
486  if (!std::isnan(epsilon))
487  fs->calculateFreeSpace(epsilon);
488 
489  clear();
490 
491  fix.clear();
492  fix.push_back(start);
493 
494  //calculateReachability(start);
495 
496  if (BR.n==0 || LR.m==0) return;
497 
498  if (!wrapRight && !wrapTop) {
499  // Open curves. Start point must be 0.0
500  if (startPoint() != Point(0,0))
501  return;
502  }
503 
504  Point b; // = opposite point
505  Q_ASSERT(wrapRight || wrapTop || (startPoint().x()==0.0 && startPoint().y()==0.0));
506 
507  if (startPoint().x()==0.0 && startPoint().y()==0.0) {
508  b.rx()=BR.n-1;
509  b.ry()=LR.m-1;
510  }
511  else if (wrapRight) {
512  Q_ASSERT(startPoint().y()==0.0);
513  b.rx()=startPoint().x();
514  b.ry()=LR.m-1;
515  }
516  else {
517  Q_ASSERT(wrapTop);
518  Q_ASSERT(startPoint().x()==0.0);
519  b.rx()=BR.n-1;
520  b.ry()=startPoint().y();
521  }
522 
523  Q_ASSERT(startPoint().x()>=0 && startPoint().x()<=(BR.n-1));
524  Q_ASSERT(b.x()>=0 && b.x()<=(BR.n-1));
525 
526  Q_ASSERT(startPoint().y()>=0 && startPoint().y()<=(LR.m-1));
527  Q_ASSERT(b.y()>=0 && b.y()<=(LR.m-1));
528 
529  //Q_ASSERT((a.x() <= b.x()) && (a.y() <= b.y()));
530 
531  // bool ra = isReachable(startPoint());
532  // bool rb = isReachable(b);
533  // Plausi-Check; Rundungsfehler sind möglich, aber tolerierbar.
534  // Q_ASSERT(ra && rb);
535 
536  fix.push_back(b);
537 
538  calculatePath();
539 }
540 
542 {
543  if (!segment)
544  return Point(); // invalid
545 
546  // for open curves, pick 0.0 as start point
547  if (!wrapRight && !wrapTop) {
548  Q_ASSERT(segment->contains(0.0));
549  return Point(0,0);
550  }
551 
552  // avoid picking a too narrow value. While perfectly legal,
553  // it will cause headaches when painting the homeomorphism.
554  double x = segment->mid();
555 
556  switch(segment->ori)
557  {
558  case HORIZONTAL:
559  switch(segment->dir)
560  {
561  case FIRST: return Point(x,0.0);
562  case SECOND: return Point(x,LR.m-1);
563  }
564  case VERTICAL:
565  switch(segment->dir)
566  {
567  case FIRST: return Point(0.0,x);
568  case SECOND: return Point(BR.n-1,x);
569  }
570  }
571  Q_ASSERT(false); // don't come here
572 }
573 
574 void FSPath::propagateColumn(int i, int j0, double bx, double by, bool allow_wrap)
575 {
576  int j=j0;
577  int j1 = (int)floor(by);
578  //if (!std::isnan(by)) j1 = ((int)floor(by));
579 
580  if (j==j1 && allow_wrap) {
581  if (j < LR.m-1 || wrapTop)
582  propagateInner(i,j, bx, NAN);
583  ++j;
584  }
585  for( ; j>=0 && j!=j1; ++j) {
586  if (j >= LR.m-1) {
587  if(!wrapTop) break;
588  j -= LR.m-1;
589  if (j==j1) break;
590  }
591 
592  if ((j+1)==j1 && is_int(by))
593  propagateInner(i,j, bx, by);
594  else
595  propagateInner(i,j, bx, NAN);
596  }
597  // wenn y > 0, müssen wir j0 zweimal besuchen (aber mit Obergrenze)
598  //if (j==j0 && (startPoint().y() > j0))
599  if (! is_int(by)) {
600  if (j1 < LR.m-1 || wrapTop)
601  propagateInner(i,j1, bx, by);
602  }
603 }
604 
605 void FSPath::propagateBottom(int i, int j0, double bx, double by, bool allow_wrap)
606 {
607  int j=j0;
608  int j1 = (int)floor(by);
609  //if (!std::isnan(by)) j1 = ((int)floor(by));
610 
611  if (j==j1 && allow_wrap) {
612  if (j < LR.m-1 || wrapTop)
613  propagateBottom(i,j, bx, NAN);
614  ++j;
615  }
616  for( ; j>=0 && j!=j1; ++j) {
617  if (j >= LR.m-1) {
618  if (!wrapTop) break;
619  j -= LR.m-1;
620  if (j==j1) break;
621  }
622 
623  if ((j+1)==j1 && is_int(by))
624  propagateBottom(i,j, bx, by);
625  else
626  propagateBottom(i,j, bx, NAN);
627  }
628  // wenn y > 0, müssen wir j0 zweimal besuchen (aber mit Obergrenze)
629  //if (j==j0 && (startPoint().y() > j0))
630  if (! is_int(by)) {
631  if (j1 < LR.m-1 || wrapTop)
632  propagateBottom(i,j1, bx, by);
633  }
634 }
635 
636 
637 bool FSPath::left_contains(int i, int j, double y, double prec) const {
638  if (j==LR.m-1)
639  return LR.at(i,j-1).contains(1.0,prec);
640  // or: LR.at(i,0).contains(0.0) ? should be identical
641  else
642  return LR.at(i,j).contains(y-j,prec);
643 }
644 
645 bool FSPath::bottom_contains(int i, int j, double x, double prec) const {
646  if (i==BR.n-1)
647  return BR.at(i-1,j).contains(1.0,prec);
648  // or: BR.at(0,j).contains(0.0) ? should be identical
649  else
650  return BR.at(i,j).contains(x-i,prec);
651 }
652 
653 bool FSPath::isReachable(int i, int j, double prec) const {
654  //Q_ASSERT(L.at(i,j).contains(0.0) == B.at(i,j).contains(0.0));
655  return left_contains(i,j,0.0,prec) || bottom_contains(i,j,0.0,prec);
656 }
657 
658 bool FSPath::isReachable(Point a, double prec) const {
659  // Point is assumed to be on an edge
660  int i = floor(a.x());
661  int j = floor(a.y());
662 
663  Q_ASSERT(a.x()==i || a.y()==j);
664  Q_ASSERT(a.x()>=0.0 && a.x()<=(BR.n-1));
665  Q_ASSERT(a.y()>=0.0 && a.y()<=(LR.m-1));
666 
667  bool l = left_contains(i,j,a.y(),prec);
668  bool b = bottom_contains(i,j,a.x(),prec);
669  return l||b;
670 }
671 
672 
673 
675 {
676  // die Spur von b nach a zurückverfolgen
677  // jeweils zur nächsten erreichbaren Kante
678  path[0].clear();
679  path[1].clear();
680  path[0].push_front(fix.last());
681 
682  int wrapped=0;
684  for(int i=fix.size()-2; i >= 0; --i) {
685  calculateReachability(fix[i], fix[i+1], fix.size()==2);
686  findPathSegment(fix[i], fix[i+1], path, wrapped);
687  clearReachability(); // fix[i], fix[i+1]
688  }
689 
690  // for better visualisation:
691  calculateReachability(fix.first(), fix.last(), true);
692  propagateBottom( (int)fix.first().x(), (int)fix.first().y(), NAN,path[0].first().y(), false);
693 
694  Q_ASSERT(wrapped <= 1);
695 
696  // if (path[0].first().x() != 0.0)
697  // std::swap(path[0],path[1]);
698 
699  if (path[0].first().x() == 0.0)
700  Q_ASSERT(are_consistent(path[0],path[1],fs->n,fs->m));
701  else
702  Q_ASSERT(are_consistent(path[1],path[0],fs->n,fs->m));
703 }
704 
705 void FSPath::findPathSegment(Point a, Point b, Curve res[2], int& wrapped)
706 {
707  int i,j;
708  double bx,by;
709 
710  int i0=floor(a.x());
711  int j0=floor(a.y());
712  bool will_wrap_x = (b.x() <= a.x());
713  bool will_wrap_y = (b.y() <= a.y());
714 
715  for(;;)
716  {
717  // wrap-around (but only once!)
718  if (!wrapped && (b.x()==0.0) && (a.x() > 0.0) && wrapRight) {
719  Q_ASSERT(!wrapped);
720  b.rx() = fs->n-1;
721  res[++wrapped].push_front(b);
722  will_wrap_x=will_wrap_y=false;
723  // update initial R-Intervals
724  propagateBottom(i0,j0,NAN,b.y(),false);
725  }
726  else if (!wrapped && (b.y()==0.0) && (a.y() > 0.0) && wrapTop) {
727  Q_ASSERT(!wrapped);
728  b.ry() = fs->m-1;
729  res[++wrapped].push_front(b);
730  will_wrap_x=will_wrap_y=false;
731  // update initial R-Intervals
732  propagateBottom(i0,j0,NAN,b.y(),false);
733  }
734  Q_ASSERT(!(will_wrap_x || will_wrap_y) || !wrapped);
735 
736  i = numeric::lower_floor(b.x());
737  j = numeric::lower_floor(b.y());
738 
739  if (i==i0 && j==j0) {
740  res[wrapped].push_front(a);
741  break;
742  }
743 
744  Q_ASSERT(will_wrap_x || (i>=i0));
745  Q_ASSERT(will_wrap_y || (j>=j0));
746 
747  const Interval& L = LR.at(i,j);
748  const Interval& B = BR.at(i,j);
749 
750  //if (!L && !B) break;
751  Q_ASSERT(L || B);
752 
753  double bx=NAN, by=NAN;
754  double bx1,bx2, by1,by2;
755  if (B) {
756  bx1 = i+B.lower();
757  bx2 = std::min(i+B.upper(),b.x());
758  if (!will_wrap_x && bx1 < a.x())
759  bx1 = a.x();
760  if (bx1 < bx2)
761  bx = (bx1+2*bx2)/3;
762  }
763  if (L) {
764  by1 = j+L.lower();
765  by2 = std::min(j+L.upper(),b.y());
766  if (!will_wrap_y && by1 < a.y())
767  by1 = a.y();
768  if (by1 < by2)
769  by = (by1+2*by2)/3;
770  }
771 
772  if (std::isnan(bx) && std::isnan(by))
773  {
774  // this is not supposed to happen. could be caused by precision issues, however.
775  // relax the homeomorphism conditions just a bit
776  if (B && ((bx1-bx2) < 4*PRECISION))
777  bx = bx2-PRECISION;
778  if (L && ((by1-by2) < 4*PRECISION))
779  by = by2-PRECISION;
780  }
781 
782  Q_ASSERT(std::isnan(by) || by < b.y());
783  Q_ASSERT(std::isnan(bx) || bx < b.x());
784 
785  Q_ASSERT(std::isnan(by) || will_wrap_y || by > a.y());
786  Q_ASSERT(std::isnan(bx) || will_wrap_x || bx > a.x());
787 
788  bool chooseL = !std::isnan(by) && (will_wrap_x || i > i0);
789  bool chooseB = !std::isnan(bx) && (will_wrap_y || j > j0);
790 
791  if (chooseL && chooseB) {
792  // choose the steepest descent
793  if ((b.x()-bx) > (b.y()-by))
794  chooseL=false;
795  }
796 
797  //if (!chooseL && !chooseB) break;
798  Q_ASSERT(chooseL || chooseB);
799 
800  if (chooseL) {
801  b = Point(i,by);
802  res[wrapped].push_front(b);
803  }
804  else if (chooseB) {
805  b = Point(bx,j);
806  res[wrapped].push_front(b);
807  }
808  }
809 }
810 
811 double FSPath::mapFromPToQ(int p) const
812 {
813  return fmod(mapFromP(p).y(),fs->m-1);
814 }
815 
816 
817 double FSPath::mapFromQToP(int q) const
818 {
819  return fmod(mapFromQ(q).x(),fs->n-1);
820 }
821 
823 {
824  int i = binSearchX(path[0],p);
825  if (i >= 0)
826  return path[0][i];
827 
828  i = binSearchX(path[1],p);
829  Q_ASSERT(i >= 0);
830  return path[1][i];
831 }
832 
834 {
835  int i = binSearchY(path[0],q);
836  if (i >= 0)
837  return path[0][i];
838 
839  i = binSearchY(path[1],q);
840  Q_ASSERT(i >= 0);
841  return path[1][i];
842 }
843 
844 
845 bool FSPath::are_consistent(Curve path0, Curve path1, int n, int m)
846 {
847  // all X and Y coordinates must be present, and in ascending order
848  int x=0, y=0;
849 
850  for(int i=0; i < path0.size(); ++i) {
851  if (is_int(path0[i].x()))
852  Q_ASSERT(path0[i].x() == x++);
853  Q_ASSERT(i==0 || path0[i].x() > path0[i-1].x());
854  }
855  // start and end point may duplicate
856  if (!path1.empty() && path1.first().x()==(x-1)) --x;
857  for(int i=0; i < path1.size(); ++i) {
858  if (is_int(path1[i].x()))
859  Q_ASSERT(path1[i].x() == x++);
860  Q_ASSERT(i==0 || path1[i].x() > path1[i-1].x());
861  }
862  Q_ASSERT(x==n);
863 
864  for(int i=0; i < path1.size(); ++i) {
865  if (is_int(path1[i].y()))
866  Q_ASSERT(path1[i].y() == y++);
867  Q_ASSERT(i==0 || path1[i].y() > path1[i-1].y());
868  }
869  // start and end point may duplicate
870  if (!path0.empty() && path0.first().y()==(y-1)) --y;
871  for(int i=0; i < path0.size(); ++i) {
872  if (is_int(path0[i].y()))
873  Q_ASSERT(path0[i].y() == y++);
874  Q_ASSERT(i==0 || path0[i].y() > path0[i-1].y());
875  }
876  Q_ASSERT(y==m);
877 
878  return true;
879 }
880 
Represents a polygon line segment from node i to j.
Definition: types.h:39
Curve path[2]
Path curves(s)
Definition: fs_path.h:58
bool propagateHorizontalEdge(Point a, double bx, double prec=PRECISION)
propagate the reachable intervals along the horizontal axis
Definition: fs_path.cpp:229
Array2D< Interval > BR
Definition: fs_path.h:54
bool left_contains(int i, int j, double y, double prec=PRECISION) const
test the reachability interval on the left of a cell
Definition: fs_path.cpp:637
static Point mapToPoint(const Curve &C, double x)
given a polygonal curve and an offset, compute the point on the curve
Definition: grid.cpp:130
void propagateBottom(int i, int j, double bx, double by)
propagate the reachable intervals within one cell. The cell is located at the bottom of the free-spac...
Definition: fs_path.cpp:367
void mapToQ(Curve path[2]) const
Definition: fs_path.cpp:142
Curve getPath(int i) const
get the feasible path for display on screen. May be split into two parts.
Definition: fs_path.cpp:36
bool wrapTop
free-space wraps at top, Q is closed
Definition: fs_path.h:50
global definitions for all algorithms.
Point startPoint() const
Definition: fs_path.h:168
const Orientation ori
horizontal or vertical
Definition: boundary.h:318
static void append(std::vector< double > &map, double x)
append a value to a vector, avoiding duplicates
Definition: fs_path.cpp:49
static double mapToSegment(double a, Point p1, Point p2)
map a relative offset to a line segment. For values in [0..1] the resulting point is one the line seg...
Definition: fs_path.cpp:44
void propagateColumn(int i, int j0, double bx, double by, bool allow_wrap)
propagate the reachable intervals along a column of the free-space diagram.
Definition: fs_path.cpp:574
double mid() const
Definition: interval.h:121
FSPath()
constructor for derived classes only
Definition: fs_path.cpp:19
double mapFromQToP(int q) const
Definition: fs_path.cpp:817
void propagateInner(int i, int j, double bx, double by)
propagate the reachable intervals within one cell. The cell is located in the inner part of the free-...
Definition: fs_path.cpp:321
void findPathSegment(Point a, Point b, Curve curve[2], int &cc)
compute one segment of the feasible path. THe path is completed from top-right backwards to bottom-le...
Definition: fs_path.cpp:705
bool bottom_contains(int i, int j, double x, double prec=PRECISION) const
test the reachability interval on the bottom of a cell
Definition: fs_path.cpp:645
bool empty() const
empty test
Definition: interval.h:79
boundary interval in the reachability structure. Represents an interval on the boundary of the FreeSp...
Definition: boundary.h:285
void calculateReachability(Point start, Point end, bool allow_wrap, double prec=PRECISION)
update L^R and B from Free-Space diagram
Definition: fs_path.cpp:429
Interval & clear()
make this an invalid interval
Definition: interval.h:197
static int binSearchY(const Curve &curve, int y)
find a point by its y coordinate
Definition: fs_path.cpp:174
double upper() const
Definition: interval.h:96
static Interval opposite(const Interval &LR, const Interval &RF)
Definition: fs_path.cpp:398
bool wrapRight
free-space wraps right, P is closed
Definition: fs_path.h:52
static bool are_consistent(Curve path0, Curve path1, int n, int m)
test if two curves are consistent with being a feasible path. Both curves must be monotone (asecning ...
Definition: fs_path.cpp:845
int lower_floor(double x, int min=0)
floor function with lower limit
Definition: numeric.h:243
QPointF Point
a point in the plane; with double floating point precision. This type is heavily used throughout all ...
Definition: types.h:14
void mapFromP(Segment pseg, Curve result[2]) const
for a line segment in P, find the mapped part of the feasible path
Definition: fs_path.cpp:55
bool contains(double x) const
containment test
Definition: boundary.h:369
QPolygonF Curve
a polygonal curve in the plane; with double floating point precision. This type is heavily used throu...
Definition: types.h:20
static void propagate(const Interval &RF, const Interval &TF, const Interval &LR, const Interval &BR, Interval &RR, Interval &TR)
propagate the reachable intervals within one cell. from bottom and left to right and top
Definition: fs_path.cpp:406
double lower() const
Definition: interval.h:92
Curve fix
fix points
Definition: fs_path.h:56
double min(double a, double b)
minimum function with checks for NAN
Definition: numeric.h:222
bool isReachable(int i, int j, double prec=PRECISION) const
follow the reachable intervals and test whether a point is reachable from the start
void mapToP(Curve path[2]) const
Definition: fs_path.cpp:125
second segment = right and top edge
Definition: boundary.h:118
void mapFromQ(Segment pseg, Curve result[2]) const
Definition: fs_path.cpp:90
const Direction dir
left/right or bottom/top
Definition: boundary.h:319
static const double PRECISION
floating point precision
Definition: fs_path.h:61
void calculatePath()
calculate the feasible path
Definition: fs_path.cpp:674
Array2D< Interval > LR
reachable intervals
Definition: fs_path.h:54
static int binSearchX(const Curve &curve, int x)
find a point by its x coordinate
Definition: fs_path.cpp:158
bool contains(double x) const
containment test (assumes closed interval, bounds inclusive)
Definition: interval.h:206
bool is_int(double x)
Definition: fs_path.cpp:15
first segment = bottom and left edge
Definition: boundary.h:117
int next_vert(int j)
Definition: fs_path.cpp:205
an interval of two double values.
Definition: interval.h:31
static void copy(Curve &dest, const Curve &source, int i, int j)
copy parts of a curve
Definition: fs_path.cpp:190
FreeSpace::ptr fs
underlying free-space
Definition: fs_path.h:48
boost::shared_ptr< FreeSpace > ptr
smart pointer to FreeSpace object
Definition: freespace.h:92
void update(Point start, double epsilon)
compute a feasible path from the free-space diagram
Definition: fs_path.cpp:484
virtual void clear()
Definition: fs_path.cpp:222
Point toPoint(Pointer segment)
pick a point from a reachability interval
Definition: fs_path.cpp:541
double mapFromPToQ(int p) const
Definition: fs_path.cpp:811
double max(double a, double b)
maximum function with checks for NAN
Definition: numeric.h:233
bool propagateVerticalEdge(Point a, double by, double prec=PRECISION)
propagate the reachable intervals along the vertical axis
Definition: fs_path.cpp:275