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 namespace cv
44 {
45
nextEdge(int edge) const46 int Subdiv2D::nextEdge(int edge) const
47 {
48 CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
49 return qedges[edge >> 2].next[edge & 3];
50 }
51
rotateEdge(int edge,int rotate) const52 int Subdiv2D::rotateEdge(int edge, int rotate) const
53 {
54 return (edge & ~3) + ((edge + rotate) & 3);
55 }
56
symEdge(int edge) const57 int Subdiv2D::symEdge(int edge) const
58 {
59 return edge ^ 2;
60 }
61
getEdge(int edge,int nextEdgeType) const62 int Subdiv2D::getEdge(int edge, int nextEdgeType) const
63 {
64 CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
65 edge = qedges[edge >> 2].next[(edge + nextEdgeType) & 3];
66 return (edge & ~3) + ((edge + (nextEdgeType >> 4)) & 3);
67 }
68
edgeOrg(int edge,CV_OUT Point2f * orgpt) const69 int Subdiv2D::edgeOrg(int edge, CV_OUT Point2f* orgpt) const
70 {
71 CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
72 int vidx = qedges[edge >> 2].pt[edge & 3];
73 if( orgpt )
74 {
75 CV_DbgAssert((size_t)vidx < vtx.size());
76 *orgpt = vtx[vidx].pt;
77 }
78 return vidx;
79 }
80
edgeDst(int edge,CV_OUT Point2f * dstpt) const81 int Subdiv2D::edgeDst(int edge, CV_OUT Point2f* dstpt) const
82 {
83 CV_DbgAssert((size_t)(edge >> 2) < qedges.size());
84 int vidx = qedges[edge >> 2].pt[(edge + 2) & 3];
85 if( dstpt )
86 {
87 CV_DbgAssert((size_t)vidx < vtx.size());
88 *dstpt = vtx[vidx].pt;
89 }
90 return vidx;
91 }
92
93
getVertex(int vertex,CV_OUT int * firstEdge) const94 Point2f Subdiv2D::getVertex(int vertex, CV_OUT int* firstEdge) const
95 {
96 CV_DbgAssert((size_t)vertex < vtx.size());
97 if( firstEdge )
98 *firstEdge = vtx[vertex].firstEdge;
99 return vtx[vertex].pt;
100 }
101
102
Subdiv2D()103 Subdiv2D::Subdiv2D()
104 {
105 validGeometry = false;
106 freeQEdge = 0;
107 freePoint = 0;
108 recentEdge = 0;
109 }
110
Subdiv2D(Rect rect)111 Subdiv2D::Subdiv2D(Rect rect)
112 {
113 validGeometry = false;
114 freeQEdge = 0;
115 freePoint = 0;
116 recentEdge = 0;
117
118 initDelaunay(rect);
119 }
120
121
QuadEdge()122 Subdiv2D::QuadEdge::QuadEdge()
123 {
124 next[0] = next[1] = next[2] = next[3] = 0;
125 pt[0] = pt[1] = pt[2] = pt[3] = 0;
126 }
127
QuadEdge(int edgeidx)128 Subdiv2D::QuadEdge::QuadEdge(int edgeidx)
129 {
130 CV_DbgAssert((edgeidx & 3) == 0);
131 next[0] = edgeidx;
132 next[1] = edgeidx+3;
133 next[2] = edgeidx+2;
134 next[3] = edgeidx+1;
135
136 pt[0] = pt[1] = pt[2] = pt[3] = 0;
137 }
138
isfree() const139 bool Subdiv2D::QuadEdge::isfree() const
140 {
141 return next[0] <= 0;
142 }
143
Vertex()144 Subdiv2D::Vertex::Vertex()
145 {
146 firstEdge = 0;
147 type = -1;
148 }
149
Vertex(Point2f _pt,bool _isvirtual,int _firstEdge)150 Subdiv2D::Vertex::Vertex(Point2f _pt, bool _isvirtual, int _firstEdge)
151 {
152 firstEdge = _firstEdge;
153 type = (int)_isvirtual;
154 pt = _pt;
155 }
156
isvirtual() const157 bool Subdiv2D::Vertex::isvirtual() const
158 {
159 return type > 0;
160 }
161
isfree() const162 bool Subdiv2D::Vertex::isfree() const
163 {
164 return type < 0;
165 }
166
splice(int edgeA,int edgeB)167 void Subdiv2D::splice( int edgeA, int edgeB )
168 {
169 int& a_next = qedges[edgeA >> 2].next[edgeA & 3];
170 int& b_next = qedges[edgeB >> 2].next[edgeB & 3];
171 int a_rot = rotateEdge(a_next, 1);
172 int b_rot = rotateEdge(b_next, 1);
173 int& a_rot_next = qedges[a_rot >> 2].next[a_rot & 3];
174 int& b_rot_next = qedges[b_rot >> 2].next[b_rot & 3];
175 std::swap(a_next, b_next);
176 std::swap(a_rot_next, b_rot_next);
177 }
178
setEdgePoints(int edge,int orgPt,int dstPt)179 void Subdiv2D::setEdgePoints(int edge, int orgPt, int dstPt)
180 {
181 qedges[edge >> 2].pt[edge & 3] = orgPt;
182 qedges[edge >> 2].pt[(edge + 2) & 3] = dstPt;
183 vtx[orgPt].firstEdge = edge;
184 vtx[dstPt].firstEdge = edge ^ 2;
185 }
186
connectEdges(int edgeA,int edgeB)187 int Subdiv2D::connectEdges( int edgeA, int edgeB )
188 {
189 int edge = newEdge();
190
191 splice(edge, getEdge(edgeA, NEXT_AROUND_LEFT));
192 splice(symEdge(edge), edgeB);
193
194 setEdgePoints(edge, edgeDst(edgeA), edgeOrg(edgeB));
195 return edge;
196 }
197
swapEdges(int edge)198 void Subdiv2D::swapEdges( int edge )
199 {
200 int sedge = symEdge(edge);
201 int a = getEdge(edge, PREV_AROUND_ORG);
202 int b = getEdge(sedge, PREV_AROUND_ORG);
203
204 splice(edge, a);
205 splice(sedge, b);
206
207 setEdgePoints(edge, edgeDst(a), edgeDst(b));
208
209 splice(edge, getEdge(a, NEXT_AROUND_LEFT));
210 splice(sedge, getEdge(b, NEXT_AROUND_LEFT));
211 }
212
triangleArea(Point2f a,Point2f b,Point2f c)213 static double triangleArea( Point2f a, Point2f b, Point2f c )
214 {
215 return ((double)b.x - a.x) * ((double)c.y - a.y) - ((double)b.y - a.y) * ((double)c.x - a.x);
216 }
217
isRightOf(Point2f pt,int edge) const218 int Subdiv2D::isRightOf(Point2f pt, int edge) const
219 {
220 Point2f org, dst;
221 edgeOrg(edge, &org);
222 edgeDst(edge, &dst);
223 double cw_area = triangleArea( pt, dst, org );
224
225 return (cw_area > 0) - (cw_area < 0);
226 }
227
newEdge()228 int Subdiv2D::newEdge()
229 {
230 if( freeQEdge <= 0 )
231 {
232 qedges.emplace_back();
233 freeQEdge = (int)(qedges.size()-1);
234 }
235 int edge = freeQEdge*4;
236 freeQEdge = qedges[edge >> 2].next[1];
237 qedges[edge >> 2] = QuadEdge(edge);
238 return edge;
239 }
240
deleteEdge(int edge)241 void Subdiv2D::deleteEdge(int edge)
242 {
243 CV_DbgAssert((size_t)(edge >> 2) < (size_t)qedges.size());
244 splice( edge, getEdge(edge, PREV_AROUND_ORG) );
245 int sedge = symEdge(edge);
246 splice(sedge, getEdge(sedge, PREV_AROUND_ORG) );
247
248 edge >>= 2;
249 qedges[edge].next[0] = 0;
250 qedges[edge].next[1] = freeQEdge;
251 freeQEdge = edge;
252 }
253
newPoint(Point2f pt,bool isvirtual,int firstEdge)254 int Subdiv2D::newPoint(Point2f pt, bool isvirtual, int firstEdge)
255 {
256 if( freePoint == 0 )
257 {
258 vtx.push_back(Vertex());
259 freePoint = (int)(vtx.size()-1);
260 }
261 int vidx = freePoint;
262 freePoint = vtx[vidx].firstEdge;
263 vtx[vidx] = Vertex(pt, isvirtual, firstEdge);
264
265 return vidx;
266 }
267
deletePoint(int vidx)268 void Subdiv2D::deletePoint(int vidx)
269 {
270 CV_DbgAssert( (size_t)vidx < vtx.size() );
271 vtx[vidx].firstEdge = freePoint;
272 vtx[vidx].type = -1;
273 freePoint = vidx;
274 }
275
locate(Point2f pt,int & _edge,int & _vertex)276 int Subdiv2D::locate(Point2f pt, int& _edge, int& _vertex)
277 {
278 CV_INSTRUMENT_REGION();
279
280 int vertex = 0;
281
282 int i, maxEdges = (int)(qedges.size() * 4);
283
284 if( qedges.size() < (size_t)4 )
285 CV_Error( CV_StsError, "Subdivision is empty" );
286
287 if( pt.x < topLeft.x || pt.y < topLeft.y || pt.x >= bottomRight.x || pt.y >= bottomRight.y )
288 CV_Error( CV_StsOutOfRange, "" );
289
290 int edge = recentEdge;
291 CV_Assert(edge > 0);
292
293 int location = PTLOC_ERROR;
294
295 int right_of_curr = isRightOf(pt, edge);
296 if( right_of_curr > 0 )
297 {
298 edge = symEdge(edge);
299 right_of_curr = -right_of_curr;
300 }
301
302 for( i = 0; i < maxEdges; i++ )
303 {
304 int onext_edge = nextEdge( edge );
305 int dprev_edge = getEdge( edge, PREV_AROUND_DST );
306
307 int right_of_onext = isRightOf( pt, onext_edge );
308 int right_of_dprev = isRightOf( pt, dprev_edge );
309
310 if( right_of_dprev > 0 )
311 {
312 if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
313 {
314 location = PTLOC_INSIDE;
315 break;
316 }
317 else
318 {
319 right_of_curr = right_of_onext;
320 edge = onext_edge;
321 }
322 }
323 else
324 {
325 if( right_of_onext > 0 )
326 {
327 if( right_of_dprev == 0 && right_of_curr == 0 )
328 {
329 location = PTLOC_INSIDE;
330 break;
331 }
332 else
333 {
334 right_of_curr = right_of_dprev;
335 edge = dprev_edge;
336 }
337 }
338 else if( right_of_curr == 0 &&
339 isRightOf( vtx[edgeDst(onext_edge)].pt, edge ) >= 0 )
340 {
341 edge = symEdge( edge );
342 }
343 else
344 {
345 right_of_curr = right_of_onext;
346 edge = onext_edge;
347 }
348 }
349 }
350
351 recentEdge = edge;
352
353 if( location == PTLOC_INSIDE )
354 {
355 Point2f org_pt, dst_pt;
356 edgeOrg(edge, &org_pt);
357 edgeDst(edge, &dst_pt);
358
359 double t1 = fabs( pt.x - org_pt.x );
360 t1 += fabs( pt.y - org_pt.y );
361 double t2 = fabs( pt.x - dst_pt.x );
362 t2 += fabs( pt.y - dst_pt.y );
363 double t3 = fabs( org_pt.x - dst_pt.x );
364 t3 += fabs( org_pt.y - dst_pt.y );
365
366 if( t1 < FLT_EPSILON )
367 {
368 location = PTLOC_VERTEX;
369 vertex = edgeOrg( edge );
370 edge = 0;
371 }
372 else if( t2 < FLT_EPSILON )
373 {
374 location = PTLOC_VERTEX;
375 vertex = edgeDst( edge );
376 edge = 0;
377 }
378 else if( (t1 < t3 || t2 < t3) &&
379 fabs( triangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
380 {
381 location = PTLOC_ON_EDGE;
382 vertex = 0;
383 }
384 }
385
386 if( location == PTLOC_ERROR )
387 {
388 edge = 0;
389 vertex = 0;
390 }
391
392 _edge = edge;
393 _vertex = vertex;
394
395 return location;
396 }
397
398
399 inline int
isPtInCircle3(Point2f pt,Point2f a,Point2f b,Point2f c)400 isPtInCircle3( Point2f pt, Point2f a, Point2f b, Point2f c)
401 {
402 const double eps = FLT_EPSILON*0.125;
403 double val = ((double)a.x * a.x + (double)a.y * a.y) * triangleArea( b, c, pt );
404 val -= ((double)b.x * b.x + (double)b.y * b.y) * triangleArea( a, c, pt );
405 val += ((double)c.x * c.x + (double)c.y * c.y) * triangleArea( a, b, pt );
406 val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * triangleArea( a, b, c );
407
408 return val > eps ? 1 : val < -eps ? -1 : 0;
409 }
410
411
insert(Point2f pt)412 int Subdiv2D::insert(Point2f pt)
413 {
414 CV_INSTRUMENT_REGION();
415
416 int curr_point = 0, curr_edge = 0, deleted_edge = 0;
417 int location = locate( pt, curr_edge, curr_point );
418
419 if( location == PTLOC_ERROR )
420 CV_Error( CV_StsBadSize, "" );
421
422 if( location == PTLOC_OUTSIDE_RECT )
423 CV_Error( CV_StsOutOfRange, "" );
424
425 if( location == PTLOC_VERTEX )
426 return curr_point;
427
428 if( location == PTLOC_ON_EDGE )
429 {
430 deleted_edge = curr_edge;
431 recentEdge = curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
432 deleteEdge(deleted_edge);
433 }
434 else if( location == PTLOC_INSIDE )
435 ;
436 else
437 CV_Error_(CV_StsError, ("Subdiv2D::locate returned invalid location = %d", location) );
438
439 assert( curr_edge != 0 );
440 validGeometry = false;
441
442 curr_point = newPoint(pt, false);
443 int base_edge = newEdge();
444 int first_point = edgeOrg(curr_edge);
445 setEdgePoints(base_edge, first_point, curr_point);
446 splice(base_edge, curr_edge);
447
448 do
449 {
450 base_edge = connectEdges( curr_edge, symEdge(base_edge) );
451 curr_edge = getEdge(base_edge, PREV_AROUND_ORG);
452 }
453 while( edgeDst(curr_edge) != first_point );
454
455 curr_edge = getEdge( base_edge, PREV_AROUND_ORG );
456
457 int i, max_edges = (int)(qedges.size()*4);
458
459 for( i = 0; i < max_edges; i++ )
460 {
461 int temp_dst = 0, curr_org = 0, curr_dst = 0;
462 int temp_edge = getEdge( curr_edge, PREV_AROUND_ORG );
463
464 temp_dst = edgeDst( temp_edge );
465 curr_org = edgeOrg( curr_edge );
466 curr_dst = edgeDst( curr_edge );
467
468 if( isRightOf( vtx[temp_dst].pt, curr_edge ) > 0 &&
469 isPtInCircle3( vtx[curr_org].pt, vtx[temp_dst].pt,
470 vtx[curr_dst].pt, vtx[curr_point].pt ) < 0 )
471 {
472 swapEdges( curr_edge );
473 curr_edge = getEdge( curr_edge, PREV_AROUND_ORG );
474 }
475 else if( curr_org == first_point )
476 break;
477 else
478 curr_edge = getEdge( nextEdge( curr_edge ), PREV_AROUND_LEFT );
479 }
480
481 return curr_point;
482 }
483
insert(const std::vector<Point2f> & ptvec)484 void Subdiv2D::insert(const std::vector<Point2f>& ptvec)
485 {
486 CV_INSTRUMENT_REGION();
487
488 for( size_t i = 0; i < ptvec.size(); i++ )
489 insert(ptvec[i]);
490 }
491
initDelaunay(Rect rect)492 void Subdiv2D::initDelaunay( Rect rect )
493 {
494 CV_INSTRUMENT_REGION();
495
496 float big_coord = 3.f * MAX( rect.width, rect.height );
497 float rx = (float)rect.x;
498 float ry = (float)rect.y;
499
500 vtx.clear();
501 qedges.clear();
502
503 recentEdge = 0;
504 validGeometry = false;
505
506 topLeft = Point2f( rx, ry );
507 bottomRight = Point2f( rx + rect.width, ry + rect.height );
508
509 Point2f ppA( rx + big_coord, ry );
510 Point2f ppB( rx, ry + big_coord );
511 Point2f ppC( rx - big_coord, ry - big_coord );
512
513 vtx.push_back(Vertex());
514 qedges.push_back(QuadEdge());
515
516 freeQEdge = 0;
517 freePoint = 0;
518
519 int pA = newPoint(ppA, false);
520 int pB = newPoint(ppB, false);
521 int pC = newPoint(ppC, false);
522
523 int edge_AB = newEdge();
524 int edge_BC = newEdge();
525 int edge_CA = newEdge();
526
527 setEdgePoints( edge_AB, pA, pB );
528 setEdgePoints( edge_BC, pB, pC );
529 setEdgePoints( edge_CA, pC, pA );
530
531 splice( edge_AB, symEdge( edge_CA ));
532 splice( edge_BC, symEdge( edge_AB ));
533 splice( edge_CA, symEdge( edge_BC ));
534
535 recentEdge = edge_AB;
536 }
537
538
clearVoronoi()539 void Subdiv2D::clearVoronoi()
540 {
541 size_t i, total = qedges.size();
542
543 for( i = 0; i < total; i++ )
544 qedges[i].pt[1] = qedges[i].pt[3] = 0;
545
546 total = vtx.size();
547 for( i = 0; i < total; i++ )
548 {
549 if( vtx[i].isvirtual() )
550 deletePoint((int)i);
551 }
552
553 validGeometry = false;
554 }
555
556
computeVoronoiPoint(Point2f org0,Point2f dst0,Point2f org1,Point2f dst1)557 static Point2f computeVoronoiPoint(Point2f org0, Point2f dst0, Point2f org1, Point2f dst1)
558 {
559 double a0 = dst0.x - org0.x;
560 double b0 = dst0.y - org0.y;
561 double c0 = -0.5*(a0 * (dst0.x + org0.x) + b0 * (dst0.y + org0.y));
562
563 double a1 = dst1.x - org1.x;
564 double b1 = dst1.y - org1.y;
565 double c1 = -0.5*(a1 * (dst1.x + org1.x) + b1 * (dst1.y + org1.y));
566
567 double det = a0 * b1 - a1 * b0;
568
569 if( det != 0 )
570 {
571 det = 1. / det;
572 return Point2f((float) ((b0 * c1 - b1 * c0) * det),
573 (float) ((a1 * c0 - a0 * c1) * det));
574 }
575
576 return Point2f(FLT_MAX, FLT_MAX);
577 }
578
579
calcVoronoi()580 void Subdiv2D::calcVoronoi()
581 {
582 // check if it is already calculated
583 if( validGeometry )
584 return;
585
586 clearVoronoi();
587 int i, total = (int)qedges.size();
588
589 // loop through all quad-edges, except for the first 3 (#1, #2, #3 - 0 is reserved for "NULL" pointer)
590 for( i = 4; i < total; i++ )
591 {
592 QuadEdge& quadedge = qedges[i];
593
594 if( quadedge.isfree() )
595 continue;
596
597 int edge0 = (int)(i*4);
598 Point2f org0, dst0, org1, dst1;
599
600 if( !quadedge.pt[3] )
601 {
602 int edge1 = getEdge( edge0, NEXT_AROUND_LEFT );
603 int edge2 = getEdge( edge1, NEXT_AROUND_LEFT );
604
605 edgeOrg(edge0, &org0);
606 edgeDst(edge0, &dst0);
607 edgeOrg(edge1, &org1);
608 edgeDst(edge1, &dst1);
609
610 Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
611
612 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
613 fabs( virt_point.y ) < FLT_MAX * 0.5 )
614 {
615 quadedge.pt[3] = qedges[edge1 >> 2].pt[3 - (edge1 & 2)] =
616 qedges[edge2 >> 2].pt[3 - (edge2 & 2)] = newPoint(virt_point, true);
617 }
618 }
619
620 if( !quadedge.pt[1] )
621 {
622 int edge1 = getEdge( edge0, NEXT_AROUND_RIGHT );
623 int edge2 = getEdge( edge1, NEXT_AROUND_RIGHT );
624
625 edgeOrg(edge0, &org0);
626 edgeDst(edge0, &dst0);
627 edgeOrg(edge1, &org1);
628 edgeDst(edge1, &dst1);
629
630 Point2f virt_point = computeVoronoiPoint(org0, dst0, org1, dst1);
631
632 if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
633 fabs( virt_point.y ) < FLT_MAX * 0.5 )
634 {
635 quadedge.pt[1] = qedges[edge1 >> 2].pt[1 + (edge1 & 2)] =
636 qedges[edge2 >> 2].pt[1 + (edge2 & 2)] = newPoint(virt_point, true);
637 }
638 }
639 }
640
641 validGeometry = true;
642 }
643
644
645 static int
isRightOf2(const Point2f & pt,const Point2f & org,const Point2f & diff)646 isRightOf2( const Point2f& pt, const Point2f& org, const Point2f& diff )
647 {
648 double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
649 return (cw_area > 0) - (cw_area < 0);
650 }
651
652
findNearest(Point2f pt,Point2f * nearestPt)653 int Subdiv2D::findNearest(Point2f pt, Point2f* nearestPt)
654 {
655 CV_INSTRUMENT_REGION();
656
657 if( !validGeometry )
658 calcVoronoi();
659
660 int vertex = 0, edge = 0;
661 int loc = locate( pt, edge, vertex );
662
663 if( loc != PTLOC_ON_EDGE && loc != PTLOC_INSIDE )
664 return vertex;
665
666 vertex = 0;
667
668 Point2f start;
669 edgeOrg(edge, &start);
670 Point2f diff = pt - start;
671
672 edge = rotateEdge(edge, 1);
673
674 int i, total = (int)vtx.size();
675
676 for( i = 0; i < total; i++ )
677 {
678 Point2f t;
679
680 for(;;)
681 {
682 CV_Assert( edgeDst(edge, &t) > 0 );
683 if( isRightOf2( t, start, diff ) >= 0 )
684 break;
685
686 edge = getEdge( edge, NEXT_AROUND_LEFT );
687 }
688
689 for(;;)
690 {
691 CV_Assert( edgeOrg( edge, &t ) > 0 );
692
693 if( isRightOf2( t, start, diff ) < 0 )
694 break;
695
696 edge = getEdge( edge, PREV_AROUND_LEFT );
697 }
698
699 Point2f tempDiff;
700 edgeDst(edge, &tempDiff);
701 edgeOrg(edge, &t);
702 tempDiff -= t;
703
704 if( isRightOf2( pt, t, tempDiff ) >= 0 )
705 {
706 vertex = edgeOrg(rotateEdge( edge, 3 ));
707 break;
708 }
709
710 edge = symEdge( edge );
711 }
712
713 if( nearestPt && vertex > 0 )
714 *nearestPt = vtx[vertex].pt;
715
716 return vertex;
717 }
718
getEdgeList(std::vector<Vec4f> & edgeList) const719 void Subdiv2D::getEdgeList(std::vector<Vec4f>& edgeList) const
720 {
721 edgeList.clear();
722
723 for( size_t i = 4; i < qedges.size(); i++ )
724 {
725 if( qedges[i].isfree() )
726 continue;
727 if( qedges[i].pt[0] > 0 && qedges[i].pt[2] > 0 )
728 {
729 Point2f org = vtx[qedges[i].pt[0]].pt;
730 Point2f dst = vtx[qedges[i].pt[2]].pt;
731 edgeList.push_back(Vec4f(org.x, org.y, dst.x, dst.y));
732 }
733 }
734 }
735
getLeadingEdgeList(std::vector<int> & leadingEdgeList) const736 void Subdiv2D::getLeadingEdgeList(std::vector<int>& leadingEdgeList) const
737 {
738 leadingEdgeList.clear();
739 int i, total = (int)(qedges.size()*4);
740 std::vector<bool> edgemask(total, false);
741
742 for( i = 4; i < total; i += 2 )
743 {
744 if( edgemask[i] )
745 continue;
746 int edge = i;
747 edgemask[edge] = true;
748 edge = getEdge(edge, NEXT_AROUND_LEFT);
749 edgemask[edge] = true;
750 edge = getEdge(edge, NEXT_AROUND_LEFT);
751 edgemask[edge] = true;
752 leadingEdgeList.push_back(i);
753 }
754 }
755
getTriangleList(std::vector<Vec6f> & triangleList) const756 void Subdiv2D::getTriangleList(std::vector<Vec6f>& triangleList) const
757 {
758 triangleList.clear();
759 int i, total = (int)(qedges.size()*4);
760 std::vector<bool> edgemask(total, false);
761 Rect2f rect(topLeft.x, topLeft.y, bottomRight.x - topLeft.x, bottomRight.y - topLeft.y);
762
763 for( i = 4; i < total; i += 2 )
764 {
765 if( edgemask[i] )
766 continue;
767 Point2f a, b, c;
768 int edge_a = i;
769 edgeOrg(edge_a, &a);
770 if ( !rect.contains(a) )
771 continue;
772 int edge_b = getEdge(edge_a, NEXT_AROUND_LEFT);
773 edgeOrg(edge_b, &b);
774 if ( !rect.contains(b) )
775 continue;
776 int edge_c = getEdge(edge_b, NEXT_AROUND_LEFT);
777 edgeOrg(edge_c, &c);
778 if ( !rect.contains(c) )
779 continue;
780 edgemask[edge_a] = true;
781 edgemask[edge_b] = true;
782 edgemask[edge_c] = true;
783 triangleList.push_back(Vec6f(a.x, a.y, b.x, b.y, c.x, c.y));
784 }
785 }
786
getVoronoiFacetList(const std::vector<int> & idx,CV_OUT std::vector<std::vector<Point2f>> & facetList,CV_OUT std::vector<Point2f> & facetCenters)787 void Subdiv2D::getVoronoiFacetList(const std::vector<int>& idx,
788 CV_OUT std::vector<std::vector<Point2f> >& facetList,
789 CV_OUT std::vector<Point2f>& facetCenters)
790 {
791 calcVoronoi();
792 facetList.clear();
793 facetCenters.clear();
794
795 std::vector<Point2f> buf;
796
797 size_t i, total;
798 if( idx.empty() )
799 i = 4, total = vtx.size();
800 else
801 i = 0, total = idx.size();
802
803 for( ; i < total; i++ )
804 {
805 int k = idx.empty() ? (int)i : idx[i];
806
807 if( vtx[k].isfree() || vtx[k].isvirtual() )
808 continue;
809 int edge = rotateEdge(vtx[k].firstEdge, 1), t = edge;
810
811 // gather points
812 buf.clear();
813 do
814 {
815 buf.push_back(vtx[edgeOrg(t)].pt);
816 t = getEdge( t, NEXT_AROUND_LEFT );
817 }
818 while( t != edge );
819
820 facetList.push_back(buf);
821 facetCenters.push_back(vtx[k].pt);
822 }
823 }
824
825
checkSubdiv() const826 void Subdiv2D::checkSubdiv() const
827 {
828 int i, j, total = (int)qedges.size();
829
830 for( i = 0; i < total; i++ )
831 {
832 const QuadEdge& qe = qedges[i];
833
834 if( qe.isfree() )
835 continue;
836
837 for( j = 0; j < 4; j++ )
838 {
839 int e = (int)(i*4 + j);
840 int o_next = nextEdge(e);
841 int o_prev = getEdge(e, PREV_AROUND_ORG );
842 int d_prev = getEdge(e, PREV_AROUND_DST );
843 int d_next = getEdge(e, NEXT_AROUND_DST );
844
845 // check points
846 CV_Assert( edgeOrg(e) == edgeOrg(o_next));
847 CV_Assert( edgeOrg(e) == edgeOrg(o_prev));
848 CV_Assert( edgeDst(e) == edgeDst(d_next));
849 CV_Assert( edgeDst(e) == edgeDst(d_prev));
850
851 if( j % 2 == 0 )
852 {
853 CV_Assert( edgeDst(o_next) == edgeOrg(d_prev));
854 CV_Assert( edgeDst(o_prev) == edgeOrg(d_next));
855 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_LEFT),NEXT_AROUND_LEFT),NEXT_AROUND_LEFT) == e );
856 CV_Assert( getEdge(getEdge(getEdge(e,NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT),NEXT_AROUND_RIGHT) == e);
857 }
858 }
859 }
860 }
861
862 }
863
864 /* End of file. */
865