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