shortest_paths.cpp

Go to the documentation of this file.

#include <shortest_paths.h>
#include <exception>
#include <fs_path.h>

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

PolygonShortestPathsFS::assert_hook1 PolygonShortestPathsFS::after_target_found = nullptr;
PolygonShortestPathsFS::assert_hook2 PolygonShortestPathsFS::after_placement_added = nullptr;


PolygonShortestPaths::PolygonShortestPaths(Triangulation& atri)
    : tri(atri), funnel(atri.size())
{ }

PolygonShortestPathsFS::PolygonShortestPathsFS(Triangulation& atri)
    : PolygonShortestPaths(atri),
      source(nullptr), d(), epsilon(NAN), validPlacements(nullptr)
{ }

void PolygonShortestPathsFS::findShortestPaths(
        Vertex_handle start_point,
        const frechet::reach::Placement* asource,
        QLineF d, double epsilon,
        frechet::reach::Graph::ptr validPlacements)
{
    this->source = asource;
    this->d = d;
    this->epsilon = epsilon;
    this->validPlacements = validPlacements;

    PolygonShortestPaths::findShortestPaths(start_point);

    this->source = nullptr;
    this->validPlacements.reset();
}

frechet::Curve PolygonShortestPaths::find1ShortestPath(double p1, double p2)
{
    int size_before = tri.size();

    frechet::reach::Placement pl2;
    pl2.fs.lower()=pl2.fs.upper()=pl2.w = p2;

    Triangulation::Vertex_handle v1 = tri.addVertex(p1);
    Triangulation::Vertex_handle v2 = tri.addTargetVertex(&pl2);

    findShortestPaths(v1);

    Curve result;
    for(Triangulation::Vertex* v=&*v2; v; v=v->prev)
        result.prepend(*v);

    tri.resize(size_before);
    return result;
}

void PolygonShortestPaths::findShortestPaths(Vertex_handle start_point)
{
    //  reset all prev pointers
    tri.clearPredecessors();
    funnel.reset(start_point);

    Face_circulator f0 (start_point,start_point->face());
    Face_circulator f = f0;
    do {
        Q_ASSERT(f!=Face_handle());
        //std::cout << *f << std::endl;
        if (tri.is_inner_face(f))
            search(start_point,f);
    } while(++f != f0);

    funnel.undo();
    Q_ASSERT(funnel.empty());
}

bool PolygonShortestPaths::updateSPSegment(Vertex_handle prev, Vertex_handle v)
{
    if (v->prev==&*prev)
        return false;    //  already set

    Q_ASSERT(v->prev==nullptr);
    v->prev = &*prev;
    return true;
}

//int iteration=0;

bool PolygonShortestPathsFS::updateSPSegment(Vertex_handle prev, Vertex_handle v)
{
    if (! PolygonShortestPaths::updateSPSegment(prev,v))
        return false;

    Q_ASSERT(this->source!=nullptr);
    Q_ASSERT(v->prev);

    bool unreachabe = v->prev->prev && !v->prev->LR && !v->prev->BR;
//  if (unreachabe) return;

    /*  Update Free-Space Intervals */
    //  TODO float128
    frechet::FreeSpace::calculateFreeSpaceSegment<accurate_float>(d.p1(), *v->prev, *v, epsilon, v->L);
    frechet::FreeSpace::calculateFreeSpaceSegment<accurate_float>(d.p2(), *v->prev, *v, epsilon, v->R);
    frechet::FreeSpace::calculateFreeSpaceSegment<accurate_float>(*v->prev, d.p1(), d.p2(), epsilon, v->B);
    frechet::FreeSpace::fixBorderCase(
            &v->L, &v->B,
            v->prev->prev ? &v->prev->L : nullptr, nullptr);
    frechet::FreeSpace::fixBorderCase(
            &v->R, nullptr,
            v->prev->prev ? &v->prev->R : nullptr, &v->B);

    /*  Update Reachability */
    if (v->prev->prev==nullptr)
        initReachability(v);
    else
        propagateReachability(v);

    bool reachable=false;
    if (v->target) {
        Q_ASSERT(v->target->fs.lower() >= 0.0);
        Q_ASSERT(v->target->fs.upper() >= v->target->fs.lower());
        //  This is a Placement target point
        //  Point is reachable -> this is a 'valid' placement
        Interval T, TR, TR2;
        FreeSpace::calculateFreeSpaceSegment<accurate_float>(*v, d.p1(), d.p2(), epsilon, T);
        FreeSpace::fixBorderCase(nullptr,&T,&v->L,nullptr);
        FreeSpace::fixBorderCase(nullptr,nullptr,&v->R,&T);

        if (v->LR || !T)
            TR = T;
        else if (v->BR)
            TR = frechet::reach::FSPath::opposite(v->BR,T);
/* TODO mirrored reachability
        if (v->RR2 || !T)
            TR2 = T;
        else if (v->BR2)
            TR2 = frechet::reach::FSPath::opposite_min(v->BR2,T);
*/
        //ASSERT_EQ(TR.contains(1.0), v->RR.contains(1.0));
        //  There is one border case, where
        //  v->RR = empty, TR = [x..1]
        //  So 1.0 is only reachable through 1 single point.

        reachable = v->RR.contains(1.0) || TR.contains(1.0);
// TODO        reachable2 = v->LR2.contains(1.0) || TR2.contains(0.0);
    }

    if (reachable) {
        validPlacements->add_range(source->gn, v->target->gn);
        //  add_valid_range(source->gn, v->target->gn);
//        Q_ASSERT(validPlacements->contains_edge(source->gn, v->target->gn));
        //  debug hook:
        Q_ASSERT(!after_placement_added || (after_placement_added(this,source,v->target),true));
    }
/*  TODO
    if (reachable2) {
        validPlacements2->add_range(flipped(source->gn) ??, v->target->gn);
    }
*/
    //  debug hook:
    Q_ASSERT(!after_target_found || (after_target_found(this,v,reachable),true));
    return true;
}

void PolygonShortestPathsFS::initReachability(Vertex_handle v)
{
    //  assumes v->L,B,R is calculated
    //  calculate reachable intervals starting at (0,0)
    bool lr = v->L.contains(0.0);
    bool br = v->B.contains(0.0);

    Q_ASSERT(lr == br);
    if (lr) {
        Q_ASSERT(br);
        v->LR = v->L;
        v->BR = v->B;
        v->RR = v->R;
    }
    else {
        Q_ASSERT(!br && !lr);
        v->LR.clear();
        v->BR.clear();
        v->RR.clear();
    }

    //  TODO mirrorerd reachability
/*
    bool rr = v->R.contains(0.0);
    br = v->B.contains(1.0);

    Q_ASSERT(rr==br);
    if (rr) {
        Q_ASSERT(br);
        v->LR2 = v->L;
        v->BR2 = v->B;
        v->RR2 = v->R;
    }
    else {
        v->LR2.clear();
        v->BR2.clear();
        v->RR2.clear();
    }
*/
}

void PolygonShortestPathsFS::propagateReachability(Vertex_handle v)
{
    if (v->prev->LR.contains(1.0) && v->L.contains(0.0))
        v->LR = v->L;
    else
        v->LR.clear();

    if (v->prev->LR)
        v->BR = v->B;
    else if (v->prev->BR && v->B)
        v->BR = frechet::reach::FSPath::opposite(v->prev->BR,v->B);
    else
        v->BR.clear();

    if (v->BR || (v->prev->RR.contains(1.0) && v->R.contains(0.0)))
        v->RR = v->R;
    else if (v->LR && v->R)
        v->RR = frechet::reach::FSPath::opposite(v->LR,v->R);
    else
        v->RR.clear();

    //  TODO mirrored reachability
/*
    if (v->prev->RR2.contains(1.0) && v->R.contains(0.0))
        v->RR2 = v->R;
    else
        v->RR2.clear();

    if (v->prev->RR2)
        v->BR2 = v->B;
    else if (v->prev->BR2 && v->B)
        v->BR2 = frechet::reach::FSPath::opposite_min(v->prev->BR2,v->B); // TODO flipping required??
     else
        v->BR2.clear();

     if (v->BR2 || (v->prev->LR2.contains(1.0) && v->L.contains(0.0)))     // TODO ??
         v->LR2 = v->L;
     else if (v->LR2 && v->R)   //  TODO ??
         v->LR2 = frechet::reach::FSPath::opposite(v->LR2,v->R);
     else
         v->LR2.clear();
*/
}

void PolygonShortestPaths::search(Vertex_handle start_point, Face_handle f)
{
    int i = f->index(start_point);
    Edge e(f,i);

    Q_ASSERT(funnel.apex()==start_point);
    Vertex_handle v1 = f->vertex(f->ccw(i));
    Vertex_handle v2 = f->vertex(f->cw(i));

    updateSPSegment(funnel.apex(), v1);
    updateSPSegment(funnel.apex(), v2);

    if (! tri.is_polygon_edge(e))
    {
        funnel.addLeft(v1);
        funnel.addRight(v2);
        search(e);
        funnel.undo();
        funnel.undo();
    }
}

void printFunnel(const PolygonShortestPaths::Funnel& funnel)
{
    for(int i=-funnel.leftSize(); i <= funnel.rightSize(); ++i) {
        if (i==0) std::cout << " apex=";
        //std::cout << funnel[i]->index << " ";
    }
    //std::cout << std::endl;
}

void PolygonShortestPaths::search(Edge e)
{
    //  candidate point is opposite e
    Vertex_handle v = tri.oppositeVertex(e);
    int t  = findTangent(v);

    updateSPSegment(funnel[t], v);

    //  next Diagonals
    Face_handle f1 = e.first;
    int i = e.second;

    Face_handle f2 = f1->neighbor(i);
    Edge d1 (f2,f2->index(funnel.leftEnd()));
    Edge d2 (f2,f2->index(funnel.rightEnd()));

    if (false/*v->index>=643*/) {
        //std::cout << v->index << "," << v->prev->index << ", " << tri.segment(e) << std::endl;
        printFunnel(funnel);
        //std::cout << f2 << std::endl;
        //std::cout << tri.segment(d1) << std::endl;
        //std::cout << tri.segment(d2) << std::endl;
    }
    Q_ASSERT(tri.segment(d1)==Segment(funnel.rightEnd()->pindex,v->pindex));
    Q_ASSERT(tri.segment(d2)==Segment(funnel.leftEnd()->pindex,v->pindex));

    if (!tri.is_polygon_edge(d1))
    {   //  Right sub-funnel
        funnel.removeLeftUpto(t);
        funnel.addLeft(v);
        search(d1);
        funnel.undo();
        funnel.undo();
    }

    if (!tri.is_polygon_edge(d2))
    {   //  Left sub-funnel
        funnel.removeRightUpto(t);
        funnel.addRight(v);
        search(d2);
        funnel.undo();
        funnel.undo();
    }
}

bool PolygonShortestPaths::isTangent(int t, Vertex_handle v) const
{
    Q_ASSERT(t!=0);
    Q_ASSERT(t >= -funnel.leftSize());
    Q_ASSERT(t <= funnel.rightSize());

    if (t > 0) {
        //  right funnel: v is on the LEFT of [ab]
        Vertex_handle a = funnel[t-1];
        Vertex_handle b = funnel[t];
        int hpt = halfplaneTest<accurate_float>(a,b,v);
        return hpt < 0;
    }
    else {
        Q_ASSERT(t < 0);
        Vertex_handle a = funnel[t+1];
        Vertex_handle b = funnel[t];
        int hpt = halfplaneTest<accurate_float>(a,b,v);
        return hpt > 0;
    }
}

int PolygonShortestPaths::findTangent(Vertex_handle v) const
{
    Q_ASSERT((assertFunnel(),true));
    //  we expect a consistent, non-trivial funnel

    //  dovetailed doubling search from left and right
    int left = funnel.leftSize();
    int right = funnel.rightSize();

    if (left>0 && isTangent(-left,v))
        return -left;
    if (right>0 && isTangent(right,v))
        return right;

    for(int incr = 1; (incr < left) || (incr < right); incr*=2)
    {
        if ((incr < left) && isTangent(-left+incr,v))
            return findTangentLeft(-left,-left+incr,v);
        //  TODO findTangentLeft(-left+incr/2,-left+incr,v);

        if ((incr < right) && isTangent(right-incr,v))
            return findTangentRight(right-incr,right,v);
        //  TODO findTangentRight(right-incr,right-incr/2,v);
    }
    if (left>0 && isTangent(-1,v))
        return findTangentLeft(-left,-1,v);
    if (right>0 && isTangent(+1,v))
        return findTangentRight(+1,right,v);

    //  if there is no tangent, split at apex:
    return 0;
}

int PolygonShortestPaths::findTangentLeft(int t1, int t2, Vertex_handle v) const
{
    //  perform binary search
    //  for max. tangent
    Q_ASSERT(t1 < 0);
    Q_ASSERT(t2 < 0);
    Q_ASSERT(t1 < t2);
    Q_ASSERT(!isTangent(t1,v));
    Q_ASSERT( isTangent(t2,v));

    while ((t1+1) < t2) {
        int t3 = (t1+t2)/2;
        if (isTangent(t3,v))
            t2=t3;
        else
            t1=t3;
    }
    return t2;
}

int PolygonShortestPaths::findTangentRight(int t1, int t2, Vertex_handle v) const
{
    //  perform binary search
    //  for max. tangent
    Q_ASSERT(t1 > 0);
    Q_ASSERT(t2 > 0);
    Q_ASSERT(t1 < t2);
    Q_ASSERT( isTangent(t1,v));
    Q_ASSERT(!isTangent(t2,v));

    while ((t1+1) < t2) {
        int t3 = (t1+t2)/2;
        if (isTangent(t3,v))
            t1=t3;
        else
            t2=t3;
    }
    return t1;
}

void PolygonShortestPaths::assertFunnel() const
{
    Q_ASSERT(funnel.size() >= 1);

    if (funnel.rightSize() >= 1) {
        Q_ASSERT(funnel[1]->prev == &*funnel[0]);
    }
    for(int t=2; t <= funnel.rightSize(); ++t)
    {
        Q_ASSERT(funnel[t]->prev == &*funnel[t-1]);
        Q_ASSERT(funnel[t-1]->prev == &*funnel[t-2]);
        Q_ASSERT(isTangent(t-1, funnel[t]));
    }
    if (funnel.leftSize() >= 1) {
        if(funnel[-1]->prev != &*funnel[0])
            throw std::exception();
        Q_ASSERT(funnel[-1]->prev == &*funnel[0]);
    }
    for(int t=2; t <= funnel.leftSize(); ++t)
    {
        Q_ASSERT(funnel[-t]->prev == &*funnel[-t+1]);
        Q_ASSERT(funnel[-t+1]->prev == &*funnel[-t+2]);
        Q_ASSERT(isTangent(-t+1,funnel[-t]));
    }
}