triangulation.cpp

Go to the documentation of this file.

#include <triangulation.h>
#include <frechet/poly/triangulation.h>
#include <frechet/poly/shortest_paths.h>

using namespace frechet;
using namespace poly;

Triangulation::Triangulation(int capacity)
    : tds(), vertices(), neighbors(), poly_n(-1)
{
    if (capacity > 0)
        vertices.reserve(capacity);
    tds.set_dimension(2);
    south_pole = tds.create_vertex(Vertex_base<TDS>(Point(NAN,NAN),SOUTH_POLE_INDEX));
}

Triangulation::Triangulation(const Triangulation& that)
: tds(that.tds), vertices(), neighbors(), poly_n(that.poly_n)
{
    //  deep copy vertices
    south_pole=Vertex_handle();
    vertices.resize(that.vertices.size());
    auto vi = tds.vertices_begin();
    auto vend = tds.vertices_end();
    for( ; vi != vend; ++vi) {
        vi->prev=nullptr;
        if (vi->pindex==SOUTH_POLE_INDEX)
            south_pole = vi;
        else if (vi->pindex >= 0)
            vertices[vi->pindex] = vi;
    }
    Q_ASSERT((south_pole!=Vertex_handle()) && south_pole->pindex==SOUTH_POLE_INDEX);

    //  DON'T deep copy neigbors (and edges)

    Q_ASSERT(assertEquals(that));
}

Triangulation::~Triangulation() 
{
    vertices.clear();
    neighbors.clear();
    tds.clear();
}


bool Triangulation::is_south_pole(Vertex_handle v)
{
    return v->pindex==SOUTH_POLE_INDEX;
}

bool Triangulation::is_outer_face(Face_handle f)
{
    return is_south_pole(f->vertex(0));
}

bool Triangulation::is_inner_face(Face_handle f)
{
    return ! is_outer_face(f);
}

bool Triangulation::is_outer_edge(Edge e)
{
    return is_outer_face(e.first) && (e.second!=0);
}

bool Triangulation::is_polygon_edge(Edge e)
{
    return is_outer_face(e.first)
        != is_outer_face(e.first->neighbor(e.second));
}

bool Triangulation::is_diagonal(Edge e)
{
    return is_inner_face(e.first)
            && is_inner_face(e.first->neighbor(e.second));
}

void Triangulation::mirror(Edge& e) const {
    e = tds.mirror_edge(e);
}

void Triangulation::mirror(EdgePair& e2) const {
    mirror(e2.first);
    mirror(e2.second);
}

void Triangulation::addTargetVertices(const frechet::reach::Placements& ps)
{
    for(const frechet::reach::Placement& p : ps)
        addTargetVertex(&p);
}

Triangulation::Vertex_handle Triangulation::addVertex(double w)
{
    Vertex_handle v;
    int i = floor(w);
    if (i == w) { //  existing vertex
        Q_ASSERT(i>=0 && i < vertices.size());
        return vertices[i];
    }
    else {
        return insertVertexOnEdge(w);
    }
}

Triangulation::Vertex_handle Triangulation::addTargetVertex(const frechet::reach::Placement* p)
{
    Vertex_handle v = addVertex(p->w);

    Q_ASSERT(p->fs.lower() >= 0);
    Q_ASSERT(p->fs.upper() >= p->fs.lower());
    v->target = p;
    return v;
}

void Triangulation::setPolygonSize(int n)
{
    this->poly_n = n;
}

int Triangulation::size() const
{
    return vertices.size();
}

void Triangulation::resize(int sz)
{
    Q_ASSERT(sz >= 0);
    if (sz < size()) {
        for (int i=vertices.size()-1; i >= sz; --i)
            removeVertexFromEdge(i);
        vertices.resize(sz);
    }
    else {
        int i = size();
        vertices.resize(sz);
        for( ; i < sz; ++i)
            vertices[i] = Vertex_handle();
    }
    //Q_ASSERT(tds.is_valid());
}



Triangulation::Vertex_handle Triangulation::createVertex(const frechet::Point &p, int i)
{
    Q_ASSERT(i >= 0);
    Vertex_handle v = (*this)[i];
    if (v == 0) {
        if (i >= vertices.size())
            resize(i+1);
        vertices[i] = v = tds.create_vertex(Vertex_base<TDS>(p,i));
    }
    return v;
}

Triangulation::Vertex_handle Triangulation::insertVertexOnEdge(double x)
{

    //  (1) start at i = floor(x). We know that there is a polygon vertex at (i+1).
    //      find the adjacent south-pole face
    int i=floor(x);
    Vertex_handle v1 = (*this)[i];

    Edge e;
    bool ok = tds.is_edge(south_pole,v1, e.first,e.second);
    Q_ASSERT(ok);

    Face_handle f;
    if (e.second==2) {
        //  wrong direction
        f = e.first->neighbor(2);
    }
    else {
        f = e.first;
    }

    //  (2) find the vertex with index < x < index+1
    Vertex_handle v2 = f->vertex(1);
    while((v2->offset != 0.0) && (v2->offset < x)) {
        //  advance
        f = f->neighbor(2);
        v2 = f->vertex(1);
    }

    //  (3) insert new vertex
    Vertex_handle v = tds.insert_in_edge(f,0);
    Point p =  numeric::between(*v1,*v2,x-i);
    addVertex(v, x, p);

    Q_ASSERT(tds.is_valid());
    return v;
}

Segment Triangulation::segment(Triangulation::Edge e)
{
    Vertex_handle v1 = e.first->vertex((e.second+1)%3);
    Vertex_handle v2 = e.first->vertex((e.second+2)%3);
    return Segment(v1->pindex,v2->pindex);
}

void Triangulation::removeVertexFromEdge(int index)
{

    Vertex_handle v1 = (*this)[index];
    Face_circulator f (v1);
    while(is_inner_face(f)) ++f;
    Q_ASSERT(is_outer_face(f));
    //std::cout << v1->index << "," << f << std::endl;

    //Face_handle f2 = f->neighbor(1);
    Edge e (f,0);
    Vertex_base<TDS> v2 = *third_vertex(f,v1,south_pole);

    Vertex_handle v = tds.join_vertices(e);
    //  keep payload data from v2
    v->setData(v2);
    //v->set_face(f2);    //  right?

    vertices[v->pindex] = v;
    vertices[index]=Vertex_handle();

    Q_ASSERT(v->is_valid());
    Q_ASSERT(v->face()->is_valid());
    Q_ASSERT(v->face()->has_vertex(v));
    Q_ASSERT(tds.is_valid());
}


void Triangulation::addVertex(Vertex_handle v, double x, Point p)
{
    vertices.resize(size()+1);
    vertices[size()-1] = v;
    v->setData(p,size()-1,-1,x);
}

Triangulation::Vertex_handle Triangulation::operator[](int i)
{
    Q_ASSERT(i >= 0);
    if (i >= vertices.size())
        return Vertex_handle();
    else
        return vertices[i];
}

const Triangulation::Vertex_handle Triangulation::operator[](int i) const
{
    Q_ASSERT(i >= 0);
    if (i >= vertices.size())
        return Vertex_handle();
    else
        return vertices[i];
}

Triangulation::EdgePair Triangulation::adjacentEdges(Triangulation::Edge e) const
{
    Q_ASSERT(e.second!=SOUTH_POLE_INDEX);
    Q_ASSERT(is_inner_face(e.first));

    if (!is_inner_face(e.first))
        e = tds.mirror_edge(e);

    return EdgePair(Edge(e.first,e.first->ccw(e.second)),
                    Edge(e.first,e.first->cw(e.second)));
}

Triangulation::EdgePair Triangulation::adjacentEdges(Edge e, int i1, int i2) const
{
    Face_handle f = e.first;
    Q_ASSERT(is_inner_face(f));
    if (!is_inner_face(f))
        f = f->neighbor(e.second);
    Q_ASSERT(is_inner_face(f));

#ifdef QT_DEBUG
    //  i1,i2 must be the incident vertices to e
    VertexPair vv = incidentVertexes(e);
    if (vv.first->pindex==i2)
        std::swap(vv.first,vv.second);
    Q_ASSERT(vv.first->pindex==i1);
    Q_ASSERT(vv.second->pindex==i2);
#endif
    Vertex_handle v1 = vertices[i1];
    Vertex_handle v2 = vertices[i2];
    return EdgePair(Edge(f,f->index(v2)),
                    Edge(f,f->index(v1)));
}

Triangulation::FacePair Triangulation::adjacentFaces(Segment s) const
{
    FacePair result;

    const Vertex_handle v1 = (*this)[s.first];
    const Vertex_handle v2 = (*this)[s.second];

    /*  TODO
     *  tds_is_edge() may need O(degree(v))
     *  later in the algorithm we want O(1)
     *
     *  speed up acces by putting all edges into a map
     */
    int i;
    if (tds.is_edge(v1,v2, result.first, i)) {
        result.second = result.first->neighbor(i);
    }
    return result;
}

Triangulation::Face_circulator Triangulation::incidentFaces(Vertex_handle v) const
{
    return tds.incident_faces(v);
}

Triangulation::Vertex_handle Triangulation::oppositeVertex(Edge e) const {
    e = tds.mirror_edge(e);
    return e.first->vertex(e.second);
}

int Triangulation::thirdVertex(int a, int b) const {
    Edge e;
    bool ok = tds.is_edge(vertices[a],vertices[b], e.first,e.second);
    Q_ASSERT(ok);
    Vertex_handle c = e.first->vertex(e.second);
    if (is_south_pole(c)) {
        e = tds.mirror_edge(e);
        c = e.first->vertex(e.second);
    }
    Q_ASSERT(!is_south_pole(c));
    return c->pindex;
}

Triangulation::Edge Triangulation::edge(int i, int j) const
{
    Vertex_handle vi = vertices[i];
    Vertex_handle vj = vertices[j];
    Edge e;
    bool ok = tds.is_edge(vi,vj, e.first,e.second);
    Q_ASSERT(ok);
    if (e.second==SOUTH_POLE_INDEX)
        e = tds.mirror_edge(e);
    Q_ASSERT(e.second!=SOUTH_POLE_INDEX);
    Q_ASSERT(is_inner_face(e.first));
    return e;
}

Triangulation::VertexPair Triangulation::incidentVertexes(Edge e) const
{
    int i1 = e.first->ccw(e.second);
    int i2 = e.first->cw(e.second);
    return VertexPair(e.first->vertex(i1),
                       e.first->vertex(i2));
}

void Triangulation::clearPredecessors() {
    auto v = tds.vertices_begin();
    auto vend = tds.vertices_end();
    for( ; v != vend; ++v)
        v->prev = nullptr;
}

void Triangulation::clearTargets() {
    auto v = tds.vertices_begin();
    auto vend = tds.vertices_end();
    for( ; v != vend; ++v)
        v->target = nullptr;
}

Triangulation::TwoVertices Triangulation::adjacentVertices(Segment s) const
{
    const Vertex_handle v1 = (*this)[s.first];
    const Vertex_handle v2 = (*this)[s.second];

    TwoVertices result;
    Edge e;
    if (tds.is_edge(v1,v2, e.first,e.second))
    {
        result.first = e.first->vertex(e.second);
        e = tds.mirror_edge(e);
        result.second = e.first->vertex(e.second);
    }

    return result;
}

Triangulation::TwoVertices Triangulation::adjacentVertices(int i) const
{
    return adjacentVertices(Segment(i,SOUTH_POLE_INDEX));
}

void Triangulation::createFace(int a, int b, int c)
{
    int count_before=countFaces();

    Vertex_handle va = (a==SOUTH_POLE_INDEX) ? south_pole : (*this)[a];
    Vertex_handle vb = (*this)[b];
    Vertex_handle vc = (*this)[c];

    TDS::Face_handle f = tds.create_face(va,vb,vc);

    //Q_ASSERT(tds.dimension()==2);
    Q_ASSERT(f != TDS::Face_handle());

    Segment sa(a,b);
    Segment sb(b,c);
    Segment sc(c,a);

    linkNeighbors(sa, Edge(f,2));
    linkNeighbors(sb, Edge(f,0));
    linkNeighbors(sc, Edge(f,1));

    if (va->face()==Face_handle()) va->set_face(f);
    if (vb->face()==Face_handle()) vb->set_face(f);
    if (vc->face()==Face_handle()) vc->set_face(f);

    Q_ASSERT(countFaces()==count_before+1);
}


void Triangulation::complement()
{
    if (neighbors.empty()) return;

    //  smallest vertex index
    int v0 = neighbors.begin()->first.first;
    //  how often have we seen the first edge?
    int first_edge=0;
    while(! neighbors.empty()) {
        auto n = neighbors.end();
        --n;
        Segment s = n->first;
        Q_ASSERT(s.first>=0);
        Q_ASSERT(s.second>=0);
        int v1 = s.first;
        int v2 = s.second;
        //  keep faces ALWAYS counter-clockwise
        //  special care needs to be taken when the first vertex is encountered
        //  sorting of the map ensures, that we encounter the [v0..]
        //  segment first
        if (v1==v0) {
            if (first_edge!=0) {
                //  wrap-around
                std::swap(v1,v2);
            }
            ++first_edge;
            Q_ASSERT(first_edge <= 2);
        }
        createFace(SOUTH_POLE_INDEX,v2,v1);
    }
    TDS::Vertex_iterator vit = tds.vertices_begin();
    Q_ASSERT(vit==south_pole);
    Q_ASSERT(south_pole->face() != Face_handle());
    Q_ASSERT(tds.is_valid());
}

void Triangulation::linkNeighbors(Segment s, Edge e)
{
    //  diagonals are visited twice.
    //  the first time, we record the face,
    //  the second time, we create the neighborhood relations
    auto n = neighbors.find(s);
    if (n==neighbors.end()) {
        //  First visit: store reference for later
        neighbors.insert(std::make_pair(s,e));
    }
    else {
        //  Second visit: recover reference and link
        Edge e2 = n->second;
        tds.set_adjacency(e.first,e.second, e2.first,e2.second);
        neighbors.erase(n);
    }
    //  if all neighboring faces have been set, the 'neighbors' map will be empty again.
    //  @see isComplete()
}

Triangulation::Vertex_handle Triangulation::third_vertex(Face_handle f, Vertex_handle v1, Vertex_handle v2)
{
    Q_ASSERT(v1!=Vertex_handle());
    Q_ASSERT(v2!=Vertex_handle());

    Q_ASSERT(f->has_vertex(v1));
    Q_ASSERT(f->has_vertex(v2));

    for(int i=0; i < 3; ++i) {
        Vertex_handle v3 = f->vertex(i);
        if (v3!=v1 && v3!=v2)
            return v3;
    }
    Q_ASSERT(false);
    return Vertex_handle();
}

Triangulation::Vertex_handle Triangulation::third_vertex(Face_handle f, Segment s)
{
    return third_vertex(f, (*this)[s.first], (*this)[s.second]);
}

bool Triangulation::is_ccw_face(Face_handle f)
{
    //  inner faces: check embedding
    return is_inner_face(f)
        && PolygonShortestPaths::halfplaneTest<double>(f->vertex(0), f->vertex(1), f->vertex(2)) < 0
        && PolygonShortestPaths::halfplaneTest<double>(f->vertex(1), f->vertex(2), f->vertex(0)) < 0
        && PolygonShortestPaths::halfplaneTest<double>(f->vertex(2), f->vertex(0), f->vertex(1)) < 0;
}
/*
bool Triangulation::is_ccw_polygon_edge(Edge e)
{
    //  only applicable to polygon edges
    Q_ASSERT(is_polygon_edge(e));

    int j = e.first->vertex((e.second+1)%3)->pindex;
    int i = e.first->vertex((e.second+2)%3)->pindex;
    return (i+1==j) || (j==0 || i==vertices.size());
}
*/
template<class TDS>
Vertex_base<TDS>::Vertex_base() : Vertex_base(Point(),-1)  { }

template<class TDS>
Vertex_base<TDS>::Vertex_base(const frechet::Point &p, int pi) : Vertex_base(p,pi,(double)pi) { }

template<class TDS>
Vertex_base<TDS>::Vertex_base(const frechet::Point &p, int pi, double offs)
        :   Point(p), 
            CGAL::Triangulation_ds_vertex_base_2<TDS>(),
            pindex(pi), dindex(-1), offset(offs),
            prev(nullptr), target(nullptr),
            L(),R(),B(), LR(),BR(),RR()

{ }

template<class TDS>
Vertex_base<TDS>::~Vertex_base() { }

Triangulation::Edge_iterator::Edge_iterator(TDS::Edge_iterator i)
    : TDS::Edge_iterator(i)
{ }

Triangulation::Vertex_handle Triangulation::vertex1(Edge e) {
    Face_handle f = e.first;
    int i = e.second;
    return f->vertex((i+1)%3);
}

Triangulation::Vertex_handle Triangulation::vertex2(Edge e) {
    Face_handle f = e.first;
    int i = e.second;
    return f->vertex((i+2)%3);
}

Triangulation::Vertex_handle Triangulation::Edge_iterator::vertex1() const {
    return Triangulation::vertex1(operator*());
}

Triangulation::Vertex_handle Triangulation::Edge_iterator::vertex2() const {
    return Triangulation::vertex2(operator*());
}

bool Triangulation::Edge_iterator::is_outer_edge() const
{
    return Triangulation::is_outer_edge(operator*());
}

bool Triangulation::Edge_iterator::is_polygon_edge() const
{
    return Triangulation::is_polygon_edge(operator*());
}

bool Triangulation::Edge_iterator::is_diagonal() const
{
    return Triangulation::is_diagonal(operator*());
}

QLineF Triangulation::Edge_iterator::line() const {
    return QLineF(*vertex1(),*vertex2());
}

Segment Triangulation::Edge_iterator::segment() const {
    return Triangulation::segment(operator*());
}

bool Triangulation::assertEquals(const Triangulation& that) const
{
    Q_ASSERT(tds.number_of_vertices()==that.tds.number_of_vertices());
    Q_ASSERT(tds.number_of_faces()==that.tds.number_of_faces());
    Q_ASSERT(tds.number_of_edges()==that.tds.number_of_edges());

    Q_ASSERT(vertices.size()==that.vertices.size());
    for(int i=0; i < vertices.size(); ++i)
    {
        Vertex_handle v1 = vertices[i];
        Vertex_handle v2 = that.vertices[i];

        Q_ASSERT((v1!=Vertex_handle()) == (v2!=Vertex_handle()));
        Q_ASSERT((v1==Vertex_handle()) || v1->assertEquals(*v2));
    }

    auto v1i = tds.vertices_begin();
    auto v1end = tds.vertices_end();
/*
    auto v2i = that.tds.vertices_begin();
    auto v2end = that.tds.vertices_end();

    for( ; v1i!=v1end && v2i!=v2end; ++v1i,++v2i)
        Q_ASSERT(v1i->assertEquals(*v2i));
*/
    for( ; v1i != v1end; ++v1i)
        if (v1i->pindex >= 0)
            Q_ASSERT(v1i == vertices[v1i->pindex]);

    Q_ASSERT(v1i==v1end);
//    Q_ASSERT(v2i==v2end);
    return true;
}

template<typename TDS>
bool Vertex_base<TDS>::assertEquals(const Vertex_base<TDS>& that)
{
    Q_ASSERT(this->pindex == that.pindex);

    if (this->pindex==SOUTH_POLE_INDEX) return true;

    Q_ASSERT((Point)*this == (Point)that);
    Q_ASSERT(this->dindex == that.dindex);
    Q_ASSERT(this->offset == that.offset);
    return true;
}


std::ostream& frechet::poly::operator<< (std::ostream& out, const Triangulation& tri)
{
    out << "{";
    auto tbegin = tri.edges_begin();
    auto tend = tri.edges_end();
    for(auto t=tbegin; t!=tend; ++t) {
        if (t!=tbegin) out << ",";
        out << t;
    }
    return out << "}";
}

std::ostream& frechet::poly::operator<< (std::ostream& out, const Triangulation::Face_handle& f)
{
    return out << "["
            <<f->vertex(0)->pindex<<"-"
            <<f->vertex(1)->pindex<<"-"
            <<f->vertex(2)->pindex<<"]";
}

std::ostream& frechet::poly::operator<< (std::ostream& out, const Triangulation::Edge& e)
{
    if (Triangulation::is_polygon_edge(e))
        out << "P";
    else if (Triangulation::is_outer_edge(e))
        out << "O";
    else if (Triangulation::is_diagonal(e))
        out << "D";

    return out << Triangulation::vertex1(e)->pindex << "--" << Triangulation::vertex2(e)->pindex;
}

std::ostream& frechet::poly::operator<< (std::ostream& out, const Triangulation::Edge_iterator& e)
{
    return frechet::poly::operator<<(out,*e);
}