22 wrapTop(false), wrapRight(false),
31 wrapTop(afs->wrapTop()),
32 wrapRight(afs->wrapRight()),
45 double x = (a-p1.x()) / (p2.x()-p1.x());
46 return (1-x)*p1.y() + x*p2.y();
51 if (map.empty() || map.back() != x)
62 Q_ASSERT(i>=0 || i==(-
path[0].size()-1));
74 Q_ASSERT(j>=0 || j==(-
path[0].size()-1));
97 Q_ASSERT(i>=0 || i==(-
path[1].size()-1));
109 Q_ASSERT(j>=0 || j==(-
path[1].size()-1));
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));
131 for(
int i=0; i < 2; ++i)
132 for(
int j=0; j <
path[i].size(); ++j)
135 Q_ASSERT(
path[0].isEmpty() ||
path[1].isEmpty()
136 ||
path[0].last() ==
path[1].first());
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));
147 for(
int i=0; i < 2; ++i)
148 for(
int j=0; j <
path[i].size(); ++j)
151 Q_ASSERT(
path[0].isEmpty() ||
path[1].isEmpty()
152 ||
path[0].last()==
path[1].first());
163 double mx = curve[mid].x();
179 double my = curve[mid].y();
192 while(i < j && i < source.size())
193 dest.push_back(source[i++]);
214 for(
int i=0; i <
LR.n; ++i)
215 for(
int j=0; j <
LR.m; ++j)
231 int j = floor(a.y());
237 int i0 = floor(a.x());
245 if (!std::isnan(bx) && (i0+F->
upper() > bx+prec))
246 LR.at(i0,j).upper() = bx-i0+prec;
248 if (F->
upper() < 1.0)
252 int i1 = (int)floor(bx);
253 if (i > i1)
return true;
255 for( ; i>=0 && i < i1; ++i)
261 int imod = i % (
BR.n-1);
262 F = &
fs->cell(imod,j).B;
263 if (F->
lower() > 0.0)
break;
266 if (!std::isnan(bx) && (i0+F->
upper() > bx+prec))
267 BR.at(imod,j).upper() = bx-i0+prec;
269 if (F->
upper() < 1.0)
break;
291 if (!std::isnan(by) && (j0+F->
upper() > by+prec))
292 LR.at(i,j0).upper() = by-j0+prec;
294 if (F->
upper() < 1.0)
298 int j1 = (int)floor(by);
299 if (j > j1)
return true;
301 for( ; j >= 0 && j < j1; ++j)
307 int jmod = j % (
LR.m-1);
308 F = &
fs->cell(i,jmod).L;
309 if (F->
lower() > 0.0)
break;
312 if (!std::isnan(by) && (jmod+F->
upper() > by+prec))
313 LR.at(i,j0).upper() = by-jmod+prec;
315 if (F->
upper() < 1.0)
break;
335 Interval &RR = this->LR.at(i + 1, j);
336 Interval &TR = this->BR.at(i, j + 1);
340 std::isnan(bx) ? RR:ignore,
341 std::isnan(by) ? TR:ignore);
343 static const double TOLERANCE =
PRECISION;
346 if (!std::isnan(bx) && (TR.
upper() > bx-i+TOLERANCE)) {
347 TR.
upper() = bx-i+TOLERANCE;
352 if (!std::isnan(by) && (RR.
upper() > by-j+TOLERANCE)) {
353 RR.
upper() = by-j+TOLERANCE;
359 if (i==this->BR.n-2 &&
wrapRight && std::isnan(bx))
360 this->LR.at(0,j) += this->LR.at(i+1,j);
363 if (j==this->LR.m-2 &&
wrapTop && std::isnan(by))
364 this->BR.at(i,0) += this->BR.at(i,j+1);
377 Interval &TR = this->BR.at(i, j + 1);
382 std::isnan(by) ? TR:ignore);
384 static const double TOLERANCE =
PRECISION;
387 if (!std::isnan(bx) && (TR.
upper() > bx-i+TOLERANCE)) {
388 TR.
upper() = bx-i+TOLERANCE;
394 if (j==this->
LR.m-2 &&
wrapTop && std::isnan(by))
395 this->BR.at(i,0) += this->BR.at(i,j+1);
402 if (!RR.valid() || RR.empty()) RR.
clear();
431 Q_ASSERT(start.x() >= 0.0 && start.x() <
fs->n-1);
432 Q_ASSERT(start.y() >= 0.0 && start.y() <
fs->m-1);
438 Q_ASSERT(
BR.n==
fs->n);
439 Q_ASSERT(
BR.m==
fs->m);
444 if (! hor_edge && ! vert_edge) {
454 int i0 = floor(start.x());
455 int j0 = floor(start.y());
457 int i1 = ((int)floor(end.x())) % (
LR.n-1);
460 if (i==i1 && allow_wrap) {
464 for( ; i >= 0 && i != i1; i =
next_hor(i)) {
486 if (!std::isnan(epsilon))
487 fs->calculateFreeSpace(epsilon);
492 fix.push_back(start);
496 if (
BR.n==0 ||
LR.m==0)
return;
524 Q_ASSERT(b.x()>=0 && b.x()<=(
BR.n-1));
527 Q_ASSERT(b.y()>=0 && b.y()<=(
LR.m-1));
554 double x = segment->
mid();
577 int j1 = (int)floor(by);
580 if (j==j1 && allow_wrap) {
585 for( ; j>=0 && j!=j1; ++j) {
592 if ((j+1)==j1 &&
is_int(by))
608 int j1 = (int)floor(by);
611 if (j==j1 && allow_wrap) {
616 for( ; j>=0 && j!=j1; ++j) {
623 if ((j+1)==j1 &&
is_int(by))
639 return LR.at(i,j-1).contains(1.0,prec);
642 return LR.at(i,j).contains(y-j,prec);
647 return BR.at(i-1,j).contains(1.0,prec);
650 return BR.at(i,j).contains(x-i,prec);
660 int i = floor(a.x());
661 int j = floor(a.y());
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));
680 path[0].push_front(
fix.last());
684 for(
int i=
fix.size()-2; i >= 0; --i) {
694 Q_ASSERT(wrapped <= 1);
699 if (
path[0].first().x() == 0.0)
712 bool will_wrap_x = (b.x() <= a.x());
713 bool will_wrap_y = (b.y() <= a.y());
718 if (!wrapped && (b.x()==0.0) && (a.x() > 0.0) &&
wrapRight) {
721 res[++wrapped].push_front(b);
722 will_wrap_x=will_wrap_y=
false;
726 else if (!wrapped && (b.y()==0.0) && (a.y() > 0.0) &&
wrapTop) {
729 res[++wrapped].push_front(b);
730 will_wrap_x=will_wrap_y=
false;
734 Q_ASSERT(!(will_wrap_x || will_wrap_y) || !wrapped);
739 if (i==i0 && j==j0) {
740 res[wrapped].push_front(a);
744 Q_ASSERT(will_wrap_x || (i>=i0));
745 Q_ASSERT(will_wrap_y || (j>=j0));
753 double bx=NAN, by=NAN;
754 double bx1,bx2, by1,by2;
758 if (!will_wrap_x && bx1 < a.x())
766 if (!will_wrap_y && by1 < a.y())
772 if (std::isnan(bx) && std::isnan(by))
782 Q_ASSERT(std::isnan(by) || by < b.y());
783 Q_ASSERT(std::isnan(bx) || bx < b.x());
785 Q_ASSERT(std::isnan(by) || will_wrap_y || by > a.y());
786 Q_ASSERT(std::isnan(bx) || will_wrap_x || bx > a.x());
788 bool chooseL = !std::isnan(by) && (will_wrap_x || i > i0);
789 bool chooseB = !std::isnan(bx) && (will_wrap_y || j > j0);
791 if (chooseL && chooseB) {
793 if ((b.x()-bx) > (b.y()-by))
798 Q_ASSERT(chooseL || chooseB);
802 res[wrapped].push_front(b);
806 res[wrapped].push_front(b);
850 for(
int i=0; i < path0.size(); ++i) {
852 Q_ASSERT(path0[i].x() == x++);
853 Q_ASSERT(i==0 || path0[i].x() > path0[i-1].x());
856 if (!path1.empty() && path1.first().x()==(x-1)) --x;
857 for(
int i=0; i < path1.size(); ++i) {
859 Q_ASSERT(path1[i].x() == x++);
860 Q_ASSERT(i==0 || path1[i].x() > path1[i-1].x());
864 for(
int i=0; i < path1.size(); ++i) {
866 Q_ASSERT(path1[i].y() == y++);
867 Q_ASSERT(i==0 || path1[i].y() > path1[i-1].y());
870 if (!path0.empty() && path0.first().y()==(y-1)) --y;
871 for(
int i=0; i < path0.size(); ++i) {
873 Q_ASSERT(path0[i].y() == y++);
874 Q_ASSERT(i==0 || path0[i].y() > path0[i-1].y());
Represents a polygon line segment from node i to j.
Curve path[2]
Path curves(s)
bool propagateHorizontalEdge(Point a, double bx, double prec=PRECISION)
propagate the reachable intervals along the horizontal axis
bool left_contains(int i, int j, double y, double prec=PRECISION) const
test the reachability interval on the left of a cell
static Point mapToPoint(const Curve &C, double x)
given a polygonal curve and an offset, compute the point on the curve
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...
void mapToQ(Curve path[2]) const
Curve getPath(int i) const
get the feasible path for display on screen. May be split into two parts.
bool wrapTop
free-space wraps at top, Q is closed
global definitions for all algorithms.
const Orientation ori
horizontal or vertical
static void append(std::vector< double > &map, double x)
append a value to a vector, avoiding duplicates
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...
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.
FSPath()
constructor for derived classes only
double mapFromQToP(int q) const
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-...
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...
bool bottom_contains(int i, int j, double x, double prec=PRECISION) const
test the reachability interval on the bottom of a cell
bool empty() const
empty test
boundary interval in the reachability structure. Represents an interval on the boundary of the FreeSp...
void calculateReachability(Point start, Point end, bool allow_wrap, double prec=PRECISION)
update L^R and B from Free-Space diagram
Interval & clear()
make this an invalid interval
static int binSearchY(const Curve &curve, int y)
find a point by its y coordinate
static Interval opposite(const Interval &LR, const Interval &RF)
bool wrapRight
free-space wraps right, P is closed
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 ...
int lower_floor(double x, int min=0)
floor function with lower limit
QPointF Point
a point in the plane; with double floating point precision. This type is heavily used throughout all ...
void mapFromP(Segment pseg, Curve result[2]) const
for a line segment in P, find the mapped part of the feasible path
bool contains(double x) const
containment test
QPolygonF Curve
a polygonal curve in the plane; with double floating point precision. This type is heavily used throu...
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
double min(double a, double b)
minimum function with checks for NAN
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
second segment = right and top edge
void mapFromQ(Segment pseg, Curve result[2]) const
const Direction dir
left/right or bottom/top
static const double PRECISION
floating point precision
void calculatePath()
calculate the feasible path
Array2D< Interval > LR
reachable intervals
static int binSearchX(const Curve &curve, int x)
find a point by its x coordinate
bool contains(double x) const
containment test (assumes closed interval, bounds inclusive)
first segment = bottom and left edge
an interval of two double values.
static void copy(Curve &dest, const Curve &source, int i, int j)
copy parts of a curve
FreeSpace::ptr fs
underlying free-space
boost::shared_ptr< FreeSpace > ptr
smart pointer to FreeSpace object
void update(Point start, double epsilon)
compute a feasible path from the free-space diagram
Point toPoint(Pointer segment)
pick a point from a reachability interval
double mapFromPToQ(int p) const
double max(double a, double b)
maximum function with checks for NAN
bool propagateVerticalEdge(Point a, double by, double prec=PRECISION)
propagate the reachable intervals along the vertical axis