| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| #include "_cv.h" |
| |
| CV_IMPL CvSubdiv2D * |
| cvCreateSubdiv2D( int subdiv_type, int header_size, |
| int vtx_size, int quadedge_size, CvMemStorage * storage ) |
| { |
| CvSubdiv2D *subdiv = 0; |
| |
| CV_FUNCNAME( "cvCleateSubdiv2D" ); |
| |
| __BEGIN__; |
| |
| if( !storage ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( header_size < (int)sizeof( *subdiv ) || |
| quadedge_size < (int)sizeof( CvQuadEdge2D ) || |
| vtx_size < (int)sizeof( CvSubdiv2DPoint )) |
| CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); |
| |
| subdiv = (CvSubdiv2D *) cvCreateGraph( subdiv_type, header_size, |
| vtx_size, quadedge_size, storage ); |
| |
| |
| __END__; |
| |
| return subdiv; |
| } |
| |
| |
| /****************************************************************************************\ |
| * Quad Edge algebra * |
| \****************************************************************************************/ |
| |
| static CvSubdiv2DEdge |
| cvSubdiv2DMakeEdge( CvSubdiv2D * subdiv ) |
| { |
| CvQuadEdge2D *edge = 0; |
| CvSubdiv2DEdge edgehandle = 0; |
| |
| CV_FUNCNAME( "cvSubdiv2DMakeEdge" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| edge = (CvQuadEdge2D*)cvSetNew( (CvSet*)subdiv->edges ); |
| CV_CHECK(); |
| |
| memset( edge->pt, 0, sizeof( edge->pt )); |
| edgehandle = (CvSubdiv2DEdge) edge; |
| |
| edge->next[0] = edgehandle; |
| edge->next[1] = edgehandle + 3; |
| edge->next[2] = edgehandle + 2; |
| edge->next[3] = edgehandle + 1; |
| |
| subdiv->quad_edges++; |
| |
| |
| __END__; |
| |
| return edgehandle; |
| } |
| |
| |
| static CvSubdiv2DPoint * |
| cvSubdiv2DAddPoint( CvSubdiv2D * subdiv, CvPoint2D32f pt, int is_virtual ) |
| { |
| CvSubdiv2DPoint *subdiv_point = 0; |
| |
| subdiv_point = (CvSubdiv2DPoint*)cvSetNew( (CvSet*)subdiv ); |
| if( subdiv_point ) |
| { |
| memset( subdiv_point, 0, subdiv->elem_size ); |
| subdiv_point->pt = pt; |
| subdiv_point->first = 0; |
| subdiv_point->flags |= is_virtual ? CV_SUBDIV2D_VIRTUAL_POINT_FLAG : 0; |
| } |
| |
| return subdiv_point; |
| } |
| |
| |
| static void |
| cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ) |
| { |
| CvSubdiv2DEdge *a_next = &CV_SUBDIV2D_NEXT_EDGE( edgeA ); |
| CvSubdiv2DEdge *b_next = &CV_SUBDIV2D_NEXT_EDGE( edgeB ); |
| CvSubdiv2DEdge a_rot = cvSubdiv2DRotateEdge( *a_next, 1 ); |
| CvSubdiv2DEdge b_rot = cvSubdiv2DRotateEdge( *b_next, 1 ); |
| CvSubdiv2DEdge *a_rot_next = &CV_SUBDIV2D_NEXT_EDGE( a_rot ); |
| CvSubdiv2DEdge *b_rot_next = &CV_SUBDIV2D_NEXT_EDGE( b_rot ); |
| CvSubdiv2DEdge t; |
| |
| CV_SWAP( *a_next, *b_next, t ); |
| CV_SWAP( *a_rot_next, *b_rot_next, t ); |
| } |
| |
| |
| static void |
| cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge, |
| CvSubdiv2DPoint * org_pt, CvSubdiv2DPoint * dst_pt ) |
| { |
| CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3); |
| |
| CV_FUNCNAME( "cvSubdiv2DSetEdgePoints" ); |
| |
| __BEGIN__; |
| |
| if( !quadedge ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| quadedge->pt[edge & 3] = org_pt; |
| quadedge->pt[(edge + 2) & 3] = dst_pt; |
| |
| |
| __END__; |
| } |
| |
| |
| static void |
| cvSubdiv2DDeleteEdge( CvSubdiv2D * subdiv, CvSubdiv2DEdge edge ) |
| { |
| CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3); |
| |
| CV_FUNCNAME( "cvSubdiv2DDeleteEdge" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv || !quadedge ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG )); |
| |
| { |
| CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge ); |
| cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG )); |
| } |
| |
| cvSetRemoveByPtr( (CvSet*)(subdiv->edges), quadedge ); |
| subdiv->quad_edges--; |
| |
| |
| __END__; |
| } |
| |
| |
| static CvSubdiv2DEdge |
| cvSubdiv2DConnectEdges( CvSubdiv2D * subdiv, CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ) |
| { |
| CvSubdiv2DEdge new_edge = 0; |
| |
| CV_FUNCNAME( "cvSubdiv2DConnectPoints" ); |
| |
| __BEGIN__; |
| |
| CvSubdiv2DPoint *orgB, *dstA; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| new_edge = cvSubdiv2DMakeEdge( subdiv ); |
| |
| cvSubdiv2DSplice( new_edge, cvSubdiv2DGetEdge( edgeA, CV_NEXT_AROUND_LEFT )); |
| cvSubdiv2DSplice( cvSubdiv2DSymEdge( new_edge ), edgeB ); |
| |
| dstA = cvSubdiv2DEdgeDst( edgeA ); |
| orgB = cvSubdiv2DEdgeOrg( edgeB ); |
| cvSubdiv2DSetEdgePoints( new_edge, dstA, orgB ); |
| |
| __END__; |
| |
| return new_edge; |
| } |
| |
| |
| static void |
| cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge ) |
| { |
| CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge ); |
| CvSubdiv2DEdge a = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG ); |
| CvSubdiv2DEdge b = cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG ); |
| CvSubdiv2DPoint *dstB, *dstA; |
| |
| cvSubdiv2DSplice( edge, a ); |
| cvSubdiv2DSplice( sym_edge, b ); |
| |
| dstA = cvSubdiv2DEdgeDst( a ); |
| dstB = cvSubdiv2DEdgeDst( b ); |
| cvSubdiv2DSetEdgePoints( edge, dstA, dstB ); |
| |
| cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( a, CV_NEXT_AROUND_LEFT )); |
| cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( b, CV_NEXT_AROUND_LEFT )); |
| } |
| |
| |
| static int |
| icvIsRightOf( CvPoint2D32f& pt, CvSubdiv2DEdge edge ) |
| { |
| CvSubdiv2DPoint *org = cvSubdiv2DEdgeOrg(edge), *dst = cvSubdiv2DEdgeDst(edge); |
| Cv32suf cw_area; |
| cw_area.f = (float)cvTriangleArea( pt, dst->pt, org->pt ); |
| |
| return (cw_area.i > 0)*2 - (cw_area.i*2 != 0); |
| } |
| |
| |
| CV_IMPL CvSubdiv2DPointLocation |
| cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt, |
| CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point ) |
| { |
| CvSubdiv2DEdge edge = 0; |
| CvSubdiv2DPoint *point = 0; |
| CvSubdiv2DPointLocation location = CV_PTLOC_ERROR; |
| |
| int i, max_edges; |
| int right_of_curr = 0; |
| |
| CV_FUNCNAME( "cvSubdiv2DLocate" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( !CV_IS_SUBDIV2D(subdiv) ) |
| CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR ); |
| |
| max_edges = subdiv->quad_edges * 4; |
| edge = subdiv->recent_edge; |
| |
| if( max_edges == 0 ) |
| CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); |
| if( !edge ) |
| CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR ); |
| |
| location = CV_PTLOC_OUTSIDE_RECT; |
| if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y || |
| pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y ) |
| CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR ); |
| |
| location = CV_PTLOC_ERROR; |
| |
| right_of_curr = icvIsRightOf( pt, edge ); |
| if( right_of_curr > 0 ) |
| { |
| edge = cvSubdiv2DSymEdge( edge ); |
| right_of_curr = -right_of_curr; |
| } |
| |
| for( i = 0; i < max_edges; i++ ) |
| { |
| CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge ); |
| CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST ); |
| |
| int right_of_onext = icvIsRightOf( pt, onext_edge ); |
| int right_of_dprev = icvIsRightOf( pt, dprev_edge ); |
| |
| if( right_of_dprev > 0 ) |
| { |
| if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) ) |
| { |
| location = CV_PTLOC_INSIDE; |
| EXIT; |
| } |
| else |
| { |
| right_of_curr = right_of_onext; |
| edge = onext_edge; |
| } |
| } |
| else |
| { |
| if( right_of_onext > 0 ) |
| { |
| if( right_of_dprev == 0 && right_of_curr == 0 ) |
| { |
| location = CV_PTLOC_INSIDE; |
| EXIT; |
| } |
| else |
| { |
| right_of_curr = right_of_dprev; |
| edge = dprev_edge; |
| } |
| } |
| else if( right_of_curr == 0 && |
| icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 ) |
| { |
| edge = cvSubdiv2DSymEdge( edge ); |
| } |
| else |
| { |
| right_of_curr = right_of_onext; |
| edge = onext_edge; |
| } |
| } |
| } |
| |
| |
| __END__; |
| |
| subdiv->recent_edge = edge; |
| |
| if( location == CV_PTLOC_INSIDE ) |
| { |
| double t1, t2, t3; |
| CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt; |
| CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt; |
| |
| t1 = fabs( pt.x - org_pt.x ); |
| t1 += fabs( pt.y - org_pt.y ); |
| t2 = fabs( pt.x - dst_pt.x ); |
| t2 += fabs( pt.y - dst_pt.y ); |
| t3 = fabs( org_pt.x - dst_pt.x ); |
| t3 += fabs( org_pt.y - dst_pt.y ); |
| |
| if( t1 < FLT_EPSILON ) |
| { |
| location = CV_PTLOC_VERTEX; |
| point = cvSubdiv2DEdgeOrg( edge ); |
| edge = 0; |
| } |
| else if( t2 < FLT_EPSILON ) |
| { |
| location = CV_PTLOC_VERTEX; |
| point = cvSubdiv2DEdgeDst( edge ); |
| edge = 0; |
| } |
| else if( (t1 < t3 || t2 < t3) && |
| fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON ) |
| { |
| location = CV_PTLOC_ON_EDGE; |
| point = 0; |
| } |
| } |
| |
| if( location == CV_PTLOC_ERROR ) |
| { |
| edge = 0; |
| point = 0; |
| } |
| |
| if( _edge ) |
| *_edge = edge; |
| if( _point ) |
| *_point = point; |
| |
| return location; |
| } |
| |
| |
| CV_INLINE int |
| icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c ) |
| { |
| double val = (a.x * a.x + a.y * a.y) * cvTriangleArea( b, c, pt ); |
| val -= (b.x * b.x + b.y * b.y) * cvTriangleArea( a, c, pt ); |
| val += (c.x * c.x + c.y * c.y) * cvTriangleArea( a, b, pt ); |
| val -= (pt.x * pt.x + pt.y * pt.y) * cvTriangleArea( a, b, c ); |
| |
| return val > FLT_EPSILON ? 1 : val < -FLT_EPSILON ? -1 : 0; |
| } |
| |
| |
| CV_IMPL CvSubdiv2DPoint * |
| cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt ) |
| { |
| CvSubdiv2DPoint *point = 0; |
| CvSubdiv2DPointLocation location = CV_PTLOC_ERROR; |
| |
| CvSubdiv2DPoint *curr_point = 0, *first_point = 0; |
| CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0; |
| int i, max_edges; |
| |
| CV_FUNCNAME( "cvSubdivDelaunay2DInsert" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( !CV_IS_SUBDIV2D(subdiv) ) |
| CV_ERROR_FROM_STATUS( CV_BADFLAG_ERR ); |
| |
| |
| location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point ); |
| |
| switch (location) |
| { |
| case CV_PTLOC_ERROR: |
| CV_ERROR_FROM_STATUS( CV_BADSIZE_ERR ); |
| |
| case CV_PTLOC_OUTSIDE_RECT: |
| CV_ERROR_FROM_STATUS( CV_BADRANGE_ERR ); |
| |
| case CV_PTLOC_VERTEX: |
| point = curr_point; |
| break; |
| |
| case CV_PTLOC_ON_EDGE: |
| deleted_edge = curr_edge; |
| subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); |
| cvSubdiv2DDeleteEdge( subdiv, deleted_edge ); |
| /* no break */ |
| |
| case CV_PTLOC_INSIDE: |
| |
| assert( curr_edge != 0 ); |
| subdiv->is_geometry_valid = 0; |
| |
| curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 ); |
| CV_CHECK(); |
| |
| base_edge = cvSubdiv2DMakeEdge( subdiv ); |
| first_point = cvSubdiv2DEdgeOrg( curr_edge ); |
| cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point ); |
| cvSubdiv2DSplice( base_edge, curr_edge ); |
| |
| do |
| { |
| base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge, |
| cvSubdiv2DSymEdge( base_edge )); |
| curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG ); |
| } |
| while( cvSubdiv2DEdgeDst( curr_edge ) != first_point ); |
| |
| curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG ); |
| |
| max_edges = subdiv->quad_edges * 4; |
| |
| for( i = 0; i < max_edges; i++ ) |
| { |
| CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0; |
| CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); |
| |
| temp_dst = cvSubdiv2DEdgeDst( temp_edge ); |
| curr_org = cvSubdiv2DEdgeOrg( curr_edge ); |
| curr_dst = cvSubdiv2DEdgeDst( curr_edge ); |
| |
| if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 && |
| icvIsPtInCircle3( curr_org->pt, temp_dst->pt, |
| curr_dst->pt, curr_point->pt ) < 0 ) |
| { |
| cvSubdiv2DSwapEdges( curr_edge ); |
| curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG ); |
| } |
| else if( curr_org == first_point ) |
| { |
| break; |
| } |
| else |
| { |
| curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ), |
| CV_PREV_AROUND_LEFT ); |
| } |
| } |
| break; |
| default: |
| assert( 0 ); |
| CV_ERROR_FROM_STATUS( CV_NOTDEFINED_ERR ); |
| } |
| |
| point = curr_point; |
| |
| |
| __END__; |
| |
| //icvSubdiv2DCheck( subdiv ); |
| |
| return point; |
| } |
| |
| |
| CV_IMPL void |
| cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect ) |
| { |
| float big_coord = 3.f * MAX( rect.width, rect.height ); |
| CvPoint2D32f ppA, ppB, ppC; |
| CvSubdiv2DPoint *pA, *pB, *pC; |
| CvSubdiv2DEdge edge_AB, edge_BC, edge_CA; |
| float rx = (float) rect.x; |
| float ry = (float) rect.y; |
| |
| CV_FUNCNAME( "cvSubdivDelaunay2DInit" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| cvClearSet( (CvSet *) (subdiv->edges) ); |
| cvClearSet( (CvSet *) subdiv ); |
| |
| subdiv->quad_edges = 0; |
| subdiv->recent_edge = 0; |
| subdiv->is_geometry_valid = 0; |
| |
| subdiv->topleft = cvPoint2D32f( rx, ry ); |
| subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height ); |
| |
| ppA = cvPoint2D32f( rx + big_coord, ry ); |
| ppB = cvPoint2D32f( rx, ry + big_coord ); |
| ppC = cvPoint2D32f( rx - big_coord, ry - big_coord ); |
| |
| pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 ); |
| pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 ); |
| pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 ); |
| |
| edge_AB = cvSubdiv2DMakeEdge( subdiv ); |
| edge_BC = cvSubdiv2DMakeEdge( subdiv ); |
| edge_CA = cvSubdiv2DMakeEdge( subdiv ); |
| |
| cvSubdiv2DSetEdgePoints( edge_AB, pA, pB ); |
| cvSubdiv2DSetEdgePoints( edge_BC, pB, pC ); |
| cvSubdiv2DSetEdgePoints( edge_CA, pC, pA ); |
| |
| cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA )); |
| cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB )); |
| cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC )); |
| |
| subdiv->recent_edge = edge_AB; |
| |
| |
| __END__; |
| } |
| |
| |
| CV_IMPL void |
| cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv ) |
| { |
| int elem_size; |
| int i, total; |
| CvSeqReader reader; |
| |
| CV_FUNCNAME( "cvClearVoronoi2D" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| /* clear pointers to voronoi points */ |
| total = subdiv->edges->total; |
| elem_size = subdiv->edges->elem_size; |
| |
| cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 ); |
| |
| for( i = 0; i < total; i++ ) |
| { |
| CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr; |
| |
| quadedge->pt[1] = quadedge->pt[3] = 0; |
| CV_NEXT_SEQ_ELEM( elem_size, reader ); |
| } |
| |
| /* remove voronoi points */ |
| total = subdiv->total; |
| elem_size = subdiv->elem_size; |
| |
| cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 ); |
| |
| for( i = 0; i < total; i++ ) |
| { |
| CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr; |
| |
| /* check for virtual point. it is also check that the point exists */ |
| if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG ) |
| { |
| cvSetRemoveByPtr( (CvSet*)subdiv, pt ); |
| } |
| CV_NEXT_SEQ_ELEM( elem_size, reader ); |
| } |
| |
| subdiv->is_geometry_valid = 0; |
| |
| |
| __END__; |
| } |
| |
| |
| CV_IMPL void |
| cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv ) |
| { |
| CvSeqReader reader; |
| int i, total, elem_size; |
| |
| CV_FUNCNAME( "cvCalcSubdivVoronoi2D" ); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| /* check if it is already calculated */ |
| if( subdiv->is_geometry_valid ) |
| EXIT; |
| |
| total = subdiv->edges->total; |
| elem_size = subdiv->edges->elem_size; |
| |
| cvClearSubdivVoronoi2D( subdiv ); |
| |
| cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 ); |
| |
| if( total <= 3 ) |
| EXIT; |
| |
| /* skip first three edges (bounding triangle) */ |
| for( i = 0; i < 3; i++ ) |
| CV_NEXT_SEQ_ELEM( elem_size, reader ); |
| |
| /* loop through all quad-edges */ |
| for( ; i < total; i++ ) |
| { |
| CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr); |
| |
| if( CV_IS_SET_ELEM( quadedge )) |
| { |
| CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2; |
| double a0, b0, c0, a1, b1, c1; |
| CvPoint2D32f virt_point; |
| CvSubdiv2DPoint *voronoi_point; |
| |
| if( !quadedge->pt[3] ) |
| { |
| edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT ); |
| edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT ); |
| |
| icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 ); |
| icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 ); |
| |
| icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point ); |
| if( fabs( virt_point.x ) < FLT_MAX * 0.5 && |
| fabs( virt_point.y ) < FLT_MAX * 0.5 ) |
| { |
| voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 ); |
| |
| quadedge->pt[3] = |
| ((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] = |
| ((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point; |
| } |
| } |
| |
| if( !quadedge->pt[1] ) |
| { |
| edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT ); |
| edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT ); |
| |
| icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 ); |
| icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 ); |
| |
| icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point ); |
| |
| if( fabs( virt_point.x ) < FLT_MAX * 0.5 && |
| fabs( virt_point.y ) < FLT_MAX * 0.5 ) |
| { |
| voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 ); |
| |
| quadedge->pt[1] = |
| ((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] = |
| ((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point; |
| } |
| } |
| } |
| |
| CV_NEXT_SEQ_ELEM( elem_size, reader ); |
| } |
| |
| subdiv->is_geometry_valid = 1; |
| |
| |
| __END__; |
| } |
| |
| |
| static int |
| icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff ) |
| { |
| Cv32suf cw_area; |
| cw_area.f = (org.x - pt.x)*diff.y - (org.y - pt.y)*diff.x; |
| return (cw_area.i > 0)*2 - (cw_area.i*2 != 0); |
| } |
| |
| |
| CV_IMPL CvSubdiv2DPoint* |
| cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt ) |
| { |
| CvSubdiv2DPoint* point = 0; |
| CvPoint2D32f start; |
| CvPoint2D32f diff; |
| CvSubdiv2DPointLocation loc; |
| CvSubdiv2DEdge edge; |
| int i; |
| |
| CV_FUNCNAME("cvFindNearestPoint2D"); |
| |
| __BEGIN__; |
| |
| if( !subdiv ) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( !CV_IS_SUBDIV2D( subdiv )) |
| CV_ERROR( CV_StsNullPtr, "" ); |
| |
| if( !subdiv->is_geometry_valid ) |
| cvCalcSubdivVoronoi2D( subdiv ); |
| |
| loc = cvSubdiv2DLocate( subdiv, pt, &edge, &point ); |
| |
| switch( loc ) |
| { |
| case CV_PTLOC_ON_EDGE: |
| case CV_PTLOC_INSIDE: |
| break; |
| default: |
| EXIT; |
| } |
| |
| point = 0; |
| |
| start = cvSubdiv2DEdgeOrg( edge )->pt; |
| diff.x = pt.x - start.x; |
| diff.y = pt.y - start.y; |
| |
| edge = cvSubdiv2DRotateEdge( edge, 1 ); |
| |
| for( i = 0; i < subdiv->total; i++ ) |
| { |
| CvPoint2D32f t; |
| |
| for(;;) |
| { |
| assert( cvSubdiv2DEdgeDst( edge )); |
| |
| t = cvSubdiv2DEdgeDst( edge )->pt; |
| if( icvIsRightOf2( t, start, diff ) >= 0 ) |
| break; |
| |
| edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT ); |
| } |
| |
| for(;;) |
| { |
| assert( cvSubdiv2DEdgeOrg( edge )); |
| |
| t = cvSubdiv2DEdgeOrg( edge )->pt; |
| if( icvIsRightOf2( t, start, diff ) < 0 ) |
| break; |
| |
| edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_LEFT ); |
| } |
| |
| { |
| CvPoint2D32f tempDiff = cvSubdiv2DEdgeDst( edge )->pt; |
| t = cvSubdiv2DEdgeOrg( edge )->pt; |
| tempDiff.x -= t.x; |
| tempDiff.y -= t.y; |
| |
| if( icvIsRightOf2( pt, t, tempDiff ) >= 0 ) |
| { |
| point = cvSubdiv2DEdgeOrg( cvSubdiv2DRotateEdge( edge, 3 )); |
| break; |
| } |
| } |
| |
| edge = cvSubdiv2DSymEdge( edge ); |
| } |
| |
| __END__; |
| |
| return point; |
| } |
| |
| /* Removed from the main interface */ |
| |
| #if 0 |
| /* Adds new isolated quadedge to the subdivision */ |
| OPENCVAPI CvSubdiv2DEdge cvSubdiv2DMakeEdge( CvSubdiv2D* subdiv ); |
| |
| |
| /* Adds new isolated point to subdivision */ |
| OPENCVAPI CvSubdiv2DPoint* cvSubdiv2DAddPoint( CvSubdiv2D* subdiv, |
| CvPoint2D32f pt, int is_virtual ); |
| |
| |
| /* Does a splice operation for two quadedges */ |
| OPENCVAPI void cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB ); |
| |
| |
| /* Assigns ending [non-virtual] points for given quadedge */ |
| OPENCVAPI void cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge, |
| CvSubdiv2DPoint* org_pt, |
| CvSubdiv2DPoint* dst_pt ); |
| |
| /* Removes quadedge from subdivision */ |
| OPENCVAPI void cvSubdiv2DDeleteEdge( CvSubdiv2D* subdiv, CvSubdiv2DEdge edge ); |
| |
| |
| /* Connects estination point of the first edge with origin point of the second edge */ |
| OPENCVAPI CvSubdiv2DEdge cvSubdiv2DConnectEdges( CvSubdiv2D* subdiv, |
| CvSubdiv2DEdge edgeA, |
| CvSubdiv2DEdge edgeB ); |
| |
| /* Swaps diagonal in two connected Delaunay facets */ |
| OPENCVAPI void cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge ); |
| #endif |
| |
| /* End of file. */ |
