modules/imgproc/src/subdivision2d.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41 #include "precomp.hpp"
  42
  43 namespace cv
  44 {
  45
  46 int Subdiv2D::nextEdge(int edge) const
  47 {
  48     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
  49     return qedges[edge >> 2].next[edge & 3];
  50 }
  51
  52 int Subdiv2D::rotateEdge(int edge, int rotate) const
  53 {
  54     return (edge & ~3) + ((edge + rotate) & 3);
  55 }
  56
  57 int Subdiv2D::symEdge(int edge) const
  58 {
  59     return edge ^ 2;
  60 }
  61
  62 int Subdiv2D::getEdge(int edge, int nextEdgeType) const
  63 {
  64     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
  65     edge = qedges[edge >> 2].next[(edge + nextEdgeType) & 3];
  66     return (edge & ~3) + ((edge + (nextEdgeType >> 4)) & 3);
  67 }
  68
  69 int Subdiv2D::edgeOrg(int edge, CV_OUT Point2f* orgpt) const
  70 {
  71     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
  72     int vidx = qedges[edge >> 2].pt[edge & 3];
  73     if( orgpt )
  74     {
  75         CV_DbgAssert((size_t)vidx < vtx.size());
  76         *orgpt = vtx[vidx].pt;
  77     }
  78     return vidx;
  79 }
  80
  81 int Subdiv2D::edgeDst(int edge, CV_OUT Point2f* dstpt) const
  82 {
  83     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
  84     int vidx = qedges[edge >> 2].pt[(edge + 2) & 3];
  85     if( dstpt )
  86     {
  87         CV_DbgAssert((size_t)vidx < vtx.size());
  88         *dstpt = vtx[vidx].pt;
  89     }
  90     return vidx;
  91 }
  92
  93
  94 Point2f Subdiv2D::getVertex(int vertex, CV_OUT int* firstEdge) const
  95 {
  96     CV_DbgAssert((size_t)vertex < vtx.size());
  97     if( firstEdge )
  98         *firstEdge = vtx[vertex].firstEdge;
  99     return vtx[vertex].pt;
 100 }
 101
 102
 103 Subdiv2D::Subdiv2D()
 104 {
 105     validGeometry = false;
 106     freeQEdge = 0;
 107     freePoint = 0;
 108     recentEdge = 0;
 109 }
 110
 111 Subdiv2D::Subdiv2D(Rect rect)
 112 {
 113     validGeometry = false;
 114     freeQEdge = 0;
 115     freePoint = 0;
 116     recentEdge = 0;
 117
 118     initDelaunay(rect);
 119 }
 120
 121
 122 Subdiv2D::QuadEdge::QuadEdge()
 123 {
 124     next[0] = next[1] = next[2] = next[3] = 0;
 125     pt[0] = pt[1] = pt[2] = pt[3] = 0;
 126 }
 127
 128 Subdiv2D::QuadEdge::QuadEdge(int edgeidx)
 129 {
 130     CV_DbgAssert((edgeidx & 3) == 0);
 131     next[0] = edgeidx;
 132     next[1] = edgeidx+3;
 133     next[2] = edgeidx+2;
 134     next[3] = edgeidx+1;
 135
 136     pt[0] = pt[1] = pt[2] = pt[3] = 0;
 137 }
 138
 139 bool Subdiv2D::QuadEdge::isfree() const
 140 {
 141     return next[0] <= 0;
 142 }
 143
 144 Subdiv2D::Vertex::Vertex()
 145 {
 146     firstEdge = 0;
 147     type = -1;
 148 }
 149
 150 Subdiv2D::Vertex::Vertex(Point2f _pt, bool _isvirtual, int _firstEdge)
 151 {
 152     firstEdge = _firstEdge;
 153     type = (int)_isvirtual;
 154     pt = _pt;
 155 }
 156
 157 bool Subdiv2D::Vertex::isvirtual() const
 158 {
 159     return type > 0;
 160 }
 161
 162 bool Subdiv2D::Vertex::isfree() const
 163 {
 164     return type < 0;
 165 }
 166
 167 void Subdiv2D::splice( int edgeA, int edgeB )
 168 {
 169     int& a_next = qedges[edgeA >> 2].next[edgeA & 3];
 170     int& b_next = qedges[edgeB >> 2].next[edgeB & 3];
 171     int a_rot = rotateEdge(a_next, 1);
 172     int b_rot = rotateEdge(b_next, 1);
 173     int& a_rot_next = qedges[a_rot >> 2].next[a_rot & 3];
 174     int& b_rot_next = qedges[b_rot >> 2].next[b_rot & 3];
 175     std::swap(a_next, b_next);
 176     std::swap(a_rot_next, b_rot_next);
 177 }
 178
 179 void Subdiv2D::setEdgePoints(int edge, int orgPt, int dstPt)
 180 {
 181     qedges[edge >> 2].pt[edge & 3] = orgPt;
 182     qedges[edge >> 2].pt[(edge + 2) & 3] = dstPt;
 183     vtx[orgPt].firstEdge = edge;
 184     vtx[dstPt].firstEdge = edge ^ 2;
 185 }
 186
 187 int Subdiv2D::connectEdges( int edgeA, int edgeB )
 188 {
 189     int edge = newEdge();
 190
 191     splice(edge, getEdge(edgeA, NEXT_AROUND_LEFT));
 192     splice(symEdge(edge), edgeB);
 193
 194     setEdgePoints(edge, edgeDst(edgeA), edgeOrg(edgeB));
 195     return edge;
 196 }
 197
 198 void Subdiv2D::swapEdges( int edge )
 199 {
 200     int sedge = symEdge(edge);
 201     int a = getEdge(edge, PREV_AROUND_ORG);
 202     int b = getEdge(sedge, PREV_AROUND_ORG);
 203
 204     splice(edge, a);
 205     splice(sedge, b);
 206
 207     setEdgePoints(edge, edgeDst(a), edgeDst(b));
 208
 209     splice(edge, getEdge(a, NEXT_AROUND_LEFT));
 210     splice(sedge, getEdge(b, NEXT_AROUND_LEFT));
 211 }
 212
 213 static double triangleArea( Point2f a, Point2f b, Point2f c )
 214 {
 215     return ((double)b.x - a.x) * ((double)c.y - a.y) - ((double)b.y - a.y) * ((double)c.x - a.x);
 216 }
 217
 218 int Subdiv2D::isRightOf(Point2f pt, int edge) const
 219 {
 220     Point2f org, dst;
 221     edgeOrg(edge, &org);
 222     edgeDst(edge, &dst);
 223     double cw_area = triangleArea( pt, dst, org );
 224
 225     return (cw_area > 0) - (cw_area < 0);
 226 }
 227
 228 int Subdiv2D::newEdge()
 229 {
 230     if( freeQEdge <= 0 )
 231     {
 232         qedges.push_back(QuadEdge());
 233         freeQEdge = (int)(qedges.size()-1);
 234     }
 235     int edge = freeQEdge*4;
 236     freeQEdge = qedges[edge >> 2].next[1];
 237     qedges[edge >> 2] = QuadEdge(edge);
 238     return edge;
 239 }
 240
 241 void Subdiv2D::deleteEdge(int edge)
 242 {
 243     CV_DbgAssert((size_t)(edge >> 2) < (size_t)qedges.size());
 244     splice( edge, getEdge(edge, PREV_AROUND_ORG) );
 245     int sedge = symEdge(edge);
 246     splice(sedge, getEdge(sedge, PREV_AROUND_ORG) );
 247
 248     edge >>= 2;
 249     qedges[edge].next[0] = 0;
 250     qedges[edge].next[1] = freeQEdge;
 251     freeQEdge = edge;
 252 }
 253
 254 int Subdiv2D::newPoint(Point2f pt, bool isvirtual, int firstEdge)
 255 {
 256     if( freePoint == 0 )
 257     {
 258         vtx.push_back(Vertex());
 259         freePoint = (int)(vtx.size()-1);
 260     }
 261     int vidx = freePoint;
 262     freePoint = vtx[vidx].firstEdge;
 263     vtx[vidx] = Vertex(pt, isvirtual, firstEdge);
 264
 265     return vidx;
 266 }
 267
 268 void Subdiv2D::deletePoint(int vidx)
 269 {
 270     CV_DbgAssert( (size_t)vidx < vtx.size() );
 271     vtx[vidx].firstEdge = freePoint;
 272     vtx[vidx].type = -1;
 273     freePoint = vidx;
 274 }
 275
 276 int Subdiv2D::locate(Point2f pt, int& _edge, int& _vertex)
 277 {
 278     int vertex = 0;
 279
 280     int i, maxEdges = (int)(qedges.size() * 4);
 281
 282     if( qedges.size() < (size_t)4 )
 283         CV_Error( CV_StsError, "Subdivision is empty" );
 284
 285     if( pt.x < topLeft.x || pt.y < topLeft.y || pt.x >= bottomRight.x || pt.y >= bottomRight.y )
 286         CV_Error( CV_StsOutOfRange, "" );
 287
 288     int edge = recentEdge;
 289     CV_Assert(edge > 0);
 290
 291     int location = PTLOC_ERROR;
 292
 293     int right_of_curr = isRightOf(pt, edge);
 294     if( right_of_curr > 0 )
 295     {
 296         edge = symEdge(edge);
 297         right_of_curr = -right_of_curr;
 298     }
 299
 300     for( i = 0; i < maxEdges; i++ )
 301     {
 302         int onext_edge = nextEdge( edge );
 303         int dprev_edge = getEdge( edge, PREV_AROUND_DST );
 304
 305         int right_of_onext = isRightOf( pt, onext_edge );
 306         int right_of_dprev = isRightOf( pt, dprev_edge );
 307
 308         if( right_of_dprev > 0 )
 309         {
 310             if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
 311             {
 312                 location = PTLOC_INSIDE;
 313                 break;
 314             }
 315             else
 316             {
 317                 right_of_curr = right_of_onext;
 318                 edge = onext_edge;
 319             }
 320         }
 321         else
 322         {
 323             if( right_of_onext > 0 )
 324             {
 325                 if( right_of_dprev == 0 && right_of_curr == 0 )
 326                 {
 327                     location = PTLOC_INSIDE;
 328                     break;
 329                 }
 330                 else
 331                 {
 332                     right_of_curr = right_of_dprev;
 333                     edge = dprev_edge;
 334                 }
 335             }
 336             else if( right_of_curr == 0 &&
 337                     isRightOf( vtx[edgeDst(onext_edge)].pt, edge ) >= 0 )
 338             {
 339                 edge = symEdge( edge );
 340             }
 341             else
 342             {
 343                 right_of_curr = right_of_onext;
 344                 edge = onext_edge;
 345             }
 346         }
 347     }
 348
 349     recentEdge = edge;
 350
 351     if( location == PTLOC_INSIDE )
 352     {
 353         Point2f org_pt, dst_pt;
 354         edgeOrg(edge, &org_pt);
 355         edgeDst(edge, &dst_pt);
 356
 357         double t1 = fabs( pt.x - org_pt.x );
 358         t1 += fabs( pt.y - org_pt.y );
 359         double t2 = fabs( pt.x - dst_pt.x );
 360         t2 += fabs( pt.y - dst_pt.y );
 361         double t3 = fabs( org_pt.x - dst_pt.x );
 362         t3 += fabs( org_pt.y - dst_pt.y );
 363
 364         if( t1 < FLT_EPSILON )
 365         {
 366             location = PTLOC_VERTEX;
 367             vertex = edgeOrg( edge );
 368             edge = 0;
 369         }
 370         else if( t2 < FLT_EPSILON )
 371         {
 372             location = PTLOC_VERTEX;
 373             vertex = edgeDst( edge );
 374             edge = 0;
 375         }
 376         else if( (t1 < t3 || t2 < t3) &&
 377                 fabs( triangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
 378         {
 379             location = PTLOC_ON_EDGE;
 380             vertex = 0;
 381         }
 382     }
 383
 384     if( location == PTLOC_ERROR )
 385     {
 386         edge = 0;
 387         vertex = 0;
 388     }
 389
 390     _edge = edge;
 391     _vertex = vertex;
 392
 393     return location;
 394 }
 395
 396
 397 inline int
 398 isPtInCircle3( Point2f pt, Point2f a, Point2f b, Point2f c)
 399 {
 400     const double eps = FLT_EPSILON*0.125;
 401     double val = ((double)a.x * a.x + (double)a.y * a.y) * triangleArea( b, c, pt );
 402     val -= ((double)b.x * b.x + (double)b.y * b.y) * triangleArea( a, c, pt );
 403     val += ((double)c.x * c.x + (double)c.y * c.y) * triangleArea( a, b, pt );
 404     val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * triangleArea( a, b, c );
 405
 406     return val > eps ? 1 : val < -eps ? -1 : 0;
 407 }
 408
 409
 410 int Subdiv2D::insert(Point2f pt)
 411 {
 412     int curr_point = 0, curr_edge = 0, deleted_edge = 0;
 413     int location = locate( pt, curr_edge, curr_point );
 414
 415     if( location == PTLOC_ERROR )
 416         CV_Error( CV_StsBadSize, "" );
 417
 418     if( location == PTLOC_OUTSIDE_RECT )
 419         CV_Error( CV_StsOutOfRange, "" );
 420
 421     if( location == PTLOC_VERTEX )
 422         return curr_point;
 423
 424     if( location == PTLOC_ON_EDGE )
 425     {
 426         deleted_edge = curr_edge;
 427         recentEdge = curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
 428         deleteEdge(deleted_edge);
 429     }
 430     else if( location == PTLOC_INSIDE )
 431         ;
 432     else
 433         CV_Error_(CV_StsError, ("Subdiv2D::locate returned invalid location = %d", location) );
 434
 435     assert( curr_edge != 0 );
 436     validGeometry = false;
 437
 438     curr_point = newPoint(pt, false);
 439     int base_edge = newEdge();
 440     int first_point = edgeOrg(curr_edge);
 441     setEdgePoints(base_edge, first_point, curr_point);
 442     splice(base_edge, curr_edge);
 443
 444     do
 445     {
 446         base_edge = connectEdges( curr_edge, symEdge(base_edge) );
 447         curr_edge = getEdge(base_edge, PREV_AROUND_ORG);
 448     }
 449     while( edgeDst(curr_edge) != first_point );
 450
 451     curr_edge = getEdge( base_edge, PREV_AROUND_ORG );
 452
 453     int i, max_edges = (int)(qedges.size()*4);
 454
 455     for( i = 0; i < max_edges; i++ )
 456     {
 457         int temp_dst = 0, curr_org = 0, curr_dst = 0;
 458         int temp_edge = getEdge( curr_edge, PREV_AROUND_ORG );
 459
 460         temp_dst = edgeDst( temp_edge );
 461         curr_org = edgeOrg( curr_edge );
 462         curr_dst = edgeDst( curr_edge );
 463
 464         if( isRightOf( vtx[temp_dst].pt, curr_edge ) > 0 &&
 465            isPtInCircle3( vtx[curr_org].pt, vtx[temp_dst].pt,
 466                          vtx[curr_dst].pt, vtx[curr_point].pt ) < 0 )
 467         {
 468             swapEdges( curr_edge );
 469             curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
 470         }
 471         else if( curr_org == first_point )
 472             break;
 473         else
 474             curr_edge = getEdge( nextEdge( curr_edge ), PREV_AROUND_LEFT );
 475     }
 476
 477     return curr_point;
 478 }
 479
 480 void Subdiv2D::insert(const std::vector<Point2f>& ptvec)
 481 {
 482     for( size_t i = 0; i < ptvec.size(); i++ )
 483         insert(ptvec[i]);
 484 }
 485
 486 void Subdiv2D::initDelaunay( Rect rect )
 487 {
 488     float big_coord = 3.f * MAX( rect.width, rect.height );
 489     float rx = (float)rect.x;
 490     float ry = (float)rect.y;
 491
 492     vtx.clear();
 493     qedges.clear();
 494
 495     recentEdge = 0;
 496     validGeometry = false;
 497
 498     topLeft = Point2f( rx, ry );
 499     bottomRight = Point2f( rx + rect.width, ry + rect.height );
 500
 501     Point2f ppA( rx + big_coord, ry );
 502     Point2f ppB( rx, ry + big_coord );
 503     Point2f ppC( rx - big_coord, ry - big_coord );
 504
 505     vtx.push_back(Vertex());
 506     qedges.push_back(QuadEdge());
 507
 508     freeQEdge = 0;
 509     freePoint = 0;
 510
 511     int pA = newPoint(ppA, false);
 512     int pB = newPoint(ppB, false);
 513     int pC = newPoint(ppC, false);
 514
 515     int edge_AB = newEdge();
 516     int edge_BC = newEdge();
 517     int edge_CA = newEdge();
 518
 519     setEdgePoints( edge_AB, pA, pB );
 520     setEdgePoints( edge_BC, pB, pC );
 521     setEdgePoints( edge_CA, pC, pA );
 522
 523     splice( edge_AB, symEdge( edge_CA ));
 524     splice( edge_BC, symEdge( edge_AB ));
 525     splice( edge_CA, symEdge( edge_BC ));
 526
 527     recentEdge = edge_AB;
 528 }
 529
 530
 531 void Subdiv2D::clearVoronoi()
 532 {
 533     size_t i, total = qedges.size();
 534
 535     for( i = 0; i < total; i++ )
 536         qedges[i].pt[1] = qedges[i].pt[3] = 0;
 537
 538     total = vtx.size();
 539     for( i = 0; i < total; i++ )
 540     {
 541         if( vtx[i].isvirtual() )
 542             deletePoint((int)i);
 543     }
 544
 545     validGeometry = false;
 546 }
 547
 548
 549 static Point2f computeVoronoiPoint(Point2f org0, Point2f dst0, Point2f org1, Point2f dst1)
 550 {
 551     double a0 = dst0.x - org0.x;
 552     double b0 = dst0.y - org0.y;
 553     double c0 = -0.5*(a0 * (dst0.x + org0.x) + b0 * (dst0.y + org0.y));
 554
 555     double a1 = dst1.x - org1.x;
 556     double b1 = dst1.y - org1.y;
 557     double c1 = -0.5*(a1 * (dst1.x + org1.x) + b1 * (dst1.y + org1.y));
 558
 559     double det = a0 * b1 - a1 * b0;
 560
 561     if( det != 0 )
 562     {
 563         det = 1. / det;
 564         return Point2f((float) ((b0 * c1 - b1 * c0) * det),
 565                        (float) ((a1 * c0 - a0 * c1) * det));
 566     }
 567
 568     return Point2f(FLT_MAX, FLT_MAX);
 569 }
 570
 571
 572 void Subdiv2D::calcVoronoi()
 573 {
 574     // check if it is already calculated
 575     if( validGeometry )
 576         return;
 577
 578     clearVoronoi();
 579     int i, total = (int)qedges.size();
 580
 581     // loop through all quad-edges, except for the first 3 (#1, #2, #3 - 0 is reserved for "NULL" pointer)
 582     for( i = 4; i < total; i++ )
 583     {
 584         QuadEdge& quadedge = qedges[i];
 585
 586         if( quadedge.isfree() )
 587             continue;
 588
 589         int edge0 = (int)(i*4);
 590         Point2f org0, dst0, org1, dst1;
 591
 592         if( !quadedge.pt[3] )
 593         {
 594             int edge1 = getEdge( edge0, NEXT_AROUND_LEFT );
 595             int edge2 = getEdge( edge1, NEXT_AROUND_LEFT );
 596
 597             edgeOrg(edge0, &org0);
 598             edgeDst(edge0, &dst0);
 599             edgeOrg(edge1, &org1);
 600             edgeDst(edge1, &dst1);
 601
 602             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
 603
 604             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 605                fabs( virt_point.y ) < FLT_MAX * 0.5 )
 606             {
 607                 quadedge.pt[3] = qedges[edge1 >> 2].pt[3 - (edge1 & 2)] =
 608                 qedges[edge2 >> 2].pt[3 - (edge2 & 2)] = newPoint(virt_point, true);
 609             }
 610         }
 611
 612         if( !quadedge.pt[1] )
 613         {
 614             int edge1 = getEdge( edge0, NEXT_AROUND_RIGHT );
 615             int edge2 = getEdge( edge1, NEXT_AROUND_RIGHT );
 616
 617             edgeOrg(edge0, &org0);
 618             edgeDst(edge0, &dst0);
 619             edgeOrg(edge1, &org1);
 620             edgeDst(edge1, &dst1);
 621
 622             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
 623
 624             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 625                fabs( virt_point.y ) < FLT_MAX * 0.5 )
 626             {
 627                 quadedge.pt[1] = qedges[edge1 >> 2].pt[1 + (edge1 & 2)] =
 628                 qedges[edge2 >> 2].pt[1 + (edge2 & 2)] = newPoint(virt_point, true);
 629             }
 630         }
 631     }
 632
 633     validGeometry = true;
 634 }
 635
 636
 637 static int
 638 isRightOf2( const Point2f& pt, const Point2f& org, const Point2f& diff )
 639 {
 640     double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
 641     return (cw_area > 0) - (cw_area < 0);
 642 }
 643
 644
 645 int Subdiv2D::findNearest(Point2f pt, Point2f* nearestPt)
 646 {
 647     if( !validGeometry )
 648         calcVoronoi();
 649
 650     int vertex = 0, edge = 0;
 651     int loc = locate( pt, edge, vertex );
 652
 653     if( loc != PTLOC_ON_EDGE && loc != PTLOC_INSIDE )
 654         return vertex;
 655
 656     vertex = 0;
 657
 658     Point2f start;
 659     edgeOrg(edge, &start);
 660     Point2f diff = pt - start;
 661
 662     edge = rotateEdge(edge, 1);
 663
 664     int i, total = (int)vtx.size();
 665
 666     for( i = 0; i < total; i++ )
 667     {
 668         Point2f t;
 669
 670         for(;;)
 671         {
 672             CV_Assert( edgeDst(edge, &t) > 0 );
 673             if( isRightOf2( t, start, diff ) >= 0 )
 674                 break;
 675
 676             edge = getEdge( edge, NEXT_AROUND_LEFT );
 677         }
 678
 679         for(;;)
 680         {
 681             CV_Assert( edgeOrg( edge, &t ) > 0 );
 682
 683             if( isRightOf2( t, start, diff ) < 0 )
 684                 break;
 685
 686             edge = getEdge( edge, PREV_AROUND_LEFT );
 687         }
 688
 689         Point2f tempDiff;
 690         edgeDst(edge, &tempDiff);
 691         edgeOrg(edge, &t);
 692         tempDiff -= t;
 693
 694         if( isRightOf2( pt, t, tempDiff ) >= 0 )
 695         {
 696             vertex = edgeOrg(rotateEdge( edge, 3 ));
 697             break;
 698         }
 699
 700         edge = symEdge( edge );
 701     }
 702
 703     if( nearestPt && vertex > 0 )
 704         *nearestPt = vtx[vertex].pt;
 705
 706     return vertex;
 707 }
 708
 709 void Subdiv2D::getEdgeList(std::vector<Vec4f>& edgeList) const
 710 {
 711     edgeList.clear();
 712
 713     for( size_t i = 4; i < qedges.size(); i++ )
 714     {
 715         if( qedges[i].isfree() )
 716             continue;
 717         if( qedges[i].pt[0] > 0 && qedges[i].pt[2] > 0 )
 718         {
 719             Point2f org = vtx[qedges[i].pt[0]].pt;
 720             Point2f dst = vtx[qedges[i].pt[2]].pt;
 721             edgeList.push_back(Vec4f(org.x, org.y, dst.x, dst.y));
 722         }
 723     }
 724 }
 725
 726 void Subdiv2D::getTriangleList(std::vector<Vec6f>& triangleList) const
 727 {
 728     triangleList.clear();
 729     int i, total = (int)(qedges.size()*4);
 730     std::vector<bool> edgemask(total, false);
 731
 732     for( i = 4; i < total; i += 2 )
 733     {
 734         if( edgemask[i] )
 735             continue;
 736         Point2f a, b, c;
 737         int edge = i;
 738         edgeOrg(edge, &a);
 739         edgemask[edge] = true;
 740         edge = getEdge(edge, NEXT_AROUND_LEFT);
 741         edgeOrg(edge, &b);
 742         edgemask[edge] = true;
 743         edge = getEdge(edge, NEXT_AROUND_LEFT);
 744         edgeOrg(edge, &c);
 745         edgemask[edge] = true;
 746         triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
 747     }
 748 }
 749
 750 void Subdiv2D::getVoronoiFacetList(const std::vector<int>& idx,
 751                                    CV_OUT std::vector<std::vector<Point2f> >& facetList,
 752                                    CV_OUT std::vector<Point2f>& facetCenters)
 753 {
 754     calcVoronoi();
 755     facetList.clear();
 756     facetCenters.clear();
 757
 758     std::vector<Point2f> buf;
 759
 760     size_t i, total;
 761     if( idx.empty() )
 762         i = 4, total = vtx.size();
 763     else
 764         i = 0, total = idx.size();
 765
 766     for( ; i < total; i++ )
 767     {
 768         int k = idx.empty() ? (int)i : idx[i];
 769
 770         if( vtx[k].isfree() || vtx[k].isvirtual() )
 771             continue;
 772         int edge = rotateEdge(vtx[k].firstEdge, 1), t = edge;
 773
 774         // gather points
 775         buf.clear();
 776         do
 777         {
 778             buf.push_back(vtx[edgeOrg(t)].pt);
 779             t = getEdge( t, NEXT_AROUND_LEFT );
 780         }
 781         while( t != edge );
 782
 783         facetList.push_back(buf);
 784         facetCenters.push_back(vtx[k].pt);
 785     }
 786 }
 787
 788
 789 void Subdiv2D::checkSubdiv() const
 790 {
 791     int i, j, total = (int)qedges.size();
 792
 793     for( i = 0; i < total; i++ )
 794     {
 795         const QuadEdge& qe = qedges[i];
 796
 797         if( qe.isfree() )
 798             continue;
 799
 800         for( j = 0; j < 4; j++ )
 801         {
 802             int e = (int)(i*4 + j);
 803             int o_next = nextEdge(e);
 804             int o_prev = getEdge(e, PREV_AROUND_ORG );
 805             int d_prev = getEdge(e, PREV_AROUND_DST );
 806             int d_next = getEdge(e, NEXT_AROUND_DST );
 807
 808             // check points
 809             CV_Assert( edgeOrg(e) == edgeOrg(o_next));
 810             CV_Assert( edgeOrg(e) == edgeOrg(o_prev));
 811             CV_Assert( edgeDst(e) == edgeDst(d_next));
 812             CV_Assert( edgeDst(e) == edgeDst(d_prev));
 813
 814             if( j % 2 == 0 )
 815             {
 816                 CV_Assert( edgeDst(o_next) == edgeOrg(d_prev));
 817                 CV_Assert( edgeDst(o_prev) == edgeOrg(d_next));
 818                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_LEFT),NEXT_AROUND_LEFT),NEXT_AROUND_LEFT) == e );
 819                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT) == e);
 820             }
 821         }
 822     }
 823 }
 824
 825 }
 826
 827 /* End of file. */