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 "_cv.h" 42 43 CV_IMPL CvSubdiv2D * 44 cvCreateSubdiv2D( int subdiv_type, int header_size, 45 int vtx_size, int quadedge_size, CvMemStorage * storage ) 46 { 47 CvSubdiv2D *subdiv = 0; 48 49 CV_FUNCNAME( "cvCleateSubdiv2D" ); 50 51 __BEGIN__; 52 53 if( !storage ) 54 CV_ERROR( CV_StsNullPtr, "" ); 55 56 if( header_size < (int)sizeof( *subdiv ) || 57 quadedge_size < (int)sizeof( CvQuadEdge2D ) || 58 vtx_size < (int)sizeof( CvSubdiv2DPoint )) 59 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); 60 61 subdiv = (CvSubdiv2D *) cvCreateGraph( subdiv_type, header_size, 62 vtx_size, quadedge_size, storage ); 63 64 65 __END__; 66 67 return subdiv; 68 } 69 70 71 /****************************************************************************************\ 72 * Quad Edge algebra * 73 \****************************************************************************************/ 74 75 static CvSubdiv2DEdge 76 cvSubdiv2DMakeEdge( CvSubdiv2D * subdiv ) 77 { 78 CvQuadEdge2D *edge = 0; 79 CvSubdiv2DEdge edgehandle = 0; 80 81 CV_FUNCNAME( "cvSubdiv2DMakeEdge" ); 82 83 __BEGIN__; 84 85 if( !subdiv ) 86 CV_ERROR( CV_StsNullPtr, "" ); 87 88 edge = (CvQuadEdge2D*)cvSetNew( (CvSet*)subdiv->edges ); 89 CV_CHECK(); 90 91 memset( edge->pt, 0, sizeof( edge->pt )); 92 edgehandle = (CvSubdiv2DEdge) edge; 93 94 edge->next[0] = edgehandle; 95 edge->next[1] = edgehandle + 3; 96 edge->next[2] = edgehandle + 2; 97 edge->next[3] = edgehandle + 1; 98 99 subdiv->quad_edges++; 100 101 102 __END__; 103 104 return edgehandle; 105 } 106 107 108 static CvSubdiv2DPoint * 109 cvSubdiv2DAddPoint( CvSubdiv2D * subdiv, CvPoint2D32f pt, int is_virtual ) 110 { 111 CvSubdiv2DPoint *subdiv_point = 0; 112 113 subdiv_point = (CvSubdiv2DPoint*)cvSetNew( (CvSet*)subdiv ); 114 if( subdiv_point ) 115 { 116 memset( subdiv_point, 0, subdiv->elem_size ); 117 subdiv_point->pt = pt; 118 subdiv_point->first = 0; 119 subdiv_point->flags |= is_virtual ? CV_SUBDIV2D_VIRTUAL_POINT_FLAG : 0; 120 } 121 122 return subdiv_point; 123 } 124 125 126 static void 127 cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ) 128 { 129 CvSubdiv2DEdge *a_next = &CV_SUBDIV2D_NEXT_EDGE( edgeA ); 130 CvSubdiv2DEdge *b_next = &CV_SUBDIV2D_NEXT_EDGE( edgeB ); 131 CvSubdiv2DEdge a_rot = cvSubdiv2DRotateEdge( *a_next, 1 ); 132 CvSubdiv2DEdge b_rot = cvSubdiv2DRotateEdge( *b_next, 1 ); 133 CvSubdiv2DEdge *a_rot_next = &CV_SUBDIV2D_NEXT_EDGE( a_rot ); 134 CvSubdiv2DEdge *b_rot_next = &CV_SUBDIV2D_NEXT_EDGE( b_rot ); 135 CvSubdiv2DEdge t; 136 137 CV_SWAP( *a_next, *b_next, t ); 138 CV_SWAP( *a_rot_next, *b_rot_next, t ); 139 } 140 141 142 static void 143 cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge, 144 CvSubdiv2DPoint * org_pt, CvSubdiv2DPoint * dst_pt ) 145 { 146 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3); 147 148 CV_FUNCNAME( "cvSubdiv2DSetEdgePoints" ); 149 150 __BEGIN__; 151 152 if( !quadedge ) 153 CV_ERROR( CV_StsNullPtr, "" ); 154 155 quadedge->pt[edge & 3] = org_pt; 156 quadedge->pt[(edge + 2) & 3] = dst_pt; 157 158 159 __END__; 160 } 161 162 163 static void 164 cvSubdiv2DDeleteEdge( CvSubdiv2D * subdiv, CvSubdiv2DEdge edge ) 165 { 166 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3); 167 168 CV_FUNCNAME( "cvSubdiv2DDeleteEdge" ); 169 170 __BEGIN__; 171 172 if( !subdiv || !quadedge ) 173 CV_ERROR( CV_StsNullPtr, "" ); 174 175 cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG )); 176 177 { 178 CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge ); 179 cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG )); 180 } 181 182 cvSetRemoveByPtr( (CvSet*)(subdiv->edges), quadedge ); 183 subdiv->quad_edges--; 184 185 186 __END__; 187 } 188 189 190 static CvSubdiv2DEdge 191 cvSubdiv2DConnectEdges( CvSubdiv2D * subdiv, CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ) 192 { 193 CvSubdiv2DEdge new_edge = 0; 194 195 CV_FUNCNAME( "cvSubdiv2DConnectPoints" ); 196 197 __BEGIN__; 198 199 CvSubdiv2DPoint *orgB, *dstA; 200 201 if( !subdiv ) 202 CV_ERROR( CV_StsNullPtr, "" ); 203 204 new_edge = cvSubdiv2DMakeEdge( subdiv ); 205 206 cvSubdiv2DSplice( new_edge, cvSubdiv2DGetEdge( edgeA, CV_NEXT_AROUND_LEFT )); 207 cvSubdiv2DSplice( cvSubdiv2DSymEdge( new_edge ), edgeB ); 208 209 dstA = cvSubdiv2DEdgeDst( edgeA ); 210 orgB = cvSubdiv2DEdgeOrg( edgeB ); 211 cvSubdiv2DSetEdgePoints( new_edge, dstA, orgB ); 212 213 __END__; 214 215 return new_edge; 216 } 217 218 219 static void 220 cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge ) 221 { 222 CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge ); 223 CvSubdiv2DEdge a = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG ); 224 CvSubdiv2DEdge b = cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG ); 225 CvSubdiv2DPoint *dstB, *dstA; 226 227 cvSubdiv2DSplice( edge, a ); 228 cvSubdiv2DSplice( sym_edge, b ); 229 230 dstA = cvSubdiv2DEdgeDst( a ); 231 dstB = cvSubdiv2DEdgeDst( b ); 232 cvSubdiv2DSetEdgePoints( edge, dstA, dstB ); 233 234 cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( a, CV_NEXT_AROUND_LEFT )); 235 cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( b, CV_NEXT_AROUND_LEFT )); 236 } 237 238 239 static int 240 icvIsRightOf( CvPoint2D32f& pt, CvSubdiv2DEdge edge ) 241 { 242 CvSubdiv2DPoint *org = cvSubdiv2DEdgeOrg(edge), *dst = cvSubdiv2DEdgeDst(edge); 243 Cv32suf cw_area; 244 cw_area.f = (float)cvTriangleArea( pt, dst->pt, org->pt ); 245 246 return (cw_area.i > 0)*2 - (cw_area.i*2 != 0); 247 } 248 249 250 CV_IMPL CvSubdiv2DPointLocation 251 cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt, 252 CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point ) 253 { 254 CvSubdiv2DEdge edge = 0; 255 CvSubdiv2DPoint *point = 0; 256 CvSubdiv2DPointLocation location = CV_PTLOC_ERROR; 257 258 int i, max_edges; 259 int right_of_curr = 0; 260 261 CV_FUNCNAME( "cvSubdiv2DLocate" ); 262 263 __BEGIN__; 264 265 if( !subdiv ) 266 CV_ERROR( CV_StsNullPtr, "" ); 267 268 if( !CV_IS_SUBDIV2D(subdiv) ) 269 CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR ); 270 271 max_edges = subdiv->quad_edges * 4; 272 edge = subdiv->recent_edge; 273 274 if( max_edges == 0 ) 275 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); 276 if( !edge ) 277 CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR ); 278 279 location = CV_PTLOC_OUTSIDE_RECT; 280 if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y || 281 pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y ) 282 CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR ); 283 284 location = CV_PTLOC_ERROR; 285 286 right_of_curr = icvIsRightOf( pt, edge ); 287 if( right_of_curr > 0 ) 288 { 289 edge = cvSubdiv2DSymEdge( edge ); 290 right_of_curr = -right_of_curr; 291 } 292 293 for( i = 0; i < max_edges; i++ ) 294 { 295 CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge ); 296 CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST ); 297 298 int right_of_onext = icvIsRightOf( pt, onext_edge ); 299 int right_of_dprev = icvIsRightOf( pt, dprev_edge ); 300 301 if( right_of_dprev > 0 ) 302 { 303 if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) ) 304 { 305 location = CV_PTLOC_INSIDE; 306 EXIT; 307 } 308 else 309 { 310 right_of_curr = right_of_onext; 311 edge = onext_edge; 312 } 313 } 314 else 315 { 316 if( right_of_onext > 0 ) 317 { 318 if( right_of_dprev == 0 && right_of_curr == 0 ) 319 { 320 location = CV_PTLOC_INSIDE; 321 EXIT; 322 } 323 else 324 { 325 right_of_curr = right_of_dprev; 326 edge = dprev_edge; 327 } 328 } 329 else if( right_of_curr == 0 && 330 icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 ) 331 { 332 edge = cvSubdiv2DSymEdge( edge ); 333 } 334 else 335 { 336 right_of_curr = right_of_onext; 337 edge = onext_edge; 338 } 339 } 340 } 341 342 343 __END__; 344 345 subdiv->recent_edge = edge; 346 347 if( location == CV_PTLOC_INSIDE ) 348 { 349 double t1, t2, t3; 350 CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt; 351 CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt; 352 353 t1 = fabs( pt.x - org_pt.x ); 354 t1 += fabs( pt.y - org_pt.y ); 355 t2 = fabs( pt.x - dst_pt.x ); 356 t2 += fabs( pt.y - dst_pt.y ); 357 t3 = fabs( org_pt.x - dst_pt.x ); 358 t3 += fabs( org_pt.y - dst_pt.y ); 359 360 if( t1 < FLT_EPSILON ) 361 { 362 location = CV_PTLOC_VERTEX; 363 point = cvSubdiv2DEdgeOrg( edge ); 364 edge = 0; 365 } 366 else if( t2 < FLT_EPSILON ) 367 { 368 location = CV_PTLOC_VERTEX; 369 point = cvSubdiv2DEdgeDst( edge ); 370 edge = 0; 371 } 372 else if( (t1 < t3 || t2 < t3) && 373 fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON ) 374 { 375 location = CV_PTLOC_ON_EDGE; 376 point = 0; 377 } 378 } 379 380 if( location == CV_PTLOC_ERROR ) 381 { 382 edge = 0; 383 point = 0; 384 } 385 386 if( _edge ) 387 *_edge = edge; 388 if( _point ) 389 *_point = point; 390 391 return location; 392 } 393 394 395 CV_INLINE int 396 icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c ) 397 { 398 double val = (a.x * a.x + a.y * a.y) * cvTriangleArea( b, c, pt ); 399 val -= (b.x * b.x + b.y * b.y) * cvTriangleArea( a, c, pt ); 400 val += (c.x * c.x + c.y * c.y) * cvTriangleArea( a, b, pt ); 401 val -= (pt.x * pt.x + pt.y * pt.y) * cvTriangleArea( a, b, c ); 402 403 return val > FLT_EPSILON ? 1 : val < -FLT_EPSILON ? -1 : 0; 404 } 405 406 407 CV_IMPL CvSubdiv2DPoint * 408 cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt ) 409 { 410 CvSubdiv2DPoint *point = 0; 411 CvSubdiv2DPointLocation location = CV_PTLOC_ERROR; 412 413 CvSubdiv2DPoint *curr_point = 0, *first_point = 0; 414 CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0; 415 int i, max_edges; 416 417 CV_FUNCNAME( "cvSubdivDelaunay2DInsert" ); 418 419 __BEGIN__; 420 421 if( !subdiv ) 422 CV_ERROR( CV_StsNullPtr, "" ); 423 424 if( !CV_IS_SUBDIV2D(subdiv) ) 425 CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR ); 426 427 428 location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point ); 429 430 switch (location) 431 { 432 case CV_PTLOC_ERROR: 433 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); 434 435 case CV_PTLOC_OUTSIDE_RECT: 436 CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR ); 437 438 case CV_PTLOC_VERTEX: 439 point = curr_point; 440 break; 441 442 case CV_PTLOC_ON_EDGE: 443 deleted_edge = curr_edge; 444 subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); 445 cvSubdiv2DDeleteEdge( subdiv, deleted_edge ); 446 /* no break */ 447 448 case CV_PTLOC_INSIDE: 449 450 assert( curr_edge != 0 ); 451 subdiv->is_geometry_valid = 0; 452 453 curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 ); 454 CV_CHECK(); 455 456 base_edge = cvSubdiv2DMakeEdge( subdiv ); 457 first_point = cvSubdiv2DEdgeOrg( curr_edge ); 458 cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point ); 459 cvSubdiv2DSplice( base_edge, curr_edge ); 460 461 do 462 { 463 base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge, 464 cvSubdiv2DSymEdge( base_edge )); 465 curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG ); 466 } 467 while( cvSubdiv2DEdgeDst( curr_edge ) != first_point ); 468 469 curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG ); 470 471 max_edges = subdiv->quad_edges * 4; 472 473 for( i = 0; i < max_edges; i++ ) 474 { 475 CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0; 476 CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); 477 478 temp_dst = cvSubdiv2DEdgeDst( temp_edge ); 479 curr_org = cvSubdiv2DEdgeOrg( curr_edge ); 480 curr_dst = cvSubdiv2DEdgeDst( curr_edge ); 481 482 if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 && 483 icvIsPtInCircle3( curr_org->pt, temp_dst->pt, 484 curr_dst->pt, curr_point->pt ) < 0 ) 485 { 486 cvSubdiv2DSwapEdges( curr_edge ); 487 curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); 488 } 489 else if( curr_org == first_point ) 490 { 491 break; 492 } 493 else 494 { 495 curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ), 496 CV_PREV_AROUND_LEFT ); 497 } 498 } 499 break; 500 default: 501 assert( 0 ); 502 CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR ); 503 } 504 505 point = curr_point; 506 507 508 __END__; 509 510 //icvSubdiv2DCheck( subdiv ); 511 512 return point; 513 } 514 515 516 CV_IMPL void 517 cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect ) 518 { 519 float big_coord = 3.f * MAX( rect.width, rect.height ); 520 CvPoint2D32f ppA, ppB, ppC; 521 CvSubdiv2DPoint *pA, *pB, *pC; 522 CvSubdiv2DEdge edge_AB, edge_BC, edge_CA; 523 float rx = (float) rect.x; 524 float ry = (float) rect.y; 525 526 CV_FUNCNAME( "cvSubdivDelaunay2DInit" ); 527 528 __BEGIN__; 529 530 if( !subdiv ) 531 CV_ERROR( CV_StsNullPtr, "" ); 532 533 cvClearSet( (CvSet *) (subdiv->edges) ); 534 cvClearSet( (CvSet *) subdiv ); 535 536 subdiv->quad_edges = 0; 537 subdiv->recent_edge = 0; 538 subdiv->is_geometry_valid = 0; 539 540 subdiv->topleft = cvPoint2D32f( rx, ry ); 541 subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height ); 542 543 ppA = cvPoint2D32f( rx + big_coord, ry ); 544 ppB = cvPoint2D32f( rx, ry + big_coord ); 545 ppC = cvPoint2D32f( rx - big_coord, ry - big_coord ); 546 547 pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 ); 548 pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 ); 549 pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 ); 550 551 edge_AB = cvSubdiv2DMakeEdge( subdiv ); 552 edge_BC = cvSubdiv2DMakeEdge( subdiv ); 553 edge_CA = cvSubdiv2DMakeEdge( subdiv ); 554 555 cvSubdiv2DSetEdgePoints( edge_AB, pA, pB ); 556 cvSubdiv2DSetEdgePoints( edge_BC, pB, pC ); 557 cvSubdiv2DSetEdgePoints( edge_CA, pC, pA ); 558 559 cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA )); 560 cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB )); 561 cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC )); 562 563 subdiv->recent_edge = edge_AB; 564 565 566 __END__; 567 } 568 569 570 CV_IMPL void 571 cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv ) 572 { 573 int elem_size; 574 int i, total; 575 CvSeqReader reader; 576 577 CV_FUNCNAME( "cvClearVoronoi2D" ); 578 579 __BEGIN__; 580 581 if( !subdiv ) 582 CV_ERROR( CV_StsNullPtr, "" ); 583 584 /* clear pointers to voronoi points */ 585 total = subdiv->edges->total; 586 elem_size = subdiv->edges->elem_size; 587 588 cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 ); 589 590 for( i = 0; i < total; i++ ) 591 { 592 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr; 593 594 quadedge->pt[1] = quadedge->pt[3] = 0; 595 CV_NEXT_SEQ_ELEM( elem_size, reader ); 596 } 597 598 /* remove voronoi points */ 599 total = subdiv->total; 600 elem_size = subdiv->elem_size; 601 602 cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 ); 603 604 for( i = 0; i < total; i++ ) 605 { 606 CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr; 607 608 /* check for virtual point. it is also check that the point exists */ 609 if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG ) 610 { 611 cvSetRemoveByPtr( (CvSet*)subdiv, pt ); 612 } 613 CV_NEXT_SEQ_ELEM( elem_size, reader ); 614 } 615 616 subdiv->is_geometry_valid = 0; 617 618 619 __END__; 620 } 621 622 623 CV_IMPL void 624 cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv ) 625 { 626 CvSeqReader reader; 627 int i, total, elem_size; 628 629 CV_FUNCNAME( "cvCalcSubdivVoronoi2D" ); 630 631 __BEGIN__; 632 633 if( !subdiv ) 634 CV_ERROR( CV_StsNullPtr, "" ); 635 636 /* check if it is already calculated */ 637 if( subdiv->is_geometry_valid ) 638 EXIT; 639 640 total = subdiv->edges->total; 641 elem_size = subdiv->edges->elem_size; 642 643 cvClearSubdivVoronoi2D( subdiv ); 644 645 cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 ); 646 647 if( total <= 3 ) 648 EXIT; 649 650 /* skip first three edges (bounding triangle) */ 651 for( i = 0; i < 3; i++ ) 652 CV_NEXT_SEQ_ELEM( elem_size, reader ); 653 654 /* loop through all quad-edges */ 655 for( ; i < total; i++ ) 656 { 657 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr); 658 659 if( CV_IS_SET_ELEM( quadedge )) 660 { 661 CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2; 662 double a0, b0, c0, a1, b1, c1; 663 CvPoint2D32f virt_point; 664 CvSubdiv2DPoint *voronoi_point; 665 666 if( !quadedge->pt[3] ) 667 { 668 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT ); 669 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT ); 670 671 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 ); 672 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 ); 673 674 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point ); 675 if( fabs( virt_point.x ) < FLT_MAX * 0.5 && 676 fabs( virt_point.y ) < FLT_MAX * 0.5 ) 677 { 678 voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 ); 679 680 quadedge->pt[3] = 681 ((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] = 682 ((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point; 683 } 684 } 685 686 if( !quadedge->pt[1] ) 687 { 688 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT ); 689 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT ); 690 691 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 ); 692 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 ); 693 694 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point ); 695 696 if( fabs( virt_point.x ) < FLT_MAX * 0.5 && 697 fabs( virt_point.y ) < FLT_MAX * 0.5 ) 698 { 699 voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 ); 700 701 quadedge->pt[1] = 702 ((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] = 703 ((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point; 704 } 705 } 706 } 707 708 CV_NEXT_SEQ_ELEM( elem_size, reader ); 709 } 710 711 subdiv->is_geometry_valid = 1; 712 713 714 __END__; 715 } 716 717 718 static int 719 icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff ) 720 { 721 Cv32suf cw_area; 722 cw_area.f = (org.x - pt.x)*diff.y - (org.y - pt.y)*diff.x; 723 return (cw_area.i > 0)*2 - (cw_area.i*2 != 0); 724 } 725 726 727 CV_IMPL CvSubdiv2DPoint* 728 cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt ) 729 { 730 CvSubdiv2DPoint* point = 0; 731 CvPoint2D32f start; 732 CvPoint2D32f diff; 733 CvSubdiv2DPointLocation loc; 734 CvSubdiv2DEdge edge; 735 int i; 736 737 CV_FUNCNAME("cvFindNearestPoint2D"); 738 739 __BEGIN__; 740 741 if( !subdiv ) 742 CV_ERROR( CV_StsNullPtr, "" ); 743 744 if( !CV_IS_SUBDIV2D( subdiv )) 745 CV_ERROR( CV_StsNullPtr, "" ); 746 747 if( !subdiv->is_geometry_valid ) 748 cvCalcSubdivVoronoi2D( subdiv ); 749 750 loc = cvSubdiv2DLocate( subdiv, pt, &edge, &point ); 751 752 switch( loc ) 753 { 754 case CV_PTLOC_ON_EDGE: 755 case CV_PTLOC_INSIDE: 756 break; 757 default: 758 EXIT; 759 } 760 761 point = 0; 762 763 start = cvSubdiv2DEdgeOrg( edge )->pt; 764 diff.x = pt.x - start.x; 765 diff.y = pt.y - start.y; 766 767 edge = cvSubdiv2DRotateEdge( edge, 1 ); 768 769 for( i = 0; i < subdiv->total; i++ ) 770 { 771 CvPoint2D32f t; 772 773 for(;;) 774 { 775 assert( cvSubdiv2DEdgeDst( edge )); 776 777 t = cvSubdiv2DEdgeDst( edge )->pt; 778 if( icvIsRightOf2( t, start, diff ) >= 0 ) 779 break; 780 781 edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT ); 782 } 783 784 for(;;) 785 { 786 assert( cvSubdiv2DEdgeOrg( edge )); 787 788 t = cvSubdiv2DEdgeOrg( edge )->pt; 789 if( icvIsRightOf2( t, start, diff ) < 0 ) 790 break; 791 792 edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_LEFT ); 793 } 794 795 { 796 CvPoint2D32f tempDiff = cvSubdiv2DEdgeDst( edge )->pt; 797 t = cvSubdiv2DEdgeOrg( edge )->pt; 798 tempDiff.x -= t.x; 799 tempDiff.y -= t.y; 800 801 if( icvIsRightOf2( pt, t, tempDiff ) >= 0 ) 802 { 803 point = cvSubdiv2DEdgeOrg( cvSubdiv2DRotateEdge( edge, 3 )); 804 break; 805 } 806 } 807 808 edge = cvSubdiv2DSymEdge( edge ); 809 } 810 811 __END__; 812 813 return point; 814 } 815 816 /* Removed from the main interface */ 817 818 #if 0 819 /* Adds new isolated quadedge to the subdivision */ 820 OPENCVAPI CvSubdiv2DEdge cvSubdiv2DMakeEdge( CvSubdiv2D* subdiv ); 821 822 823 /* Adds new isolated point to subdivision */ 824 OPENCVAPI CvSubdiv2DPoint* cvSubdiv2DAddPoint( CvSubdiv2D* subdiv, 825 CvPoint2D32f pt, int is_virtual ); 826 827 828 /* Does a splice operation for two quadedges */ 829 OPENCVAPI void cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ); 830 831 832 /* Assigns ending [non-virtual] points for given quadedge */ 833 OPENCVAPI void cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge, 834 CvSubdiv2DPoint* org_pt, 835 CvSubdiv2DPoint* dst_pt ); 836 837 /* Removes quadedge from subdivision */ 838 OPENCVAPI void cvSubdiv2DDeleteEdge( CvSubdiv2D* subdiv, CvSubdiv2DEdge edge ); 839 840 841 /* Connects estination point of the first edge with origin point of the second edge */ 842 OPENCVAPI CvSubdiv2DEdge cvSubdiv2DConnectEdges( CvSubdiv2D* subdiv, 843 CvSubdiv2DEdge edgeA, 844 CvSubdiv2DEdge edgeB ); 845 846 /* Swaps diagonal in two connected Delaunay facets */ 847 OPENCVAPI void cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge ); 848 #endif 849 850 /* End of file. */ 851
