modules/imgproc/src/subdivision2d.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41 #include "precomp.hpp"
  42
  43 CV_IMPL CvSubdiv2D *
  44 cvCreateSubdiv2D( int subdiv_type, int header_size,
  45                   int vtx_size, int quadedge_size, CvMemStorage * storage )
  46 {
  47     if( !storage )
  48         CV_Error( CV_StsNullPtr, "" );
  49
  50     if( header_size < (int)sizeof( CvSubdiv2D ) ||
  51         quadedge_size < (int)sizeof( CvQuadEdge2D ) ||
  52         vtx_size < (int)sizeof( CvSubdiv2DPoint ))
  53         CV_Error( CV_StsBadSize, "" );
  54
  55     return (CvSubdiv2D *)cvCreateGraph( subdiv_type, header_size,
  56                                         vtx_size, quadedge_size, storage );
  57 }
  58
  59
  60 /****************************************************************************************\
  61 *                                    Quad Edge  algebra                                  *
  62 \****************************************************************************************/
  63
  64 static CvSubdiv2DEdge
  65 cvSubdiv2DMakeEdge( CvSubdiv2D * subdiv )
  66 {
  67     if( !subdiv )
  68         CV_Error( CV_StsNullPtr, "" );
  69
  70     CvQuadEdge2D* edge = (CvQuadEdge2D*)cvSetNew( (CvSet*)subdiv->edges );
  71     memset( edge->pt, 0, sizeof( edge->pt ));
  72     CvSubdiv2DEdge edgehandle = (CvSubdiv2DEdge) edge;
  73
  74     edge->next[0] = edgehandle;
  75     edge->next[1] = edgehandle + 3;
  76     edge->next[2] = edgehandle + 2;
  77     edge->next[3] = edgehandle + 1;
  78
  79     subdiv->quad_edges++;
  80     return edgehandle;
  81 }
  82
  83
  84 static CvSubdiv2DPoint *
  85 cvSubdiv2DAddPoint( CvSubdiv2D * subdiv, CvPoint2D32f pt, int is_virtual )
  86 {
  87     CvSubdiv2DPoint* subdiv_point = (CvSubdiv2DPoint*)cvSetNew( (CvSet*)subdiv );
  88     if( subdiv_point )
  89     {
  90         memset( subdiv_point, 0, subdiv->elem_size );
  91         subdiv_point->pt = pt;
  92         subdiv_point->first = 0;
  93         subdiv_point->flags |= is_virtual ? CV_SUBDIV2D_VIRTUAL_POINT_FLAG : 0;
  94                 subdiv_point->id = -1;
  95     }
  96
  97     return subdiv_point;
  98 }
  99
 100
 101 static void
 102 cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
 103 {
 104     CvSubdiv2DEdge *a_next = &CV_SUBDIV2D_NEXT_EDGE( edgeA );
 105     CvSubdiv2DEdge *b_next = &CV_SUBDIV2D_NEXT_EDGE( edgeB );
 106     CvSubdiv2DEdge a_rot = cvSubdiv2DRotateEdge( *a_next, 1 );
 107     CvSubdiv2DEdge b_rot = cvSubdiv2DRotateEdge( *b_next, 1 );
 108     CvSubdiv2DEdge *a_rot_next = &CV_SUBDIV2D_NEXT_EDGE( a_rot );
 109     CvSubdiv2DEdge *b_rot_next = &CV_SUBDIV2D_NEXT_EDGE( b_rot );
 110     CvSubdiv2DEdge t;
 111
 112     CV_SWAP( *a_next, *b_next, t );
 113     CV_SWAP( *a_rot_next, *b_rot_next, t );
 114 }
 115
 116
 117 static void
 118 cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge,
 119                          CvSubdiv2DPoint * org_pt, CvSubdiv2DPoint * dst_pt )
 120 {
 121     CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
 122
 123     if( !quadedge )
 124         CV_Error( CV_StsNullPtr, "" );
 125
 126     quadedge->pt[edge & 3] = org_pt;
 127     quadedge->pt[(edge + 2) & 3] = dst_pt;
 128 }
 129
 130
 131 static void
 132 cvSubdiv2DDeleteEdge( CvSubdiv2D * subdiv, CvSubdiv2DEdge edge )
 133 {
 134     CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
 135
 136     if( !subdiv || !quadedge )
 137         CV_Error( CV_StsNullPtr, "" );
 138
 139     cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG ));
 140
 141     CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
 142     cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG ));
 143
 144     cvSetRemoveByPtr( (CvSet*)(subdiv->edges), quadedge );
 145     subdiv->quad_edges--;
 146 }
 147
 148
 149 static CvSubdiv2DEdge
 150 cvSubdiv2DConnectEdges( CvSubdiv2D * subdiv, CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
 151 {
 152     if( !subdiv )
 153         CV_Error( CV_StsNullPtr, "" );
 154
 155     CvSubdiv2DEdge new_edge = cvSubdiv2DMakeEdge( subdiv );
 156
 157     cvSubdiv2DSplice( new_edge, cvSubdiv2DGetEdge( edgeA, CV_NEXT_AROUND_LEFT ));
 158     cvSubdiv2DSplice( cvSubdiv2DSymEdge( new_edge ), edgeB );
 159
 160     CvSubdiv2DPoint* dstA = cvSubdiv2DEdgeDst( edgeA );
 161     CvSubdiv2DPoint* orgB = cvSubdiv2DEdgeOrg( edgeB );
 162     cvSubdiv2DSetEdgePoints( new_edge, dstA, orgB );
 163
 164     return new_edge;
 165 }
 166
 167
 168 static void
 169 cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge )
 170 {
 171     CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
 172     CvSubdiv2DEdge a = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG );
 173     CvSubdiv2DEdge b = cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG );
 174     CvSubdiv2DPoint *dstB, *dstA;
 175
 176     cvSubdiv2DSplice( edge, a );
 177     cvSubdiv2DSplice( sym_edge, b );
 178
 179     dstA = cvSubdiv2DEdgeDst( a );
 180     dstB = cvSubdiv2DEdgeDst( b );
 181     cvSubdiv2DSetEdgePoints( edge, dstA, dstB );
 182
 183     cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( a, CV_NEXT_AROUND_LEFT ));
 184     cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( b, CV_NEXT_AROUND_LEFT ));
 185 }
 186
 187
 188 static int
 189 icvIsRightOf( CvPoint2D32f& pt, CvSubdiv2DEdge edge )
 190 {
 191     CvSubdiv2DPoint *org = cvSubdiv2DEdgeOrg(edge), *dst = cvSubdiv2DEdgeDst(edge);
 192     double cw_area = cvTriangleArea( pt, dst->pt, org->pt );
 193
 194     return (cw_area > 0) - (cw_area < 0);
 195 }
 196
 197
 198 CV_IMPL CvSubdiv2DPointLocation
 199 cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt,
 200                   CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point )
 201 {
 202     CvSubdiv2DPoint *point = 0;
 203     int right_of_curr = 0;
 204
 205     if( !subdiv )
 206         CV_Error( CV_StsNullPtr, "" );
 207
 208     if( !CV_IS_SUBDIV2D(subdiv) )
 209         CV_Error( CV_StsBadFlag, "" );
 210
 211     int i, max_edges = subdiv->quad_edges * 4;
 212     CvSubdiv2DEdge edge = subdiv->recent_edge;
 213
 214     if( max_edges == 0 )
 215         CV_Error( CV_StsBadSize, "" );
 216     CV_Assert(edge != 0);
 217
 218     if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y ||
 219         pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y )
 220         CV_Error( CV_StsOutOfRange, "" );
 221
 222     CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
 223
 224     right_of_curr = icvIsRightOf( pt, edge );
 225     if( right_of_curr > 0 )
 226     {
 227         edge = cvSubdiv2DSymEdge( edge );
 228         right_of_curr = -right_of_curr;
 229     }
 230
 231     for( i = 0; i < max_edges; i++ )
 232     {
 233         CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge );
 234         CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST );
 235
 236         int right_of_onext = icvIsRightOf( pt, onext_edge );
 237         int right_of_dprev = icvIsRightOf( pt, dprev_edge );
 238
 239         if( right_of_dprev > 0 )
 240         {
 241             if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
 242             {
 243                 location = CV_PTLOC_INSIDE;
 244                 goto exit;
 245             }
 246             else
 247             {
 248                 right_of_curr = right_of_onext;
 249                 edge = onext_edge;
 250             }
 251         }
 252         else
 253         {
 254             if( right_of_onext > 0 )
 255             {
 256                 if( right_of_dprev == 0 && right_of_curr == 0 )
 257                 {
 258                     location = CV_PTLOC_INSIDE;
 259                     goto exit;
 260                 }
 261                 else
 262                 {
 263                     right_of_curr = right_of_dprev;
 264                     edge = dprev_edge;
 265                 }
 266             }
 267             else if( right_of_curr == 0 &&
 268                      icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 )
 269             {
 270                 edge = cvSubdiv2DSymEdge( edge );
 271             }
 272             else
 273             {
 274                 right_of_curr = right_of_onext;
 275                 edge = onext_edge;
 276             }
 277         }
 278     }
 279 exit:
 280
 281     subdiv->recent_edge = edge;
 282
 283     if( location == CV_PTLOC_INSIDE )
 284     {
 285         double t1, t2, t3;
 286         CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt;
 287         CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt;
 288
 289         t1 = fabs( pt.x - org_pt.x );
 290         t1 += fabs( pt.y - org_pt.y );
 291         t2 = fabs( pt.x - dst_pt.x );
 292         t2 += fabs( pt.y - dst_pt.y );
 293         t3 = fabs( org_pt.x - dst_pt.x );
 294         t3 += fabs( org_pt.y - dst_pt.y );
 295
 296         if( t1 < FLT_EPSILON )
 297         {
 298             location = CV_PTLOC_VERTEX;
 299             point = cvSubdiv2DEdgeOrg( edge );
 300             edge = 0;
 301         }
 302         else if( t2 < FLT_EPSILON )
 303         {
 304             location = CV_PTLOC_VERTEX;
 305             point = cvSubdiv2DEdgeDst( edge );
 306             edge = 0;
 307         }
 308         else if( (t1 < t3 || t2 < t3) &&
 309                  fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
 310         {
 311             location = CV_PTLOC_ON_EDGE;
 312             point = 0;
 313         }
 314     }
 315
 316     if( location == CV_PTLOC_ERROR )
 317     {
 318         edge = 0;
 319         point = 0;
 320     }
 321
 322     if( _edge )
 323         *_edge = edge;
 324     if( _point )
 325         *_point = point;
 326
 327     return location;
 328 }
 329
 330
 331 CV_INLINE int
 332 icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c )
 333 {
 334     const double eps = FLT_EPSILON*0.125;
 335     double val = ((double)a.x * a.x + (double)a.y * a.y) * cvTriangleArea( b, c, pt );
 336     val -= ((double)b.x * b.x + (double)b.y * b.y) * cvTriangleArea( a, c, pt );
 337     val += ((double)c.x * c.x + (double)c.y * c.y) * cvTriangleArea( a, b, pt );
 338     val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * cvTriangleArea( a, b, c );
 339
 340     return val > eps ? 1 : val < -eps ? -1 : 0;
 341 }
 342
 343
 344 CV_IMPL CvSubdiv2DPoint *
 345 cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt )
 346 {
 347     CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
 348
 349     CvSubdiv2DPoint *curr_point = 0, *first_point = 0;
 350     CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0;
 351     int i, max_edges;
 352
 353     if( !subdiv )
 354         CV_Error( CV_StsNullPtr, "" );
 355
 356     if( !CV_IS_SUBDIV2D(subdiv) )
 357         CV_Error( CV_StsBadFlag, "" );
 358
 359     location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point );
 360
 361     switch (location)
 362     {
 363     case CV_PTLOC_ERROR:
 364         CV_Error( CV_StsBadSize, "" );
 365
 366     case CV_PTLOC_OUTSIDE_RECT:
 367         CV_Error( CV_StsOutOfRange, "" );
 368
 369     case CV_PTLOC_VERTEX:
 370         break;
 371
 372     case CV_PTLOC_ON_EDGE:
 373         deleted_edge = curr_edge;
 374         subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 375         cvSubdiv2DDeleteEdge( subdiv, deleted_edge );
 376         /* no break */
 377
 378     case CV_PTLOC_INSIDE:
 379
 380         assert( curr_edge != 0 );
 381         subdiv->is_geometry_valid = 0;
 382
 383         curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 );
 384         base_edge = cvSubdiv2DMakeEdge( subdiv );
 385         first_point = cvSubdiv2DEdgeOrg( curr_edge );
 386         cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point );
 387         cvSubdiv2DSplice( base_edge, curr_edge );
 388
 389         do
 390         {
 391             base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge,
 392                                                 cvSubdiv2DSymEdge( base_edge ));
 393             curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
 394         }
 395         while( cvSubdiv2DEdgeDst( curr_edge ) != first_point );
 396
 397         curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
 398
 399         max_edges = subdiv->quad_edges * 4;
 400
 401         for( i = 0; i < max_edges; i++ )
 402         {
 403             CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0;
 404             CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 405
 406             temp_dst = cvSubdiv2DEdgeDst( temp_edge );
 407             curr_org = cvSubdiv2DEdgeOrg( curr_edge );
 408             curr_dst = cvSubdiv2DEdgeDst( curr_edge );
 409
 410             if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 &&
 411                 icvIsPtInCircle3( curr_org->pt, temp_dst->pt,
 412                                   curr_dst->pt, curr_point->pt ) < 0 )
 413             {
 414                 cvSubdiv2DSwapEdges( curr_edge );
 415                 curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
 416             }
 417             else if( curr_org == first_point )
 418             {
 419                 break;
 420             }
 421             else
 422             {
 423                 curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ),
 424                                                CV_PREV_AROUND_LEFT );
 425             }
 426         }
 427         break;
 428     default:
 429         CV_Error_(CV_StsError, ("cvSubdiv2DLocate returned invalid location = %d", location) );
 430     }
 431
 432     return curr_point;
 433 }
 434
 435
 436 CV_IMPL void
 437 cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect )
 438 {
 439     float big_coord = 3.f * MAX( rect.width, rect.height );
 440     CvPoint2D32f ppA, ppB, ppC;
 441     CvSubdiv2DPoint *pA, *pB, *pC;
 442     CvSubdiv2DEdge edge_AB, edge_BC, edge_CA;
 443     float rx = (float) rect.x;
 444     float ry = (float) rect.y;
 445
 446     if( !subdiv )
 447         CV_Error( CV_StsNullPtr, "" );
 448
 449     cvClearSet( (CvSet *) (subdiv->edges) );
 450     cvClearSet( (CvSet *) subdiv );
 451
 452     subdiv->quad_edges = 0;
 453     subdiv->recent_edge = 0;
 454     subdiv->is_geometry_valid = 0;
 455
 456     subdiv->topleft = cvPoint2D32f( rx, ry );
 457     subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height );
 458
 459     ppA = cvPoint2D32f( rx + big_coord, ry );
 460     ppB = cvPoint2D32f( rx, ry + big_coord );
 461     ppC = cvPoint2D32f( rx - big_coord, ry - big_coord );
 462
 463     pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 );
 464     pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 );
 465     pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 );
 466
 467     edge_AB = cvSubdiv2DMakeEdge( subdiv );
 468     edge_BC = cvSubdiv2DMakeEdge( subdiv );
 469     edge_CA = cvSubdiv2DMakeEdge( subdiv );
 470
 471     cvSubdiv2DSetEdgePoints( edge_AB, pA, pB );
 472     cvSubdiv2DSetEdgePoints( edge_BC, pB, pC );
 473     cvSubdiv2DSetEdgePoints( edge_CA, pC, pA );
 474
 475     cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA ));
 476     cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB ));
 477     cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC ));
 478
 479     subdiv->recent_edge = edge_AB;
 480 }
 481
 482
 483 CV_IMPL void
 484 cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv )
 485 {
 486     int elem_size;
 487     int i, total;
 488     CvSeqReader reader;
 489
 490     if( !subdiv )
 491         CV_Error( CV_StsNullPtr, "" );
 492
 493     /* clear pointers to voronoi points */
 494     total = subdiv->edges->total;
 495     elem_size = subdiv->edges->elem_size;
 496
 497     cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
 498
 499     for( i = 0; i < total; i++ )
 500     {
 501         CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr;
 502
 503         quadedge->pt[1] = quadedge->pt[3] = 0;
 504         CV_NEXT_SEQ_ELEM( elem_size, reader );
 505     }
 506
 507     /* remove voronoi points */
 508     total = subdiv->total;
 509     elem_size = subdiv->elem_size;
 510
 511     cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 );
 512
 513     for( i = 0; i < total; i++ )
 514     {
 515         CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr;
 516
 517         /* check for virtual point. it is also check that the point exists */
 518         if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG )
 519         {
 520             cvSetRemoveByPtr( (CvSet*)subdiv, pt );
 521         }
 522         CV_NEXT_SEQ_ELEM( elem_size, reader );
 523     }
 524
 525     subdiv->is_geometry_valid = 0;
 526 }
 527
 528
 529 CV_IMPL void
 530 cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv )
 531 {
 532     CvSeqReader reader;
 533     int i, total, elem_size;
 534
 535     if( !subdiv )
 536         CV_Error( CV_StsNullPtr, "" );
 537
 538     /* check if it is already calculated */
 539     if( subdiv->is_geometry_valid )
 540         return;
 541
 542     total = subdiv->edges->total;
 543     elem_size = subdiv->edges->elem_size;
 544
 545     cvClearSubdivVoronoi2D( subdiv );
 546
 547     cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
 548
 549     if( total <= 3 )
 550         return;
 551
 552     /* skip first three edges (bounding triangle) */
 553     for( i = 0; i < 3; i++ )
 554         CV_NEXT_SEQ_ELEM( elem_size, reader );
 555
 556     /* loop through all quad-edges */
 557     for( ; i < total; i++ )
 558     {
 559         CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr);
 560
 561         if( CV_IS_SET_ELEM( quadedge ))
 562         {
 563             CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2;
 564             double a0, b0, c0, a1, b1, c1;
 565             CvPoint2D32f virt_point;
 566             CvSubdiv2DPoint *voronoi_point;
 567
 568             if( !quadedge->pt[3] )
 569             {
 570                 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT );
 571                 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT );
 572
 573                 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
 574                 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
 575
 576                 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
 577                 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 578                     fabs( virt_point.y ) < FLT_MAX * 0.5 )
 579                 {
 580                     voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
 581
 582                     quadedge->pt[3] =
 583                         ((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] =
 584                         ((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point;
 585                 }
 586             }
 587
 588             if( !quadedge->pt[1] )
 589             {
 590                 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT );
 591                 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT );
 592
 593                 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
 594                 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
 595
 596                 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
 597
 598                 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
 599                     fabs( virt_point.y ) < FLT_MAX * 0.5 )
 600                 {
 601                     voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
 602
 603                     quadedge->pt[1] =
 604                         ((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] =
 605                         ((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point;
 606                 }
 607             }
 608         }
 609
 610         CV_NEXT_SEQ_ELEM( elem_size, reader );
 611     }
 612
 613     subdiv->is_geometry_valid = 1;
 614 }
 615
 616
 617 static int
 618 icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff )
 619 {
 620     double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
 621     return (cw_area > 0) - (cw_area < 0);
 622 }
 623
 624
 625 CV_IMPL CvSubdiv2DPoint*
 626 cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt )
 627 {
 628     CvSubdiv2DPoint* point = 0;
 629     CvPoint2D32f start;
 630     CvPoint2D32f diff;
 631     CvSubdiv2DPointLocation loc;
 632     CvSubdiv2DEdge edge;
 633     int i;
 634
 635     if( !subdiv )
 636         CV_Error( CV_StsNullPtr, "" );
 637
 638     if( !CV_IS_SUBDIV2D( subdiv ))
 639         CV_Error( CV_StsNullPtr, "" );
 640
 641     if( subdiv->edges->active_count <= 3 )
 642         return 0;
 643
 644     if( !subdiv->is_geometry_valid )
 645         cvCalcSubdivVoronoi2D( subdiv );
 646
 647     loc = cvSubdiv2DLocate( subdiv, pt, &edge, &point );
 648
 649     switch( loc )
 650     {
 651     case CV_PTLOC_ON_EDGE:
 652     case CV_PTLOC_INSIDE:
 653         break;
 654     default:
 655         return point;
 656     }
 657
 658     point = 0;
 659
 660     start = cvSubdiv2DEdgeOrg( edge )->pt;
 661     diff.x = pt.x - start.x;
 662     diff.y = pt.y - start.y;
 663
 664     edge = cvSubdiv2DRotateEdge( edge, 1 );
 665
 666     for( i = 0; i < subdiv->total; i++ )
 667     {
 668         CvPoint2D32f t;
 669
 670         for(;;)
 671         {
 672             assert( cvSubdiv2DEdgeDst( edge ));
 673
 674             t = cvSubdiv2DEdgeDst( edge )->pt;
 675             if( icvIsRightOf2( t, start, diff ) >= 0 )
 676                 break;
 677
 678             edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT );
 679         }
 680
 681         for(;;)
 682         {
 683             assert( cvSubdiv2DEdgeOrg( edge ));
 684
 685             t = cvSubdiv2DEdgeOrg( edge )->pt;
 686             if( icvIsRightOf2( t, start, diff ) < 0 )
 687                 break;
 688
 689             edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_LEFT );
 690         }
 691
 692         {
 693             CvPoint2D32f tempDiff = cvSubdiv2DEdgeDst( edge )->pt;
 694             t = cvSubdiv2DEdgeOrg( edge )->pt;
 695             tempDiff.x -= t.x;
 696             tempDiff.y -= t.y;
 697
 698             if( icvIsRightOf2( pt, t, tempDiff ) >= 0 )
 699             {
 700                 point = cvSubdiv2DEdgeOrg( cvSubdiv2DRotateEdge( edge, 3 ));
 701                 break;
 702             }
 703         }
 704
 705         edge = cvSubdiv2DSymEdge( edge );
 706     }
 707
 708     return point;
 709 }
 710
 711
 712 namespace cv
 713 {
 714
 715 int Subdiv2D::nextEdge(int edge) const
 716 {
 717     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
 718     return qedges[edge >> 2].next[edge & 3];
 719 }
 720
 721 int Subdiv2D::rotateEdge(int edge, int rotate) const
 722 {
 723     return (edge & ~3) + ((edge + rotate) & 3);
 724 }
 725
 726 int Subdiv2D::symEdge(int edge) const
 727 {
 728     return edge ^ 2;
 729 }
 730
 731 int Subdiv2D::getEdge(int edge, int nextEdgeType) const
 732 {
 733     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
 734     edge = qedges[edge >> 2].next[(edge + nextEdgeType) & 3];
 735     return (edge & ~3) + ((edge + (nextEdgeType >> 4)) & 3);
 736 }
 737
 738 int Subdiv2D::edgeOrg(int edge, CV_OUT Point2f* orgpt) const
 739 {
 740     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
 741     int vidx = qedges[edge >> 2].pt[edge & 3];
 742     if( orgpt )
 743     {
 744         CV_DbgAssert((size_t)vidx < vtx.size());
 745         *orgpt = vtx[vidx].pt;
 746     }
 747     return vidx;
 748 }
 749
 750 int Subdiv2D::edgeDst(int edge, CV_OUT Point2f* dstpt) const
 751 {
 752     CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
 753     int vidx = qedges[edge >> 2].pt[(edge + 2) & 3];
 754     if( dstpt )
 755     {
 756         CV_DbgAssert((size_t)vidx < vtx.size());
 757         *dstpt = vtx[vidx].pt;
 758     }
 759     return vidx;
 760 }
 761
 762
 763 Point2f Subdiv2D::getVertex(int vertex, CV_OUT int* firstEdge) const
 764 {
 765     CV_DbgAssert((size_t)vertex < vtx.size());
 766     if( firstEdge )
 767         *firstEdge = vtx[vertex].firstEdge;
 768     return vtx[vertex].pt;
 769 }
 770
 771
 772 Subdiv2D::Subdiv2D()
 773 {
 774     validGeometry = false;
 775     freeQEdge = 0;
 776     freePoint = 0;
 777     recentEdge = 0;
 778 }
 779
 780 Subdiv2D::Subdiv2D(Rect rect)
 781 {
 782     validGeometry = false;
 783     freeQEdge = 0;
 784     freePoint = 0;
 785     recentEdge = 0;
 786
 787     initDelaunay(rect);
 788 }
 789
 790
 791 Subdiv2D::QuadEdge::QuadEdge()
 792 {
 793     next[0] = next[1] = next[2] = next[3] = 0;
 794     pt[0] = pt[1] = pt[2] = pt[3] = 0;
 795 }
 796
 797 Subdiv2D::QuadEdge::QuadEdge(int edgeidx)
 798 {
 799     CV_DbgAssert((edgeidx & 3) == 0);
 800     next[0] = edgeidx;
 801     next[1] = edgeidx+3;
 802     next[2] = edgeidx+2;
 803     next[3] = edgeidx+1;
 804
 805     pt[0] = pt[1] = pt[2] = pt[3] = 0;
 806 }
 807
 808 bool Subdiv2D::QuadEdge::isfree() const
 809 {
 810     return next[0] <= 0;
 811 }
 812
 813 Subdiv2D::Vertex::Vertex()
 814 {
 815     firstEdge = 0;
 816     type = -1;
 817 }
 818
 819 Subdiv2D::Vertex::Vertex(Point2f _pt, bool _isvirtual, int _firstEdge)
 820 {
 821     firstEdge = _firstEdge;
 822     type = (int)_isvirtual;
 823     pt = _pt;
 824 }
 825
 826 bool Subdiv2D::Vertex::isvirtual() const
 827 {
 828     return type > 0;
 829 }
 830
 831 bool Subdiv2D::Vertex::isfree() const
 832 {
 833     return type < 0;
 834 }
 835
 836 void Subdiv2D::splice( int edgeA, int edgeB )
 837 {
 838     int& a_next = qedges[edgeA >> 2].next[edgeA & 3];
 839     int& b_next = qedges[edgeB >> 2].next[edgeB & 3];
 840     int a_rot = rotateEdge(a_next, 1);
 841     int b_rot = rotateEdge(b_next, 1);
 842     int& a_rot_next = qedges[a_rot >> 2].next[a_rot & 3];
 843     int& b_rot_next = qedges[b_rot >> 2].next[b_rot & 3];
 844     std::swap(a_next, b_next);
 845     std::swap(a_rot_next, b_rot_next);
 846 }
 847
 848 void Subdiv2D::setEdgePoints(int edge, int orgPt, int dstPt)
 849 {
 850     qedges[edge >> 2].pt[edge & 3] = orgPt;
 851     qedges[edge >> 2].pt[(edge + 2) & 3] = dstPt;
 852     vtx[orgPt].firstEdge = edge;
 853     vtx[dstPt].firstEdge = edge ^ 2;
 854 }
 855
 856 int Subdiv2D::connectEdges( int edgeA, int edgeB )
 857 {
 858     int edge = newEdge();
 859
 860     splice(edge, getEdge(edgeA, NEXT_AROUND_LEFT));
 861     splice(symEdge(edge), edgeB);
 862
 863     setEdgePoints(edge, edgeDst(edgeA), edgeOrg(edgeB));
 864     return edge;
 865 }
 866
 867 void Subdiv2D::swapEdges( int edge )
 868 {
 869     int sedge = symEdge(edge);
 870     int a = getEdge(edge, PREV_AROUND_ORG);
 871     int b = getEdge(sedge, PREV_AROUND_ORG);
 872
 873     splice(edge, a);
 874     splice(sedge, b);
 875
 876     setEdgePoints(edge, edgeDst(a), edgeDst(b));
 877
 878     splice(edge, getEdge(a, NEXT_AROUND_LEFT));
 879     splice(sedge, getEdge(b, NEXT_AROUND_LEFT));
 880 }
 881
 882 int Subdiv2D::isRightOf(Point2f pt, int edge) const
 883 {
 884     Point2f org, dst;
 885     edgeOrg(edge, &org);
 886     edgeDst(edge, &dst);
 887     double cw_area = cvTriangleArea( pt, dst, org );
 888
 889     return (cw_area > 0) - (cw_area < 0);
 890 }
 891
 892
 893 int Subdiv2D::newEdge()
 894 {
 895     if( freeQEdge <= 0 )
 896     {
 897         qedges.push_back(QuadEdge());
 898         freeQEdge = (int)(qedges.size()-1);
 899     }
 900     int edge = freeQEdge*4;
 901     freeQEdge = qedges[edge >> 2].next[1];
 902     qedges[edge >> 2] = QuadEdge(edge);
 903     return edge;
 904 }
 905
 906 void Subdiv2D::deleteEdge(int edge)
 907 {
 908     CV_DbgAssert((size_t)(edge >> 2) < (size_t)qedges.size());
 909     splice( edge, getEdge(edge, PREV_AROUND_ORG) );
 910     int sedge = symEdge(edge);
 911     splice(sedge, getEdge(sedge, PREV_AROUND_ORG) );
 912
 913     edge >>= 2;
 914     qedges[edge].next[0] = 0;
 915     qedges[edge].next[1] = freeQEdge;
 916     freeQEdge = edge;
 917 }
 918
 919 int Subdiv2D::newPoint(Point2f pt, bool isvirtual, int firstEdge)
 920 {
 921     if( freePoint == 0 )
 922     {
 923         vtx.push_back(Vertex());
 924         freePoint = (int)(vtx.size()-1);
 925     }
 926     int vidx = freePoint;
 927     freePoint = vtx[vidx].firstEdge;
 928     vtx[vidx] = Vertex(pt, isvirtual, firstEdge);
 929
 930     return vidx;
 931 }
 932
 933 void Subdiv2D::deletePoint(int vidx)
 934 {
 935     CV_DbgAssert( (size_t)vidx < vtx.size() );
 936     vtx[vidx].firstEdge = freePoint;
 937     vtx[vidx].type = -1;
 938     freePoint = vidx;
 939 }
 940
 941 int Subdiv2D::locate(Point2f pt, int& _edge, int& _vertex)
 942 {
 943     int vertex = 0;
 944
 945     int i, maxEdges = (int)(qedges.size() * 4);
 946
 947     if( qedges.size() < (size_t)4 )
 948         CV_Error( CV_StsError, "Subdivision is empty" );
 949
 950     if( pt.x < topLeft.x || pt.y < topLeft.y || pt.x >= bottomRight.x || pt.y >= bottomRight.y )
 951         CV_Error( CV_StsOutOfRange, "" );
 952
 953     int edge = recentEdge;
 954     CV_Assert(edge > 0);
 955
 956     int location = PTLOC_ERROR;
 957
 958     int right_of_curr = isRightOf(pt, edge);
 959     if( right_of_curr > 0 )
 960     {
 961         edge = symEdge(edge);
 962         right_of_curr = -right_of_curr;
 963     }
 964
 965     for( i = 0; i < maxEdges; i++ )
 966     {
 967         int onext_edge = nextEdge( edge );
 968         int dprev_edge = getEdge( edge, PREV_AROUND_DST );
 969
 970         int right_of_onext = isRightOf( pt, onext_edge );
 971         int right_of_dprev = isRightOf( pt, dprev_edge );
 972
 973         if( right_of_dprev > 0 )
 974         {
 975             if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
 976             {
 977                 location = PTLOC_INSIDE;
 978                 break;
 979             }
 980             else
 981             {
 982                 right_of_curr = right_of_onext;
 983                 edge = onext_edge;
 984             }
 985         }
 986         else
 987         {
 988             if( right_of_onext > 0 )
 989             {
 990                 if( right_of_dprev == 0 && right_of_curr == 0 )
 991                 {
 992                     location = PTLOC_INSIDE;
 993                     break;
 994                 }
 995                 else
 996                 {
 997                     right_of_curr = right_of_dprev;
 998                     edge = dprev_edge;
 999                 }
1000             }
1001             else if( right_of_curr == 0 &&
1002                     isRightOf( vtx[edgeDst(onext_edge)].pt, edge ) >= 0 )
1003             {
1004                 edge = symEdge( edge );
1005             }
1006             else
1007             {
1008                 right_of_curr = right_of_onext;
1009                 edge = onext_edge;
1010             }
1011         }
1012     }
1013
1014     recentEdge = edge;
1015
1016     if( location == PTLOC_INSIDE )
1017     {
1018         Point2f org_pt, dst_pt;
1019         edgeOrg(edge, &org_pt);
1020         edgeDst(edge, &dst_pt);
1021
1022         double t1 = fabs( pt.x - org_pt.x );
1023         t1 += fabs( pt.y - org_pt.y );
1024         double t2 = fabs( pt.x - dst_pt.x );
1025         t2 += fabs( pt.y - dst_pt.y );
1026         double t3 = fabs( org_pt.x - dst_pt.x );
1027         t3 += fabs( org_pt.y - dst_pt.y );
1028
1029         if( t1 < FLT_EPSILON )
1030         {
1031             location = PTLOC_VERTEX;
1032             vertex = edgeOrg( edge );
1033             edge = 0;
1034         }
1035         else if( t2 < FLT_EPSILON )
1036         {
1037             location = PTLOC_VERTEX;
1038             vertex = edgeDst( edge );
1039             edge = 0;
1040         }
1041         else if( (t1 < t3 || t2 < t3) &&
1042                 fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
1043         {
1044             location = PTLOC_ON_EDGE;
1045             vertex = 0;
1046         }
1047     }
1048
1049     if( location == PTLOC_ERROR )
1050     {
1051         edge = 0;
1052         vertex = 0;
1053     }
1054
1055     _edge = edge;
1056     _vertex = vertex;
1057
1058     return location;
1059 }
1060
1061
1062 inline int
1063 isPtInCircle3( Point2f pt, Point2f a, Point2f b, Point2f c)
1064 {
1065     const double eps = FLT_EPSILON*0.125;
1066     double val = ((double)a.x * a.x + (double)a.y * a.y) * cvTriangleArea( b, c, pt );
1067     val -= ((double)b.x * b.x + (double)b.y * b.y) * cvTriangleArea( a, c, pt );
1068     val += ((double)c.x * c.x + (double)c.y * c.y) * cvTriangleArea( a, b, pt );
1069     val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * cvTriangleArea( a, b, c );
1070
1071     return val > eps ? 1 : val < -eps ? -1 : 0;
1072 }
1073
1074
1075 int Subdiv2D::insert(Point2f pt)
1076 {
1077     int curr_point = 0, curr_edge = 0, deleted_edge = 0;
1078     int location = locate( pt, curr_edge, curr_point );
1079
1080     if( location == PTLOC_ERROR )
1081         CV_Error( CV_StsBadSize, "" );
1082
1083     if( location == PTLOC_OUTSIDE_RECT )
1084         CV_Error( CV_StsOutOfRange, "" );
1085
1086     if( location == PTLOC_VERTEX )
1087         return curr_point;
1088
1089     if( location == PTLOC_ON_EDGE )
1090     {
1091         deleted_edge = curr_edge;
1092         recentEdge = curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
1093         deleteEdge(deleted_edge);
1094     }
1095     else if( location == PTLOC_INSIDE )
1096         ;
1097     else
1098         CV_Error_(CV_StsError, ("Subdiv2D::locate returned invalid location = %d", location) );
1099
1100     assert( curr_edge != 0 );
1101     validGeometry = false;
1102
1103     curr_point = newPoint(pt, false);
1104     int base_edge = newEdge();
1105     int first_point = edgeOrg(curr_edge);
1106     setEdgePoints(base_edge, first_point, curr_point);
1107     splice(base_edge, curr_edge);
1108
1109     do
1110     {
1111         base_edge = connectEdges( curr_edge, symEdge(base_edge) );
1112         curr_edge = getEdge(base_edge, PREV_AROUND_ORG);
1113     }
1114     while( edgeDst(curr_edge) != first_point );
1115
1116     curr_edge = getEdge( base_edge, PREV_AROUND_ORG );
1117
1118     int i, max_edges = (int)(qedges.size()*4);
1119
1120     for( i = 0; i < max_edges; i++ )
1121     {
1122         int temp_dst = 0, curr_org = 0, curr_dst = 0;
1123         int temp_edge = getEdge( curr_edge, PREV_AROUND_ORG );
1124
1125         temp_dst = edgeDst( temp_edge );
1126         curr_org = edgeOrg( curr_edge );
1127         curr_dst = edgeDst( curr_edge );
1128
1129         if( isRightOf( vtx[temp_dst].pt, curr_edge ) > 0 &&
1130            isPtInCircle3( vtx[curr_org].pt, vtx[temp_dst].pt,
1131                          vtx[curr_dst].pt, vtx[curr_point].pt ) < 0 )
1132         {
1133             swapEdges( curr_edge );
1134             curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
1135         }
1136         else if( curr_org == first_point )
1137             break;
1138         else
1139             curr_edge = getEdge( nextEdge( curr_edge ), PREV_AROUND_LEFT );
1140     }
1141
1142     return curr_point;
1143 }
1144
1145 void Subdiv2D::insert(const vector<Point2f>& ptvec)
1146 {
1147     for( size_t i = 0; i < ptvec.size(); i++ )
1148         insert(ptvec[i]);
1149 }
1150
1151 void Subdiv2D::initDelaunay( Rect rect )
1152 {
1153     float big_coord = 3.f * MAX( rect.width, rect.height );
1154     float rx = (float)rect.x;
1155     float ry = (float)rect.y;
1156
1157     vtx.clear();
1158     qedges.clear();
1159
1160     recentEdge = 0;
1161     validGeometry = false;
1162
1163     topLeft = Point2f( rx, ry );
1164     bottomRight = Point2f( rx + rect.width, ry + rect.height );
1165
1166     Point2f ppA( rx + big_coord, ry );
1167     Point2f ppB( rx, ry + big_coord );
1168     Point2f ppC( rx - big_coord, ry - big_coord );
1169
1170     vtx.push_back(Vertex());
1171     qedges.push_back(QuadEdge());
1172
1173     freeQEdge = 0;
1174     freePoint = 0;
1175
1176     int pA = newPoint(ppA, false);
1177     int pB = newPoint(ppB, false);
1178     int pC = newPoint(ppC, false);
1179
1180     int edge_AB = newEdge();
1181     int edge_BC = newEdge();
1182     int edge_CA = newEdge();
1183
1184     setEdgePoints( edge_AB, pA, pB );
1185     setEdgePoints( edge_BC, pB, pC );
1186     setEdgePoints( edge_CA, pC, pA );
1187
1188     splice( edge_AB, symEdge( edge_CA ));
1189     splice( edge_BC, symEdge( edge_AB ));
1190     splice( edge_CA, symEdge( edge_BC ));
1191
1192     recentEdge = edge_AB;
1193 }
1194
1195
1196 void Subdiv2D::clearVoronoi()
1197 {
1198     size_t i, total = qedges.size();
1199
1200     for( i = 0; i < total; i++ )
1201         qedges[i].pt[1] = qedges[i].pt[3] = 0;
1202
1203     total = vtx.size();
1204     for( i = 0; i < total; i++ )
1205     {
1206         if( vtx[i].isvirtual() )
1207             deletePoint((int)i);
1208     }
1209
1210     validGeometry = false;
1211 }
1212
1213
1214 static Point2f computeVoronoiPoint(Point2f org0, Point2f dst0, Point2f org1, Point2f dst1)
1215 {
1216     double a0 = dst0.x - org0.x;
1217     double b0 = dst0.y - org0.y;
1218     double c0 = -0.5*(a0 * (dst0.x + org0.x) + b0 * (dst0.y + org0.y));
1219
1220     double a1 = dst1.x - org1.x;
1221     double b1 = dst1.y - org1.y;
1222     double c1 = -0.5*(a1 * (dst1.x + org1.x) + b1 * (dst1.y + org1.y));
1223
1224     double det = a0 * b1 - a1 * b0;
1225
1226     if( det != 0 )
1227     {
1228         det = 1. / det;
1229         return Point2f((float) ((b0 * c1 - b1 * c0) * det),
1230                        (float) ((a1 * c0 - a0 * c1) * det));
1231     }
1232
1233     return Point2f(FLT_MAX, FLT_MAX);
1234 }
1235
1236
1237 void Subdiv2D::calcVoronoi()
1238 {
1239     // check if it is already calculated
1240     if( validGeometry )
1241         return;
1242
1243     clearVoronoi();
1244     int i, total = (int)qedges.size();
1245
1246     // loop through all quad-edges, except for the first 3 (#1, #2, #3 - 0 is reserved for "NULL" pointer)
1247     for( i = 4; i < total; i++ )
1248     {
1249         QuadEdge& quadedge = qedges[i];
1250
1251         if( quadedge.isfree() )
1252             continue;
1253
1254         int edge0 = (int)(i*4);
1255         Point2f org0, dst0, org1, dst1;
1256
1257         if( !quadedge.pt[3] )
1258         {
1259             int edge1 = getEdge( edge0, NEXT_AROUND_LEFT );
1260             int edge2 = getEdge( edge1, NEXT_AROUND_LEFT );
1261
1262             edgeOrg(edge0, &org0);
1263             edgeDst(edge0, &dst0);
1264             edgeOrg(edge1, &org1);
1265             edgeDst(edge1, &dst1);
1266
1267             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
1268
1269             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
1270                fabs( virt_point.y ) < FLT_MAX * 0.5 )
1271             {
1272                 quadedge.pt[3] = qedges[edge1 >> 2].pt[3 - (edge1 & 2)] =
1273                 qedges[edge2 >> 2].pt[3 - (edge2 & 2)] = newPoint(virt_point, true);
1274             }
1275         }
1276
1277         if( !quadedge.pt[1] )
1278         {
1279             int edge1 = getEdge( edge0, NEXT_AROUND_RIGHT );
1280             int edge2 = getEdge( edge1, NEXT_AROUND_RIGHT );
1281
1282             edgeOrg(edge0, &org0);
1283             edgeDst(edge0, &dst0);
1284             edgeOrg(edge1, &org1);
1285             edgeDst(edge1, &dst1);
1286
1287             Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
1288
1289             if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
1290                fabs( virt_point.y ) < FLT_MAX * 0.5 )
1291             {
1292                 quadedge.pt[1] = qedges[edge1 >> 2].pt[1 + (edge1 & 2)] =
1293                 qedges[edge2 >> 2].pt[1 + (edge2 & 2)] = newPoint(virt_point, true);
1294             }
1295         }
1296     }
1297
1298     validGeometry = true;
1299 }
1300
1301
1302 static int
1303 isRightOf2( const Point2f& pt, const Point2f& org, const Point2f& diff )
1304 {
1305     double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
1306     return (cw_area > 0) - (cw_area < 0);
1307 }
1308
1309
1310 int Subdiv2D::findNearest(Point2f pt, Point2f* nearestPt)
1311 {
1312     if( !validGeometry )
1313         calcVoronoi();
1314
1315     int vertex = 0, edge = 0;
1316     int loc = locate( pt, edge, vertex );
1317
1318     if( loc != PTLOC_ON_EDGE && loc != PTLOC_INSIDE )
1319         return vertex;
1320
1321     vertex = 0;
1322
1323     Point2f start;
1324     edgeOrg(edge, &start);
1325     Point2f diff = pt - start;
1326
1327     edge = rotateEdge(edge, 1);
1328
1329     int i, total = (int)vtx.size();
1330
1331     for( i = 0; i < total; i++ )
1332     {
1333         Point2f t;
1334
1335         for(;;)
1336         {
1337             CV_Assert( edgeDst(edge, &t) > 0 );
1338             if( isRightOf2( t, start, diff ) >= 0 )
1339                 break;
1340
1341             edge = getEdge( edge, NEXT_AROUND_LEFT );
1342         }
1343
1344         for(;;)
1345         {
1346             CV_Assert( edgeOrg( edge, &t ) > 0 );
1347
1348             if( isRightOf2( t, start, diff ) < 0 )
1349                 break;
1350
1351             edge = getEdge( edge, PREV_AROUND_LEFT );
1352         }
1353
1354         Point2f tempDiff;
1355         edgeDst(edge, &tempDiff);
1356         edgeOrg(edge, &t);
1357         tempDiff -= t;
1358
1359         if( isRightOf2( pt, t, tempDiff ) >= 0 )
1360         {
1361             vertex = edgeOrg(rotateEdge( edge, 3 ));
1362             break;
1363         }
1364
1365         edge = symEdge( edge );
1366     }
1367
1368     if( nearestPt && vertex > 0 )
1369         *nearestPt = vtx[vertex].pt;
1370
1371     return vertex;
1372 }
1373
1374 void Subdiv2D::getEdgeList(vector<Vec4f>& edgeList) const
1375 {
1376     edgeList.clear();
1377
1378     for( size_t i = 4; i < qedges.size(); i++ )
1379     {
1380         if( qedges[i].isfree() )
1381             continue;
1382         if( qedges[i].pt[0] > 0 && qedges[i].pt[2] > 0 )
1383         {
1384             Point2f org = vtx[qedges[i].pt[0]].pt;
1385             Point2f dst = vtx[qedges[i].pt[2]].pt;
1386             edgeList.push_back(Vec4f(org.x, org.y, dst.x, dst.y));
1387         }
1388     }
1389 }
1390
1391 void Subdiv2D::getTriangleList(vector<Vec6f>& triangleList) const
1392 {
1393     triangleList.clear();
1394     int i, total = (int)(qedges.size()*4);
1395     vector<bool> edgemask(total, false);
1396
1397     for( i = 4; i < total; i += 2 )
1398     {
1399         if( edgemask[i] )
1400             continue;
1401         Point2f a, b, c;
1402         int edge = i;
1403         edgeOrg(edge, &a);
1404         edgemask[edge] = true;
1405         edge = getEdge(edge, NEXT_AROUND_LEFT);
1406         edgeOrg(edge, &b);
1407         edgemask[edge] = true;
1408         edge = getEdge(edge, NEXT_AROUND_LEFT);
1409         edgeOrg(edge, &c);
1410         edgemask[edge] = true;
1411         triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
1412     }
1413 }
1414
1415 void Subdiv2D::getVoronoiFacetList(const vector<int>& idx,
1416                                    CV_OUT vector<vector<Point2f> >& facetList,
1417                                    CV_OUT vector<Point2f>& facetCenters)
1418 {
1419     calcVoronoi();
1420     facetList.clear();
1421     facetCenters.clear();
1422
1423     vector<Point2f> buf;
1424
1425     size_t i, total;
1426     if( idx.empty() )
1427         i = 4, total = vtx.size();
1428     else
1429         i = 0, total = idx.size();
1430
1431     for( ; i < total; i++ )
1432     {
1433         int k = idx.empty() ? (int)i : idx[i];
1434
1435         if( vtx[k].isfree() || vtx[k].isvirtual() )
1436             continue;
1437         int edge = rotateEdge(vtx[k].firstEdge, 1), t = edge;
1438
1439         // gather points
1440         buf.clear();
1441         do
1442         {
1443             buf.push_back(vtx[edgeOrg(t)].pt);
1444             t = getEdge( t, NEXT_AROUND_LEFT );
1445         }
1446         while( t != edge );
1447
1448         facetList.push_back(buf);
1449         facetCenters.push_back(vtx[k].pt);
1450     }
1451 }
1452
1453
1454 void Subdiv2D::checkSubdiv() const
1455 {
1456     int i, j, total = (int)qedges.size();
1457
1458     for( i = 0; i < total; i++ )
1459     {
1460         const QuadEdge& qe = qedges[i];
1461
1462         if( qe.isfree() )
1463             continue;
1464
1465         for( j = 0; j < 4; j++ )
1466         {
1467             int e = (int)(i*4 + j);
1468             int o_next = nextEdge(e);
1469             int o_prev = getEdge(e, PREV_AROUND_ORG );
1470             int d_prev = getEdge(e, PREV_AROUND_DST );
1471             int d_next = getEdge(e, NEXT_AROUND_DST );
1472
1473             // check points
1474             CV_Assert( edgeOrg(e) == edgeOrg(o_next));
1475             CV_Assert( edgeOrg(e) == edgeOrg(o_prev));
1476             CV_Assert( edgeDst(e) == edgeDst(d_next));
1477             CV_Assert( edgeDst(e) == edgeDst(d_prev));
1478
1479             if( j % 2 == 0 )
1480             {
1481                 CV_Assert( edgeDst(o_next) == edgeOrg(d_prev));
1482                 CV_Assert( edgeDst(o_prev) == edgeOrg(d_next));
1483                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_LEFT),NEXT_AROUND_LEFT),NEXT_AROUND_LEFT) == e );
1484                 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT) == e);
1485             }
1486         }
1487     }
1488 }
1489
1490 }
1491
1492 /* End of file. */