cxmathfuncs.c

Xilinx ISE&EDK 8.2平台的人脸检测系统设计

ICV_DEF_SQR_MAGNITUDE_FUNC( 32f, float, float )
ICV_DEF_SQR_MAGNITUDE_FUNC( 64f, double, double )

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

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

    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, 0 ));

        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, 0 ));

        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*)cvStackAlloc( block_size*sizeof(float));
        y_buffer = (float*)cvStackAlloc( block_size*sizeof(float));
    }
    else if( depth == CV_32F && mag )
    {
        mag_buffer = (float*)cvStackAlloc( 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__;
}


/****************************************************************************************\
*                                  Polar -> Cartezian                                    *
\****************************************************************************************/
#if 0
static CvStatus CV_STDCALL
icvSinCos_32f( const float *angle,float *sinval, float* cosval,
                int len, int angle_in_degrees )
{
    const int N = 64;

    static const double sin_table[] =
    { 
     0.00000000000000000000,     0.09801714032956060400,
     0.19509032201612825000,     0.29028467725446233000,
     0.38268343236508978000,     0.47139673682599764000,
     0.55557023301960218000,     0.63439328416364549000,
     0.70710678118654746000,     0.77301045336273699000,
     0.83146961230254524000,     0.88192126434835494000,
     0.92387953251128674000,     0.95694033573220894000,
     0.98078528040323043000,     0.99518472667219682000,
     1.00000000000000000000,     0.99518472667219693000,
     0.98078528040323043000,     0.95694033573220894000,
     0.92387953251128674000,     0.88192126434835505000,
     0.83146961230254546000,     0.77301045336273710000,
     0.70710678118654757000,     0.63439328416364549000,
     0.55557023301960218000,     0.47139673682599786000,
     0.38268343236508989000,     0.29028467725446239000,
     0.19509032201612861000,     0.09801714032956082600,
     0.00000000000000012246,    -0.09801714032956059000,
    -0.19509032201612836000,    -0.29028467725446211000,
    -0.38268343236508967000,    -0.47139673682599764000,
    -0.55557023301960196000,    -0.63439328416364527000,
    -0.70710678118654746000,    -0.77301045336273666000,
    -0.83146961230254524000,    -0.88192126434835494000,
    -0.92387953251128652000,    -0.95694033573220882000,
    -0.98078528040323032000,    -0.99518472667219693000,
    -1.00000000000000000000,    -0.99518472667219693000,
    -0.98078528040323043000,    -0.95694033573220894000,
    -0.92387953251128663000,    -0.88192126434835505000,
    -0.83146961230254546000,    -0.77301045336273688000,
    -0.70710678118654768000,    -0.63439328416364593000,
    -0.55557023301960218000,    -0.47139673682599792000,
    -0.38268343236509039000,    -0.29028467725446250000,
    -0.19509032201612872000,    -0.09801714032956050600,
    };

    static const double k2 = (2*CV_PI)/N;
    
    static const double sin_a0 = -0.166630293345647*k2*k2*k2;
    static const double sin_a2 = k2;

    static const double cos_a0 = -0.499818138450326*k2*k2;
    /*static const double cos_a2 =  1;*/

    double k1;
    int i;

    if( !angle_in_degrees )
        k1 = N/(2*CV_PI);
    else
        k1 = N/360.;

    for( i = 0; i < len; i++ )
    {
        double t = angle[i]*k1;
        int it = cvRound(t);
        t -= it;
        int sin_idx = it & (N - 1);
        int cos_idx = (N/4 - sin_idx) & (N - 1);

        double sin_b = (sin_a0*t*t + sin_a2)*t;
        double cos_b = cos_a0*t*t + 1;

        double sin_a = sin_table[sin_idx];
        double cos_a = sin_table[cos_idx];

        double sin_val = sin_a*cos_b + cos_a*sin_b;
        double cos_val = cos_a*cos_b - sin_a*sin_b;

        sinval[i] = (float)sin_val;
        cosval[i] = (float)cos_val;
    }

    return CV_OK;
}

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

    __BEGIN__

    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;
    
    if( !CV_IS_MAT(angle))
        CV_CALL( angle = cvGetMat( angle, &anglestub, &coi4 ));

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

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

        if( !CV_ARE_TYPES_EQ( angle, mag ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedFormats );

        if( !CV_ARE_SIZES_EQ( angle, mag ) )
            CV_ERROR_FROM_CODE( CV_StsUnmatchedSizes );

        cont_flag &= mag->type;
    }

    if( xmat )
    {
        if( !CV_IS_MAT(xmat))
            CV_CALL( xmat = cvGetMat( xmat, &xstub, &coi1 ));

        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 &= xmat->type;
    }

    if( ymat )
    {
        if( !CV_IS_MAT(ymat))