initial (buggy) C++ version of Delaunay triangulation

samples/cpp/delaunay2.cpp

@@ -0,0 +1,955 @@
+#include <opencv2/opencv.hpp>
+#include <iostream>
+
+namespace cv
+{
+
+class CV_EXPORTS_W Subdiv2D
+{
+public:
+    
+    enum
+    {
+        PTLOC_ERROR = -2,
+        PTLOC_OUTSIDE_RECT = -1,
+        PTLOC_INSIDE = 0,
+        PTLOC_VERTEX = 1,
+        PTLOC_ON_EDGE = 2
+    };
+    
+    enum
+    {
+        NEXT_AROUND_ORG   = 0x00,
+        NEXT_AROUND_DST   = 0x22,
+        PREV_AROUND_ORG   = 0x11,
+        PREV_AROUND_DST   = 0x33,
+        NEXT_AROUND_LEFT  = 0x13,
+        NEXT_AROUND_RIGHT = 0x31,
+        PREV_AROUND_LEFT  = 0x20,
+        PREV_AROUND_RIGHT = 0x02
+    };
+    
+    CV_WRAP Subdiv2D();
+    CV_WRAP Subdiv2D(Rect rect);
+    CV_WRAP void initDelaunay(Rect rect);
+    
+    CV_WRAP int insert(Point2f pt);
+    CV_WRAP void insert(const vector<Point2f>& ptvec);
+    CV_WRAP int locate(Point2f pt, CV_OUT int& edge, CV_OUT int& vertex);
+    
+    CV_WRAP int findNearest(Point2f pt, CV_OUT Point2f* nearestPt=0);
+    CV_WRAP void getTriangleList(CV_OUT vector<Vec6f>& triangleList);
+    CV_WRAP void getVoronoiFacetList(const vector<int>& idx, CV_OUT vector<vector<Point2f> >& facetList);
+    
+    CV_WRAP Point2f getVertex(int vertex, CV_OUT int* firstEdge=0) const;
+    
+    CV_WRAP int getEdge( int edge, int nextEdgeType ) const;
+    CV_WRAP int nextEdge(int edge) const;
+    CV_WRAP int rotateEdge(int edge, int rotate) const;
+    CV_WRAP int symEdge(int edge) const;
+    CV_WRAP int edgeOrg(int edge, CV_OUT Point2f* orgpt=0) const;
+    CV_WRAP int edgeDst(int edge, CV_OUT Point2f* dstpt=0) const;
+    
+protected:
+    int newEdge();
+    void deleteEdge(int edge);
+    int newPoint(Point2f pt, bool isvirtual, int firstEdge=0);
+    void deletePoint(int vtx);
+    void setEdgePoints( int edge, int orgPt, int dstPt );
+    void splice( int edgeA, int edgeB );
+    int connectEdges( int edgeA, int edgeB );
+    void swapEdges( int edge );
+    int isRightOf(Point2f pt, int edge) const;
+    void calcVoronoi();
+    void clearVoronoi();
+    
+    struct CV_EXPORTS Vertex
+    {
+        Vertex();
+        Vertex(Point2f pt, bool _isvirtual, int _firstEdge=0);
+        bool isvirtual() const;
+        bool isfree() const;
+        int firstEdge;
+        int type;
+        Point2f pt;
+    };
+    struct CV_EXPORTS QuadEdge
+    {
+        QuadEdge();
+        QuadEdge(int edgeidx);
+        bool isfree() const;
+        int next[4];
+        int pt[4];
+    };
+    
+    vector<Vertex> vtx;
+    vector<QuadEdge> qedges;
+    int freeQEdge;
+    int freePoint;
+    bool validGeometry;
+    
+    int recentEdge;
+    Point2f topLeft;
+    Point2f bottomRight;
+};
+
+
+int Subdiv2D::nextEdge(int edge) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    return qedges[edge >> 2].next[edge & 3];
+}
+
+int Subdiv2D::rotateEdge(int edge, int rotate) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    return (edge & ~3) + ((edge + rotate) & 3);
+}
+
+int Subdiv2D::symEdge(int edge) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    return edge ^ 2;
+}
+
+int Subdiv2D::getEdge(int edge, int nextEdgeType) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    edge = qedges[edge >> 2].next[(edge + nextEdgeType) & 3];
+    return (edge & ~3) + ((edge + (nextEdgeType >> 4)) & 3);
+}
+    
+int Subdiv2D::edgeOrg(int edge, CV_OUT Point2f* orgpt) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    int vidx = qedges[edge >> 2].pt[edge & 3];
+    if( orgpt )
+    {
+        CV_DbgAssert((size_t)vidx < vtx.size());
+        *orgpt = vtx[vidx].pt;
+    }
+    return vidx;
+}
+    
+int Subdiv2D::edgeDst(int edge, CV_OUT Point2f* dstpt) const
+{
+    CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
+    int vidx = qedges[edge >> 2].pt[(edge + 2) & 3];
+    if( dstpt )
+    {
+        CV_DbgAssert((size_t)vidx < vtx.size());
+        *dstpt = vtx[vidx].pt;
+    }
+    return vidx;
+}
+
+    
+Point2f Subdiv2D::getVertex(int vertex, CV_OUT int* firstEdge) const
+{
+    CV_DbgAssert((size_t)vertex < vtx.size());
+    if( firstEdge )
+        *firstEdge = vtx[vertex].firstEdge;
+    return vtx[vertex].pt;
+}
+    
+    
+Subdiv2D::Subdiv2D()
+{
+    validGeometry = false;
+    freeQEdge = 0;
+    freePoint = 0;
+    recentEdge = 0;
+}
+    
+Subdiv2D::Subdiv2D(Rect rect)
+{
+    validGeometry = false;
+    freeQEdge = 0;
+    freePoint = 0;
+    recentEdge = 0;
+    
+    initDelaunay(rect);
+}
+    
+
+Subdiv2D::QuadEdge::QuadEdge()
+{
+    next[0] = next[1] = next[2] = next[3] = 0;
+    pt[0] = pt[1] = pt[2] = pt[3] = 0;
+}
+    
+Subdiv2D::QuadEdge::QuadEdge(int edgeidx)
+{
+    next[0] = edgeidx;
+    next[1] = edgeidx+3;
+    next[2] = edgeidx+2;
+    next[3] = edgeidx+1;
+    
+    pt[0] = pt[1] = pt[2] = pt[3] = 0;
+}
+
+bool Subdiv2D::QuadEdge::isfree() const
+{
+    return next[0] <= 0;
+}
+    
+Subdiv2D::Vertex::Vertex()
+{
+    firstEdge = 0;
+    type = -1;
+}
+    
+Subdiv2D::Vertex::Vertex(Point2f _pt, bool _isvirtual, int _firstEdge)
+{
+    firstEdge = _firstEdge;
+    type = (int)_isvirtual;
+    pt = _pt;
+}
+
+bool Subdiv2D::Vertex::isvirtual() const
+{
+    return type > 0;
+}
+    
+bool Subdiv2D::Vertex::isfree() const
+{
+    return firstEdge <= 0;
+}
+    
+void Subdiv2D::splice( int edgeA, int edgeB )
+{
+    int& a_next = qedges[edgeA >> 2].next[edgeA & 3];
+    int& b_next = qedges[edgeB >> 2].next[edgeB & 3];
+    int a_rot = rotateEdge(a_next, 1);
+    int b_rot = rotateEdge(b_next, 1);
+    int& a_rot_next = qedges[a_rot >> 2].next[a_rot & 3];
+    int& b_rot_next = qedges[b_rot >> 2].next[b_rot & 3];
+    std::swap(a_next, b_next);
+    std::swap(a_rot_next, b_rot_next);
+}
+
+void Subdiv2D::setEdgePoints(int edge, int orgPt, int dstPt)
+{
+    qedges[edge >> 2].pt[edge & 3] = orgPt;
+    qedges[edge >> 2].pt[(edge + 2) & 3] = dstPt;
+}
+    
+int Subdiv2D::connectEdges( int edgeA, int edgeB )
+{
+    int edge = newEdge();
+    
+    splice(edge, getEdge(edgeA, NEXT_AROUND_LEFT));
+    splice(symEdge(edge), edgeB);
+    
+    setEdgePoints(edge, edgeDst(edgeA), edgeOrg(edgeB));
+    return edge;
+}
+    
+void Subdiv2D::swapEdges( int edge )
+{
+    int sedge = symEdge(edge);
+    int a = getEdge(edge, PREV_AROUND_ORG);
+    int b = getEdge(sedge, PREV_AROUND_ORG);
+    
+    splice(edge, a);
+    splice(sedge, b);
+    
+    setEdgePoints(edge, edgeDst(a), edgeDst(b));
+
+    splice(edge, getEdge(a, NEXT_AROUND_LEFT));
+    splice(sedge, getEdge(b, NEXT_AROUND_LEFT));
+}
+   
+int Subdiv2D::isRightOf(Point2f pt, int edge) const
+{
+    Point2f org, dst;
+    edgeOrg(edge, &org);
+    edgeDst(edge, &dst);
+    double cw_area = cvTriangleArea( pt, dst, org );
+    
+    return (cw_area > 0) - (cw_area < 0);
+}
+
+    
+int Subdiv2D::newEdge()
+{
+    if( freeQEdge == 0 )
+    {
+        qedges.push_back(QuadEdge());
+        freeQEdge = (int)(qedges.size()-1);
+    }
+    int edge = freeQEdge*4;
+    freeQEdge = qedges[edge >> 2].next[1];
+    qedges[edge >> 2] = QuadEdge(edge);
+    return edge;
+}
+    
+void Subdiv2D::deleteEdge(int edge)
+{
+    CV_DbgAssert((size_t)(edge >> 2) < (size_t)qedges.size());
+    splice( edge, getEdge(edge, PREV_AROUND_ORG) );
+    int sedge = symEdge(edge);
+    splice(sedge, getEdge(sedge, PREV_AROUND_ORG) );
+
+    edge >>= 2;
+    qedges[edge].next[0] = -1;
+    qedges[edge].next[1] = freeQEdge;
+    freeQEdge = edge;
+}
+    
+int Subdiv2D::newPoint(Point2f pt, bool isvirtual, int firstEdge)
+{
+    if( freePoint == 0 )
+    {
+        vtx.push_back(Vertex());
+        freePoint = (int)(vtx.size()-1);
+        vtx[freePoint].type = -1;
+        vtx[freePoint].firstEdge = 0;
+    }
+    int vidx = freePoint;
+    freePoint = vtx[vidx].firstEdge;
+    vtx[vidx] = Vertex(pt, isvirtual, firstEdge);
+    
+    return vidx;
+}
+
+void Subdiv2D::deletePoint(int vidx)
+{
+    CV_DbgAssert( (size_t)vidx < vtx.size() );
+    vtx[vidx].firstEdge = freePoint;
+    vtx[vidx].type = -1;
+    freePoint = vidx;
+}
+    
+int Subdiv2D::locate(Point2f pt, int& _edge, int& _vertex)
+{
+    int vertex = 0;
+        
+    int i, maxEdges = (int)(qedges.size() * 4);
+    int edge = recentEdge;
+    
+    CV_Assert(edge > 0);
+    
+    if( pt.x < topLeft.x || pt.y < topLeft.y || pt.x >= bottomRight.x || pt.y >= bottomRight.y )
+        CV_Error( CV_StsOutOfRange, "" );
+    
+    int location = PTLOC_ERROR;
+    
+    int right_of_curr = isRightOf(pt, edge);
+    if( right_of_curr > 0 )
+    {
+        edge = symEdge(edge);
+        right_of_curr = -right_of_curr;
+    }
+    
+    for( i = 0; i < maxEdges; i++ )
+    {
+        int onext_edge = nextEdge( edge );
+        int dprev_edge = getEdge( edge, PREV_AROUND_DST );
+        
+        int right_of_onext = isRightOf( pt, onext_edge );
+        int right_of_dprev = isRightOf( pt, dprev_edge );
+        
+        if( right_of_dprev > 0 )
+        {
+            if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
+            {
+                location = PTLOC_INSIDE;
+                break;
+            }
+            else
+            {
+                right_of_curr = right_of_onext;
+                edge = onext_edge;
+            }
+        }
+        else
+        {
+            if( right_of_onext > 0 )
+            {
+                if( right_of_dprev == 0 && right_of_curr == 0 )
+                {
+                    location = PTLOC_INSIDE;
+                    break;
+                }
+                else
+                {
+                    right_of_curr = right_of_dprev;
+                    edge = dprev_edge;
+                }
+            }
+            else if( right_of_curr == 0 &&
+                    isRightOf( vtx[edgeDst(onext_edge)].pt, edge ) >= 0 )
+            {
+                edge = symEdge( edge );
+            }
+            else
+            {
+                right_of_curr = right_of_onext;
+                edge = onext_edge;
+            }
+        }
+    }
+    
+    recentEdge = edge;
+    
+    if( location == PTLOC_INSIDE )
+    {
+        Point2f org_pt, dst_pt;
+        edgeOrg(edge, &org_pt);
+        edgeDst(edge, &dst_pt);
+        
+        double t1 = fabs( pt.x - org_pt.x );
+        t1 += fabs( pt.y - org_pt.y );
+        double t2 = fabs( pt.x - dst_pt.x );
+        t2 += fabs( pt.y - dst_pt.y );
+        double t3 = fabs( org_pt.x - dst_pt.x );
+        t3 += fabs( org_pt.y - dst_pt.y );
+        
+        if( t1 < FLT_EPSILON )
+        {
+            location = PTLOC_VERTEX;
+            vertex = edgeOrg( edge );
+            edge = 0;
+        }
+        else if( t2 < FLT_EPSILON )
+        {
+            location = PTLOC_VERTEX;
+            vertex = edgeDst( edge );
+            edge = 0;
+        }
+        else if( (t1 < t3 || t2 < t3) &&
+                fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
+        {
+            location = PTLOC_ON_EDGE;
+            vertex = 0;
+        }
+    }
+    
+    if( location == PTLOC_ERROR )
+    {
+        edge = 0;
+        vertex = 0;
+    }
+    
+    _edge = edge;
+    _vertex = vertex;
+    
+    return location;
+}
+    
+    
+inline bool
+isPtInCircle3( Point2f pt, Point2f a, Point2f b, Point2f c)
+{
+    const double eps = FLT_EPSILON*0.125;
+    double val = ((double)a.x * a.x + (double)a.y * a.y) * cvTriangleArea( b, c, pt );
+    val -= ((double)b.x * b.x + (double)b.y * b.y) * cvTriangleArea( a, c, pt );
+    val += ((double)c.x * c.x + (double)c.y * c.y) * cvTriangleArea( a, b, pt );
+    val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * cvTriangleArea( a, b, c );
+    
+    return val > eps ? 1 : val < -eps ? -1 : 0;
+}
+    
+
+int Subdiv2D::insert(Point2f pt)
+{
+    int curr_point = 0, curr_edge = 0, deleted_edge = 0;
+    int location = locate( pt, curr_edge, curr_point );
+    
+    if( location == PTLOC_ERROR )
+        CV_Error( CV_StsBadSize, "" );
+        
+    if( location == PTLOC_OUTSIDE_RECT )
+        CV_Error( CV_StsOutOfRange, "" );
+    
+    if( location == PTLOC_VERTEX )
+        return curr_point;
+    
+    if( location == PTLOC_ON_EDGE )
+    {
+        deleted_edge = curr_edge;
+        recentEdge = curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
+        deleteEdge(deleted_edge);
+    }
+    else if( location == PTLOC_INSIDE )
+        ;
+    else
+        CV_Error_(CV_StsError, ("Subdiv2D::locate returned invalid location = %d", location) );
+        
+    assert( curr_edge != 0 );
+    validGeometry = false;
+    
+    curr_point = newPoint(pt, false);
+    int base_edge = newEdge();
+    int first_point = edgeOrg(curr_edge);
+    setEdgePoints(base_edge, first_point, curr_point);
+    splice(base_edge, curr_edge);
+    
+    do
+    {
+        base_edge = connectEdges( curr_edge, symEdge(base_edge) );
+        curr_edge = getEdge(base_edge, PREV_AROUND_ORG);
+    }
+    while( edgeDst(curr_edge) != first_point );
+    
+    curr_edge = getEdge( base_edge, PREV_AROUND_ORG );
+    
+    int i, max_edges = qedges.size()*4;
+    
+    for( i = 0; i < max_edges; i++ )
+    {
+        int temp_dst = 0, curr_org = 0, curr_dst = 0;
+        int temp_edge = getEdge( curr_edge, PREV_AROUND_ORG );
+        
+        temp_dst = edgeDst( temp_edge );
+        curr_org = edgeOrg( curr_edge );
+        curr_dst = edgeDst( curr_edge );
+        
+        if( isRightOf( vtx[temp_dst].pt, curr_edge ) > 0 &&
+            isPtInCircle3( vtx[curr_org].pt, vtx[temp_dst].pt,
+                           vtx[curr_dst].pt, vtx[curr_point].pt ) < 0 )
+        {
+            swapEdges( curr_edge );
+            curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
+        }
+        else if( curr_org == first_point )
+            break;
+        else
+            curr_edge = getEdge( nextEdge( curr_edge ), PREV_AROUND_LEFT );
+    }
+    
+    return curr_point;
+}
+
+void Subdiv2D::insert(const vector<Point2f>& ptvec)
+{
+    for( size_t i = 0; i < ptvec.size(); i++ )
+        insert(ptvec[i]);
+}
+    
+void Subdiv2D::initDelaunay( Rect rect )
+{
+    float big_coord = 3.f * MAX( rect.width, rect.height );
+    float rx = (float)rect.x;
+    float ry = (float)rect.y;
+    
+    vtx.clear();
+    qedges.clear();
+    
+    recentEdge = 0;
+    validGeometry = false;
+    
+    topLeft = Point2f( rx, ry );
+    bottomRight = Point2f( rx + rect.width, ry + rect.height );
+    
+    Point2f ppA( rx + big_coord, ry );
+    Point2f ppB( rx, ry + big_coord );
+    Point2f ppC( rx - big_coord, ry - big_coord );
+    
+    vtx.push_back(Vertex());
+    qedges.push_back(QuadEdge());
+    
+    freeQEdge = 0;
+    freePoint = 0;
+    
+    int pA = newPoint(ppA, false);
+    int pB = newPoint(ppB, false);
+    int pC = newPoint(ppC, false);
+    
+    int edge_AB = newEdge();
+    int edge_BC = newEdge();
+    int edge_CA = newEdge();
+    
+    setEdgePoints( edge_AB, pA, pB );
+    setEdgePoints( edge_BC, pB, pC );
+    setEdgePoints( edge_CA, pC, pA );
+    
+    splice( edge_AB, symEdge( edge_CA ));
+    splice( edge_BC, symEdge( edge_AB ));
+    splice( edge_CA, symEdge( edge_BC ));
+    
+    recentEdge = edge_AB;
+}
+
+
+void Subdiv2D::clearVoronoi()
+{
+    size_t i, total = qedges.size();
+    
+    for( i = 0; i < total; i++ )
+        qedges[i].pt[1] = qedges[i].pt[3] = 0;
+    
+    total = vtx.size();
+    for( i = 0; i < total; i++ )
+    {
+        if( vtx[i].isvirtual() )
+            deletePoint((int)i);
+    }
+    
+    validGeometry = false;
+}
+
+    
+static Point2f computeVoronoiPoint(Point2f org0, Point2f dst0, Point2f org1, Point2f dst1)
+{
+    double a0 = dst0.x - org0.x;
+    double b0 = dst0.y - org0.y;
+    double c0 = -0.5*(a0 * (dst0.x + org0.x) + b0 * (dst0.y + org0.y));
+    
+    double a1 = dst1.x - org1.x;
+    double b1 = dst1.y - org1.y;
+    double c1 = -0.5*(a1 * (dst1.x + org1.x) + b1 * (dst1.y + org1.y));
+    
+    double det = a0 * b1 - a1 * b0;
+    
+    if( det != 0 )
+    {
+        det = 1. / det;
+        return Point2f((float) ((b0 * c1 - b1 * c0) * det),
+                       (float) ((a1 * c0 - a0 * c1) * det));
+    }
+    
+    return Point2f(FLT_MAX, FLT_MAX);
+}
+    
+
+void Subdiv2D::calcVoronoi()
+{
+    // check if it is already calculated
+    if( validGeometry )
+        return;
+    
+    clearVoronoi();
+    int i, total = (int)qedges.size();
+    
+    // loop through all quad-edges, except for the first 3 (#1, #2, #3 - 0 is reserved for "NULL" pointer)
+    for( i = 4; i < total; i++ )
+    {
+        QuadEdge& quadedge = qedges[i];
+        
+        if( quadedge.isfree() )
+            continue;
+        
+        int edge0 = (int)(i*4);
+        Point2f org0, dst0, org1, dst1;
+        
+        if( !quadedge.pt[3] )
+        {
+            int edge1 = getEdge( edge0, NEXT_AROUND_LEFT );
+            int edge2 = getEdge( edge1, NEXT_AROUND_LEFT );
+            
+            edgeOrg(edge0, &org0);
+            edgeDst(edge0, &dst0);
+            edgeOrg(edge1, &org1);
+            edgeDst(edge1, &dst1);
+            
+            Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
+            
+            if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
+                fabs( virt_point.y ) < FLT_MAX * 0.5 )
+            {
+                quadedge.pt[3] = qedges[edge1 >> 2].pt[3 - (edge1 & 2)] =
+                    qedges[edge2 >> 2].pt[3 - (edge2 & 2)] = newPoint(virt_point, true);
+            }
+        }
+        
+        if( !quadedge.pt[1] )
+        {
+            int edge1 = getEdge( edge0, NEXT_AROUND_RIGHT );
+            int edge2 = getEdge( edge1, NEXT_AROUND_RIGHT );
+            
+            edgeOrg(edge0, &org0);
+            edgeDst(edge0, &dst0);
+            edgeOrg(edge1, &org1);
+            edgeDst(edge1, &dst1);
+            
+            Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
+            
+            if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
+                fabs( virt_point.y ) < FLT_MAX * 0.5 )
+            {
+                quadedge.pt[1] = qedges[edge1 >> 2].pt[1 + (edge1 & 2)] = 
+                qedges[edge2 >> 2].pt[1 + (edge2 & 2)] = newPoint(virt_point, true);
+            }
+        }
+    }
+    
+    validGeometry = true;
+}
+
+
+static int
+isRightOf2( const Point2f& pt, const Point2f& org, const Point2f& diff )
+{
+    double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
+    return (cw_area > 0) - (cw_area < 0);
+}
+
+
+int Subdiv2D::findNearest(Point2f pt, Point2f* nearestPt)
+{
+    if( !validGeometry )
+        calcVoronoi();
+    
+    int vertex = 0, edge = 0;
+    int loc = locate( pt, edge, vertex );
+    
+    if( loc != PTLOC_ON_EDGE && loc != PTLOC_INSIDE )
+        return vertex;
+    
+    vertex = 0;
+    
+    Point2f start;
+    edgeOrg(edge, &start);
+    Point2f diff = pt - start;
+    
+    edge = rotateEdge(edge, 1);
+    
+    int i, total = (int)vtx.size();
+    
+    for( i = 0; i < total; i++ )
+    {
+        Point2f t;
+        
+        for(;;)
+        {
+            CV_Assert( edgeDst(edge, &t) > 0 );
+            if( isRightOf2( t, start, diff ) >= 0 )
+                break;
+            
+            edge = getEdge( edge, NEXT_AROUND_LEFT );
+        }
+        
+        for(;;)
+        {
+            CV_Assert( edgeOrg( edge, &t ) > 0 );
+            
+            if( isRightOf2( t, start, diff ) < 0 )
+                break;
+            
+            edge = getEdge( edge, PREV_AROUND_LEFT );
+        }
+        
+        Point2f tempDiff;
+        edgeDst(edge, &tempDiff);
+        edgeOrg(edge, &t);
+        tempDiff -= t;
+        
+        if( isRightOf2( pt, t, tempDiff ) >= 0 )
+        {
+            vertex = edgeOrg(rotateEdge( edge, 3 ));
+            break;
+        }
+        
+        edge = symEdge( edge );
+    }
+    
+    if( nearestPt && vertex > 0 )
+        *nearestPt = vtx[vertex].pt;
+    
+    return vertex;
+}
+
+void Subdiv2D::getTriangleList(vector<Vec6f>& triangleList)
+{
+    vector<bool> processed(vtx.size(), false);
+    processed[0] = true;
+    
+    calcVoronoi();
+    triangleList.clear();
+    
+    for( size_t i = 4; i < qedges.size(); i++ )
+    {
+        if( qedges[i].isfree() )
+            continue;
+        int e0 = (int)(i*4), e1 = rotateEdge(e0, 1), e;
+        int vidx0 = edgeOrg(e1), vidx1 = edgeDst(e1);
+        Point2f a, b, c;
+        if( !processed[vidx0] )
+        {
+            edgeOrg(e0, &a);
+            edgeDst(e0, &b);
+            e = getEdge(e0, NEXT_AROUND_LEFT);
+            edgeDst(e, &c);
+            triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
+            processed[vidx0] = true;
+        }
+        if( !processed[vidx1] )
+        {
+            edgeDst(e0, &a);
+            edgeOrg(e0, &b);
+            e = getEdge(e0, PREV_AROUND_RIGHT);
+            edgeOrg(e, &c);
+            triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
+            processed[vidx1] = true;
+        }
+    }
+}
+
+void Subdiv2D::getVoronoiFacetList(const vector<int>& idx, CV_OUT vector<vector<Point2f> >& facetList)
+{
+    calcVoronoi();
+    facetList.clear();
+    
+    vector<Point2f> buf;
+    
+    size_t i, total;
+    if( idx.empty() )
+        i = 4, total = vtx.size();
+    else
+        i = 0, total = idx.size();
+    
+    for( ; i < total; i++ )
+    {
+        int k = idx.empty() ? (int)i : idx[i];
+        
+        if( vtx[k].isvirtual() )
+            continue;
+        int edge = rotateEdge(vtx[k].firstEdge, 1), t = edge;       
+
+        // gather points
+        buf.clear();
+        do
+        {
+            buf.push_back(vtx[edgeOrg(t)].pt);
+            t = getEdge( t, NEXT_AROUND_LEFT );
+        }
+        while( t != edge );
+        
+        facetList.push_back(buf);
+    }
+}
+        
+}
+
+using namespace cv;
+using namespace std;
+
+static void help()
+{
+	cout << "\nThis program demostrates iterative construction of\n"
+           "delaunay triangulation and voronoi tesselation.\n"
+           "It draws a random set of points in an image and then delaunay triangulates them.\n"
+           "Usage: \n"
+           "./delaunay \n"
+           "\nThis program builds the traingulation interactively, you may stop this process by\n"
+           "hitting any key.\n";
+}
+
+
+static void draw_subdiv_point( Mat& img, Point2f fp, Scalar color )
+{
+    circle( img, fp, 3, color, CV_FILLED, 8, 0 );
+}
+
+static void draw_subdiv( Mat& img, Subdiv2D& subdiv, Scalar delaunay_color )
+{
+    vector<Vec6f> triangleList;
+    subdiv.getTriangleList(triangleList);
+    vector<Point> pt(3);
+    
+    for( size_t i = 0; i < triangleList.size(); i++ )
+    {
+        Vec6f t = triangleList[i];
+        pt[0] = Point(cvRound(t[0]), cvRound(t[1]));
+        pt[1] = Point(cvRound(t[2]), cvRound(t[3]));
+        pt[2] = Point(cvRound(t[4]), cvRound(t[5]));
+        line(img, pt[0], pt[1], delaunay_color, 1, CV_AA, 0);
+        line(img, pt[1], pt[2], delaunay_color, 1, CV_AA, 0);
+        line(img, pt[2], pt[0], delaunay_color, 1, CV_AA, 0);
+    }
+}
+
+static void locate_point( Mat& img, Subdiv2D& subdiv, Point2f fp, Scalar active_color )
+{
+    int e0=0, vertex=0;
+    
+    subdiv.locate(fp, e0, vertex);
+    
+    if( e0 > 0 )
+    {
+        int e = e0;
+        do
+        {
+            Point2f org, dst;
+            if( subdiv.edgeOrg(e, &org) > 0 && subdiv.edgeDst(e, &dst) > 0 )
+                line( img, org, dst, active_color, 3, CV_AA, 0 );
+                
+            e = subdiv.getEdge(e, Subdiv2D::NEXT_AROUND_LEFT);
+        }
+        while( e != e0 );
+    }
+    
+    draw_subdiv_point( img, fp, active_color );
+}
+
+
+void paint_voronoi( Mat& img, Subdiv2D& subdiv )
+{
+    vector<vector<Point2f> > facets;
+    subdiv.getVoronoiFacetList(vector<int>(), facets);
+    
+    vector<Point> ifacet;
+    vector<vector<Point> > ifacets(1);
+    
+    for( size_t i = 0; i < facets.size(); i++ )
+    {
+        ifacet.resize(facets[i].size());
+        for( size_t j = 0; j < facets[i].size(); j++ )
+            ifacet[j] = facets[i][j];
+    
+        Scalar color;
+        color[0] = rand() & 256;
+        color[1] = rand() & 256;
+        color[2] = rand() & 256;
+        fillConvexPoly(img, ifacet, color, 8, 0);
+        
+        ifacets[0] = ifacet;
+        polylines(img, ifacets, true, Scalar(), 1, CV_AA, 0);
+    }
+}
+
+
+int main( int, char** )
+{
+    help();
+    
+    Scalar active_facet_color(0, 0, 255), delaunay_color(255,255,255);
+    Rect rect(0, 0, 600, 600);
+    
+    Subdiv2D subdiv(rect);
+    Mat img(rect.size(), CV_8UC3);
+    
+    img = Scalar::all(0);
+    string win = "Delaunay Demo";
+    imshow(win, img);
+    
+    for( int i = 0; i < 200; i++ )
+    {
+        Point2f fp( (float)(rand()%(rect.width-10)+5),
+                    (float)(rand()%(rect.height-10)+5));
+        
+        locate_point( img, subdiv, fp, active_facet_color );
+        imshow( win, img );
+        
+        if( waitKey( 100 ) >= 0 )
+            break;
+        
+        subdiv.insert(fp);
+        img = Scalar::all(0);
+        draw_subdiv( img, subdiv, delaunay_color );
+        imshow( win, img );
+        
+        if( waitKey( 100 ) >= 0 )
+            break;
+    }
+    
+    img = Scalar::all(0);
+    paint_voronoi( img, subdiv );
+    imshow( win, img );
+        
+    waitKey(0);
+    
+    return 0;
+}