vcs.maemo.org Git - opencv/blob - src/cv/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( CV_StsBadSize, "" );
  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( CV_StsBadFlag, "" );
 270
 271     max_edges = subdiv->quad_edges * 4;
 272     edge = subdiv->recent_edge;
 273
 274     if( max_edges == 0 )
 275         CV_ERROR( CV_StsBadSize, "" );
 276     CV_ASSERT(edge != 0);
 277
 278     location = CV_PTLOC_OUTSIDE_RECT;
 279     if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y ||
 280         pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y )
 281         CV_ERROR( CV_StsOutOfRange, "" );
 282
 283     location = CV_PTLOC_ERROR;
 284
 285     right_of_curr = icvIsRightOf( pt, edge );
 286     if( right_of_curr > 0 )
 287     {
 288         edge = cvSubdiv2DSymEdge( edge );
 289         right_of_curr = -right_of_curr;
 290     }
 291
 292     for( i = 0; i < max_edges; i++ )
 293     {
 294         CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge );
 295         CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST );
 296
 297         int right_of_onext = icvIsRightOf( pt, onext_edge );
 298         int right_of_dprev = icvIsRightOf( pt, dprev_edge );
 299
 300         if( right_of_dprev > 0 )
 301         {
 302             if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
 303             {
 304                 location = CV_PTLOC_INSIDE;
 305                 EXIT;
 306             }
 307             else
 308             {
 309                 right_of_curr = right_of_onext;
 310                 edge = onext_edge;
 311             }
 312         }
 313         else
 314         {
 315             if( right_of_onext > 0 )
 316             {
 317                 if( right_of_dprev == 0 && right_of_curr == 0 )
 318                 {
 319                     location = CV_PTLOC_INSIDE;
 320                     EXIT;
 321                 }
 322                 else
 323                 {
 324                     right_of_curr = right_of_dprev;
 325                     edge = dprev_edge;
 326                 }
 327             }
 328             else if( right_of_curr == 0 &&
 329                      icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 )
 330             {
 331                 edge = cvSubdiv2DSymEdge( edge );
 332             }
 333             else
 334             {
 335                 right_of_curr = right_of_onext;
 336                 edge = onext_edge;
 337             }
 338         }
 339     }
 340
 341
 342     __END__;
 343
 344     subdiv->recent_edge = edge;
 345
 346     if( location == CV_PTLOC_INSIDE )
 347     {
 348         double t1, t2, t3;
 349         CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt;
 350         CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt;
 351
 352         t1 = fabs( pt.x - org_pt.x );
 353         t1 += fabs( pt.y - org_pt.y );
 354         t2 = fabs( pt.x - dst_pt.x );
 355         t2 += fabs( pt.y - dst_pt.y );
 356         t3 = fabs( org_pt.x - dst_pt.x );
 357         t3 += fabs( org_pt.y - dst_pt.y );
 358
 359         if( t1 < FLT_EPSILON )
 360         {
 361             location = CV_PTLOC_VERTEX;
 362             point = cvSubdiv2DEdgeOrg( edge );
 363             edge = 0;
 364         }
 365         else if( t2 < FLT_EPSILON )
 366         {
 367             location = CV_PTLOC_VERTEX;
 368             point = cvSubdiv2DEdgeDst( edge );
 369             edge = 0;
 370         }
 371         else if( (t1 < t3 || t2 < t3) &&
 372                  fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
 373         {
 374             location = CV_PTLOC_ON_EDGE;
 375             point = 0;
 376         }
 377     }
 378
 379     if( location == CV_PTLOC_ERROR )
 380     {
 381         edge = 0;
 382         point = 0;
 383     }
 384
 385     if( _edge )
 386         *_edge = edge;
 387     if( _point )
 388         *_point = point;
 389
 390     return location;
 391 }
 392
 393
 394 CV_INLINE int
 395 icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c )
 396 {
 397     double val = (a.x * a.x + a.y * a.y) * cvTriangleArea( b, c, pt );
 398     val -= (b.x * b.x + b.y * b.y) * cvTriangleArea( a, c, pt );
 399     val += (c.x * c.x + c.y * c.y) * cvTriangleArea( a, b, pt );
 400     val -= (pt.x * pt.x + pt.y * pt.y) * cvTriangleArea( a, b, c );
 401
 402     return val > FLT_EPSILON ? 1 : val < -FLT_EPSILON ? -1 : 0;
 403 }
 404
 405
 406 CV_IMPL CvSubdiv2DPoint *
 407 cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt )
 408 {
 409     CvSubdiv2DPoint *point = 0;
 410     CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
 411
 412     CvSubdiv2DPoint *curr_point = 0, *first_point = 0;
 413     CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0;
 414     int i, max_edges;
 415
 416     CV_FUNCNAME( "cvSubdivDelaunay2DInsert" );
 417
 418     __BEGIN__;
 419
 420     if( !subdiv )
 421         CV_ERROR( CV_StsNullPtr, "" );
 422
 423     if( !CV_IS_SUBDIV2D(subdiv) )
 424         CV_ERROR( CV_StsBadFlag, "" );
 425
 426     location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point );
 427
 428     switch (location)
 429     {
 430     case CV_PTLOC_ERROR:
 431         CV_ERROR( CV_StsBadSize, "" );
 432
 433     case CV_PTLOC_OUTSIDE_RECT:
 434         CV_ERROR( CV_StsOutOfRange, "" );
 435
 436     case CV_PTLOC_VERTEX:
 437         point = curr_point;
 438         break;
 439
 440     case CV_PTLOC_ON_EDGE:
 441         deleted_edge = curr_edge;
 442         subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 443         cvSubdiv2DDeleteEdge( subdiv, deleted_edge );
 444         /* no break */
 445
 446     case CV_PTLOC_INSIDE:
 447
 448         assert( curr_edge != 0 );
 449         subdiv->is_geometry_valid = 0;
 450
 451         curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 );
 452         CV_CHECK();
 453
 454         base_edge = cvSubdiv2DMakeEdge( subdiv );
 455         first_point = cvSubdiv2DEdgeOrg( curr_edge );
 456         cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point );
 457         cvSubdiv2DSplice( base_edge, curr_edge );
 458
 459         do
 460         {
 461             base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge,
 462                                                 cvSubdiv2DSymEdge( base_edge ));
 463             curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
 464         }
 465         while( cvSubdiv2DEdgeDst( curr_edge ) != first_point );
 466
 467         curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
 468
 469         max_edges = subdiv->quad_edges * 4;
 470
 471         for( i = 0; i < max_edges; i++ )
 472         {
 473             CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0;
 474             CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 475
 476             temp_dst = cvSubdiv2DEdgeDst( temp_edge );
 477             curr_org = cvSubdiv2DEdgeOrg( curr_edge );
 478             curr_dst = cvSubdiv2DEdgeDst( curr_edge );
 479
 480             if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 &&
 481                 icvIsPtInCircle3( curr_org->pt, temp_dst->pt,
 482                                   curr_dst->pt, curr_point->pt ) < 0 )
 483             {
 484                 cvSubdiv2DSwapEdges( curr_edge );
 485                 curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 486             }
 487             else if( curr_org == first_point )
 488             {
 489                 break;
 490             }
 491             else
 492             {
 493                 curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ),
 494                                                CV_PREV_AROUND_LEFT );
 495             }
 496         }
 497         break;
 498     default:
 499         CV_Error_(CV_StsError, ("cvSubdiv2DLocate returned invalid location = %d", location) );
 500     }
 501
 502     point = curr_point;
 503
 504
 505     __END__;
 506
 507     //icvSubdiv2DCheck( subdiv );
 508
 509     return point;
 510 }
 511
 512
 513 CV_IMPL void
 514 cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect )
 515 {
 516     float big_coord = 3.f * MAX( rect.width, rect.height );
 517     CvPoint2D32f ppA, ppB, ppC;
 518     CvSubdiv2DPoint *pA, *pB, *pC;
 519     CvSubdiv2DEdge edge_AB, edge_BC, edge_CA;
 520     float rx = (float) rect.x;
 521     float ry = (float) rect.y;
 522
 523     CV_FUNCNAME( "cvSubdivDelaunay2DInit" );
 524
 525     __BEGIN__;
 526
 527     if( !subdiv )
 528         CV_ERROR( CV_StsNullPtr, "" );
 529
 530     cvClearSet( (CvSet *) (subdiv->edges) );
 531     cvClearSet( (CvSet *) subdiv );
 532
 533     subdiv->quad_edges = 0;
 534     subdiv->recent_edge = 0;
 535     subdiv->is_geometry_valid = 0;
 536
 537     subdiv->topleft = cvPoint2D32f( rx, ry );
 538     subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height );
 539
 540     ppA = cvPoint2D32f( rx + big_coord, ry );
 541     ppB = cvPoint2D32f( rx, ry + big_coord );
 542     ppC = cvPoint2D32f( rx - big_coord, ry - big_coord );
 543
 544     pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 );
 545     pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 );
 546     pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 );
 547
 548     edge_AB = cvSubdiv2DMakeEdge( subdiv );
 549     edge_BC = cvSubdiv2DMakeEdge( subdiv );
 550     edge_CA = cvSubdiv2DMakeEdge( subdiv );
 551
 552     cvSubdiv2DSetEdgePoints( edge_AB, pA, pB );
 553     cvSubdiv2DSetEdgePoints( edge_BC, pB, pC );
 554     cvSubdiv2DSetEdgePoints( edge_CA, pC, pA );
 555
 556     cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA ));
 557     cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB ));
 558     cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC ));
 559
 560     subdiv->recent_edge = edge_AB;
 561
 562
 563     __END__;
 564 }
 565
 566
 567 CV_IMPL void
 568 cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv )
 569 {
 570     int elem_size;
 571     int i, total;
 572     CvSeqReader reader;
 573
 574     CV_FUNCNAME( "cvClearVoronoi2D" );
 575
 576     __BEGIN__;
 577
 578     if( !subdiv )
 579         CV_ERROR( CV_StsNullPtr, "" );
 580
 581     /* clear pointers to voronoi points */
 582     total = subdiv->edges->total;
 583     elem_size = subdiv->edges->elem_size;
 584
 585     cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
 586
 587     for( i = 0; i < total; i++ )
 588     {
 589         CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr;
 590
 591         quadedge->pt[1] = quadedge->pt[3] = 0;
 592         CV_NEXT_SEQ_ELEM( elem_size, reader );
 593     }
 594
 595     /* remove voronoi points */
 596     total = subdiv->total;
 597     elem_size = subdiv->elem_size;
 598
 599     cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 );
 600
 601     for( i = 0; i < total; i++ )
 602     {
 603         CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr;
 604
 605         /* check for virtual point. it is also check that the point exists */
 606         if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG )
 607         {
 608             cvSetRemoveByPtr( (CvSet*)subdiv, pt );
 609         }
 610         CV_NEXT_SEQ_ELEM( elem_size, reader );
 611     }
 612
 613     subdiv->is_geometry_valid = 0;
 614
 615
 616     __END__;
 617 }
 618
 619
 620 CV_IMPL void
 621 cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv )
 622 {
 623     CvSeqReader reader;
 624     int i, total, elem_size;
 625
 626     CV_FUNCNAME( "cvCalcSubdivVoronoi2D" );
 627
 628     __BEGIN__;
 629
 630     if( !subdiv )
 631         CV_ERROR( CV_StsNullPtr, "" );
 632
 633     /* check if it is already calculated */
 634     if( subdiv->is_geometry_valid )
 635         EXIT;
 636
 637     total = subdiv->edges->total;
 638     elem_size = subdiv->edges->elem_size;
 639
 640     cvClearSubdivVoronoi2D( subdiv );
 641
 642     cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
 643
 644     if( total <= 3 )
 645         EXIT;
 646
 647     /* skip first three edges (bounding triangle) */
 648     for( i = 0; i < 3; i++ )
 649         CV_NEXT_SEQ_ELEM( elem_size, reader );
 650
 651     /* loop through all quad-edges */
 652     for( ; i < total; i++ )
 653     {
 654         CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr);
 655
 656         if( CV_IS_SET_ELEM( quadedge ))
 657         {
 658             CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2;
 659             double a0, b0, c0, a1, b1, c1;
 660             CvPoint2D32f virt_point;
 661             CvSubdiv2DPoint *voronoi_point;
 662
 663             if( !quadedge->pt[3] )
 664             {
 665                 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT );
 666                 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT );
 667
 668                 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
 669                 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
 670
 671                 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
 672                 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 673                     fabs( virt_point.y ) < FLT_MAX * 0.5 )
 674                 {
 675                     voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
 676
 677                     quadedge->pt[3] =
 678                         ((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] =
 679                         ((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point;
 680                 }
 681             }
 682
 683             if( !quadedge->pt[1] )
 684             {
 685                 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT );
 686                 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT );
 687
 688                 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
 689                 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
 690
 691                 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
 692
 693                 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 694                     fabs( virt_point.y ) < FLT_MAX * 0.5 )
 695                 {
 696                     voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
 697
 698                     quadedge->pt[1] =
 699                         ((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] =
 700                         ((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point;
 701                 }
 702             }
 703         }
 704
 705         CV_NEXT_SEQ_ELEM( elem_size, reader );
 706     }
 707
 708     subdiv->is_geometry_valid = 1;
 709
 710
 711     __END__;
 712 }
 713
 714
 715 static int
 716 icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff )
 717 {
 718     Cv32suf cw_area;
 719     cw_area.f = (org.x - pt.x)*diff.y - (org.y - pt.y)*diff.x;
 720     return (cw_area.i > 0)*2 - (cw_area.i*2 != 0);
 721 }
 722
 723
 724 CV_IMPL CvSubdiv2DPoint*
 725 cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt )
 726 {
 727     CvSubdiv2DPoint* point = 0;
 728     CvPoint2D32f start;
 729     CvPoint2D32f diff;
 730     CvSubdiv2DPointLocation loc;
 731     CvSubdiv2DEdge edge;
 732     int i;
 733
 734     CV_FUNCNAME("cvFindNearestPoint2D");
 735
 736     __BEGIN__;
 737
 738     if( !subdiv )
 739         CV_ERROR( CV_StsNullPtr, "" );
 740
 741     if( !CV_IS_SUBDIV2D( subdiv ))
 742         CV_ERROR( CV_StsNullPtr, "" );
 743
 744     if( subdiv->edges->active_count <= 3 )
 745         EXIT;
 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. */