fs_path.cpp

Go to the documentation of this file.

#include <poly_path.h>
#include <fs_path.h>
#include <QtGlobal>
#include <grid.h>
#include <iostream>

using namespace frechet;
using namespace reach;


const double FSPath::PRECISION = 1e-9;


inline bool is_int(double x) {
    return x==((int)x);
}

FSPath::FSPath()
:   LR(0,0), BR(0,0),
    fs(), fix(),
    wrapTop(false), wrapRight(false),
    path{Curve(),Curve()}
{ }

FSPath::FSPath(FreeSpace::ptr afs)
:   LR(afs->n,afs->m),
    BR(afs->n,afs->m),
    fs(afs),
    fix(),
    wrapTop(afs->wrapTop()),
    wrapRight(afs->wrapRight()),
    path{Curve(),Curve()}
{ }

Curve FSPath::getPath(int i) const {
    Q_ASSERT(i>=0);
    if (i<2)
        return path[i];
    else
        return Curve();
}

double FSPath::mapToSegment(double a, Point p1, Point p2) {
    double x = (a-p1.x()) / (p2.x()-p1.x());
    return (1-x)*p1.y() + x*p2.y();
}

void FSPath::append(std::vector<double>& map, double x)
{
    if (map.empty() || map.back() != x)
        map.push_back(x);
}

void FSPath::mapFromP(Segment pseg, Curve result[2]) const
{
    result[0].clear();
    result[1].clear();

    int i,j;
    i = binSearchX(path[0],pseg.first);
    Q_ASSERT(i>=0 || i==(-path[0].size()-1));

    if (i < 0) {
        //  both in second part
        i = binSearchX(path[1],pseg.first);
        Q_ASSERT(i >= 0);
        j = binSearchX(path[1],pseg.second);
        Q_ASSERT(j >= 0);
        copy(result[1], path[1], i,j+1);
    }
    else {
        j = binSearchX(path[0],pseg.second);
        Q_ASSERT(j>=0 || j==(-path[0].size()-1));
        if (j >= 0) {
            //  both in first part
            copy(result[0],path[0], i,j+1);
        }
        else {
            //  first part + second part
            j = binSearchX(path[1],pseg.second);
            Q_ASSERT(j >= 0);

            copy(result[0],path[0],i,path[0].size());
            copy(result[1],path[1],0,j+1);
        }
    }
}

void FSPath::mapFromQ(Segment pseg, Curve result[2]) const
{
    result[0].clear();
    result[1].clear();

    int i,j;
    i = binSearchY(path[1],pseg.first);
    Q_ASSERT(i>=0 || i==(-path[1].size()-1));

    if (i < 0) {
        //  both in first part
        i = binSearchY(path[0],pseg.first);
        Q_ASSERT(i >= 0);
        j = binSearchY(path[0],pseg.second);
        Q_ASSERT(j >= 0);
        copy(result[0],path[0], i,j+1);
    }
    else {
        j = binSearchY(path[1],pseg.second);
        Q_ASSERT(j>=0 || j==(-path[1].size()-1));
        if (j >= 0) {
            //  both in second part
            copy(result[1],path[1], i,j+1);
        }
        else {
            //  second part + first part
            j = binSearchY(path[0],pseg.second);
            Q_ASSERT(j >= 0);

            copy(result[0],path[1], i,path[1].size());
            copy(result[1],path[0], 0,j+1);
        }
    }
}

void FSPath::mapToP(Curve path[2]) const
{
    Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
            || fmod(path[0].last().x(),fs->n-1) == fmod(path[1].first().x(),fs->n-1));

    const Curve& P = fs->P;
    for(int i=0; i < 2; ++i)
        for(int j=0; j < path[i].size(); ++j)
            path[i][j] = frechet::Grid::mapToPoint(P,path[i][j].x());

    Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
            || path[0].last() == path[1].first());

    path[0] += path[1];
    path[1].clear();
}

void FSPath::mapToQ(Curve path[2]) const {
    Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
            || fmod(path[0].last().y(),fs->m-1)==fmod(path[1].first().y(),fs->m-1));

    const Curve& Q = fs->Q;
    for(int i=0; i < 2; ++i)
        for(int j=0; j < path[i].size(); ++j)
            path[i][j] = frechet::Grid::mapToPoint(Q,path[i][j].y());

    Q_ASSERT(path[0].isEmpty() || path[1].isEmpty()
            || path[0].last()==path[1].first());

    path[0] += path[1];
    path[1].clear();
}

int FSPath::binSearchX(const Curve& curve, int x) {
    int lo=0;
    int hi=curve.size();
    while(lo < hi) {
        int mid = (lo+hi)/2;
        double mx = curve[mid].x();
        if (x==mx)
            return mid;
        if (x > mx)
            lo = mid+1;
        else
            hi = mid;
    }
    return -lo-1;
}

int FSPath::binSearchY(const Curve& curve, int y) {
    int lo=0;
    int hi=curve.size();
    while(lo < hi) {
        int mid = (lo+hi)/2;
        double my = curve[mid].y();
        if (y==my)
            return mid;
        if (y > my)
            lo = mid+1;
        else
            hi = mid;
    }
    return -lo-1;
}

void FSPath::copy(Curve& dest, const Curve& source, int i, int j)
{
    while(i < j && i < source.size())
        dest.push_back(source[i++]);
}



int FSPath::next_hor(int i) {
    if (++i==BR.n-1)
        return wrapRight ? 0 : -1;
    else
        return i;
}

int FSPath::next_vert(int j) {
    if (++j==LR.m-1)
        return wrapTop ? 0 : -1;
    else
        return j;
}


void FSPath::clearReachability() {
    for(int i=0; i < LR.n; ++i)
        for(int j=0; j < LR.m; ++j)
        {
            LR.at(i,j).clear();
            BR.at(i,j).clear();
        }
}

void FSPath::clear() {
    clearReachability();
    fix.clear();
    path[0].clear();
    path[1].clear();
}

bool FSPath::propagateHorizontalEdge(Point a, double bx, double prec)
{
    int j = floor(a.y());
    if (a.y() > j)
        return false;
        //  Startpunkt liegt nicht auf einer horizontalen Kante

    //  start at start.x()
    int i0 = floor(a.x());
    double x = a.x()-i0;

    const Interval* F = &fs->cell(i0,j).B;
    if (!F->contains(x,prec))
        return false;

    BR.at(i0,j) = {x,F->upper()};
    if (!std::isnan(bx) && (i0+F->upper() > bx+prec))
        LR.at(i0,j).upper() = bx-i0+prec;

    if (F->upper() < 1.0)
        return true;

    int i = i0+1;
    int i1 = (int)floor(bx); // % (BR.n-1);
    if (i > i1) return true;

    for( ; i>=0 && i < i1; ++i)
    {
//        if (i==i1) {
            // ? BR.at(i,j).upper() = (bx-i1);
//            break;
//        }
        int imod = i % (BR.n-1);
        F = &fs->cell(imod,j).B;
        if (F->lower() > 0.0) break;

        BR.at(imod,j) = *F;
        if (!std::isnan(bx) && (i0+F->upper() > bx+prec))
            BR.at(imod,j).upper() = bx-i0+prec;

        if (F->upper() < 1.0) break;
    }

    return true;
}

bool FSPath::propagateVerticalEdge(Point a, double by, double prec)
{
    int i = (int)a.x();
    if (a.x() > i)
        return false;
        //  Startpunkt liegt nicht auf einer vertikalen Kante

    //  start at start.y()
    int j0 = (int)a.y();
    double y = a.y()-j0;

    const Interval* F = &fs->cell(i,j0).L;
    if (!F->contains(y,prec))
        return false;   //  Start liegt nicht im Freespace

    LR.at(i,j0) = {y,F->upper()};
    if (!std::isnan(by) && (j0+F->upper() > by+prec))
        LR.at(i,j0).upper() = by-j0+prec;

    if (F->upper() < 1.0)
        return true;

    int j = j0+1;
    int j1 = (int)floor(by);// % (LR.m-1);
    if (j > j1) return true;

    for( ; j >= 0 && j < j1; ++j)
    {
//        if (j==j1) {
//            // ? LR.at(i,j).upper() = (by-j);
//            break;
//        }
        int jmod = j % (LR.m-1);
        F = &fs->cell(i,jmod).L;
        if (F->lower() > 0.0) break;

        LR.at(i,jmod) = *F;
        if (!std::isnan(by) && (jmod+F->upper() > by+prec))
            LR.at(i,j0).upper() = by-jmod+prec;

        if (F->upper() < 1.0) break;
    }

    return true;
}

void FSPath::propagateInner(int i, int j, double bx, double by)
{
    if (j >= LR.m-1) {
        Q_ASSERT(wrapTop);
        j -= LR.m-1;
    }

    //  Innere Zelle: rechte und obere Kante (i+1,j+1) berechnen
    const Interval &LR = this->LR.at(i, j);
    const Interval &BR = this->BR.at(i, j);

    const Interval &RF = fs->cell(i + 1, j).L;
    const Interval &TF = fs->cell(i, j + 1).B;

    Interval &RR = this->LR.at(i + 1, j);
    Interval &TR = this->BR.at(i, j + 1);

    Interval ignore;
    propagate(RF,TF, LR,BR,
            std::isnan(bx) ? RR:ignore,
            std::isnan(by) ? TR:ignore);

    static const double TOLERANCE = PRECISION;
    //  clip to upper bounds
    //  (slight tolerance improves propagation, actually)
    if (!std::isnan(bx) && (TR.upper() > bx-i+TOLERANCE)) {
        TR.upper() = bx-i+TOLERANCE;
        if (TR.empty())
            TR.clear();
    }

    if (!std::isnan(by) && (RR.upper() > by-j+TOLERANCE)) {
        RR.upper() = by-j+TOLERANCE;
        if (RR.empty())
            RR.clear();
    }

    //  wrap-around: (n-1) == 0  (unless cut-off)
    if (i==this->BR.n-2 && /*(startPoint().x() > 0.0) &&*/ wrapRight && std::isnan(bx))
        this->LR.at(0,j) += this->LR.at(i+1,j);

    //  wrap-around: (m-1) == 0  (
    if (j==this->LR.m-2 && /*(startPoint().y() > 0.0) &&*/ wrapTop && std::isnan(by))
        this->BR.at(i,0) += this->BR.at(i,j+1);
}

void FSPath::propagateBottom(int i, int j, double bx, double by)
{
    if (j >= LR.m-1) {
        Q_ASSERT(wrapTop);
        j -= LR.m-1;
    }

    //  Innere Zelle: rechte und obere Kante (i+1,j+1) berechnen
    const Interval &BR = this->BR.at(i, j);
    const Interval &TF = fs->cell(i, j + 1).B;
    Interval &TR = this->BR.at(i, j + 1);

    Interval ignore;
    propagate(ignore,TF, ignore,BR,
              ignore,
              std::isnan(by) ? TR:ignore);

    static const double TOLERANCE = PRECISION;
    //  clip to upper bounds
    //  (slight tolerance improves propagation, actually)
    if (!std::isnan(bx) && (TR.upper() > bx-i+TOLERANCE)) {
        TR.upper() = bx-i+TOLERANCE;
        if (TR.empty())
            TR.clear();
    }

    //  wrap-around: (m-1) == 0  (
    if (j==this->LR.m-2 && /*(startPoint().y() > 0.0) &&*/ wrapTop && std::isnan(by))
        this->BR.at(i,0) += this->BR.at(i,j+1);
}

Interval FSPath::opposite(const Interval& LR, const Interval& RF)
{
    Interval RR(std::max(LR.lower(),RF.lower()),
                RF.upper());
    if (!RR.valid() || RR.empty()) RR.clear();
    return RR;
}

void FSPath::propagate(const Interval& RF, const Interval& TF,
                       const Interval& LR, const Interval& BR,
                        Interval& RR, Interval& TR)
{
    if (!LR && !BR) {
        //  kein Pfad "nonaccessible"
        //RR.clear();
        //TR.clear();
    }
    else if (LR && BR) {
        RR = RF;
        TR = TF;
    }
    else if (LR) {  // ! BR && LR
        TR = TF;
        RR = opposite(LR,RF);
    }
    else { // !LR && BR
        RR = RF;
        TR = opposite(BR,TF);
    }
}

void FSPath::calculateReachability(Point start, Point end, bool allow_wrap, double prec)
{
    Q_ASSERT(start.x() >= 0.0 && start.x() < fs->n-1);
    Q_ASSERT(start.y() >= 0.0 && start.y() < fs->m-1);

    //  calculate reachable intervals from start point
    //  assume start point is on an edge (right?)
    Q_ASSERT(is_int(start.x()) || is_int(start.y()));

    Q_ASSERT(BR.n==fs->n);
    Q_ASSERT(BR.m==fs->m);

    //  Reachability an den Rändern
    bool hor_edge = propagateHorizontalEdge(start,end.x(),prec);
    bool vert_edge = propagateVerticalEdge(start,end.y(),prec);
    if (! hor_edge && ! vert_edge) {
        /*  Der Startpunkt liegt nicht im Free-Space, oder nicht auf einer Kante.

            Ein Startpunkt im Inneren wäre möglich, ist aber nicht vorgesehen.
            Wir müssten den Abstand aus P,Q ermitteln, dann die R,T Intervalle.
         */
        return;
    }

    //  Spalte für Spalte propagieren
    int i0 = floor(start.x());
    int j0 = floor(start.y());
    //  TODO mit Obergrenze
    int i1 = ((int)floor(end.x())) % (LR.n-1);

    int i=i0;
    if (i==i1 && allow_wrap) {
        propagateColumn(i,j0, NAN, end.y(),allow_wrap);
        i = next_hor(i);
    }
    for( ; i >= 0 && i != i1; i = next_hor(i)) {
        double bx = NAN;//(next_hor(i)==i1 && is_int(end.x())) ? end.x() : NAN;
        propagateColumn(i,j0, bx, end.y(),allow_wrap);
    }
    //if (i==i0 && (start.x() > i0))
    if (! is_int(end.x()))
        propagateColumn(i1,j0, end.x(), end.y(),allow_wrap);

    //  Konsistenzcheck
/*  for(i=0; i < BR.n; ++i)
        for(j=0; j < LR.m; ++j)
        {
            //Q_ASSERT(L.at(i,j).contains(0) == B.at(i,j).contains(0));   //  wenn 0 \in L, dann auch \in B
            //Q_ASSERT(!L.at(i,j).valid() || !L.at(i,j).empty());
            //Q_ASSERT(!B.at(i,j).valid() || !B.at(i,j).empty());
        }
*/

}

void FSPath::update(Point start, double epsilon)
{
    if (!std::isnan(epsilon))
        fs->calculateFreeSpace(epsilon);

    clear();

    fix.clear();
    fix.push_back(start);

    //calculateReachability(start);

    if (BR.n==0 || LR.m==0) return;

    if (!wrapRight && !wrapTop) {
        //  Open curves. Start point must be 0.0
        if (startPoint() != Point(0,0))
            return;
    }

    Point b; // = opposite point
    Q_ASSERT(wrapRight || wrapTop || (startPoint().x()==0.0 && startPoint().y()==0.0));

    if (startPoint().x()==0.0 && startPoint().y()==0.0) {
        b.rx()=BR.n-1;
        b.ry()=LR.m-1;
    }
    else if (wrapRight) {
        Q_ASSERT(startPoint().y()==0.0);
        b.rx()=startPoint().x();
        b.ry()=LR.m-1;
    }
    else {
        Q_ASSERT(wrapTop);
        Q_ASSERT(startPoint().x()==0.0);
        b.rx()=BR.n-1;
        b.ry()=startPoint().y();
    }

    Q_ASSERT(startPoint().x()>=0 && startPoint().x()<=(BR.n-1));
    Q_ASSERT(b.x()>=0 && b.x()<=(BR.n-1));

    Q_ASSERT(startPoint().y()>=0 && startPoint().y()<=(LR.m-1));
    Q_ASSERT(b.y()>=0 && b.y()<=(LR.m-1));

    //Q_ASSERT((a.x() <= b.x()) && (a.y() <= b.y()));

   // bool ra = isReachable(startPoint());
   // bool rb = isReachable(b);
    //  Plausi-Check; Rundungsfehler sind möglich, aber tolerierbar.
   // Q_ASSERT(ra && rb);

    fix.push_back(b);

    calculatePath();
}

Point FSPath::toPoint(Pointer segment)
{
    if (!segment)
        return Point(); //  invalid

    //  for open curves, pick 0.0 as start point
    if (!wrapRight && !wrapTop) {
        Q_ASSERT(segment->contains(0.0));
        return Point(0,0);
    }

    //  avoid picking a too narrow value. While perfectly legal,
    //  it will cause headaches when painting the homeomorphism.
    double x = segment->mid();

    switch(segment->ori)
    {
    case HORIZONTAL:
        switch(segment->dir)
        {
        case FIRST:     return Point(x,0.0);
        case SECOND:    return Point(x,LR.m-1);
        }
    case VERTICAL:
        switch(segment->dir)
        {
        case FIRST:     return Point(0.0,x);
        case SECOND:    return Point(BR.n-1,x);
        }
    }
    Q_ASSERT(false);    //  don't come here
}

void FSPath::propagateColumn(int i, int j0, double bx, double by, bool allow_wrap)
{
    int j=j0;
    int j1 = (int)floor(by);
    //if (!std::isnan(by)) j1 = ((int)floor(by));

    if (j==j1 && allow_wrap) {
        if (j < LR.m-1 || wrapTop)
            propagateInner(i,j, bx, NAN);
        ++j;
    }
    for( ; j>=0 && j!=j1; ++j) {
        if (j >= LR.m-1) {
            if(!wrapTop) break;
            j -= LR.m-1;
            if (j==j1) break;
        }

        if ((j+1)==j1 && is_int(by))
            propagateInner(i,j, bx, by);
        else
            propagateInner(i,j, bx, NAN);
    }
    // wenn y > 0, müssen wir j0 zweimal besuchen (aber mit Obergrenze)
    //if (j==j0 && (startPoint().y() > j0))
    if (! is_int(by)) {
        if (j1 < LR.m-1 || wrapTop)
            propagateInner(i,j1, bx, by);
    }
}

void FSPath::propagateBottom(int i, int j0, double bx, double by, bool allow_wrap)
{
    int j=j0;
    int j1 = (int)floor(by);
    //if (!std::isnan(by)) j1 = ((int)floor(by));

    if (j==j1 && allow_wrap) {
        if (j < LR.m-1 || wrapTop)
            propagateBottom(i,j, bx, NAN);
        ++j;
    }
    for( ; j>=0 && j!=j1; ++j) {
        if (j >= LR.m-1) {
            if (!wrapTop) break;
            j -= LR.m-1;
            if (j==j1) break;
        }

        if ((j+1)==j1 && is_int(by))
            propagateBottom(i,j, bx, by);
        else
            propagateBottom(i,j, bx, NAN);
    }
    // wenn y > 0, müssen wir j0 zweimal besuchen (aber mit Obergrenze)
    //if (j==j0 && (startPoint().y() > j0))
    if (! is_int(by)) {
        if (j1 < LR.m-1 || wrapTop)
            propagateBottom(i,j1, bx, by);
    }
}


bool FSPath::left_contains(int i, int j, double y, double prec) const {
    if (j==LR.m-1)
        return LR.at(i,j-1).contains(1.0,prec);
    // or: LR.at(i,0).contains(0.0) ? should be identical
    else
        return LR.at(i,j).contains(y-j,prec);
}

bool FSPath::bottom_contains(int i, int j, double x, double prec) const {
    if (i==BR.n-1)
        return BR.at(i-1,j).contains(1.0,prec);
    // or: BR.at(0,j).contains(0.0) ? should be identical
    else
        return BR.at(i,j).contains(x-i,prec);
}

bool FSPath::isReachable(int i, int j, double prec) const {
    //Q_ASSERT(L.at(i,j).contains(0.0) == B.at(i,j).contains(0.0));
    return left_contains(i,j,0.0,prec) || bottom_contains(i,j,0.0,prec);
}

bool FSPath::isReachable(Point a, double prec) const {
    //  Point is assumed to be on an edge
    int i = floor(a.x());
    int j = floor(a.y());

    Q_ASSERT(a.x()==i || a.y()==j);
    Q_ASSERT(a.x()>=0.0 && a.x()<=(BR.n-1));
    Q_ASSERT(a.y()>=0.0 && a.y()<=(LR.m-1));

    bool l = left_contains(i,j,a.y(),prec);
    bool b = bottom_contains(i,j,a.x(),prec);
    return l||b;
}



void FSPath::calculatePath()
{
    //  die Spur von b nach a zurückverfolgen
    //  jeweils zur nächsten erreichbaren Kante
    path[0].clear();
    path[1].clear();
    path[0].push_front(fix.last());

    int wrapped=0;
    clearReachability();
    for(int i=fix.size()-2; i >= 0; --i) {
        calculateReachability(fix[i], fix[i+1], fix.size()==2);
        findPathSegment(fix[i], fix[i+1], path, wrapped);
        clearReachability();    // fix[i], fix[i+1]
    }

    //  for better visualisation:
    calculateReachability(fix.first(), fix.last(), true);
    propagateBottom( (int)fix.first().x(), (int)fix.first().y(), NAN,path[0].first().y(), false);

    Q_ASSERT(wrapped <= 1);    

    //    if (path[0].first().x() != 0.0)
    //    std::swap(path[0],path[1]);

    if (path[0].first().x() == 0.0)
        Q_ASSERT(are_consistent(path[0],path[1],fs->n,fs->m));
    else
        Q_ASSERT(are_consistent(path[1],path[0],fs->n,fs->m));
}

void FSPath::findPathSegment(Point a, Point b, Curve res[2], int& wrapped)
{       
    int i,j;
    double bx,by;

    int i0=floor(a.x());
    int j0=floor(a.y());
    bool will_wrap_x = (b.x() <= a.x());
    bool will_wrap_y = (b.y() <= a.y());

    for(;;)
    {
        //  wrap-around (but only once!)
        if (!wrapped && (b.x()==0.0) && (a.x() > 0.0) && wrapRight) {
            Q_ASSERT(!wrapped);
            b.rx() = fs->n-1;
            res[++wrapped].push_front(b);
            will_wrap_x=will_wrap_y=false;
            //  update initial R-Intervals
            propagateBottom(i0,j0,NAN,b.y(),false);
        }
        else if (!wrapped && (b.y()==0.0) && (a.y() > 0.0) && wrapTop) {
            Q_ASSERT(!wrapped);
            b.ry() = fs->m-1;
            res[++wrapped].push_front(b);
            will_wrap_x=will_wrap_y=false;
            //  update initial R-Intervals
            propagateBottom(i0,j0,NAN,b.y(),false);
        }
        Q_ASSERT(!(will_wrap_x || will_wrap_y) || !wrapped);

        i = numeric::lower_floor(b.x());
        j = numeric::lower_floor(b.y());

        if (i==i0 && j==j0) {
            res[wrapped].push_front(a);
            break;
        }

        Q_ASSERT(will_wrap_x || (i>=i0));
        Q_ASSERT(will_wrap_y || (j>=j0));

        const Interval& L = LR.at(i,j);
        const Interval& B = BR.at(i,j);

        //if (!L && !B) break;
        Q_ASSERT(L || B);

        double bx=NAN, by=NAN;
        double bx1,bx2, by1,by2;
        if (B) {
            bx1 = i+B.lower();
            bx2 = std::min(i+B.upper(),b.x());
            if (!will_wrap_x && bx1 < a.x())
                bx1 = a.x();
            if (bx1 < bx2)
                bx = (bx1+2*bx2)/3;
        }
        if (L) {
            by1 = j+L.lower();
            by2 = std::min(j+L.upper(),b.y());
            if (!will_wrap_y && by1 < a.y())
                by1 = a.y();
            if (by1 < by2)
                by = (by1+2*by2)/3;
        }

        if (std::isnan(bx) && std::isnan(by))
        {
            //  this is not supposed to happen. could be caused by precision issues, however.
            //  relax the homeomorphism conditions just a bit
            if (B && ((bx1-bx2) < 4*PRECISION))
                bx = bx2-PRECISION;
            if (L && ((by1-by2) < 4*PRECISION))
                by = by2-PRECISION;
        }

        Q_ASSERT(std::isnan(by) || by < b.y());
        Q_ASSERT(std::isnan(bx) || bx < b.x());

        Q_ASSERT(std::isnan(by) || will_wrap_y || by > a.y());
        Q_ASSERT(std::isnan(bx) || will_wrap_x || bx > a.x());

        bool chooseL = !std::isnan(by) && (will_wrap_x || i > i0);
        bool chooseB = !std::isnan(bx) && (will_wrap_y || j > j0);

        if (chooseL && chooseB) {
            //  choose the steepest descent
            if ((b.x()-bx) > (b.y()-by))
                chooseL=false;
        }

        //if (!chooseL && !chooseB) break;
        Q_ASSERT(chooseL || chooseB);

        if (chooseL) {
            b = Point(i,by);
            res[wrapped].push_front(b);
        }
        else if (chooseB) {
            b = Point(bx,j);
            res[wrapped].push_front(b);
        }
    }
}

double FSPath::mapFromPToQ(int p) const
{
    return fmod(mapFromP(p).y(),fs->m-1);
}


double FSPath::mapFromQToP(int q) const
{
    return fmod(mapFromQ(q).x(),fs->n-1);
}

Point FSPath::mapFromP(int p) const
{
    int i = binSearchX(path[0],p);
    if (i >= 0)
        return path[0][i];

    i = binSearchX(path[1],p);
    Q_ASSERT(i >= 0);
    return path[1][i];
}

Point FSPath::mapFromQ(int q) const
{
    int i = binSearchY(path[0],q);
    if (i >= 0)
        return path[0][i];

    i = binSearchY(path[1],q);
    Q_ASSERT(i >= 0);
    return path[1][i];
}


bool FSPath::are_consistent(Curve path0, Curve path1, int n, int m)
{
    //  all X and Y coordinates must be present, and in ascending order
    int x=0, y=0;

    for(int i=0; i < path0.size(); ++i) {
        if (is_int(path0[i].x()))
            Q_ASSERT(path0[i].x() == x++);
        Q_ASSERT(i==0 || path0[i].x() > path0[i-1].x());
    }
    //  start and end point may duplicate
    if (!path1.empty() && path1.first().x()==(x-1)) --x;
    for(int i=0; i < path1.size(); ++i) {
        if (is_int(path1[i].x()))
            Q_ASSERT(path1[i].x() == x++);
        Q_ASSERT(i==0 || path1[i].x() > path1[i-1].x());
    }
    Q_ASSERT(x==n);

    for(int i=0; i < path1.size(); ++i) {
        if (is_int(path1[i].y()))
            Q_ASSERT(path1[i].y() == y++);
        Q_ASSERT(i==0 || path1[i].y() > path1[i-1].y());
    }
    //  start and end point may duplicate
    if (!path0.empty() && path0.first().y()==(y-1)) --y;
    for(int i=0; i < path0.size(); ++i) {
        if (is_int(path0[i].y()))
            Q_ASSERT(path0[i].y() == y++);
        Q_ASSERT(i==0 || path0[i].y() > path0[i-1].y());
    }
    Q_ASSERT(y==m);

    return true;
}