CV_IMPL CvSubdiv2DPoint* cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt ) { CvSubdiv2DPoint* point = 0; CvPoint2D32f start; CvPoint2D32f diff; CvSubdiv2DPointLocation loc; CvSubdiv2DEdge edge; int i; if( !subdiv ) CV_Error( CV_StsNullPtr, "" ); if( !CV_IS_SUBDIV2D( subdiv )) CV_Error( CV_StsNullPtr, "" ); if( subdiv->edges->active_count <= 3 ) return 0; 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: return point; } 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 ); } return point; }
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; if( !subdiv ) CV_Error( CV_StsNullPtr, "" ); if( !CV_IS_SUBDIV2D(subdiv) ) CV_Error( CV_StsBadFlag, "" ); location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point ); switch (location) { case CV_PTLOC_ERROR: CV_Error( CV_StsBadSize, "" ); case CV_PTLOC_OUTSIDE_RECT: CV_Error( CV_StsOutOfRange, "" ); 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 ); 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: CV_Error_(CV_StsError, ("cvSubdiv2DLocate returned invalid location = %d", location) ); } return curr_point; }
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 CvSubdiv2DPointLocation cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt, CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point ) { CvSubdiv2DPoint *point = 0; int right_of_curr = 0; if( !subdiv ) CV_Error( CV_StsNullPtr, "" ); if( !CV_IS_SUBDIV2D(subdiv) ) CV_Error( CV_StsBadFlag, "" ); int i, max_edges = subdiv->quad_edges * 4; CvSubdiv2DEdge edge = subdiv->recent_edge; if( max_edges == 0 ) CV_Error( CV_StsBadSize, "" ); CV_Assert(edge != 0); if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y || pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y ) CV_Error( CV_StsOutOfRange, "" ); CvSubdiv2DPointLocation 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; goto 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; goto 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; } } } exit: 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_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; }