ordfilt2.c

来自「有关matlab的电子书籍有一定的帮助希望有用」· C语言 代码 · 共 680 行 · 第 1/2 页

        }
    }

    return(aa);
}

uint8_T **MakeZeroPaddedInput8(uint8_T **xx, int mx, int nx, int md, int nd) {
    uint8_T **aa;
    int maa;
    int naa;
    int rowOrigin;
    int colOrigin;
    int row;
    int col;

    maa = mx + md - 1;
    naa = nx + nd - 1;

    rowOrigin = (md - 1) / 2;
    colOrigin = (nd - 1) / 2;

    aa = Make2SubscriptMatrix8((uint8_T *) mxCalloc(maa*naa, sizeof(uint8_T)),
                               maa, naa);

    for (col = 0; col < nx; col++) {
        for (row = 0; row < mx; row++) {
             aa[col+colOrigin][row+rowOrigin] = xx[col][row];
        }
    }

    return(aa);
}

#ifndef NDEBUG
/* comparison function cmp() is used in debugging call to qsort below. */
int cmp(const void *keyval, const void *datum)
{
    double val1;
    double val2;
    
    val1 = *((double *) keyval);
    val2 = *((double *) datum);
    
    if (val1 < val2)
    {
        return(-1);
    }
    else if (val1 == val2)
    {
        return(0);
    }
    else
    {
        return(1);
    }
}
#endif /* NDEBUG */


static void
ordfilt8(const mxArray *Y, const mxArray *X, int order,
        const mxArray *Domain, int domainSize, 
        const mxArray *S, bool offsetsAreZero) {

    uint8_T *y;
    uint8_T *x;
    int mx;
    int nx;
    double *d;
    int md;
    int nd;
    uint8_T **aa;         /* zero-padded input */
    double **dd;         /* 2-subscript domain array */
    uint8_T **yy;         /* 2-subscript output array */
    double *vector;      /* values passed to order_select() */
    int *domainRows;     /* row indices for nonzero domain elements */
    int *domainCols;     /* column indices for nonzero domain elements */
    int domainRowOrigin; /* center of domain */
    int domainColOrigin; /* center of domain */
    int counter;
    int row;
    int col;
    int r;
    int c;

    mxAssert(offsetsAreZero, "Called uint8 case with nonzero offsets");

    y = (uint8_T *) mxGetData(Y);
    x = (uint8_T *) mxGetData(X);
    mx = mxGetM(X);
    nx = mxGetN(X);
    d = (double *) mxGetData(Domain);
    md = mxGetM(Domain);
    nd = mxGetN(Domain);

    aa = MakeZeroPaddedInput8(Make2SubscriptMatrix8(x, mx, nx), 
                              mx, nx, md, nd);

    yy = Make2SubscriptMatrix8(y, mx, nx);

    /* Compute domain info */
    domainRowOrigin = (md - 1) / 2;
    domainColOrigin = (nd - 1) / 2;
    domainRows = (int *) mxCalloc(domainSize, sizeof(int));
    domainCols = (int *) mxCalloc(domainSize, sizeof(int));
    dd = Make2SubscriptMatrix(d, md, nd);
    counter = 0;
    for (row = 0; row < md; row++) {
        for (col = 0; col < nd; col++) {
            if (dd[col][row] != 0.0) {
                domainRows[counter] = row;
                domainCols[counter] = col;
                counter++;
            }
        }
    }
    
    vector = mxCalloc(domainSize, sizeof(double));
    
    for (col = 0; col < nx; col++) {
        for (row = 0; row < mx; row++) {
            /* Fill the vector */
            for (counter = 0; counter < domainSize; counter++) {
                r = domainRows[counter];
                c = domainCols[counter];
                vector[counter] = (double) aa[col+c][row+r];
            }
            yy[col][row] = (uint8_T) order_select(vector, domainSize, order);
#ifndef NDEBUG
            /* Check result against C lib function qsort() */
            for (counter = 0; counter < domainSize; counter++) {
                r = domainRows[counter];
                c = domainCols[counter];
                vector[counter] = (double) aa[col+c][row+r];
            }
            qsort((void *) vector, (size_t) domainSize, sizeof(double), cmp);
            mxAssert(((double) yy[col][row]) == vector[order-1], "");
#endif /* NDEBUG */
        }
    }

    mxFree((void *) vector); 
}

static void
ordfilt(const mxArray *Y, const mxArray *X, int order,
        const mxArray *Domain, int domainSize, 
        const mxArray *S, bool offsetsAreZero) {

    double *y;
    double *x;
    int mx;
    int nx;
    double *d;
    int md;
    int nd;
    double *s;
    double **aa;         /* zero-padded input */
    double **dd;         /* 2-subscript domain array */
    double **ss;         /* 2-subscript offsets array */
    double **yy;         /* 2-subscript output array */
    double *vector;      /* values passed to order_select() */
    int *domainRows;     /* row indices for nonzero domain elements */
    int *domainCols;     /* column indices for nonzero domain elements */
    int domainRowOrigin; /* center of domain */
    int domainColOrigin; /* center of domain */
    int counter;
    int row;
    int col;
    int r;
    int c;

    y = (double *) mxGetData(Y);
    x = (double *) mxGetData(X);
    mx = mxGetM(X);
    nx = mxGetN(X);
    d = (double *) mxGetData(Domain);
    md = mxGetM(Domain);
    nd = mxGetN(Domain);

    aa = MakeZeroPaddedInput(Make2SubscriptMatrix(x, mx, nx), mx, nx, md, nd);

    yy = Make2SubscriptMatrix(y, mx, nx);

    /* Compute domain info */
    domainRowOrigin = (md - 1) / 2;
    domainColOrigin = (nd - 1) / 2;
    domainRows = (int *) mxCalloc(domainSize, sizeof(int));
    domainCols = (int *) mxCalloc(domainSize, sizeof(int));
    dd = Make2SubscriptMatrix(d, md, nd);
    counter = 0;
    for (row = 0; row < md; row++) {
        for (col = 0; col < nd; col++) {
            if (dd[col][row] != 0.0) {
                domainRows[counter] = row;
                domainCols[counter] = col;
                counter++;
            }
        }
    }
    
    if (! offsetsAreZero) {
        s = (double *) mxGetData(S);
        ss = Make2SubscriptMatrix(s, md, nd);
    }

    vector = mxCalloc(domainSize, sizeof(double));
    
    if (! offsetsAreZero) {
	for (col = 0; col < nx; col++) {
            for (row = 0; row < mx; row++) {
                /* Fill the vector */
                for (counter = 0; counter < domainSize; counter++) {
                    r = domainRows[counter];
                    c = domainCols[counter];
                    vector[counter] = aa[col+c][row+r] + ss[c][r];
                }
                yy[col][row] = order_select(vector, domainSize, order);
#ifndef NDEBUG
                /* Check result against C lib function qsort() */
                for (counter = 0; counter < domainSize; counter++) {
                    r = domainRows[counter];
                    c = domainCols[counter];
                    vector[counter] = aa[col+c][row+r] + ss[c][r];
                }
                qsort((void *) vector, (size_t) domainSize, 
                      sizeof(double), cmp);
                mxAssert(yy[col][row] == vector[order-1], "");
#endif /* NDEBUG */
            }
        }
        mexAddFlops(nx * mx * domainSize);
    } else {
        for (col = 0; col < nx; col++) {
            for (row = 0; row < mx; row++) {
                /* Fill the vector */
                for (counter = 0; counter < domainSize; counter++) {
                    r = domainRows[counter];
                    c = domainCols[counter];
                    vector[counter] = aa[col+c][row+r];
                }
                yy[col][row] = order_select(vector, domainSize, order);
#ifndef NDEBUG
                /* Check result against C lib function qsort() */
                for (counter = 0; counter < domainSize; counter++) {
                    r = domainRows[counter];
                    c = domainCols[counter];
                    vector[counter] = aa[col+c][row+r];
                }
                qsort((void *) vector, (size_t) domainSize, 
                      sizeof(double), cmp);
                mxAssert(yy[col][row] == vector[order-1], "");
#endif /* NDEBUG */
            }
        }
    }

    mxFree((void *) vector);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    const mxArray *X_in;
    const mxArray *X;
    const mxArray *Order_in;
    mxArray *Order;
    const mxArray *Domain_in;
    mxArray *Domain;
    const mxArray *S_in;
    mxArray *S;
    mxArray *Y;
    int domainSize;
    int order;           /* desired order statistic */
    bool offsetsAreZero; /* additive offsets flag */

    ValidateInputs(nlhs, nrhs, prhs, &X_in, &Order_in, &Domain_in,
                   &S_in);

    Order = ConvertToDouble(Order_in);  /* exploits MATLAB's copy-by-ref */
    Domain = ConvertToDouble(Domain_in);
    if (S_in != NULL) {
        S = ConvertToDouble(S_in);
    } else {
        S = NULL;
    }

    domainSize = NumNonzeros(Domain);
    if (domainSize == 0) {
        mexErrMsgTxt("DOMAIN must have at least one nonzero element");
    }

    order = GetOrder(Order, domainSize);

    /* Additive offsets? */
    if ((S == NULL) || (NumNonzeros(S) == 0)) {
        offsetsAreZero = true;
    } else {
        offsetsAreZero = false;
    }

    /* If additive offsets, convert input to double */
    if (offsetsAreZero) {
        X = (mxArray *) X_in;
    } else {
        X = ConvertToDouble(X_in);
    }

    /* Initialize outputs */
    Y = mxCreateNumericArray(mxGetNumberOfDimensions(X),
                             mxGetDimensions(X),
                             mxGetClassID(X),
                             mxREAL);
    if (mxIsLogical(X) && offsetsAreZero) {
        mxSetLogical(Y);
    }

    /* If the output matrix is empty we don't need to do anything. */
    if (! mxIsEmpty(Y)) {
        if (mxIsDouble(X)) {
            ordfilt(Y, X, order, Domain, domainSize, S, offsetsAreZero);
        } else if (mxIsUint8(X)) {
            ordfilt8(Y, X, order, Domain, domainSize, S, offsetsAreZero);
        } else {
            mxAssert(1, "Class of X is not 'double' or 'uint8'");
        }
    }

    /* Done!  Give back the answer and clean up. */
    plhs[0] = Y;
    if (X != X_in) {
        mxDestroyArray((mxArray *) X);
    }
    mxDestroyArray((mxArray *) Order);
    mxDestroyArray((mxArray *) Domain);
    if (S != NULL) {
        mxDestroyArray((mxArray *) S);
    }
}