1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41 #include "_cv.h"
42
43 CV_IMPL CvSubdiv2D *
cvCreateSubdiv2D(int subdiv_type,int header_size,int vtx_size,int quadedge_size,CvMemStorage * storage)44 cvCreateSubdiv2D( int subdiv_type, int header_size,
45 int vtx_size, int quadedge_size, CvMemStorage * storage )
46 {
47 CvSubdiv2D *subdiv = 0;
48
49 CV_FUNCNAME( "cvCleateSubdiv2D" );
50
51 __BEGIN__;
52
53 if( !storage )
54 CV_ERROR( CV_StsNullPtr, "" );
55
56 if( header_size < (int)sizeof( *subdiv ) ||
57 quadedge_size < (int)sizeof( CvQuadEdge2D ) ||
58 vtx_size < (int)sizeof( CvSubdiv2DPoint ))
59 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );
60
61 subdiv = (CvSubdiv2D *) cvCreateGraph( subdiv_type, header_size,
62 vtx_size, quadedge_size, storage );
63
64
65 __END__;
66
67 return subdiv;
68 }
69
70
71 /****************************************************************************************\
72 * Quad Edge algebra *
73 \****************************************************************************************/
74
75 static CvSubdiv2DEdge
cvSubdiv2DMakeEdge(CvSubdiv2D * subdiv)76 cvSubdiv2DMakeEdge( CvSubdiv2D * subdiv )
77 {
78 CvQuadEdge2D *edge = 0;
79 CvSubdiv2DEdge edgehandle = 0;
80
81 CV_FUNCNAME( "cvSubdiv2DMakeEdge" );
82
83 __BEGIN__;
84
85 if( !subdiv )
86 CV_ERROR( CV_StsNullPtr, "" );
87
88 edge = (CvQuadEdge2D*)cvSetNew( (CvSet*)subdiv->edges );
89 CV_CHECK();
90
91 memset( edge->pt, 0, sizeof( edge->pt ));
92 edgehandle = (CvSubdiv2DEdge) edge;
93
94 edge->next[0] = edgehandle;
95 edge->next[1] = edgehandle + 3;
96 edge->next[2] = edgehandle + 2;
97 edge->next[3] = edgehandle + 1;
98
99 subdiv->quad_edges++;
100
101
102 __END__;
103
104 return edgehandle;
105 }
106
107
108 static CvSubdiv2DPoint *
cvSubdiv2DAddPoint(CvSubdiv2D * subdiv,CvPoint2D32f pt,int is_virtual)109 cvSubdiv2DAddPoint( CvSubdiv2D * subdiv, CvPoint2D32f pt, int is_virtual )
110 {
111 CvSubdiv2DPoint *subdiv_point = 0;
112
113 subdiv_point = (CvSubdiv2DPoint*)cvSetNew( (CvSet*)subdiv );
114 if( subdiv_point )
115 {
116 memset( subdiv_point, 0, subdiv->elem_size );
117 subdiv_point->pt = pt;
118 subdiv_point->first = 0;
119 subdiv_point->flags |= is_virtual ? CV_SUBDIV2D_VIRTUAL_POINT_FLAG : 0;
120 }
121
122 return subdiv_point;
123 }
124
125
126 static void
cvSubdiv2DSplice(CvSubdiv2DEdge edgeA,CvSubdiv2DEdge edgeB)127 cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
128 {
129 CvSubdiv2DEdge *a_next = &CV_SUBDIV2D_NEXT_EDGE( edgeA );
130 CvSubdiv2DEdge *b_next = &CV_SUBDIV2D_NEXT_EDGE( edgeB );
131 CvSubdiv2DEdge a_rot = cvSubdiv2DRotateEdge( *a_next, 1 );
132 CvSubdiv2DEdge b_rot = cvSubdiv2DRotateEdge( *b_next, 1 );
133 CvSubdiv2DEdge *a_rot_next = &CV_SUBDIV2D_NEXT_EDGE( a_rot );
134 CvSubdiv2DEdge *b_rot_next = &CV_SUBDIV2D_NEXT_EDGE( b_rot );
135 CvSubdiv2DEdge t;
136
137 CV_SWAP( *a_next, *b_next, t );
138 CV_SWAP( *a_rot_next, *b_rot_next, t );
139 }
140
141
142 static void
cvSubdiv2DSetEdgePoints(CvSubdiv2DEdge edge,CvSubdiv2DPoint * org_pt,CvSubdiv2DPoint * dst_pt)143 cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge,
144 CvSubdiv2DPoint * org_pt, CvSubdiv2DPoint * dst_pt )
145 {
146 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
147
148 CV_FUNCNAME( "cvSubdiv2DSetEdgePoints" );
149
150 __BEGIN__;
151
152 if( !quadedge )
153 CV_ERROR( CV_StsNullPtr, "" );
154
155 quadedge->pt[edge & 3] = org_pt;
156 quadedge->pt[(edge + 2) & 3] = dst_pt;
157
158
159 __END__;
160 }
161
162
163 static void
cvSubdiv2DDeleteEdge(CvSubdiv2D * subdiv,CvSubdiv2DEdge edge)164 cvSubdiv2DDeleteEdge( CvSubdiv2D * subdiv, CvSubdiv2DEdge edge )
165 {
166 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
167
168 CV_FUNCNAME( "cvSubdiv2DDeleteEdge" );
169
170 __BEGIN__;
171
172 if( !subdiv || !quadedge )
173 CV_ERROR( CV_StsNullPtr, "" );
174
175 cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG ));
176
177 {
178 CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
179 cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG ));
180 }
181
182 cvSetRemoveByPtr( (CvSet*)(subdiv->edges), quadedge );
183 subdiv->quad_edges--;
184
185
186 __END__;
187 }
188
189
190 static CvSubdiv2DEdge
cvSubdiv2DConnectEdges(CvSubdiv2D * subdiv,CvSubdiv2DEdge edgeA,CvSubdiv2DEdge edgeB)191 cvSubdiv2DConnectEdges( CvSubdiv2D * subdiv, CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
192 {
193 CvSubdiv2DEdge new_edge = 0;
194
195 CV_FUNCNAME( "cvSubdiv2DConnectPoints" );
196
197 __BEGIN__;
198
199 CvSubdiv2DPoint *orgB, *dstA;
200
201 if( !subdiv )
202 CV_ERROR( CV_StsNullPtr, "" );
203
204 new_edge = cvSubdiv2DMakeEdge( subdiv );
205
206 cvSubdiv2DSplice( new_edge, cvSubdiv2DGetEdge( edgeA, CV_NEXT_AROUND_LEFT ));
207 cvSubdiv2DSplice( cvSubdiv2DSymEdge( new_edge ), edgeB );
208
209 dstA = cvSubdiv2DEdgeDst( edgeA );
210 orgB = cvSubdiv2DEdgeOrg( edgeB );
211 cvSubdiv2DSetEdgePoints( new_edge, dstA, orgB );
212
213 __END__;
214
215 return new_edge;
216 }
217
218
219 static void
cvSubdiv2DSwapEdges(CvSubdiv2DEdge edge)220 cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge )
221 {
222 CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
223 CvSubdiv2DEdge a = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG );
224 CvSubdiv2DEdge b = cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG );
225 CvSubdiv2DPoint *dstB, *dstA;
226
227 cvSubdiv2DSplice( edge, a );
228 cvSubdiv2DSplice( sym_edge, b );
229
230 dstA = cvSubdiv2DEdgeDst( a );
231 dstB = cvSubdiv2DEdgeDst( b );
232 cvSubdiv2DSetEdgePoints( edge, dstA, dstB );
233
234 cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( a, CV_NEXT_AROUND_LEFT ));
235 cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( b, CV_NEXT_AROUND_LEFT ));
236 }
237
238
239 static int
icvIsRightOf(CvPoint2D32f & pt,CvSubdiv2DEdge edge)240 icvIsRightOf( CvPoint2D32f& pt, CvSubdiv2DEdge edge )
241 {
242 CvSubdiv2DPoint *org = cvSubdiv2DEdgeOrg(edge), *dst = cvSubdiv2DEdgeDst(edge);
243 Cv32suf cw_area;
244 cw_area.f = (float)cvTriangleArea( pt, dst->pt, org->pt );
245
246 return (cw_area.i > 0)*2 - (cw_area.i*2 != 0);
247 }
248
249
250 CV_IMPL CvSubdiv2DPointLocation
cvSubdiv2DLocate(CvSubdiv2D * subdiv,CvPoint2D32f pt,CvSubdiv2DEdge * _edge,CvSubdiv2DPoint ** _point)251 cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt,
252 CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point )
253 {
254 CvSubdiv2DEdge edge = 0;
255 CvSubdiv2DPoint *point = 0;
256 CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
257
258 int i, max_edges;
259 int right_of_curr = 0;
260
261 CV_FUNCNAME( "cvSubdiv2DLocate" );
262
263 __BEGIN__;
264
265 if( !subdiv )
266 CV_ERROR( CV_StsNullPtr, "" );
267
268 if( !CV_IS_SUBDIV2D(subdiv) )
269 CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR );
270
271 max_edges = subdiv->quad_edges * 4;
272 edge = subdiv->recent_edge;
273
274 if( max_edges == 0 )
275 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );
276 if( !edge )
277 CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR );
278
279 location = CV_PTLOC_OUTSIDE_RECT;
280 if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y ||
281 pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y )
282 CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR );
283
284 location = CV_PTLOC_ERROR;
285
286 right_of_curr = icvIsRightOf( pt, edge );
287 if( right_of_curr > 0 )
288 {
289 edge = cvSubdiv2DSymEdge( edge );
290 right_of_curr = -right_of_curr;
291 }
292
293 for( i = 0; i < max_edges; i++ )
294 {
295 CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge );
296 CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST );
297
298 int right_of_onext = icvIsRightOf( pt, onext_edge );
299 int right_of_dprev = icvIsRightOf( pt, dprev_edge );
300
301 if( right_of_dprev > 0 )
302 {
303 if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
304 {
305 location = CV_PTLOC_INSIDE;
306 EXIT;
307 }
308 else
309 {
310 right_of_curr = right_of_onext;
311 edge = onext_edge;
312 }
313 }
314 else
315 {
316 if( right_of_onext > 0 )
317 {
318 if( right_of_dprev == 0 && right_of_curr == 0 )
319 {
320 location = CV_PTLOC_INSIDE;
321 EXIT;
322 }
323 else
324 {
325 right_of_curr = right_of_dprev;
326 edge = dprev_edge;
327 }
328 }
329 else if( right_of_curr == 0 &&
330 icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 )
331 {
332 edge = cvSubdiv2DSymEdge( edge );
333 }
334 else
335 {
336 right_of_curr = right_of_onext;
337 edge = onext_edge;
338 }
339 }
340 }
341
342
343 __END__;
344
345 subdiv->recent_edge = edge;
346
347 if( location == CV_PTLOC_INSIDE )
348 {
349 double t1, t2, t3;
350 CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt;
351 CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt;
352
353 t1 = fabs( pt.x - org_pt.x );
354 t1 += fabs( pt.y - org_pt.y );
355 t2 = fabs( pt.x - dst_pt.x );
356 t2 += fabs( pt.y - dst_pt.y );
357 t3 = fabs( org_pt.x - dst_pt.x );
358 t3 += fabs( org_pt.y - dst_pt.y );
359
360 if( t1 < FLT_EPSILON )
361 {
362 location = CV_PTLOC_VERTEX;
363 point = cvSubdiv2DEdgeOrg( edge );
364 edge = 0;
365 }
366 else if( t2 < FLT_EPSILON )
367 {
368 location = CV_PTLOC_VERTEX;
369 point = cvSubdiv2DEdgeDst( edge );
370 edge = 0;
371 }
372 else if( (t1 < t3 || t2 < t3) &&
373 fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
374 {
375 location = CV_PTLOC_ON_EDGE;
376 point = 0;
377 }
378 }
379
380 if( location == CV_PTLOC_ERROR )
381 {
382 edge = 0;
383 point = 0;
384 }
385
386 if( _edge )
387 *_edge = edge;
388 if( _point )
389 *_point = point;
390
391 return location;
392 }
393
394
395 CV_INLINE int
icvIsPtInCircle3(CvPoint2D32f pt,CvPoint2D32f a,CvPoint2D32f b,CvPoint2D32f c)396 icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c )
397 {
398 double val = (a.x * a.x + a.y * a.y) * cvTriangleArea( b, c, pt );
399 val -= (b.x * b.x + b.y * b.y) * cvTriangleArea( a, c, pt );
400 val += (c.x * c.x + c.y * c.y) * cvTriangleArea( a, b, pt );
401 val -= (pt.x * pt.x + pt.y * pt.y) * cvTriangleArea( a, b, c );
402
403 return val > FLT_EPSILON ? 1 : val < -FLT_EPSILON ? -1 : 0;
404 }
405
406
407 CV_IMPL CvSubdiv2DPoint *
cvSubdivDelaunay2DInsert(CvSubdiv2D * subdiv,CvPoint2D32f pt)408 cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt )
409 {
410 CvSubdiv2DPoint *point = 0;
411 CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
412
413 CvSubdiv2DPoint *curr_point = 0, *first_point = 0;
414 CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0;
415 int i, max_edges;
416
417 CV_FUNCNAME( "cvSubdivDelaunay2DInsert" );
418
419 __BEGIN__;
420
421 if( !subdiv )
422 CV_ERROR( CV_StsNullPtr, "" );
423
424 if( !CV_IS_SUBDIV2D(subdiv) )
425 CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR );
426
427
428 location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point );
429
430 switch (location)
431 {
432 case CV_PTLOC_ERROR:
433 CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR );
434
435 case CV_PTLOC_OUTSIDE_RECT:
436 CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR );
437
438 case CV_PTLOC_VERTEX:
439 point = curr_point;
440 break;
441
442 case CV_PTLOC_ON_EDGE:
443 deleted_edge = curr_edge;
444 subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
445 cvSubdiv2DDeleteEdge( subdiv, deleted_edge );
446 /* no break */
447
448 case CV_PTLOC_INSIDE:
449
450 assert( curr_edge != 0 );
451 subdiv->is_geometry_valid = 0;
452
453 curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 );
454 CV_CHECK();
455
456 base_edge = cvSubdiv2DMakeEdge( subdiv );
457 first_point = cvSubdiv2DEdgeOrg( curr_edge );
458 cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point );
459 cvSubdiv2DSplice( base_edge, curr_edge );
460
461 do
462 {
463 base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge,
464 cvSubdiv2DSymEdge( base_edge ));
465 curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
466 }
467 while( cvSubdiv2DEdgeDst( curr_edge ) != first_point );
468
469 curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
470
471 max_edges = subdiv->quad_edges * 4;
472
473 for( i = 0; i < max_edges; i++ )
474 {
475 CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0;
476 CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
477
478 temp_dst = cvSubdiv2DEdgeDst( temp_edge );
479 curr_org = cvSubdiv2DEdgeOrg( curr_edge );
480 curr_dst = cvSubdiv2DEdgeDst( curr_edge );
481
482 if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 &&
483 icvIsPtInCircle3( curr_org->pt, temp_dst->pt,
484 curr_dst->pt, curr_point->pt ) < 0 )
485 {
486 cvSubdiv2DSwapEdges( curr_edge );
487 curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
488 }
489 else if( curr_org == first_point )
490 {
491 break;
492 }
493 else
494 {
495 curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ),
496 CV_PREV_AROUND_LEFT );
497 }
498 }
499 break;
500 default:
501 assert( 0 );
502 CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR );
503 }
504
505 point = curr_point;
506
507
508 __END__;
509
510 //icvSubdiv2DCheck( subdiv );
511
512 return point;
513 }
514
515
516 CV_IMPL void
cvInitSubdivDelaunay2D(CvSubdiv2D * subdiv,CvRect rect)517 cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect )
518 {
519 float big_coord = 3.f * MAX( rect.width, rect.height );
520 CvPoint2D32f ppA, ppB, ppC;
521 CvSubdiv2DPoint *pA, *pB, *pC;
522 CvSubdiv2DEdge edge_AB, edge_BC, edge_CA;
523 float rx = (float) rect.x;
524 float ry = (float) rect.y;
525
526 CV_FUNCNAME( "cvSubdivDelaunay2DInit" );
527
528 __BEGIN__;
529
530 if( !subdiv )
531 CV_ERROR( CV_StsNullPtr, "" );
532
533 cvClearSet( (CvSet *) (subdiv->edges) );
534 cvClearSet( (CvSet *) subdiv );
535
536 subdiv->quad_edges = 0;
537 subdiv->recent_edge = 0;
538 subdiv->is_geometry_valid = 0;
539
540 subdiv->topleft = cvPoint2D32f( rx, ry );
541 subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height );
542
543 ppA = cvPoint2D32f( rx + big_coord, ry );
544 ppB = cvPoint2D32f( rx, ry + big_coord );
545 ppC = cvPoint2D32f( rx - big_coord, ry - big_coord );
546
547 pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 );
548 pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 );
549 pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 );
550
551 edge_AB = cvSubdiv2DMakeEdge( subdiv );
552 edge_BC = cvSubdiv2DMakeEdge( subdiv );
553 edge_CA = cvSubdiv2DMakeEdge( subdiv );
554
555 cvSubdiv2DSetEdgePoints( edge_AB, pA, pB );
556 cvSubdiv2DSetEdgePoints( edge_BC, pB, pC );
557 cvSubdiv2DSetEdgePoints( edge_CA, pC, pA );
558
559 cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA ));
560 cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB ));
561 cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC ));
562
563 subdiv->recent_edge = edge_AB;
564
565
566 __END__;
567 }
568
569
570 CV_IMPL void
cvClearSubdivVoronoi2D(CvSubdiv2D * subdiv)571 cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv )
572 {
573 int elem_size;
574 int i, total;
575 CvSeqReader reader;
576
577 CV_FUNCNAME( "cvClearVoronoi2D" );
578
579 __BEGIN__;
580
581 if( !subdiv )
582 CV_ERROR( CV_StsNullPtr, "" );
583
584 /* clear pointers to voronoi points */
585 total = subdiv->edges->total;
586 elem_size = subdiv->edges->elem_size;
587
588 cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
589
590 for( i = 0; i < total; i++ )
591 {
592 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr;
593
594 quadedge->pt[1] = quadedge->pt[3] = 0;
595 CV_NEXT_SEQ_ELEM( elem_size, reader );
596 }
597
598 /* remove voronoi points */
599 total = subdiv->total;
600 elem_size = subdiv->elem_size;
601
602 cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 );
603
604 for( i = 0; i < total; i++ )
605 {
606 CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr;
607
608 /* check for virtual point. it is also check that the point exists */
609 if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG )
610 {
611 cvSetRemoveByPtr( (CvSet*)subdiv, pt );
612 }
613 CV_NEXT_SEQ_ELEM( elem_size, reader );
614 }
615
616 subdiv->is_geometry_valid = 0;
617
618
619 __END__;
620 }
621
622
623 CV_IMPL void
cvCalcSubdivVoronoi2D(CvSubdiv2D * subdiv)624 cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv )
625 {
626 CvSeqReader reader;
627 int i, total, elem_size;
628
629 CV_FUNCNAME( "cvCalcSubdivVoronoi2D" );
630
631 __BEGIN__;
632
633 if( !subdiv )
634 CV_ERROR( CV_StsNullPtr, "" );
635
636 /* check if it is already calculated */
637 if( subdiv->is_geometry_valid )
638 EXIT;
639
640 total = subdiv->edges->total;
641 elem_size = subdiv->edges->elem_size;
642
643 cvClearSubdivVoronoi2D( subdiv );
644
645 cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
646
647 if( total <= 3 )
648 EXIT;
649
650 /* skip first three edges (bounding triangle) */
651 for( i = 0; i < 3; i++ )
652 CV_NEXT_SEQ_ELEM( elem_size, reader );
653
654 /* loop through all quad-edges */
655 for( ; i < total; i++ )
656 {
657 CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr);
658
659 if( CV_IS_SET_ELEM( quadedge ))
660 {
661 CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2;
662 double a0, b0, c0, a1, b1, c1;
663 CvPoint2D32f virt_point;
664 CvSubdiv2DPoint *voronoi_point;
665
666 if( !quadedge->pt[3] )
667 {
668 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT );
669 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT );
670
671 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
672 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
673
674 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
675 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
676 fabs( virt_point.y ) < FLT_MAX * 0.5 )
677 {
678 voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
679
680 quadedge->pt[3] =
681 ((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] =
682 ((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point;
683 }
684 }
685
686 if( !quadedge->pt[1] )
687 {
688 edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT );
689 edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT );
690
691 icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
692 icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
693
694 icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
695
696 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
697 fabs( virt_point.y ) < FLT_MAX * 0.5 )
698 {
699 voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
700
701 quadedge->pt[1] =
702 ((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] =
703 ((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point;
704 }
705 }
706 }
707
708 CV_NEXT_SEQ_ELEM( elem_size, reader );
709 }
710
711 subdiv->is_geometry_valid = 1;
712
713
714 __END__;
715 }
716
717
718 static int
icvIsRightOf2(const CvPoint2D32f & pt,const CvPoint2D32f & org,const CvPoint2D32f & diff)719 icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff )
720 {
721 Cv32suf cw_area;
722 cw_area.f = (org.x - pt.x)*diff.y - (org.y - pt.y)*diff.x;
723 return (cw_area.i > 0)*2 - (cw_area.i*2 != 0);
724 }
725
726
727 CV_IMPL CvSubdiv2DPoint*
cvFindNearestPoint2D(CvSubdiv2D * subdiv,CvPoint2D32f pt)728 cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt )
729 {
730 CvSubdiv2DPoint* point = 0;
731 CvPoint2D32f start;
732 CvPoint2D32f diff;
733 CvSubdiv2DPointLocation loc;
734 CvSubdiv2DEdge edge;
735 int i;
736
737 CV_FUNCNAME("cvFindNearestPoint2D");
738
739 __BEGIN__;
740
741 if( !subdiv )
742 CV_ERROR( CV_StsNullPtr, "" );
743
744 if( !CV_IS_SUBDIV2D( subdiv ))
745 CV_ERROR( CV_StsNullPtr, "" );
746
747 if( !subdiv->is_geometry_valid )
748 cvCalcSubdivVoronoi2D( subdiv );
749
750 loc = cvSubdiv2DLocate( subdiv, pt, &edge, &point );
751
752 switch( loc )
753 {
754 case CV_PTLOC_ON_EDGE:
755 case CV_PTLOC_INSIDE:
756 break;
757 default:
758 EXIT;
759 }
760
761 point = 0;
762
763 start = cvSubdiv2DEdgeOrg( edge )->pt;
764 diff.x = pt.x - start.x;
765 diff.y = pt.y - start.y;
766
767 edge = cvSubdiv2DRotateEdge( edge, 1 );
768
769 for( i = 0; i < subdiv->total; i++ )
770 {
771 CvPoint2D32f t;
772
773 for(;;)
774 {
775 assert( cvSubdiv2DEdgeDst( edge ));
776
777 t = cvSubdiv2DEdgeDst( edge )->pt;
778 if( icvIsRightOf2( t, start, diff ) >= 0 )
779 break;
780
781 edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT );
782 }
783
784 for(;;)
785 {
786 assert( cvSubdiv2DEdgeOrg( edge ));
787
788 t = cvSubdiv2DEdgeOrg( edge )->pt;
789 if( icvIsRightOf2( t, start, diff ) < 0 )
790 break;
791
792 edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_LEFT );
793 }
794
795 {
796 CvPoint2D32f tempDiff = cvSubdiv2DEdgeDst( edge )->pt;
797 t = cvSubdiv2DEdgeOrg( edge )->pt;
798 tempDiff.x -= t.x;
799 tempDiff.y -= t.y;
800
801 if( icvIsRightOf2( pt, t, tempDiff ) >= 0 )
802 {
803 point = cvSubdiv2DEdgeOrg( cvSubdiv2DRotateEdge( edge, 3 ));
804 break;
805 }
806 }
807
808 edge = cvSubdiv2DSymEdge( edge );
809 }
810
811 __END__;
812
813 return point;
814 }
815
816 /* Removed from the main interface */
817
818 #if 0
819 /* Adds new isolated quadedge to the subdivision */
820 OPENCVAPI CvSubdiv2DEdge cvSubdiv2DMakeEdge( CvSubdiv2D* subdiv );
821
822
823 /* Adds new isolated point to subdivision */
824 OPENCVAPI CvSubdiv2DPoint* cvSubdiv2DAddPoint( CvSubdiv2D* subdiv,
825 CvPoint2D32f pt, int is_virtual );
826
827
828 /* Does a splice operation for two quadedges */
829 OPENCVAPI void cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB );
830
831
832 /* Assigns ending [non-virtual] points for given quadedge */
833 OPENCVAPI void cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge,
834 CvSubdiv2DPoint* org_pt,
835 CvSubdiv2DPoint* dst_pt );
836
837 /* Removes quadedge from subdivision */
838 OPENCVAPI void cvSubdiv2DDeleteEdge( CvSubdiv2D* subdiv, CvSubdiv2DEdge edge );
839
840
841 /* Connects estination point of the first edge with origin point of the second edge */
842 OPENCVAPI CvSubdiv2DEdge cvSubdiv2DConnectEdges( CvSubdiv2D* subdiv,
843 CvSubdiv2DEdge edgeA,
844 CvSubdiv2DEdge edgeB );
845
846 /* Swaps diagonal in two connected Delaunay facets */
847 OPENCVAPI void cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge );
848 #endif
849
850 /* End of file. */
851