cvconvhull.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 818 行 · 第 1/2 页

/*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"
#include "_cvdatastructs.h"

static int
icvSklansky_32s( CvPoint** array, int start, int end, int* stack, int nsign, int sign2 )
{
    int incr = end > start ? 1 : -1;
    /* prepare first triangle */
    int pprev = start, pcur = pprev + incr, pnext = pcur + incr;
    int stacksize = 3;

    if( start == end ||
        (array[start]->x == array[end]->x &&
         array[start]->y == array[end]->y) )
    {
        stack[0] = start;
        return 1;
    }

    stack[0] = pprev;
    stack[1] = pcur;
    stack[2] = pnext;

    end += incr; /* make end = afterend */

    while( pnext != end )
    {
        /* check the angle p1,p2,p3 */
        int cury = array[pcur]->y;
        int nexty = array[pnext]->y;
        int by = nexty - cury;

        if( CV_SIGN(by) != nsign )
        {
            int ax = array[pcur]->x - array[pprev]->x;
            int bx = array[pnext]->x - array[pcur]->x;
            int ay = cury - array[pprev]->y;
            int convexity = ay*bx - ax*by;/* if >0 then convex angle */

            if( CV_SIGN(convexity) != -sign2 && (ax != 0 || ay != 0))
            {
                pprev = pcur;
                pcur = pnext;
                pnext += incr;
                stack[stacksize] = pnext;
                stacksize++;
            }
            else
            {
                if( pprev == start )
                {
                    pcur = pnext;
                    stack[1] = pcur;
                    pnext += incr;
                    stack[2] = pnext;
                }
                else
                {
                    stack[stacksize-2] = pnext;
                    pcur = pprev;
                    pprev = stack[stacksize-4];
                    stacksize--;
                }
            }
        }
        else
        {
            pnext += incr;
            stack[stacksize-1] = pnext;
        }
    }

    return --stacksize;
}


static int
icvSklansky_32f( CvPoint2D32f** array, int start, int end, int* stack, int nsign, int sign2 )
{
    int incr = end > start ? 1 : -1;
    /* prepare first triangle */
    int pprev = start, pcur = pprev + incr, pnext = pcur + incr;
    int stacksize = 3;

    if( start == end ||
        (array[start]->x == array[end]->x &&
         array[start]->y == array[end]->y) )
    {
        stack[0] = start;
        return 1;
    }

    stack[0] = pprev;
    stack[1] = pcur;
    stack[2] = pnext;

    end += incr; /* make end = afterend */

    while( pnext != end )
    {
        /* check the angle p1,p2,p3 */
        float cury = array[pcur]->y;
        float nexty = array[pnext]->y;
        float by = nexty - cury;

        if( CV_SIGN( by ) != nsign )
        {
            float ax = array[pcur]->x - array[pprev]->x;
            float bx = array[pnext]->x - array[pcur]->x;
            float ay = cury - array[pprev]->y;
            float convexity = ay*bx - ax*by;/* if >0 then convex angle */

            if( CV_SIGN( convexity ) != -sign2 && (ax != 0 || ay != 0))
            {
                pprev = pcur;
                pcur = pnext;
                pnext += incr;
                stack[stacksize] = pnext;
                stacksize++;
            }
            else
            {
                if( pprev == start )
                {
                    pcur = pnext;
                    stack[1] = pcur;
                    pnext += incr;
                    stack[2] = pnext;

                }
                else
                {
                    stack[stacksize-2] = pnext;
                    pcur = pprev;
                    pprev = stack[stacksize-4];
                    stacksize--;
                }
            }
        }
        else
        {
            pnext += incr;
            stack[stacksize-1] = pnext;
        }
    }

    return --stacksize;
}

typedef int (*sklansky_func)( CvPoint** points, int start, int end,
                              int* stack, int sign, int sign2 );

#define cmp_pts( ptr1, ptr2 ) \
( ptr1->x < ptr2->x  ||       \
  ptr1->x == ptr2->x &&       \
  ptr1->y < ptr2->y  )

CV_IMPLEMENT_QSORT( icvSortPointsByPointers_32s, CvPoint *, cmp_pts );
CV_IMPLEMENT_QSORT( icvSortPointsByPointers_32f, CvPoint2D32f *, cmp_pts );

static void
icvCalcAndWritePtIndices( CvPoint** pointer, int* stack, int start, int end,
                          CvSeq* ptseq, CvSeqWriter* writer )
{
    CV_FUNCNAME( "icvCalcAndWritePtIndices" );

    __BEGIN__;
    
    int i, incr = start < end ? 1 : -1;
    int idx, first_idx = ptseq->first->start_index;

    for( i = start; i != end; i += incr )
    {
        CvPoint* ptr = (CvPoint*)pointer[stack[i]];
        CvSeqBlock* block = ptseq->first;
        while( (unsigned)(idx = ptr - (CvPoint*)block->data) >= (unsigned)block->count )
        {
            block = block->next;
            if( block == ptseq->first )
                CV_ERROR( CV_StsError, "Internal error" );
        }
        idx += block->start_index - first_idx;
        CV_WRITE_SEQ_ELEM( idx, *writer );
    }

    __END__;
}


CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
               int orientation, int return_points )
{
    CvSeq* hull = 0;
    CvPoint** pointer = 0;
    int* stack = 0;
    
    CV_FUNCNAME( "cvConvexHull2" );
    
    __BEGIN__;

    CvMat* mat = 0;
    CvSeqReader reader;
    CvSeqWriter writer;
    CvContour contour_header, hull_header;
    CvSeqBlock block, hullblock;
    CvSeq* ptseq = 0;
    CvSeq* hullseq = 0;
    int is_float;
    int* t_stack;
    int t_count;
    int i, miny_ind = 0, maxy_ind = 0;
    int hulltype;
    sklansky_func sklansky;

    if( CV_IS_SEQ( array ))
    {
        ptseq = (CvSeq*)array;
        if( !CV_IS_SEQ_POINT_SET( ptseq ))
            CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
        if( hull_storage == 0 )
            hull_storage = ptseq->storage;
    }
    else
    {
        CV_CALL( ptseq = icvPointSeqFromMat(
            CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
    }

    if( CV_IS_STORAGE( hull_storage ))
    {
        if( return_points )
        {
            CV_CALL( hullseq = cvCreateSeq(
                CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE(ptseq)|
                CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
                sizeof(CvContour), sizeof(CvPoint),(CvMemStorage*)hull_storage ));
        }
        else
        {
            CV_CALL( hullseq = cvCreateSeq(
                CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE_PPOINT|
                CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
                sizeof(CvContour), sizeof(CvPoint*), (CvMemStorage*)hull_storage ));
        }
    }
    else
    {
        if( !CV_IS_MAT( hull_storage ))
            CV_ERROR(CV_StsBadArg, "Destination must be valid memory storage or matrix");

        mat = (CvMat*)hull_storage;

        if( mat->cols != 1 && mat->rows != 1 || !CV_IS_MAT_CONT(mat->type))
            CV_ERROR( CV_StsBadArg,
            "The hull matrix should be continuous and have a single row or a single column" );

        if( mat->cols + mat->rows - 1 < ptseq->total )
            CV_ERROR( CV_StsBadSize, "The hull matrix size might be not enough to fit the hull" );

        if( CV_MAT_TYPE(mat->type) != CV_SEQ_ELTYPE(ptseq) &&
            CV_MAT_TYPE(mat->type) != CV_32SC1 )
            CV_ERROR( CV_StsUnsupportedFormat,
            "The hull matrix must have the same type as input or 32sC1 (integers)" );

        CV_CALL( hullseq = cvMakeSeqHeaderForArray(
            CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
            sizeof(contour_header), CV_ELEM_SIZE(mat->type), mat->data.ptr,
            mat->cols + mat->rows - 1, (CvSeq*)&hull_header, &hullblock ));

        cvClearSeq( hullseq );
    }

    if( ptseq->total == 0 )
    {
        if( mat )
            CV_ERROR( CV_StsBadSize,
            "Point sequence can not be empty if the output is matrix" );
        EXIT;
    }

    cvStartAppendToSeq( hullseq, &writer );

    is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
    hulltype = CV_SEQ_ELTYPE(hullseq);
    sklansky = !is_float ? (sklansky_func)icvSklansky_32s :
                           (sklansky_func)icvSklansky_32f;

    CV_CALL( pointer = (CvPoint**)cvAlloc( ptseq->total*sizeof(pointer[0]) ));
    CV_CALL( stack = (int*)cvAlloc( (ptseq->total + 2)*sizeof(stack[0]) ));

    cvStartReadSeq( ptseq, &reader );

    for( i = 0; i < ptseq->total; i++ )
    {
        pointer[i] = (CvPoint*)reader.ptr;
        CV_NEXT_SEQ_ELEM( ptseq->elem_size, reader );
    }

    if( ptseq->total == 1 )
    {
        if( hulltype == CV_SEQ_ELTYPE_PPOINT )
        {
            CV_WRITE_SEQ_ELEM( pointer[0], writer );
        }
        else if( hulltype == CV_SEQ_ELTYPE_INDEX )
        {
            int index = 0;
            CV_WRITE_SEQ_ELEM( index, writer );
        }
        else
        {
            CvPoint pt = pointer[0][0];
            CV_WRITE_SEQ_ELEM( pt, writer );
        }
        goto finish_hull;
    }

    if( !is_float )
        icvSortPointsByPointers_32s( pointer, ptseq->total, 0 );
    else
        icvSortPointsByPointers_32f( (CvPoint2D32f**)pointer, ptseq->total, 0 );

    /* find top and bottom */
    for( i = 1; i < ptseq->total; i++ )
    {
        int y = pointer[i]->y;
        if( pointer[miny_ind]->y > y )
            miny_ind = i;
        if( pointer[maxy_ind]->y < y )
            maxy_ind = i;
    }

    /*upper half */
    {
        int *tl_stack = stack;
        int tl_count = sklansky( pointer, 0, maxy_ind, tl_stack, -1, 1 );
        int *tr_stack = tl_stack + tl_count;
        int tr_count = sklansky( pointer, ptseq->total - 1, maxy_ind, tr_stack, -1, -1 );

        /* gather upper part of convex hull to output */
        if( orientation == CV_COUNTER_CLOCKWISE )
        {
            CV_SWAP( tl_stack, tr_stack, t_stack );
            CV_SWAP( tl_count, tr_count, t_count );
        }

        if( hulltype == CV_SEQ_ELTYPE_PPOINT )
        {
            for( i = 0; i < tl_count - 1; i++ )
                CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]], writer );

            for( i = tr_count - 1; i > 0; i-- )
                CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]], writer );
        }
        else if( hulltype == CV_SEQ_ELTYPE_INDEX )
        {
            CV_CALL( icvCalcAndWritePtIndices( pointer, tl_stack,
                                               0, tl_count-1, ptseq, &writer ));
            CV_CALL( icvCalcAndWritePtIndices( pointer, tr_stack,
                                               tr_count-1, 0, ptseq, &writer ));
        }
        else
        {
            for( i = 0; i < tl_count - 1; i++ )
                CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]][0], writer );

            for( i = tr_count - 1; i > 0; i-- )