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. */ 828
