cvsubdivision2d.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

    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. */