cm2bmp.c

MatLab图像传感器网络仿真平台WiSNAP

/*
 * File:  cm2bmp
 *
 * Description:     Routines to convert from various FOURCC 
 *                  formats to 24 bit BMP
 *
 * macros for YUV->RGB are from Independent JPEG Group
 */

#include "cm2bmp.h"
#include "cmApi.h"

#define YUV_TO_B(Y,U,V) \
      (int) (1.164*(Y) + 2.018*((U) - 128))

#define YUV_TO_G(Y,U,V) \
      (int) (1.164*(Y) - 0.813*((V) - 128) - 0.391*((U) - 128))

#define YUV_TO_R(Y,U,V) \
      (int) (1.164*(Y) + 1.596*((V) - 128))

#define RANGE_LIMIT(X)\
              if (X > 255)      \
                  X = 255;      \
              else if (X < 0)   \
                  X = 0;
/*
       R = Y + (1.4075 * (V - 128));
       G = Y - (0.3455 * (U - 128) - (0.7169 * (V - 128));
       B = Y + (1.7790 * (U - 128);
 */

/*
 * Function: cm2cmp
 *
 * Description: Convert the format to BMP
 *
 * Input Parameters:
 *
 * Output Parameters:
 *
 */
int cm2bmp (unsigned char *buffer_in_p, int buffer_in_len, unsigned char *buffer_out_p, int *buffer_out_len_p, ImageType_t type, int width, int height)
{
    BITMAPFILEHEADER    bfh;
    BITMAPINFOHEADER    bih;
    unsigned char *buf_p = buffer_out_p;
    int status  = 0;

    /* Initialize the header */
    memset (&bfh, 0, sizeof (bfh));
    bfh.bfType    = 0x4d42; //'MB';
    bfh.bfSize    = sizeof (bfh) + sizeof (bih) + buffer_in_len;
    bfh.bfOffBits = sizeof (bih) + sizeof (bfh);

    memcpy (buf_p, &bfh, sizeof (bfh));
    *buffer_out_len_p = sizeof (bfh);
    buf_p = buf_p + *buffer_out_len_p;

    /* Initialize the info header */
    memset (&bih, 0, sizeof (bih));
    bih.biSize     = sizeof (bih);
    bih.biWidth    = width;
    bih.biHeight   = height;
    bih.biPlanes   = 1;
    bih.biBitCount = 24;

    memcpy (buf_p, &bih, sizeof (bih));
    *buffer_out_len_p = *buffer_out_len_p + sizeof (bih);
    buf_p = buf_p + sizeof(bih);

    /* convert to bmp */
    switch (type) {
        case CM_TYPE_RGB888:
            rgb888_bgr (width, height, buffer_in_p, buf_p);
            break;
        case CM_TYPE_RAWBPA:
        case CM_TYPE_RAW:
            raw_bgr (width, height, buffer_in_p, buf_p);
            break;
        default:
            uyvy_bgr (width, height, buffer_in_p, buf_p);
            break;
    }

    *buffer_out_len_p = *buffer_out_len_p + buffer_in_len;

    return status;
}
/*
 * Function: uyvy_bgr
 *
 * Description: Convert from YUV 4:2:2 to bmp
 *
 * There are 16 bits per pixel (Y sample at every pixel, U and V sampled at
 * every second pixel horizontally on every line)
 */
void
uyvy_bgr(UINT width,
         UINT height,
         unsigned char *uyvy,
         unsigned char *rgb_buff
        )
{
    int pad;
    int half_width = width >> 1;
    int width_offset = width  <<1;
    unsigned char  *bmp = rgb_buff;
    int col;
    unsigned char  *src;
    int row_width = width * 3; // 3 bytes per pixel

    int B0, G0, R0, B1, G1, R1;

    int Y1p, Y2p, Up, Upb, Upg, Vp, Vpg, Vpr;

    //
    // calculate number of bytes necessary to pad each row
    // to 4 byte boundary
    //

    pad = 0;
    while (row_width & 0x3)
    {
        ++pad;
        ++row_width;
    }

    // point to end of buffer (image will be flipped vertically)

    uyvy = uyvy + (width * height * 2);

    do
    {
        uyvy -= width_offset;  // back up one row
        src   = uyvy;
        col   = half_width;

        do
        {
            // even column
            //
            Up  = src[0] - 128;             // max = 127, min = -128
            Upb = (2066 * Up) >> 10;        // max = 256, min = -258
            Upg = ( 400 * Up) >> 10;        // max =  49, min = - 50 

            Y1p = (1192 * src[1]) >> 10;    // max = 296, min =    0

            Vp  = src[2] - 128;             // max = 127, min = -128
            Vpg = ( 833 * Vp) >> 10;        // max = 103, min = -104
            Vpr = (1634 * Vp) >> 10;        // max = 202, min = -204

            Y2p = (1192 * src[3]) >> 10;    // max = 296, min =    0

            B0 = Y1p + Upb;            // max = 552, min = -258
            G0 = Y1p - Vpg - Upg;      // max = 450, min = -152
            R0 = Y1p + Vpr;

            // odd column
            //
            B1 = Y2p + Upb;
            G1 = Y2p - Vpg - Upg;
            R1 = Y2p + Vpr;

            RANGE_LIMIT (B0);
            RANGE_LIMIT (G0);
            RANGE_LIMIT (R0);

            RANGE_LIMIT (B1);
            RANGE_LIMIT (G1);
            RANGE_LIMIT (R1);

            bmp[0] = (unsigned char) B0;
            bmp[1] = (unsigned char) G0;
            bmp[2] = (unsigned char) R0;

            bmp[3] = (unsigned char) B1;
            bmp[4] = (unsigned char) G1;
            bmp[5] = (unsigned char) R1;

            src += 4;
            bmp += 6;
        } while (--col > 0);

        bmp += pad;

    } while (--height > 0);
}

/*
 * Function: yuyv_bgr
 *
 * Description: Convert YUV 4:2:2 with different ordering to bmp
 */
void
yuyv_bgr(UINT width,
         UINT height,
         unsigned char  *uyvy,
         unsigned char  *rgb_buff
        )
{
    int pad;
    int half_width;
    int width_offset;
    unsigned char *bmp;

    {
        //
        // calculate number of bytes necessary to pad each row
        // to 4 byte boundary
        //

        int row_width = width * 3; // 3 bytes per pixel
        pad = 0;

        while (row_width & 0x3) {
            ++pad;
            ++row_width;
        }

    }

    // point to end of buffer (image will be flipped vertically)

    uyvy = uyvy + (width * height * 2);

    half_width = width >> 1;
    width_offset = width  * 2;
    bmp = rgb_buff;

    do {
        int col;
        unsigned char  *src;

        uyvy -= width_offset;  // back up one row

        src = uyvy;

        col = half_width;

        do {
            int B, G, R;
            unsigned char  Y1, Y2, U, V;

            // even column
            //

            Y1 = src[0];
            U  = src[1];
            Y2 = src[2];
            V  = src[3];

//            U  = src[1];
//            Y1 = src[0];
//            V  = src[3];
//            Y2 = src[2];

            src += 4;

            B = YUV_TO_B(Y1, U, V);
            G = YUV_TO_G(Y1, U, V);
            R = YUV_TO_R(Y1, U, V);

            RANGE_LIMIT (B);
            RANGE_LIMIT (G);
            RANGE_LIMIT (R);

            bmp[2] = (unsigned char) R;
            bmp[1] = (unsigned char) G;
            bmp[0] = (unsigned char) B;

            // odd column
            //

            B = YUV_TO_B(Y2, U, V);
            G = YUV_TO_G(Y2, U, V);
            R = YUV_TO_R(Y2, U, V);

            RANGE_LIMIT (B);
            RANGE_LIMIT (G);
            RANGE_LIMIT (R);

            bmp[5] = (unsigned char) R;
            bmp[4] = (unsigned char) G;
            bmp[3] = (unsigned char) B;

            bmp += 6;

        } while (--col > 0);

        /*
        if (pad) {
            memset (bmp, 0, pad);
            bmp += pad;
        }
        */
        bmp += pad;

    } while (--height > 0);

}


/*
 * Function: iyuv_bgr
 *
 * Description:
 */
void
iyuv_bgr(UINT width,
         UINT height,
         unsigned char  *iyuv,
         unsigned char  *bmp
        )
{
    int pad;
    int frame_size;
    int even_row;
    unsigned char  *Yptr;
    unsigned char  *Uptr;
    unsigned char  *Vptr;
    int half_width;

    pad = 0;
    while (width % 4)
        ++pad;

    frame_size = width * height;

    half_width = width >> 1;

    Yptr = iyuv + frame_size;  // flip image vertically
    Uptr = Yptr + (frame_size >> 2) - half_width;
    Vptr = Uptr + (frame_size >> 2);

    even_row = 1;
    do {
        int col;
        unsigned char  *_U;
        unsigned char  *_V;
        unsigned char  *_Y;

        Yptr -= width;

        col = half_width;

        _Y = Yptr;
        _U = Uptr;
        _V = Vptr;

        do {
            int B, G, R;
            int B2, G2, R2;
            unsigned char  Y, U, V;

            // even column
            //

            Y = *_Y++;
            U = *_U;
            V = *_V;

            B = YUV_TO_B(Y, U, V);
            G = YUV_TO_G(Y, U, V);
            R = YUV_TO_R(Y, U, V);

            RANGE_LIMIT (B);
            RANGE_LIMIT (G);
            RANGE_LIMIT (R);

            bmp[2] = (unsigned char) R;
            bmp[1] = (unsigned char) G;
            bmp[0] = (unsigned char) B;

            // odd column
            //

            Y = *_Y++;
            U = *_U++;
            V = *_V++;

            B2 = YUV_TO_B(Y, U, V);
            G2 = YUV_TO_G(Y, U, V);
            R2 = YUV_TO_R(Y, U, V);

            RANGE_LIMIT (B2);
            RANGE_LIMIT (G2);
            RANGE_LIMIT (R2);

            bmp[5] = (unsigned char) R2;
            bmp[4] = (unsigned char) G2;
            bmp[3] = (unsigned char) B2;

            bmp += 6;

        } while (--col > 0);

        even_row = !even_row;

        if (even_row) {
            Uptr -= half_width;
            Vptr -= half_width;
        }

        if (pad) {
            memset (bmp, 0, pad);
            bmp += pad;
        }

    } while (--height > 0);

}

void rgb888_bgr (UINT width,
                 UINT height,
                 unsigned char *rgb888,
                 unsigned char *rgb_buff)
{
    unsigned char *praw,
                  *prawRowBegin,
                  *prawRowEnd,
                  *pwin;

    UINT imageSize = 3 * width * height;
    UINT stride    = 3 * width;

    for (prawRowBegin = rgb888 + imageSize - stride,
         pwin = rgb_buff;
         prawRowBegin != rgb888 - stride;
         prawRowBegin -= stride)
    {
        prawRowEnd = prawRowBegin + stride;
        for (praw = prawRowBegin; praw != prawRowEnd; praw += 3)
        {
            *pwin++ = *(praw + 2);  // ->B
            *pwin++ = *(praw + 1);  // ->G
            *pwin++ = *praw;        // ->R
        }
    }
}

void raw_bgr (UINT width,
              UINT height,
              unsigned char *raw,
              unsigned char *rgb_buff)
{
    unsigned char *praw,
                  *prawRowBegin,
                  *prawRowEnd,
                  *pwin;

    UINT imageSize = width * height;

    for (prawRowBegin = raw + imageSize - width,
         pwin = rgb_buff;
         prawRowBegin != raw - width;
         prawRowBegin -= width)
    {
        prawRowEnd = prawRowBegin + width;
        for (praw = prawRowBegin; praw != prawRowEnd; praw++)
        {
            *pwin++ = *praw;        // ->B
            *pwin++ = *praw;        // ->G
            *pwin++ = *praw;        // ->R
        }
    }
}

void raw10_bgr (UINT width,
                UINT height,
                unsigned char *raw,
                unsigned char *rgb_buff)
{
    unsigned char *praw,
                  *prawRowBegin,
                  *prawRowEnd,
                  *pwin;

    UINT imageSize = 2 * width * height;
    UINT stride    = 2 * width;

    for (prawRowBegin = raw + imageSize - stride,
         pwin = rgb_buff;
         prawRowBegin != raw - stride;
         prawRowBegin -= stride)
    {
        prawRowEnd = prawRowBegin + stride;
        for (praw = prawRowBegin; praw != prawRowEnd; praw++)
        {
            *pwin++ = *++praw;        // ->B
            *pwin++ = *praw;        // ->G
            *pwin++ = *praw;        // ->R
        }
    }
}