cvmatmul.cpp.svn-base

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

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


/////////////////////////////// Functions Declaration ////////////////////////////////////

#define IPCV_MATMULADD( flavor, arrtype )                                               \
IPCVAPI( CvStatus,                                                                      \
icvMatMulAdd_##flavor,( const arrtype* src1, int step1, const arrtype* src2, int step2, \
                        const arrtype* src3, int step3, arrtype* dst, int dststep,      \
                        CvSize srcsize, CvSize dstsize ))


IPCV_MATMULADD( 32f_C1R, float )
IPCV_MATMULADD( 32f_C2R, float )
IPCV_MATMULADD( 64f_C1R, double )
IPCV_MATMULADD( 64f_C2R, double )


#undef IPCV_MATMULADD

#define IPCV_MATMULADD_CONST( flavor, arrtype, scalartype )                         \
IPCVAPI( CvStatus,                                                                  \
icvMatMulAddC_##flavor,( const arrtype* src, int step, arrtype* dst, int dststep,   \
                        CvSize size, const scalartype* mat ))

IPCV_MATMULADD_CONST( 8u_C2R, uchar, double )
IPCV_MATMULADD_CONST( 8u_C3R, uchar, double )
IPCV_MATMULADD_CONST( 8u_C4R, uchar, double )

IPCV_MATMULADD_CONST( 32s_C2R, int, double )
IPCV_MATMULADD_CONST( 32s_C3R, int, double )
IPCV_MATMULADD_CONST( 32s_C4R, int, double )

IPCV_MATMULADD_CONST( 32f_C2R, float, double )
IPCV_MATMULADD_CONST( 32f_C3R, float, double )
IPCV_MATMULADD_CONST( 32f_C4R, float, double )

IPCV_MATMULADD_CONST( 64f_C2R, double, double )
IPCV_MATMULADD_CONST( 64f_C3R, double, double )
IPCV_MATMULADD_CONST( 64f_C4R, double, double )

#undef IPCV_MATMULADD_CONST


#define IPCV_MULADD_CONST( flavor, arrtype, scalartype )                                \
IPCVAPI( CvStatus,                                                                      \
icvMulAddC_##flavor,( const arrtype* src1, int step1, const arrtype* src2, int step2,   \
                      arrtype* dst, int dststep, CvSize size, const scalartype* val ))

IPCV_MULADD_CONST( 32f_C1R, float, double )
IPCV_MULADD_CONST( 32f_C2R, float, double )
IPCV_MULADD_CONST( 64f_C1R, double, double )
IPCV_MULADD_CONST( 64f_C2R, double, double )

#undef IPCV_MULADD_CONST

/****************************************************************************************\
*                                        cvMatMulAdd                                     *
\****************************************************************************************/

#define _load_real_( temp, ptr, k )   \
    ((temp)[0] = (ptr) ? (ptr)[k] : 0)

#define _load_complex_( temp, ptr, k )\
    ((temp)[0] = (ptr) ? (ptr)[k] : 0, (temp)[1] = (ptr) ? (ptr)[(k)+1] : 0)

#define _mul_add_real_( a, b, s ) \
    ((s)[0] += (a)[0]*(b)[0])

#define _mul_add_complex_( a, b, s )            \
    ((s)[0] += (a)[0]*(b)[0] - (a)[1]*(b)[1],   \
    (s)[1] += (a)[0]*(b)[1] + (a)[1]*(b)[0])

#define _store_real_( temp, ptr, arrtype ) \
    ((ptr)[0] = (arrtype)(temp)[0])

#define _store_complex_( temp, ptr, arrtype ) \
    ((ptr)[0] = (arrtype)(temp)[0], (ptr)[1] = (arrtype)(temp)[1])


#define ICV_DEF_MATMULADD_FUNC( flavor, arrtype, temptype,                              \
                                _mul_add_macro_, _load_macro_, _store_macro_, cn )      \
IPCVAPI_IMPL( CvStatus,                                                                 \
icvMatMulAdd_##flavor,( const arrtype* src1, int step1, const arrtype* src2, int step2, \
                        const arrtype* src3, int step3, arrtype* dst, int step,         \
                        CvSize srcsize, CvSize dstsize ))                               \
{                                                                                       \
    for( ; dstsize.height--; (char*&)src1 += step1, (char*&)src3 += step3,              \
                             (char*&)dst += step )                                      \
    {                                                                                   \
        int i, j;                                                                       \
        for( i = 0; i < dstsize.width; i++ )                                            \
        {                                                                               \
            temptype sum[cn];                                                           \
            const arrtype* src2t = src2 + i*(cn);                                       \
                                                                                        \
            _load_macro_( sum, src3, i*(cn) );                                          \
                                                                                        \
            for( j = 0; j < srcsize.width; j++, (char*&)src2t += step2 )                \
            {                                                                           \
                _mul_add_macro_( (src1 + j*(cn)), src2t, sum );                         \
            }                                                                           \
                                                                                        \
            _store_macro_( sum, dst + i*(cn), arrtype );                                \
        }                                                                               \
    }                                                                                   \
                                                                                        \
    return CV_OK;                                                                       \
}


ICV_DEF_MATMULADD_FUNC( 32f_C1R, float, double, _mul_add_real_,
                        _load_real_, _store_real_, 1)
ICV_DEF_MATMULADD_FUNC( 64f_C1R, double, double, _mul_add_real_,
                        _load_real_,_store_real_, 1)
ICV_DEF_MATMULADD_FUNC( 32f_C2R, float, double, _mul_add_real_,
                        _load_real_, _store_real_, 2)
ICV_DEF_MATMULADD_FUNC( 64f_C2R, double, double, _mul_add_complex_,
                        _load_complex_, _store_complex_, 2 )


typedef CvStatus (CV_STDCALL *CvMatMulAddFunc)( const void* src1, int step1,
                   const void* src2, int step2, const void* src3, int step3,
                   void* dst, int dststep, CvSize srcsize, CvSize dstsize );


#define ICV_DEF_INIT_MATMULADD_TAB( FUNCNAME )              \
static void icvInitMatMulAddTable( CvBigFuncTable* table )  \
{                                                           \
    table->fn_2d[CV_32FC1] = (void*)icv##FUNCNAME##_32f_C1R;\
    table->fn_2d[CV_32FC2] = (void*)icv##FUNCNAME##_32f_C2R;\
                                                            \
    table->fn_2d[CV_64FC1] = (void*)icv##FUNCNAME##_64f_C1R;\
    table->fn_2d[CV_64FC2] = (void*)icv##FUNCNAME##_64f_C2R;\
}


ICV_DEF_INIT_MATMULADD_TAB( MatMulAdd )


CV_IMPL  void
cvMatMulAdd( const CvArr* src1arr, const CvArr* src2arr,
             const CvArr* src3arr, CvArr* dstarr )
{
    static CvBigFuncTable mmuladd_tab;
    static int inittab = 0;
    
    uchar* buffer = 0;
    int local_alloc = 0;
    
    CV_FUNCNAME( "cvMatMulAdd" );

    __BEGIN__;

    CvMat stub1, *src1 = (CvMat*)src1arr;
    CvMat stub2, *src2 = (CvMat*)src2arr;
    CvMat stub3, *src3 = (CvMat*)src3arr;
    CvMat stub, *dst = (CvMat*)dstarr;
    int type;

    if( !CV_IS_MAT( src1 ))
    {
        int coi = 0;
        CV_CALL( src1 = cvGetMat( src1, &stub1, &coi ));

        if( coi != 0 )
            CV_ERROR( CV_BadCOI, "" );
    }

    if( !CV_IS_MAT( src2 ))
    {
        int coi = 0;
        CV_CALL( src2 = cvGetMat( src2, &stub2, &coi ));

        if( coi != 0 )
            CV_ERROR( CV_BadCOI, "" );
    }

    if( !CV_IS_MAT( dst ))
    {
        int coi = 0;
        CV_CALL( dst = cvGetMat( dst, &stub, &coi ));

        if( coi != 0 )
            CV_ERROR( CV_BadCOI, "" );
    }

    if( src3 )
    {
        if( !CV_IS_MAT( src3 ))
        {
            int coi = 0;
            CV_CALL( src3 = cvGetMat( src3, &stub3, &coi ));

            if( coi != 0 )
                CV_ERROR( CV_BadCOI, "" );
        }

        if( !CV_ARE_TYPES_EQ( src3, dst ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( !CV_ARE_SIZES_EQ( src3, dst ))
            CV_ERROR( CV_StsUnmatchedSizes, "" );
    }
    else
    {
        src3 = &stub3;
        src3->data.ptr = 0;
        src3->step = 0;
        src3->type = CV_MAT_CONT_FLAG;
    }

    if( !CV_ARE_TYPES_EQ( src1, src2 ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( !CV_ARE_TYPES_EQ( src1, dst ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( src1->width != src2->height ||
        src1->height != dst->height ||
        src2->width != dst->width )
        CV_ERROR( CV_StsUnmatchedSizes, "" );

    type = CV_MAT_TYPE( src1->type );

    // check case of a single equation and small matrix
    if( src2->width == 1 && src1->width <= 3 && src1->width == src1->height &&
        CV_IS_MAT_CONT( src1->type & src2->type & src3->type & dst->type ))
    {
        uchar* data1 = src1->data.ptr;
        uchar* data2 = src2->data.ptr;
        uchar* data3 = src3->data.ptr;
        uchar* datad = dst->data.ptr;

        #undef A
        #undef X
        #undef B
        #undef Y
        #undef N
        #undef arrtype
        #define A(y,x)  ((arrtype*)data1)[(x)+(y)*N]
        #define X(y)    ((arrtype*)data2)[y]
        #define B(y)    ((arrtype*)data3)[y]
        #define Y(y)    ((arrtype*)datad)[y]

        if( type == CV_32FC1 )
        {
            if( src1->width == 2 )
            {
                #undef N
                #define N  2

                #undef arrtype                
                #define arrtype float

                float t[2];

                if( data3 )
                {
                    t[0] = (float)(A(0,0)*X(0) + A(0,1)*X(1) + B(0));
                    t[1] = (float)(A(1,0)*X(0) + A(1,1)*X(1) + B(1));
                }
                else
                {
                    t[0] = (float)(A(0,0)*X(0) + A(0,1)*X(1));
                    t[1] = (float)(A(1,0)*X(0) + A(1,1)*X(1));
                }

                Y(0) = t[0];
                Y(1) = t[1];
            }
            else if( src1->width == 3 )
            {
                #undef N
                #define N  3

                float t[3];