git.maemo.org Git - opencv/blob - cv/src/cvsubdivision2d.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 "_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. */