poly_path.cpp

Go to the documentation of this file.

#include <poly_path.h>
#include <frechet/poly/poly_path.h>
#include <frechet/poly/algorithm.h>
#include <spirolator.h>

using namespace frechet;
using namespace poly;
using namespace reach;

PolygonFSPath::PolygonFSPath(FreeSpace::ptr fs)
    : FSPath(fs)
{
    wrapTop=false;
}

void PolygonFSPath::update(
        const Graph::ptr G,
        double aneps)
{
    if (!std::isnan(aneps))
        fs->calculateFreeSpace(aneps);

    Q_ASSERT(G);
    gmodel = G->graphModel();

    FSPath::clear();
    drillDown(G);   //  calculate fix points

    calculatePath();
    gmodel.reset();
}


void PolygonFSPath::drillDown(Graph::ptr G)
{
    Q_ASSERT(G->graphModel()==gmodel);

    //  find start/end interval
    int p = G->foundDiagonalElement();
    Interval start_ival = mapInterval(HORIZONTAL,p);
    Q_ASSERT(start_ival);
    //  don't pick start point at interval border; pick middle instead
    double start = (start_ival.lower()+start_ival.upper())/2;
    start = fmod(start,fs->n-1);

    fix.clear();
    addFixPoint(Point(start,0.0));
    //  end point is mapped to Q(0), again

    //  G consists of three merged parts A B A'
    Graph::ptr A = G->origin.A;
    Graph::ptr B = G->origin.B;

    B->synchFromGpu();

    //   find the connecting points  A-B-A'
    IndexPair k = findSolutionConnectingPoints(A, p, B);
    Q_ASSERT(k.first <= k.second);

    addFixPoint(A->hmask().upper,k.first);

    drillDown(B, k.first, k.second);

    addFixPoint(B->hmask().upper,k.second);
    //  End Point
    if (fix.size()>1 && fix.last().x()==start)
        fix.last().ry() = fs->m-1;   //  replace last point
    else
        addFixPoint(Point(start,fs->m-1));

    Q_ASSERT(is_consistent(fix));
}


void PolygonFSPath::drillDown(Graph::ptr M, int k1, int k2)
{
    if (M->origin.operation==Graph::Origin::MERGE2)
    {
        Graph::ptr A = M->origin.A;
        Graph::ptr B = M->origin.B;

        A->synchFromGpu();
        B->synchFromGpu();

        Q_ASSERT(Graph::is_adjacent_to(*A,*B));

        int k = findConnectingPoint(A,k1, B,k2);
        Q_ASSERT(k >= k1 && k <= k2);

        drillDown(A, k1, k);

        addFixPoint(A->hmask().upper,k);

        drillDown(B, k, k2);
    }
}

int PolygonFSPath::findConnectingPoint(
        Graph::ptr A, int ia,
        Graph::ptr B, int ib) const
{
    Q_ASSERT(A->graphModel()==gmodel);
    Q_ASSERT(B->graphModel()==gmodel);

    Interval ival1 = mapInterval(VERTICAL,ia);
    Interval ival2 = mapInterval(VERTICAL,ib);
    Q_ASSERT(ival1.lower() < ival2.upper());

    IndexRange r = A->find_next_range(VERTICAL,ia, VERTICAL,0);
    for( ; ! r.empty(); r = A->find_next_range(VERTICAL,ia, VERTICAL,r.upper))
    {
        Spirolator k (r.lower,r.upper);
        for ( ; k; ++k) {
            if (B->contains_edge(VERTICAL, k, VERTICAL, ib)) {
                Interval ival3 = mapInterval(VERTICAL, k);
                //  avoid picking too narrow bounds. While perfectly legal,
                //  it will cause headaches when painting the homeomorphism.
                if (ival3.lower() == ival3.upper())
                    continue;
//                if (ival3.upper() >= ival1.lower() && ival3.lower() <= ival2.upper()) {
                Q_ASSERT(A->contains_edge(VERTICAL, ia, VERTICAL, k));
                Q_ASSERT(B->contains_edge(VERTICAL, k, VERTICAL, ib));
                return k;
//                }
            }
        }
    }
    Q_ASSERT(false);
    return -1;
}

void PolygonFSPath::addFixPoint(int pi, int qi) {
    //  map to curve offset
    int p = mapPoint(HORIZONTAL,pi);
    p = p % (fs->n-1);

    Interval q = mapInterval(VERTICAL,qi);
    double y,y1,y2;
    if (fix.isEmpty())
        addFixPoint(Point(p,q.lower()));
    else if (q.lower() > fix.last().y()) {
        //  avoid picking too narrow bounds. While perfectly legal,
        //  it will cause headaches when painting the homeomorphism.
        double y = (q.lower()+q.upper())/2;
        addFixPoint(Point(p,y));
    }
    else {
        y1 = std::max(q.lower(),fix.last().y());
        Q_ASSERT(q.upper() > y1);
        y = (y1+q.upper())/2;
        Q_ASSERT(q.contains(y));
        addFixPoint(Point(p,y));
    }
}

void PolygonFSPath::addFixPoint(Point p)
{
    if (!fix.empty() && p==fix.last())
        return; //  skip duplicates
    if (fix.empty()) {
        fix.push_back(p);
        return;
    }
    //  the homeomprphism must be bijective !!
    QPointF last = fix.last();
    Q_ASSERT(p.x() != last.x()); //   greater modulo n
    Q_ASSERT(p.y() > last.y());
    fix.push_back(p);
}

Interval PolygonFSPath::mapInterval(Orientation o, int p) const
{
    const std::vector<Interval>& reverse = gmodel->reverseMap(o);
    return reverse[p % reverse.size()];
}
double PolygonFSPath::mapPoint(Orientation o, int p) const
{
    return poly::Algorithm::pickIntervalPoint(mapInterval(o,p));
}

PolygonFSPath::IndexPair PolygonFSPath::findSolutionConnectingPoints(Graph::ptr A, int i, Graph::ptr B) const
{
    GraphModel::ptr model = A->graphModel();

    Q_ASSERT(Graph::is_adjacent_to(*A,*B));
    Q_ASSERT(Graph::is_adjacent_to(*B,*A));

    Q_ASSERT(A->origin.operation==Graph::Origin::RG);
    //Q_ASSERT(B->origin.operation!=Graph::Origin::RG);

    IndexRange r1 = A->find_next_range(HORIZONTAL,i, VERTICAL,0);
    for( ; ! r1.empty(); r1 = A->find_next_range(HORIZONTAL,i, VERTICAL,r1.upper))
    {
        Spirolator k1 (r1.lower,r1.upper);
        for ( ; k1; ++k1) {
            // k.first must map to an interval > 0
            Interval ival1 = mapInterval(VERTICAL, k1);
            if (ival1.upper() <= 0.0) continue;

            Q_ASSERT(A->contains_edge(HORIZONTAL, i, VERTICAL, k1));

            IndexRange r2 = B->find_next_range(VERTICAL, k1, VERTICAL, k1);
            for (; !r2.empty(); r2 = B->find_next_range(VERTICAL, k1, VERTICAL, r2.upper))
            {
                Spirolator k2(r2.lower,r2.upper);
                for ( ; k2; ++k2)
                    if (A->contains_edge(VERTICAL, k2, HORIZONTAL, i)) {
                        // intervals must be strictly monotonous
                        Interval ival2 = mapInterval(VERTICAL, k2);
                        if (ival2.lower() >= fs->m - 1) continue;
                        if (ival2.upper() <= ival1.lower()) continue;
                        return IndexPair(k1,k2);
                    }
            }
        }
    }
/*  naive implementation
    for(k.first=0; k.first < v1; ++k.first)
        for(k.second=0; k.second < v1; ++k.second)
            if (A->contains_edge(HORIZONTAL,i, VERTICAL,k.first)
                && B->contains_edge(VERTICAL,k.first, VERTICAL,k.second)
                && A->contains_edge(VERTICAL,k.second,HORIZONTAL,i))
                return k;
*/
    Q_ASSERT(false);
    return IndexPair(-1,-1);
}

Curve PolygonFSPath::findShortestPathP(int qa, int qb, Triangulation& tri)
{
    Curve result;
    //  map points from P to Q
    double pa = mapFromQToP(qa);
    double pb = mapFromQToP(qb);

    //  create a shortest path tree searcher
    PolygonShortestPaths shortestPaths(tri);
    return shortestPaths.find1ShortestPath(pa,pb);
}

Curve PolygonFSPath::findShortestPathQ(int pa, int pb, Triangulation& tri)
{
    Curve result;
    //  map points from P to Q
    double qa = mapFromPToQ(pa);
    double qb = mapFromPToQ(pb);

    //  create a shortest path tree searcher
    PolygonShortestPaths shortestPaths(tri);
    return shortestPaths.find1ShortestPath(qa,qb);
}




bool PolygonFSPath::is_consistent(Curve fix)
{
//    for(int i=0; i < fix.size(); ++i)
//        std::cout << "["<<fix[i].x()<<","<<fix[i].y()<<"]" << std::endl;
//    std::cout << std::endl;
    //  consistency check. fix must not contain duplicates
    for(int i=0; i < fix.size(); ++i) {
        Q_ASSERT(fix.indexOf(fix[i])==i);
    }

    //  must be strictly monotonous (modulo n)
    int wrapped=0;
    for(int i=1; i < fix.size(); ++i)
    {
        Q_ASSERT(fix[i].y() > fix[i-1].y());
        if (fix[i].x() < fix[i-1].x()) wrapped++;
        Q_ASSERT(fix[i].x() > fix[i-1].x() || (wrapped==1));
    }
    return true;
}