subdivision2d.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include "precomp.hpp"
00042 
00043 namespace cv
00044 {
00045 
00046 int Subdiv2D::nextEdge(int edge) const
00047 {
00048     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
00049     return qedges[edge >> 2].next[edge & 3];
00050 }
00051 
00052 int Subdiv2D::rotateEdge(int edge, int rotate) const
00053 {
00054     return (edge & ~3) + ((edge + rotate) & 3);
00055 }
00056 
00057 int Subdiv2D::symEdge(int edge) const
00058 {
00059     return edge ^ 2;
00060 }
00061 
00062 int Subdiv2D::getEdge(int edge, int nextEdgeType) const
00063 {
00064     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
00065     edge = qedges[edge >> 2].next[(edge + nextEdgeType) & 3];
00066     return (edge & ~3) + ((edge + (nextEdgeType >> 4)) & 3);
00067 }
00068 
00069 int Subdiv2D::edgeOrg(int edge, CV_OUT Point2f* orgpt) const
00070 {
00071     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
00072     int vidx = qedges[edge >> 2].pt[edge & 3];
00073     if( orgpt )
00074     {
00075         CV_DbgAssert((size_t)vidx < vtx.size());
00076         *orgpt = vtx[vidx].pt;
00077     }
00078     return vidx;
00079 }
00080 
00081 int Subdiv2D::edgeDst(int edge, CV_OUT Point2f* dstpt) const
00082 {
00083     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
00084     int vidx = qedges[edge >> 2].pt[(edge + 2) & 3];
00085     if( dstpt )
00086     {
00087         CV_DbgAssert((size_t)vidx < vtx.size());
00088         *dstpt = vtx[vidx].pt;
00089     }
00090     return vidx;
00091 }
00092 
00093 
00094 Point2f Subdiv2D::getVertex(int vertex, CV_OUT int* firstEdge) const
00095 {
00096     CV_DbgAssert((size_t)vertex < vtx.size());
00097     if( firstEdge )
00098         *firstEdge = vtx[vertex].firstEdge;
00099     return vtx[vertex].pt;
00100 }
00101 
00102 
00103 Subdiv2D::Subdiv2D()
00104 {
00105     validGeometry = false;
00106     freeQEdge = 0;
00107     freePoint = 0;
00108     recentEdge = 0;
00109 }
00110 
00111 Subdiv2D::Subdiv2D(Rect rect)
00112 {
00113     validGeometry = false;
00114     freeQEdge = 0;
00115     freePoint = 0;
00116     recentEdge = 0;
00117 
00118     initDelaunay(rect);
00119 }
00120 
00121 
00122 Subdiv2D::QuadEdge::QuadEdge()
00123 {
00124     next[0] = next[1] = next[2] = next[3] = 0;
00125     pt[0] = pt[1] = pt[2] = pt[3] = 0;
00126 }
00127 
00128 Subdiv2D::QuadEdge::QuadEdge(int edgeidx)
00129 {
00130     CV_DbgAssert((edgeidx & 3) == 0);
00131     next[0] = edgeidx;
00132     next[1] = edgeidx+3;
00133     next[2] = edgeidx+2;
00134     next[3] = edgeidx+1;
00135 
00136     pt[0] = pt[1] = pt[2] = pt[3] = 0;
00137 }
00138 
00139 bool Subdiv2D::QuadEdge::isfree() const
00140 {
00141     return next[0] <= 0;
00142 }
00143 
00144 Subdiv2D::Vertex::Vertex()
00145 {
00146     firstEdge = 0;
00147     type = -1;
00148 }
00149 
00150 Subdiv2D::Vertex::Vertex(Point2f _pt, bool _isvirtual, int _firstEdge)
00151 {
00152     firstEdge = _firstEdge;
00153     type = (int)_isvirtual;
00154     pt = _pt;
00155 }
00156 
00157 bool Subdiv2D::Vertex::isvirtual() const
00158 {
00159     return type > 0;
00160 }
00161 
00162 bool Subdiv2D::Vertex::isfree() const
00163 {
00164     return type < 0;
00165 }
00166 
00167 void Subdiv2D::splice( int edgeA, int edgeB )
00168 {
00169     int& a_next = qedges[edgeA >> 2].next[edgeA & 3];
00170     int& b_next = qedges[edgeB >> 2].next[edgeB & 3];
00171     int a_rot = rotateEdge(a_next, 1);
00172     int b_rot = rotateEdge(b_next, 1);
00173     int& a_rot_next = qedges[a_rot >> 2].next[a_rot & 3];
00174     int& b_rot_next = qedges[b_rot >> 2].next[b_rot & 3];
00175     std::swap(a_next, b_next);
00176     std::swap(a_rot_next, b_rot_next);
00177 }
00178 
00179 void Subdiv2D::setEdgePoints(int edge, int orgPt, int dstPt)
00180 {
00181     qedges[edge >> 2].pt[edge & 3] = orgPt;
00182     qedges[edge >> 2].pt[(edge + 2) & 3] = dstPt;
00183     vtx[orgPt].firstEdge = edge;
00184     vtx[dstPt].firstEdge = edge ^ 2;
00185 }
00186 
00187 int Subdiv2D::connectEdges( int edgeA, int edgeB )
00188 {
00189     int edge = newEdge();
00190 
00191     splice(edge, getEdge(edgeA, NEXT_AROUND_LEFT));
00192     splice(symEdge(edge), edgeB);
00193 
00194     setEdgePoints(edge, edgeDst(edgeA), edgeOrg(edgeB));
00195     return edge;
00196 }
00197 
00198 void Subdiv2D::swapEdges( int edge )
00199 {
00200     int sedge = symEdge(edge);
00201     int a = getEdge(edge, PREV_AROUND_ORG);
00202     int b = getEdge(sedge, PREV_AROUND_ORG);
00203 
00204     splice(edge, a);
00205     splice(sedge, b);
00206 
00207     setEdgePoints(edge, edgeDst(a), edgeDst(b));
00208 
00209     splice(edge, getEdge(a, NEXT_AROUND_LEFT));
00210     splice(sedge, getEdge(b, NEXT_AROUND_LEFT));
00211 }
00212 
00213 static double triangleArea( Point2f a, Point2f b, Point2f c )
00214 {
00215     return ((double)b.x - a.x) * ((double)c.y - a.y) - ((double)b.y - a.y) * ((double)c.x - a.x);
00216 }
00217 
00218 int Subdiv2D::isRightOf(Point2f pt, int edge) const
00219 {
00220     Point2f org, dst;
00221     edgeOrg(edge, &org);
00222     edgeDst(edge, &dst);
00223     double cw_area = triangleArea( pt, dst, org );
00224 
00225     return (cw_area > 0) - (cw_area < 0);
00226 }
00227 
00228 int Subdiv2D::newEdge()
00229 {
00230     if( freeQEdge <= 0 )
00231     {
00232         qedges.push_back(QuadEdge());
00233         freeQEdge = (int)(qedges.size()-1);
00234     }
00235     int edge = freeQEdge*4;
00236     freeQEdge = qedges[edge >> 2].next[1];
00237     qedges[edge >> 2] = QuadEdge(edge);
00238     return edge;
00239 }
00240 
00241 void Subdiv2D::deleteEdge(int edge)
00242 {
00243     CV_DbgAssert((size_t)(edge >> 2) < (size_t)qedges.size());
00244     splice( edge, getEdge(edge, PREV_AROUND_ORG) );
00245     int sedge = symEdge(edge);
00246     splice(sedge, getEdge(sedge, PREV_AROUND_ORG) );
00247 
00248     edge >>= 2;
00249     qedges[edge].next[0] = 0;
00250     qedges[edge].next[1] = freeQEdge;
00251     freeQEdge = edge;
00252 }
00253 
00254 int Subdiv2D::newPoint(Point2f pt, bool isvirtual, int firstEdge)
00255 {
00256     if( freePoint == 0 )
00257     {
00258         vtx.push_back(Vertex());
00259         freePoint = (int)(vtx.size()-1);
00260     }
00261     int vidx = freePoint;
00262     freePoint = vtx[vidx].firstEdge;
00263     vtx[vidx] = Vertex(pt, isvirtual, firstEdge);
00264 
00265     return vidx;
00266 }
00267 
00268 void Subdiv2D::deletePoint(int vidx)
00269 {
00270     CV_DbgAssert( (size_t)vidx < vtx.size() );
00271     vtx[vidx].firstEdge = freePoint;
00272     vtx[vidx].type = -1;
00273     freePoint = vidx;
00274 }
00275 
00276 int Subdiv2D::locate(Point2f pt, int& _edge, int& _vertex)
00277 {
00278     int vertex = 0;
00279 
00280     int i, maxEdges = (int)(qedges.size() * 4);
00281 
00282     if( qedges.size() < (size_t)4 )
00283         CV_Error( CV_StsError, "Subdivision is empty" );
00284 
00285     if( pt.x < topLeft.x || pt.y < topLeft.y || pt.x >= bottomRight.x || pt.y >= bottomRight.y )
00286         CV_Error( CV_StsOutOfRange, "" );
00287 
00288     int edge = recentEdge;
00289     CV_Assert(edge > 0);
00290 
00291     int location = PTLOC_ERROR;
00292 
00293     int right_of_curr = isRightOf(pt, edge);
00294     if( right_of_curr > 0 )
00295     {
00296         edge = symEdge(edge);
00297         right_of_curr = -right_of_curr;
00298     }
00299 
00300     for( i = 0; i < maxEdges; i++ )
00301     {
00302         int onext_edge = nextEdge( edge );
00303         int dprev_edge = getEdge( edge, PREV_AROUND_DST );
00304 
00305         int right_of_onext = isRightOf( pt, onext_edge );
00306         int right_of_dprev = isRightOf( pt, dprev_edge );
00307 
00308         if( right_of_dprev > 0 )
00309         {
00310             if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
00311             {
00312                 location = PTLOC_INSIDE;
00313                 break;
00314             }
00315             else
00316             {
00317                 right_of_curr = right_of_onext;
00318                 edge = onext_edge;
00319             }
00320         }
00321         else
00322         {
00323             if( right_of_onext > 0 )
00324             {
00325                 if( right_of_dprev == 0 && right_of_curr == 0 )
00326                 {
00327                     location = PTLOC_INSIDE;
00328                     break;
00329                 }
00330                 else
00331                 {
00332                     right_of_curr = right_of_dprev;
00333                     edge = dprev_edge;
00334                 }
00335             }
00336             else if( right_of_curr == 0 &&
00337                     isRightOf( vtx[edgeDst(onext_edge)].pt, edge ) >= 0 )
00338             {
00339                 edge = symEdge( edge );
00340             }
00341             else
00342             {
00343                 right_of_curr = right_of_onext;
00344                 edge = onext_edge;
00345             }
00346         }
00347     }
00348 
00349     recentEdge = edge;
00350 
00351     if( location == PTLOC_INSIDE )
00352     {
00353         Point2f org_pt, dst_pt;
00354         edgeOrg(edge, &org_pt);
00355         edgeDst(edge, &dst_pt);
00356 
00357         double t1 = fabs( pt.x - org_pt.x );
00358         t1 += fabs( pt.y - org_pt.y );
00359         double t2 = fabs( pt.x - dst_pt.x );
00360         t2 += fabs( pt.y - dst_pt.y );
00361         double t3 = fabs( org_pt.x - dst_pt.x );
00362         t3 += fabs( org_pt.y - dst_pt.y );
00363 
00364         if( t1 < FLT_EPSILON )
00365         {
00366             location = PTLOC_VERTEX;
00367             vertex = edgeOrg( edge );
00368             edge = 0;
00369         }
00370         else if( t2 < FLT_EPSILON )
00371         {
00372             location = PTLOC_VERTEX;
00373             vertex = edgeDst( edge );
00374             edge = 0;
00375         }
00376         else if( (t1 < t3 || t2 < t3) &&
00377                 fabs( triangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
00378         {
00379             location = PTLOC_ON_EDGE;
00380             vertex = 0;
00381         }
00382     }
00383 
00384     if( location == PTLOC_ERROR )
00385     {
00386         edge = 0;
00387         vertex = 0;
00388     }
00389 
00390     _edge = edge;
00391     _vertex = vertex;
00392 
00393     return location;
00394 }
00395 
00396 
00397 inline int
00398 isPtInCircle3( Point2f pt, Point2f a, Point2f b, Point2f c)
00399 {
00400     const double eps = FLT_EPSILON*0.125;
00401     double val = ((double)a.x * a.x + (double)a.y * a.y) * triangleArea( b, c, pt );
00402     val -= ((double)b.x * b.x + (double)b.y * b.y) * triangleArea( a, c, pt );
00403     val += ((double)c.x * c.x + (double)c.y * c.y) * triangleArea( a, b, pt );
00404     val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * triangleArea( a, b, c );
00405 
00406     return val > eps ? 1 : val < -eps ? -1 : 0;
00407 }
00408 
00409 
00410 int Subdiv2D::insert(Point2f pt)
00411 {
00412     int curr_point = 0, curr_edge = 0, deleted_edge = 0;
00413     int location = locate( pt, curr_edge, curr_point );
00414 
00415     if( location == PTLOC_ERROR )
00416         CV_Error( CV_StsBadSize, "" );
00417 
00418     if( location == PTLOC_OUTSIDE_RECT )
00419         CV_Error( CV_StsOutOfRange, "" );
00420 
00421     if( location == PTLOC_VERTEX )
00422         return curr_point;
00423 
00424     if( location == PTLOC_ON_EDGE )
00425     {
00426         deleted_edge = curr_edge;
00427         recentEdge = curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
00428         deleteEdge(deleted_edge);
00429     }
00430     else if( location == PTLOC_INSIDE )
00431         ;
00432     else
00433         CV_Error_(CV_StsError, ("Subdiv2D::locate returned invalid location = %d", location) );
00434 
00435     assert( curr_edge != 0 );
00436     validGeometry = false;
00437 
00438     curr_point = newPoint(pt, false);
00439     int base_edge = newEdge();
00440     int first_point = edgeOrg(curr_edge);
00441     setEdgePoints(base_edge, first_point, curr_point);
00442     splice(base_edge, curr_edge);
00443 
00444     do
00445     {
00446         base_edge = connectEdges( curr_edge, symEdge(base_edge) );
00447         curr_edge = getEdge(base_edge, PREV_AROUND_ORG);
00448     }
00449     while( edgeDst(curr_edge) != first_point );
00450 
00451     curr_edge = getEdge( base_edge, PREV_AROUND_ORG );
00452 
00453     int i, max_edges = (int)(qedges.size()*4);
00454 
00455     for( i = 0; i < max_edges; i++ )
00456     {
00457         int temp_dst = 0, curr_org = 0, curr_dst = 0;
00458         int temp_edge = getEdge( curr_edge, PREV_AROUND_ORG );
00459 
00460         temp_dst = edgeDst( temp_edge );
00461         curr_org = edgeOrg( curr_edge );
00462         curr_dst = edgeDst( curr_edge );
00463 
00464         if( isRightOf( vtx[temp_dst].pt, curr_edge ) > 0 &&
00465            isPtInCircle3( vtx[curr_org].pt, vtx[temp_dst].pt,
00466                          vtx[curr_dst].pt, vtx[curr_point].pt ) < 0 )
00467         {
00468             swapEdges( curr_edge );
00469             curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
00470         }
00471         else if( curr_org == first_point )
00472             break;
00473         else
00474             curr_edge = getEdge( nextEdge( curr_edge ), PREV_AROUND_LEFT );
00475     }
00476 
00477     return curr_point;
00478 }
00479 
00480 void Subdiv2D::insert(const std::vector<Point2f>& ptvec)
00481 {
00482     for( size_t i = 0; i < ptvec.size(); i++ )
00483         insert(ptvec[i]);
00484 }
00485 
00486 void Subdiv2D::initDelaunay( Rect rect )
00487 {
00488     float big_coord = 3.f * MAX( rect.width, rect.height );
00489     float rx = (float)rect.x;
00490     float ry = (float)rect.y;
00491 
00492     vtx.clear();
00493     qedges.clear();
00494 
00495     recentEdge = 0;
00496     validGeometry = false;
00497 
00498     topLeft = Point2f( rx, ry );
00499     bottomRight = Point2f( rx + rect.width, ry + rect.height );
00500 
00501     Point2f ppA( rx + big_coord, ry );
00502     Point2f ppB( rx, ry + big_coord );
00503     Point2f ppC( rx - big_coord, ry - big_coord );
00504 
00505     vtx.push_back(Vertex());
00506     qedges.push_back(QuadEdge());
00507 
00508     freeQEdge = 0;
00509     freePoint = 0;
00510 
00511     int pA = newPoint(ppA, false);
00512     int pB = newPoint(ppB, false);
00513     int pC = newPoint(ppC, false);
00514 
00515     int edge_AB = newEdge();
00516     int edge_BC = newEdge();
00517     int edge_CA = newEdge();
00518 
00519     setEdgePoints( edge_AB, pA, pB );
00520     setEdgePoints( edge_BC, pB, pC );
00521     setEdgePoints( edge_CA, pC, pA );
00522 
00523     splice( edge_AB, symEdge( edge_CA ));
00524     splice( edge_BC, symEdge( edge_AB ));
00525     splice( edge_CA, symEdge( edge_BC ));
00526 
00527     recentEdge = edge_AB;
00528 }
00529 
00530 
00531 void Subdiv2D::clearVoronoi()
00532 {
00533     size_t i, total = qedges.size();
00534 
00535     for( i = 0; i < total; i++ )
00536         qedges[i].pt[1] = qedges[i].pt[3] = 0;
00537 
00538     total = vtx.size();
00539     for( i = 0; i < total; i++ )
00540     {
00541         if( vtx[i].isvirtual() )
00542             deletePoint((int)i);
00543     }
00544 
00545     validGeometry = false;
00546 }
00547 
00548 
00549 static Point2f computeVoronoiPoint(Point2f org0, Point2f dst0, Point2f org1, Point2f dst1)
00550 {
00551     double a0 = dst0.x - org0.x;
00552     double b0 = dst0.y - org0.y;
00553     double c0 = -0.5*(a0 * (dst0.x + org0.x) + b0 * (dst0.y + org0.y));
00554 
00555     double a1 = dst1.x - org1.x;
00556     double b1 = dst1.y - org1.y;
00557     double c1 = -0.5*(a1 * (dst1.x + org1.x) + b1 * (dst1.y + org1.y));
00558 
00559     double det = a0 * b1 - a1 * b0;
00560 
00561     if( det != 0 )
00562     {
00563         det = 1. / det;
00564         return Point2f((float) ((b0 * c1 - b1 * c0) * det),
00565                        (float) ((a1 * c0 - a0 * c1) * det));
00566     }
00567 
00568     return Point2f(FLT_MAX, FLT_MAX);
00569 }
00570 
00571 
00572 void Subdiv2D::calcVoronoi()
00573 {
00574     // check if it is already calculated
00575     if( validGeometry )
00576         return;
00577 
00578     clearVoronoi();
00579     int i, total = (int)qedges.size();
00580 
00581     // loop through all quad-edges, except for the first 3 (#1, #2, #3 - 0 is reserved for "NULL" pointer)
00582     for( i = 4; i < total; i++ )
00583     {
00584         QuadEdge& quadedge = qedges[i];
00585 
00586         if( quadedge.isfree() )
00587             continue;
00588 
00589         int edge0 = (int)(i*4);
00590         Point2f org0, dst0, org1, dst1;
00591 
00592         if( !quadedge.pt[3] )
00593         {
00594             int edge1 = getEdge( edge0, NEXT_AROUND_LEFT );
00595             int edge2 = getEdge( edge1, NEXT_AROUND_LEFT );
00596 
00597             edgeOrg(edge0, &org0);
00598             edgeDst(edge0, &dst0);
00599             edgeOrg(edge1, &org1);
00600             edgeDst(edge1, &dst1);
00601 
00602             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
00603 
00604             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
00605                fabs( virt_point.y ) < FLT_MAX * 0.5 )
00606             {
00607                 quadedge.pt[3] = qedges[edge1 >> 2].pt[3 - (edge1 & 2)] =
00608                 qedges[edge2 >> 2].pt[3 - (edge2 & 2)] = newPoint(virt_point, true);
00609             }
00610         }
00611 
00612         if( !quadedge.pt[1] )
00613         {
00614             int edge1 = getEdge( edge0, NEXT_AROUND_RIGHT );
00615             int edge2 = getEdge( edge1, NEXT_AROUND_RIGHT );
00616 
00617             edgeOrg(edge0, &org0);
00618             edgeDst(edge0, &dst0);
00619             edgeOrg(edge1, &org1);
00620             edgeDst(edge1, &dst1);
00621 
00622             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
00623 
00624             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
00625                fabs( virt_point.y ) < FLT_MAX * 0.5 )
00626             {
00627                 quadedge.pt[1] = qedges[edge1 >> 2].pt[1 + (edge1 & 2)] =
00628                 qedges[edge2 >> 2].pt[1 + (edge2 & 2)] = newPoint(virt_point, true);
00629             }
00630         }
00631     }
00632 
00633     validGeometry = true;
00634 }
00635 
00636 
00637 static int
00638 isRightOf2( const Point2f& pt, const Point2f& org, const Point2f& diff )
00639 {
00640     double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
00641     return (cw_area > 0) - (cw_area < 0);
00642 }
00643 
00644 
00645 int Subdiv2D::findNearest(Point2f pt, Point2f* nearestPt)
00646 {
00647     if( !validGeometry )
00648         calcVoronoi();
00649 
00650     int vertex = 0, edge = 0;
00651     int loc = locate( pt, edge, vertex );
00652 
00653     if( loc != PTLOC_ON_EDGE && loc != PTLOC_INSIDE )
00654         return vertex;
00655 
00656     vertex = 0;
00657 
00658     Point2f start;
00659     edgeOrg(edge, &start);
00660     Point2f diff = pt - start;
00661 
00662     edge = rotateEdge(edge, 1);
00663 
00664     int i, total = (int)vtx.size();
00665 
00666     for( i = 0; i < total; i++ )
00667     {
00668         Point2f t;
00669 
00670         for(;;)
00671         {
00672             CV_Assert( edgeDst(edge, &t) > 0 );
00673             if( isRightOf2( t, start, diff ) >= 0 )
00674                 break;
00675 
00676             edge = getEdge( edge, NEXT_AROUND_LEFT );
00677         }
00678 
00679         for(;;)
00680         {
00681             CV_Assert( edgeOrg( edge, &t ) > 0 );
00682 
00683             if( isRightOf2( t, start, diff ) < 0 )
00684                 break;
00685 
00686             edge = getEdge( edge, PREV_AROUND_LEFT );
00687         }
00688 
00689         Point2f tempDiff;
00690         edgeDst(edge, &tempDiff);
00691         edgeOrg(edge, &t);
00692         tempDiff -= t;
00693 
00694         if( isRightOf2( pt, t, tempDiff ) >= 0 )
00695         {
00696             vertex = edgeOrg(rotateEdge( edge, 3 ));
00697             break;
00698         }
00699 
00700         edge = symEdge( edge );
00701     }
00702 
00703     if( nearestPt && vertex > 0 )
00704         *nearestPt = vtx[vertex].pt;
00705 
00706     return vertex;
00707 }
00708 
00709 void Subdiv2D::getEdgeList(std::vector<Vec4f>& edgeList) const
00710 {
00711     edgeList.clear();
00712 
00713     for( size_t i = 4; i < qedges.size(); i++ )
00714     {
00715         if( qedges[i].isfree() )
00716             continue;
00717         if( qedges[i].pt[0] > 0 && qedges[i].pt[2] > 0 )
00718         {
00719             Point2f org = vtx[qedges[i].pt[0]].pt;
00720             Point2f dst = vtx[qedges[i].pt[2]].pt;
00721             edgeList.push_back(Vec4f(org.x, org.y, dst.x, dst.y));
00722         }
00723     }
00724 }
00725 
00726 void Subdiv2D::getTriangleList(std::vector<Vec6f>& triangleList) const
00727 {
00728     triangleList.clear();
00729     int i, total = (int)(qedges.size()*4);
00730     std::vector<bool> edgemask(total, false);
00731 
00732     for( i = 4; i < total; i += 2 )
00733     {
00734         if( edgemask[i] )
00735             continue;
00736         Point2f a, b, c;
00737         int edge = i;
00738         edgeOrg(edge, &a);
00739         edgemask[edge] = true;
00740         edge = getEdge(edge, NEXT_AROUND_LEFT);
00741         edgeOrg(edge, &b);
00742         edgemask[edge] = true;
00743         edge = getEdge(edge, NEXT_AROUND_LEFT);
00744         edgeOrg(edge, &c);
00745         edgemask[edge] = true;
00746         triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
00747     }
00748 }
00749 
00750 void Subdiv2D::getVoronoiFacetList(const std::vector<int>& idx,
00751                                    CV_OUT std::vector<std::vector<Point2f> >& facetList,
00752                                    CV_OUT std::vector<Point2f>& facetCenters)
00753 {
00754     calcVoronoi();
00755     facetList.clear();
00756     facetCenters.clear();
00757 
00758     std::vector<Point2f> buf;
00759 
00760     size_t i, total;
00761     if( idx.empty() )
00762         i = 4, total = vtx.size();
00763     else
00764         i = 0, total = idx.size();
00765 
00766     for( ; i < total; i++ )
00767     {
00768         int k = idx.empty() ? (int)i : idx[i];
00769 
00770         if( vtx[k].isfree() || vtx[k].isvirtual() )
00771             continue;
00772         int edge = rotateEdge(vtx[k].firstEdge, 1), t = edge;
00773 
00774         // gather points
00775         buf.clear();
00776         do
00777         {
00778             buf.push_back(vtx[edgeOrg(t)].pt);
00779             t = getEdge( t, NEXT_AROUND_LEFT );
00780         }
00781         while( t != edge );
00782 
00783         facetList.push_back(buf);
00784         facetCenters.push_back(vtx[k].pt);
00785     }
00786 }
00787 
00788 
00789 void Subdiv2D::checkSubdiv() const
00790 {
00791     int i, j, total = (int)qedges.size();
00792 
00793     for( i = 0; i < total; i++ )
00794     {
00795         const QuadEdge& qe = qedges[i];
00796 
00797         if( qe.isfree() )
00798             continue;
00799 
00800         for( j = 0; j < 4; j++ )
00801         {
00802             int e = (int)(i*4 + j);
00803             int o_next = nextEdge(e);
00804             int o_prev = getEdge(e, PREV_AROUND_ORG );
00805             int d_prev = getEdge(e, PREV_AROUND_DST );
00806             int d_next = getEdge(e, NEXT_AROUND_DST );
00807 
00808             // check points
00809             CV_Assert( edgeOrg(e) == edgeOrg(o_next));
00810             CV_Assert( edgeOrg(e) == edgeOrg(o_prev));
00811             CV_Assert( edgeDst(e) == edgeDst(d_next));
00812             CV_Assert( edgeDst(e) == edgeDst(d_prev));
00813 
00814             if( j % 2 == 0 )
00815             {
00816                 CV_Assert( edgeDst(o_next) == edgeOrg(d_prev));
00817                 CV_Assert( edgeDst(o_prev) == edgeOrg(d_next));
00818                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_LEFT),NEXT_AROUND_LEFT),NEXT_AROUND_LEFT) == e );
00819                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT) == e);
00820             }
00821         }
00822     }
00823 }
00824 
00825 }
00826 
00827 /* End of file. */
00828