cvsubdivision2d.cpp

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

/*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 );