algorithm.cpp

Go to the documentation of this file.

#include <algorithm.h>
#include <poly_utils.h>
#include <poly_path.h>
#include <algorithm>
#include <shortest_paths.h>
#include <structure.h>
#include <grid.h>
#include <iostream>
#include <app/concurrency.h>
#include <app/workerthread.h>

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

Algorithm::Algorithm()
    : P(new CurveData()),
      Q(new CurveData()),
      initial_status(NOT_SET_UP),
      _status(NOT_SET_UP),
      graphModel(),
      _placements(),
      _validPlacements(),
      CRG_map(),
      cancelFlag(nullptr),
      local_fs(),
      criticalValues()
{ }

Algorithm::~Algorithm()
{
    delete P;
    delete Q;
}

const Segments& Algorithm::c_diagonals() const
{
    return P->_c_diagonals;
}

bool Algorithm::is_c_diagonal(Segment s) const {
    return c_diagonals().find(s) != c_diagonals().end();
}

bool Algorithm::is_c_diagonal(int di, int dj) const {
    return is_c_diagonal(Segment(di,dj));
}

Algorithm::CurveData::CurveData()
    :   curve(),
      partition(),
      _c_diagonals(),
      _d_points(),
      triangulation(), display_triangulation(nullptr),
      utils(new PolygonUtilities()), reflex()
{}
Algorithm::CurveData::~CurveData()
{
    delete utils;
    delete display_triangulation;
}


Algorithm::Status Algorithm::setup(frechet::Curve& aP,
                                  frechet::Curve& aQ,
                                  bool fixOrientation)
{
    P->curve = aP;
    Q->curve = aQ;

    /*
     * Ein paar Voraussetzungen
     * */

    if (P->curve.size() <= 1) return setInitialStatus(P_EMPTY);
    if (Q->curve.size() <= 1) return setInitialStatus(Q_EMPTY);

    //  (1) beide Kurven müssen geschlossen sein
    if (!P->curve.isClosed()) return setInitialStatus(P_NOT_CLOSED);
    if (!Q->curve.isClosed()) return setInitialStatus(Q_NOT_CLOSED);

    delete P->utils;
    delete Q->utils;

    P->utils = new PolygonUtilities (P->curve);
    Q->utils = new PolygonUtilities (Q->curve);

    //  (2) beide Kurven müssen einfache Polygone sein
    if (!P->utils->is_simple()) return setInitialStatus(P_NOT_SIMPLE);
    if (!Q->utils->is_simple()) return setInitialStatus(Q_NOT_SIMPLE);

    //  (3) Orientierung gegen den Uhrzeigersinn (lässt sich ggf. ändern)
    if (!P->utils->is_counter_clockwise() && fixOrientation) {
        std::reverse(aP.begin(),aP.end());
        *P->utils = PolygonUtilities(P->curve = aP);
    }
    if (!Q->utils->is_counter_clockwise() && fixOrientation) {
        std::reverse(aQ.begin(),aQ.end());
        *Q->utils = PolygonUtilities(Q->curve = aQ);
    }

    if (!P->utils->is_counter_clockwise())
        return setInitialStatus(P_NOT_COUNTER_CLOCKWISE);
    if (!Q->utils->is_counter_clockwise())
        return setInitialStatus(Q_NOT_COUNTER_CLOCKWISE);

    //  (4) wenn ein Polygon konvex ist, können wir den
    //  vereinfachten Algorithmus für Kurven anwenden
    if (P->utils->is_convex()) return setInitialStatus(P_CONVEX);
    if (Q->utils->is_convex()) return setInitialStatus(Q_CONVEX);

    return setInitialStatus(SET_UP);
}


bool Algorithm::decompose(bool optimal)
{
    //  1. in konvexe Teile zerlegen
    Q_ASSERT(optimal==false);
    P->utils->decompose(PolygonUtilities::APPROX_HERTEL, true);
    P->utils->partition(P->partition);

    Q->utils->decompose(PolygonUtilities::APPROX_HERTEL, true);
    Q->utils->partition(Q->partition);


    return (P->partitions() <= Q->partitions());
}

void Algorithm::swap()
{
    std::swap(P,Q);    
}

void Algorithm::triangulate()
{
    //  pre-calculate some stuff
    P->calculate_c_diagonals(P->_c_diagonals);

    /*  for P we need a triangulation of the simplified polygon
     *  (c- and t-diagonals, contracted edges)
     * */
    P->display_triangulation = new Triangulation();
    P->display_triangulation->setPolygonSize(P->n());
    P->utils->fanTriangulate(*P->display_triangulation);

    P->triangulation.setPolygonSize(P->n());
    int convex_parts = P->utils->simplify();
    if (convex_parts > 0) {
        P->utils->fanTriangulate(P->triangulation);
    }
    else {
        //  degenerated triangulation consisting of just two vertexes
        Q_ASSERT(P->_c_diagonals.size()==1);
        int p1 = P->_c_diagonals.begin()->first;
        int p2 = P->_c_diagonals.begin()->second;
        P->triangulation.createVertex(P->curve[p1],p1);
        P->triangulation.createVertex(P->curve[p2],p2);
    }
    P->calculate_d_points();
#ifdef QT_DEBUG
    //  All triangulation points must be d-points
    for(int i=0; i < P->triangulation.size(); ++i)
    {
        if (P->triangulation[i]!=Triangulation::Vertex_handle()) {
            Q_ASSERT(P->triangulation[i]->dindex >= 0);
        }
    }
#endif

    /*  for Q we need a complete triangulation  */
    Q->triangulation.setPolygonSize(Q->n());
    Q->utils->fanTriangulate(Q->triangulation);
    /*  Reflex vertices are used to find critical values.
     *  We allow some tolerance to find "almost" reflex vertices.
     *  Better have too many Critical Values than missing one !!
     */
    Q->utils->findReflexVertices(Q->reflex,1e-3);
    //  since Q is not convex, there must be reflex vertices
    Q_ASSERT(Q->reflex.size() >= 1);

    delete P->utils;
    P->utils = nullptr;

    delete Q->utils;
    Q->utils = nullptr;
}

int Algorithm::CurveData::calculate_c_diagonals(Segments& result)
{
    result.clear();
    for(const Polygon& p : partition)
    {
        //  c-diagonals are [even-odd] entries
        for(int i=0; i < p.size(); i += 2)
        if (p[i] < p[i+1])
        {   //  each diagonal appears exactly twice. we store it only once.
            Segment seg(p[i],p[i+1]);
            result.insert(seg);
            Q_ASSERT(seg.is_diagonal(n()));
        }
    }
    //  no need to get rid of duplicates
    //Q_ASSERT(removeDuplicates(result)==0);
    return (int)result.size();
}

int Algorithm::CurveData::calculate_d_points()
{
    std::set<int> set;

    //  partitions contains all the c-diagonals
    for(const Polygon& p : partition)
        for(int v : p)
            set.insert(v);

    _d_points.clear();
    std::copy(set.begin(),set.end(), std::back_inserter(_d_points));

    for(int i=0; i < _d_points.size(); ++i) {
        int di = _d_points[i];
        Q_ASSERT(di >= 0 && di < n());
        //  reverse link Vertex to index into _d_points
        triangulation[di]->dindex = i;
    }
    return (int) _d_points.size();
}

reach::Pointer Algorithm::decideCurve( FreeSpace::ptr afs,
                             FSPath::ptr path,
                             double,
                             bool update_status)
{
    //Q_ASSERT(!update_status);
    if (afs->n==0 || afs->m==0
     || initial_status==P_EMPTY || initial_status==Q_EMPTY) {
        if (update_status)
            setStatus(NO);
        return reach::Pointer();
    }

    if (update_status) setStatus(RUNNING_DECIDE_CURVE);
    this->fs = afs;

    int concurrency = ConcurrencyContext::countThreads();
    frechet::reach::Structure rstruct(fs,concurrency,cancelFlag);
    frechet::reach::Pointer startPointer = rstruct.calculate();
    if (path)
        updatePath(path,startPointer,NAN);

    this->fs.reset();
    if (update_status)
        setStatus(startPointer ? YES:NO);

    testCancelFlag();
    return startPointer;
}

void Algorithm::updatePath(reach::FSPath::ptr path, reach::Pointer startPointer, double epsilon)
{
    if (startPointer) {
        //  find a feasible path
        path->update(path->toPoint(startPointer),epsilon);
        //Q_ASSERT(ok);
    }
    else {
        path->clear();
    }
}

void Algorithm::testCancelFlag() {
    if (cancelFlag && *cancelFlag)
    {
        resetStatus();
        throw InterruptedException();
    }
}

Algorithm::GraphPtr Algorithm::createValidPlacementGraph(int di, int dj)
{
    //  Valid placement Graph for diagonal (di,dj)
    GraphPtr valid = newGraph(graphModel);
    valid->setOriginPlacement(di,dj);
    valid->allocate(VERTICAL,VERTICAL);
    //  Note: CGraph stores only the vertical section. Horizontal is ignored.
    _validPlacements.insert(std::make_pair(mapKey(di,dj), valid));
    return valid;
}

void Algorithm::calculateValidPlacements(
        poly::PolygonShortestPathsFS& spt,
        double epsilon, int di, int dj)
{
    int i = P->triangulation[di]->dindex;   // index into _d_points and placements. 0 <= i < l
    int j = P->triangulation[dj]->dindex;   // 0 <= j < l

    Q_ASSERT(i>=0);
    Q_ASSERT(j>=0);

    GraphPtr valid = createValidPlacementGraph(di,dj);
    //_validPlacements.find(mapKey(di,dj))->second;
    Q_ASSERT(valid);

    Triangulation& sptri = spt.triangulation();
    Q_ASSERT(sptri.assertEquals(Q->triangulation));

    int size_before1,size_before2;
    size_before1 = sptri.size();

    sptri.addTargetVertices(_placements[j]);

    static const double PRECISION=1e-9;
    auto wi = _placements[i].begin();
    auto wiend = _placements[i].end();
    for( ; wi!=wiend; ++wi)
    {
        QLineF dline (P->curve[di], P->curve [dj]);
        //  does wi coincide with on the the wj's ?
        //  (Guibas can't find it, if source==target)
        auto wj = _placements[j].begin();
        auto wjend = _placements[j].end();
        for ( ; wj != wjend; ++wj)
            if (wi->fs.intersects(wj->fs))
            {
                double w = (wi->fs & wj->fs).mid();
                Point q = Grid::mapToPoint(Q->curve, w);
                Q_ASSERT(numeric::euclidean_distance<double>(dline.p1(),q) <= epsilon+PRECISION);
                Q_ASSERT(numeric::euclidean_distance<double>(dline.p2(),q) <= epsilon+PRECISION);

                valid->add_range(wi->gn,wj->gn);
                //valid->add_valid_range(wi.gn,wj.gn);
                Q_ASSERT(!PolygonShortestPathsFS::after_placement_added
                      || (PolygonShortestPathsFS::after_placement_added(&spt, &*wi, &*wj),true));
            }

        testCancelFlag();

        //  Berechne Shortest-Path-Tree + Free-Space
        //  Speichere die erreichbaren Kombinationen in validPlacements
        size_before2 = sptri.size();

        Triangulation::Vertex_handle start = sptri.addVertex(wi->w);
        spt.findShortestPaths(start, &*wi, dline, epsilon, valid);

        sptri.resize(size_before2);
    }

    //  release empty matrices    
    valid->finalize();

    sptri.resize(size_before1);
    sptri.clearTargets();
}

Triangulation::Edge Algorithm::diagonalEdge(int i) const
{
    if (l() <= 2)
        return Triangulation::Edge();

    int n = fs->n - 1;
    int di = d(i) % n;
    int di1 = d(i - 1) % n;
    //Q_ASSERT(P->triangulation.countFaces()>0);
    return P->triangulation.edge(di, di1);
    //std::cout << e.first << " " << e.second << " " << e.first->vertex(e.second)->pindex << std::endl;
}
/*
int Algorithm::findFeasibleStart(GraphPtr G)
{
    //  Result G is confined to the HH segment (which is symmetrical)
    //  The solution is supposed to lie on the diagonal of G
//    IndexRange r = G->hmask();

    //  Note: conceptually, if the start point is at x, the end point must be at x+n,
    //  which is -through the design of G- at the same location.
    G->searchDiagonalElement();
    return G->foundDiagonalElement();
}
*/


Algorithm::Status Algorithm::setInitialStatus(Status st)
{
    initial_status = st;    //  base status of the curve. does not change later.
    return setStatus(st);
}

Algorithm::Status Algorithm::setStatus(Status st)
{
    Status old_stat = _status;
    _status = st;
    if (_status != old_stat)
        emit statusChanged();
    return _status;
}

void Algorithm::resetStatus(bool was_interrupted) {
    if (_status <= RUNNING && !was_interrupted) {
        //  exit from RUNNING status
        //  is only possibly after interruption
        return;
    }

    if (_status<=RUNNING || _status==YES || _status==NO)
    {
        //  revert to initial state
        Q_ASSERT(initial_status!=NOT_SET_UP);
        setStatus(initial_status);
        cleanup();
    }
}

Algorithm::GraphPtr Algorithm::decidePolygon(FreeSpace::ptr afs,
        FSPath::ptr path, double epsilon,
        bool update_status)
{
    /*  Precondition: free-space-diagram for epsilon computed
     * */
    switch(_status) {
        case NOT_SET_UP:
        default:
            if (_status<=RUNNING)
                { Q_ASSERT(!update_status); }
            else
                { Q_ASSERT(false); }
            break;

        case P_EMPTY:
        case Q_EMPTY:
        case P_NOT_CLOSED:
        case Q_NOT_CLOSED:
        case P_NOT_SIMPLE:
        case Q_NOT_SIMPLE:
        case P_NOT_COUNTER_CLOCKWISE:
        case Q_NOT_COUNTER_CLOCKWISE:
            //emit decidePolygonFinished();
            return GraphPtr();

        case SET_UP:
        case YES:
        case NO:
            /* expected */
            break;

        case P_CONVEX:
        case Q_CONVEX:
            Q_ASSERT(false);
            //  call decideCurve() instead
            break;
    }

    cleanup();
    if (update_status) setStatus(RUNNING_DECIDE_POLY);
    //
    this->fs = afs;

    graphModel.reset(new GraphModel());
    graphModel->init(fs);   //  TODO parallel ??
    if (graphModel->empty()) {
        //  Überhaupt keine erreichbaren Intervalle
        if (update_status) setStatus(NO);
        return GraphPtr();
    }

    printDebugTimer("GraphModel");
    if (update_status) {
        std::cout << "    Data:         " << "matrix size (VV) = " << graphModel->count2(VERTICAL) << std::endl;
    }

    /*  Steps 7-11
     *  compute and store valid placements
     */
    calculateValidPlacements(epsilon);
    printDebugTimer("Valid Placements");

    /*  Decision algorithm
     *  Steps 1-2 are already done
     *  Step 3 - a new Free-Space diagram has just been calculated
     */

    /*  Steps 4-6
     *  RG vorberechnen (optional)
     */
    calculateReachabilityGraphs();
    printTimer("RGraphs",update_status);

    //  Populate the left part of the Reachability Structure
    //  The right part is identical.
    testCancelFlag();

    /*  Steps 12-16 */
    GraphPtr result = findFeasiblePath();

    printDebugTimer("Main Loop");

    /*   Update FSPath (if requested) */
    if (path) {
        //  FSPath is very sensitive to round-off errors.
        //  Because it is only needed for visualisation, we can afford
        //  a bit of tolerance.
        updatePath(path,result, round_up(epsilon,32));
    }

    //FrechetViewApplication::instance()->popTimer("Decide Polygon");

    if (update_status)
        setStatus( result ? YES:NO );

    if (path && update_status)
        emit pathUpdated((bool)result); //  only after status has changed

#ifndef QT_DEBUG
    //  Debug mode: keep all the stuff for debug analysis
    //  Release mode: free unneeded memory.
    //  Note: that Result keeps a pointer to the solution graph, so it won't get lost.
    cleanup();
#endif
    return result;
}

void Algorithm::updatePath(reach::FSPath::ptr path, GraphPtr result, double epsilon)
{
    poly::PolygonFSPath* poly_path = dynamic_cast<poly::PolygonFSPath*> (path.get());
    Q_ASSERT(poly_path);
    if (result) {
        poly_path->update(result, epsilon);
    }
    else {
        poly_path->clear();
    }
}


void Algorithm::cleanup() {
    //Q_ASSERT(_status!=RUNNING);
    //  TODO clean up OpenCL pending jobs
    _placements.resize(0);
    _placements.resize(l());
    _validPlacements.clear();
    CRG_map.clear();
}

double Algorithm::pickIntervalPoint(const Interval& j, int m)
{
    // if possible, pick a vertex
    int ci = (int)ceil(j.lower());
    if (ci > j.upper()) {
        //  pick a non-integral point
        return j.lower();
    }
    else if (ci == m) {
        //  border case: don't pick fs->m-1, could be confused with vertex 0
        return max(j.lower(),m-1/1024.0);
    }
    else {
        //  pick integral point
        return ci;
    }
}

void Algorithm::calculatePlacements(int di, Placements& result)
{    
    std::list<Interval> ivals = fs->collectFreeVerticalIntervals(di);

    result.resize(ivals.size());
    std::list<Interval>::iterator j = ivals.begin();

    for(int i=0; i < ivals.size(); ++i,++j)
    {
        Q_ASSERT(j->lower() >= 0);
        Q_ASSERT(j->upper() >= j->lower());
        Q_ASSERT(j->upper() <= fs->m);

        Placement& p = result[i];
        p.fs = *j;
        p.w = pickIntervalPoint(*j,fs->m-1);
        Q_ASSERT(j->contains(p.w));
        Q_ASSERT(p.w!=(fs->m-1));

        //  map to GraphModel
        p.gn = graphModel->map(VERTICAL,p.fs,true);
    }
}

int Algorithm::mapKey(int di, int dj)
{
    return dj * P->n() + di;
}


Algorithm::GraphPtr Algorithm::getValidPlacements(int di, int dj)
{
    int h = mapKey(di,dj);
    auto p = _validPlacements.find(h);
    Q_ASSERT(p != _validPlacements.end());
    return p->second;
 }


Algorithm::GraphPtr Algorithm::calculate_RG(int i)
{
    //  calculate a reachability structure for column [i..j] (mod n)
    int n = fs->n-1;
    int di = d(i) % n;
    int dj1 = d(i+1) % n;
    int dj2 = (dj1 >= di) ? dj1 : (dj1+n);
    //  TODO right ?

    //  j is exclusive
    frechet::reach::Structure str(fs);
    str.calculateColumns( di, dj2 );
    //  Turn into a Graph
    //std::cout << "RG("<<i<<","<<(i+1)<<")" <<std::endl;

    //  Graph Model boundaries
    //int hcount1 = graphModel->count1(HORIZONTAL);
    int h0 = graphModel->map_lower(HORIZONTAL,di);
    int h1 = graphModel->map_lower(HORIZONTAL,dj1);
    int h2 = graphModel->map_lower(HORIZONTAL,dj2);
    int h3 = graphModel->lowerShiftedHorizontally(h1);

    Q_ASSERT((h1==h2) || (h2==h3));

    double s0 = str.bottom().first()->lower();
    double s1 = str.bottom().last()->upper();

    int H0 = graphModel->map_lower(HORIZONTAL,s0);
    int H2 = graphModel->map_lower(HORIZONTAL,s1);

    Q_ASSERT(h2==H2);

    GraphPtr result = newGraph(graphModel, str);

    Q_ASSERT(result->hmask().lower==H0);
    Q_ASSERT(result->hmask().upper==H2);

    result->setOriginRG(i % l());
    result->finalize();
    return result;
}


std::ostream& frechet::poly::operator<< (std::ostream& out, const Segment& seg)
{
    return out << "["<<seg.first<<".."<<seg.second<<"]";
}