cxmathfuncs.cpp

来自「将OpenCV移植到DSP上」· C++ 代码 · 共 1,547 行 · 第 1/5 页

/****************************************************************************************\
*                                  Cartezian -> Polar                                    *
\****************************************************************************************/

CV_IMPL void
cvCartToPolar( const CvArr* xarr, const CvArr* yarr,
               CvArr* magarr, CvArr* anglearr,
               int angle_in_degrees )
{
    CV_FUNCNAME( "cvCartToPolar" );

    __BEGIN__;

    float* mag_buffer = 0;
    float* x_buffer = 0;
    float* y_buffer = 0;
    int block_size = 0;
    CvMat xstub, *xmat = (CvMat*)xarr;
    CvMat ystub, *ymat = (CvMat*)yarr;
    CvMat magstub, *mag = (CvMat*)magarr;
    CvMat anglestub, *angle = (CvMat*)anglearr;
    int coi1 = 0, coi2 = 0, coi3 = 0, coi4 = 0;
    int depth;
    CvSize size;
    int x, y;
    int cont_flag = CV_MAT_CONT_FLAG;
    
    if( !CV_IS_MAT(xmat))
        CV_CALL( xmat = cvGetMat( xmat, &xstub, &coi1 ));

    if( !CV_IS_MAT(ymat))
        CV_CALL( ymat = cvGetMat( ymat, &ystub, &coi2 ));

    if( !CV_ARE_TYPES_EQ( xmat, ymat ) )
        CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );

    if( !CV_ARE_SIZES_EQ( xmat, ymat ) )
        CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );

    depth = CV_MAT_DEPTH( xmat->type );
    if( depth < CV_32F )
        CV_ERROR( CV_StsUnsupportedFormat, "" );

    if( mag )
    {
        CV_CALL( mag = cvGetMat( mag, &magstub, &coi3 ));

        if( !CV_ARE_TYPES_EQ( mag, xmat ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );
        
        if( !CV_ARE_SIZES_EQ( mag, xmat ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );
        cont_flag = mag->type;
    }

    if( angle )
    {
        CV_CALL( angle = cvGetMat( angle, &anglestub, &coi4 ));

        if( !CV_ARE_TYPES_EQ( angle, xmat ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );
        
        if( !CV_ARE_SIZES_EQ( angle, xmat ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );
        cont_flag &= angle->type;
    }

    if( coi1 != 0 || coi2 != 0 || coi3 != 0 || coi4 != 0 )
        CV_ERROR( CV_BadCOI, "" );

    size = cvGetMatSize(xmat);
    size.width *= CV_MAT_CN(xmat->type);

    if( CV_IS_MAT_CONT( xmat->type & ymat->type & cont_flag ))
    {
        size.width *= size.height;
        size.height = 1;
    }

    block_size = MIN( size.width, ICV_MATH_BLOCK_SIZE );
    if( depth == CV_64F && angle )
    {
        x_buffer = (float*)cvAlloc( block_size*sizeof(float));
        y_buffer = (float*)cvAlloc( block_size*sizeof(float));
    }
    else if( depth == CV_32F && mag )
    {
        mag_buffer = (float*)cvAlloc( block_size*sizeof(float));
    }

    if( depth == CV_32F )
    {
        for( y = 0; y < size.height; y++ )
        {
            float* x_data = (float*)(xmat->data.ptr + xmat->step*y);
            float* y_data = (float*)(ymat->data.ptr + ymat->step*y);
            float* mag_data = mag ? (float*)(mag->data.ptr + mag->step*y) : 0;
            float* angle_data = angle ? (float*)(angle->data.ptr + angle->step*y) : 0;

            for( x = 0; x < size.width; x += block_size )
            {
                int len = MIN( size.width - x, block_size );

                if( mag )
                    icvSqrMagnitude_32f( x_data + x, y_data + x, mag_buffer, len );

                if( angle )
                {
                    icvFastArctan_32f( y_data + x, x_data + x, angle_data + x, len );
                    if( !angle_in_degrees )
                        icvScale_32f( angle_data + x, angle_data + x,
                                      len, (float)(CV_PI/180.), 0 );
                }

                if( mag )
                    icvSqrt_32f( mag_buffer, mag_data + x, len );
            }
        }
    }
    else
    {
        for( y = 0; y < size.height; y++ )
        {
            double* x_data = (double*)(xmat->data.ptr + xmat->step*y);
            double* y_data = (double*)(ymat->data.ptr + ymat->step*y);
            double* mag_data = mag ? (double*)(mag->data.ptr + mag->step*y) : 0;
            double* angle_data = angle ? (double*)(angle->data.ptr + angle->step*y) : 0;

            for( x = 0; x < size.width; x += block_size )
            {
                int len = MIN( size.width - x, block_size );

                if( angle )
                {
                    icvCvt_64f32f( x_data + x, x_buffer, len );
                    icvCvt_64f32f( y_data + x, y_buffer, len );
                }

                if( mag )
                {
                    icvSqrMagnitude_64f( x_data + x, y_data + x, mag_data + x, len );
                    icvSqrt_64f( mag_data + x, mag_data + x, len );
                }

                if( angle )
                {
                    icvFastArctan_32f( y_buffer, x_buffer, x_buffer, len );
                    if( !angle_in_degrees )
                        icvScale_32f( x_buffer, x_buffer, len, (float)(CV_PI/180.), 0 );
                    icvCvt_32f64f( x_buffer, angle_data + x, len );
                }
            }
        }
    }

    __END__;
}


/****************************************************************************************\
*                                    P O W E R                                           *
\****************************************************************************************/

#define ICV_DEF_IPOW_OP( flavor, arrtype, worktype, cast_macro )                    \
static CvStatus CV_STDCALL                                                          \
icvIPow_##flavor( const arrtype* src, arrtype* dst, int len, int power )            \
{                                                                                   \
    int i;                                                                          \
                                                                                    \
    for( i = 0; i < len; i++ )                                                      \
    {                                                                               \
        worktype a = 1, b = src[i];                                                 \
        int p = power;                                                              \
        while( p > 1 )                                                              \
        {                                                                           \
            if( p & 1 )                                                             \
                a *= b;                                                             \
            b *= b;                                                                 \
            p >>= 1;                                                                \
        }                                                                           \
                                                                                    \
        a *= b;                                                                     \
        dst[i] = cast_macro(a);                                                     \
    }                                                                               \
                                                                                    \
    return CV_OK;                                                                   \
}


ICV_DEF_IPOW_OP( 8u, uchar, int, CV_CAST_8U )
ICV_DEF_IPOW_OP( 16u, ushort, int, CV_CAST_16U )
ICV_DEF_IPOW_OP( 16s, short, int, CV_CAST_16S )
ICV_DEF_IPOW_OP( 32s, int, int, CV_CAST_32S )
ICV_DEF_IPOW_OP( 32f, float, double, CV_CAST_32F )
ICV_DEF_IPOW_OP( 64f, double, double, CV_CAST_64F )

#define icvIPow_8s 0

CV_DEF_INIT_FUNC_TAB_1D( IPow )

typedef CvStatus (CV_STDCALL * CvIPowFunc)( const void* src, void* dst, int len, int power );
typedef CvStatus (CV_STDCALL * CvSqrtFunc)( const void* src, void* dst, int len );

CV_IMPL void cvPow( const CvArr* srcarr, CvArr* dstarr, double power )
{
    static CvFuncTable ipow_tab;
    static int inittab = 0;
    
    CV_FUNCNAME( "cvPow" );

    __BEGIN__;

    void* temp_buffer = 0;
    int block_size = 0;
    CvMat srcstub, *src = (CvMat*)srcarr;
    CvMat dststub, *dst = (CvMat*)dstarr;
    int coi1 = 0, coi2 = 0;
    int depth;
    CvSize size;
    int x, y;
    int ipower = cvRound( power );
    int is_ipower = 0;
    
    if( !CV_IS_MAT(src))
        CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));

    if( !CV_IS_MAT(dst))
        CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));

    if( coi1 != 0 || coi2 != 0 )
        CV_ERROR( CV_BadCOI, "" );

    if( !CV_ARE_TYPES_EQ( src, dst ))
        CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );

    if( !CV_ARE_SIZES_EQ( src, dst ) )
        CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );

    depth = CV_MAT_DEPTH( src->type );

    if( fabs(ipower - power) < DBL_EPSILON )
    {
        if( !inittab )
        {
            icvInitIPowTable( &ipow_tab );
            inittab = 1;
        }

        if( ipower < 0 )
        {
            CV_CALL( cvDiv( 0, src, dst ));
            
            if( ipower == -1 )
                EXIT;
            ipower = -ipower;
            src = dst;
        }
        
        switch( ipower )
        {
        case 0:
            cvSet( dst, cvScalarAll(1));
            EXIT;
        case 1:
            cvCopy( src, dst );
            EXIT;
        case 2:
            cvMul( src, src, dst );
            EXIT;
        default:
            is_ipower = 1;
        }
    }
    else if( depth < CV_32F )
        CV_ERROR( CV_StsUnsupportedFormat,
        "Fractional or negative integer power factor can be used "
        "with floating-point types only");

    size = cvGetMatSize(src);
    size.width *= CV_MAT_CN(src->type);

    if( CV_IS_MAT_CONT( src->type & dst->type ))
    {
        size.width *= size.height;
        size.height = 1;
    }

    if( is_ipower )
    {
        CvIPowFunc pow_func = (CvIPowFunc)ipow_tab.fn_2d[depth];
        if( !pow_func )
            CV_ERROR( CV_StsUnsupportedFormat, "The data type is not supported" );
        
        for( y = 0; y < size.height; y++ )
        {
            uchar* src_data = src->data.ptr + src->step*y;
            uchar* dst_data = dst->data.ptr + dst->step*y;

            pow_func( src_data, dst_data, size.width, ipower );
        }
    }
    else if( fabs(fabs(power) - 0.5) < DBL_EPSILON )
    {
        CvSqrtFunc sqrt_func = power < 0 ? 
            (depth == CV_32F ? (CvSqrtFunc)icvInvSqrt_32f : (CvSqrtFunc)icvInvSqrt_64f) :
            (depth == CV_32F ? (CvSqrtFunc)icvSqrt_32f : (CvSqrtFunc)icvSqrt_64f);

        for( y = 0; y < size.height; y++ )