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