conversions.c

Go to the documentation of this file.

/*
 * Copyright (C) 2000-2003 Damien Douxchamps  <ddouxchamps@users.sf.net>
 *                         Dan Dennedy  <dan@dennedy.org>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <string.h>
#include "conversions.h"

#if !defined (WIN32)
  // This doesn't need to be extern on Windows (why does it need to be elsewhere?)
  extern void swab();
#endif

// The following #define is there for the users who experience green/purple
// images in the display. This seems to be a videocard driver problem.

//#define YUYV // instead of the standard UYVY
#define UYVY // it conforms to the yuv422 structure in the cmvision class.

// color conversion functions from Bart Nabbe.
// corrected by Damien: bad coeficients in YUV2RGB
#define YUV2RGB(y, u, v, r, g, b)\
  r = y + ((v*1436) >>10);\
  g = y - ((u*352 + v*731) >> 10);\
  b = y + ((u*1814) >> 10);\
  r = r < 0 ? 0 : r;\
  g = g < 0 ? 0 : g;\
  b = b < 0 ? 0 : b;\
  r = r > 255 ? 255 : r;\
  g = g > 255 ? 255 : g;\
  b = b > 255 ? 255 : b
  

#define RGB2YUV(r, g, b, y, u, v)\
  y = (306*r + 601*g + 117*b)  >> 10;\
  u = ((-172*r - 340*g + 512*b) >> 10)  + 128;\
  v = ((512*r - 429*g - 83*b) >> 10) + 128;\
  y = y < 0 ? 0 : y;\
  u = u < 0 ? 0 : u;\
  v = v < 0 ? 0 : v;\
  y = y > 255 ? 255 : y;\
  u = u > 255 ? 255 : u;\
  v = v > 255 ? 255 : v

#define CLIP(in, out)\
{\
   in = in < 0 ? 0 : in;\
   in = in > 255 ? 255 : in;\
   out=in;\
}
  
/**********************************************************************
 *
 *  CONVERSION FUNCTIONS TO UYVY 
 *
 **********************************************************************/

void
yuyv2uyvy(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
#ifdef YUYV
  swab(src, dest, NumPixels << 1);
#else
  memcpy(dest,src, NumPixels<<1);
#endif
}

void
uyvy2yuyv(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
#ifdef YUYV
  swab(src, dest, NumPixels << 1);
#else
  memcpy(dest,src, NumPixels<<1);
#endif
}
void
uyyvyy2uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i=NumPixels + (NumPixels >> 1)-1;
  register int j=(NumPixels << 1)-1;
  register int y0, y1, y2, y3, u, v;

  while (i > 0) {
    y3 = src[i--];
    y2 = src[i--];
    v  = src[i--];
    y1 = src[i--];
    y0 = src[i--];
    u  = src[i--];
#ifdef YUYV
    dest[j--] = v;
    dest[j--] = y3;
    dest[j--] = u;
    dest[j--] = y2;
    
    dest[j--] = v;
    dest[j--] = y1;
    dest[j--] = u;
    dest[j--] = y0;
#else // UYVY
    dest[j--] = y3;
    dest[j--] = v;
    dest[j--] = y2;
    dest[j--] = u;
    
    dest[j--] = y1;
    dest[j--] = v;
    dest[j--] = y0;
    dest[j--] = u;
#endif
  }
}

void
uyv2uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels + (NumPixels << 1)-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1;

  while (i > 0) {
    v1 = src[i--];
    y1 = src[i--];
    u1 = src[i--];
    v0 = src[i--];
    y0 = src[i--];
    u0 = src[i--];
    
#ifdef YUYV
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y1;
    dest[j--] = (u0+u1) >> 1;
    dest[j--] = y0;
#else // UYVY
    dest[j--] = y1;
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y0;
    dest[j--] = (u0+u1) >> 1;
#endif
    }
}


void
y2uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i= NumPixels-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1;

  while (i > 0) {
    y1 = src[i--];
    y0 = src[i--];
#ifdef YUYV
    dest[j--] = 128;
    dest[j--] = y1;
    dest[j--] = 128;
    dest[j--] = y0;
#else // UYVY
    dest[j--] = y1;
    dest[j--] = 128;
    dest[j--] = y0;
    dest[j--] = 128;
#endif
    }
}

void
y162uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels, int bits)
{
  register int i = (NumPixels << 1)-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1;
  //fprintf(stderr,"Conv: Pix: %d Bpp: %d\n",NumPixels, bits);
  while (i > 0) {
    //fprintf(stderr,"%d ",i);
    y1 = src[i--];
    y1 = (y1 + (((int)src[i--])<<8))>>(bits-8);
    y0 = src[i--];
    y0 = (y0 + (((int)src[i--])<<8))>>(bits-8);
#ifdef YUYV
    dest[j--] = 128;
    dest[j--] = y1;
    dest[j--] = 128;
    dest[j--] = y0;
#else // UYVY
    dest[j--] = y1;
    dest[j--] = 128;
    dest[j--] = y0;
    dest[j--] = 128;
#endif
  }
}

void
y162y (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels, int bits)
{
  register int i = (NumPixels<<1)-1;
  register int j = NumPixels-1;
  register int y;

  while (i > 0) {
    y = src[i--];
    dest[j--] = (y + (src[i--]<<8))>>(bits-8);
  }
}

void
rgb2uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels + ( NumPixels << 1 )-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1 ;
  register int r, g, b;

  while (i > 0) {
    b = (unsigned char) src[i--];
    g = (unsigned char) src[i--];
    r = (unsigned char) src[i--];
    RGB2YUV (r, g, b, y0, u0 , v0);
    b = (unsigned char) src[i--];
    g = (unsigned char) src[i--];
    r = (unsigned char) src[i--];
    RGB2YUV (r, g, b, y1, u1 , v1);
#ifdef YUYV
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y0;
    dest[j--] = (u0+u1) >> 1;
    dest[j--] = y1;
#else // UYVY
    dest[j--] = y0;
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y1;
    dest[j--] = (u0+u1) >> 1;
#endif
  }
}

void
rgb482uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = ( (NumPixels + ( NumPixels << 1 )) << 1 ) -1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1 ;
  register int r, g, b;

  while (i > 0) {
    i--;
    b = (unsigned char) src[i--];
    i--;
    g = (unsigned char) src[i--];
    i--;
    r = (unsigned char) src[i--];
    i--;
    RGB2YUV (r, g, b, y0, u0 , v0);
    b = (unsigned char) src[i--];
    i--;
    g = (unsigned char) src[i--];
    i--;
    r = (unsigned char) src[i--];
    RGB2YUV (r, g, b, y1, u1 , v1);
    
#ifdef YUYV
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y0;
    dest[j--] = (u0+u1) >> 1;
    dest[j--] = y1;
#else // UYVY
    dest[j--] = y0;
    dest[j--] = (v0+v1) >> 1;
    dest[j--] = y1;
    dest[j--] = (u0+u1) >> 1;
#endif
  }
}

/**********************************************************************
 *
 *  CONVERSION FUNCTIONS TO RGB 24bpp 
 *
 **********************************************************************/

void
rgb482rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = ((NumPixels + ( NumPixels << 1 )) << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;

  while (i > 0) {
    i--;
    dest[j--]=src[i--];
    i--;
    dest[j--]=src[i--];
    i--;
    dest[j--]=src[i--];
  }
}


void
uyv2rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels + ( NumPixels << 1 ) -1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y, u, v;
  register int r, g, b;

  while (i > 0) {
    v = (unsigned char) src[i--] - 128;
    y = (unsigned char) src[i--];
    u = (unsigned char) src[i--] - 128;
    YUV2RGB (y, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;  
  }
}

void
uyvy2rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y0, y1, u, v;
  register int r, g, b;

  while (i > 0) {
    y1 = (unsigned char) src[i--];
    v  = (unsigned char) src[i--] - 128;
    y0 = (unsigned char) src[i--];
    u  = (unsigned char) src[i--] - 128;
    YUV2RGB (y1, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
    YUV2RGB (y0, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
  }
}


void
uyyvyy2rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels + ( NumPixels >> 1 )-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y0, y1, y2, y3, u, v;
  register int r, g, b;
  
  while (i > 0) {
    y3 = (unsigned char) src[i--];
    y2 = (unsigned char) src[i--];
    v  = (unsigned char) src[i--] - 128;
    y1 = (unsigned char) src[i--];
    y0 = (unsigned char) src[i--];
    u  = (unsigned char) src[i--] - 128;
    YUV2RGB (y3, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
    YUV2RGB (y2, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
    YUV2RGB (y1, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
    YUV2RGB (y0, u, v, r, g, b);
    dest[j--] = b;
    dest[j--] = g;
    dest[j--] = r;
  }
}

void
y2rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y;

  while (i > 0) {
    y = (unsigned char) src[i--];
    dest[j--] = y;
    dest[j--] = y;
    dest[j--] = y;
  }
}

void
y162rgb (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels, int bits)
{
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y;

  while (i > 0) {
    y = src[i--];
    y = (y + (src[i--]<<8))>>(bits-8);
    dest[j--] = y;
    dest[j--] = y;
    dest[j--] = y;
  }
}

/****************************************************************
 *     Color conversion functions for cameras that can          * 
 * output raw-Bayer pattern images, such as some Basler and     *
 * Point Grey camera. Most of the algos presented here com from *
 * http://ise0.Stanford.EDU/~tingchen/main.htm and have been    *
 * converted from Matlab to C and extended to all elementary    *
 * patterns.                                                    *
 ****************************************************************/

void
BayerNearestNeighbor(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;

  // sx and sy should be even
  switch (type) {
  case BAYER_PATTERN_GRBG:
  case BAYER_PATTERN_BGGR:
    outR=&dest[0];
    outG=&dest[1];
    outB=&dest[2];
    break;
  case BAYER_PATTERN_GBRG:
  case BAYER_PATTERN_RGGB:
    outR=&dest[2];
    outG=&dest[1];
    outB=&dest[0];
    break;
  default:
    outR=NULL;outG=NULL;outB=NULL;
    break;
  }
  
  switch (type) {
  case BAYER_PATTERN_GRBG: //-------------------------------------------
  case BAYER_PATTERN_GBRG:
    // copy original RGB data to output images
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        outG[(i*sx+j)*3]=src[i*sx+j];
        outG[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
        outR[(i*sx+j+1)*3]=src[i*sx+j+1];
        outB[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
      }
    }
    // R channel
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx-1;j+=2) {
        outR[(i*sx+j)*3]=outR[(i*sx+j+1)*3];
        outR[((i+1)*sx+j+1)*3]=outR[(i*sx+j+1)*3];
        outR[((i+1)*sx+j)*3]=outR[(i*sx+j+1)*3];
      }
    }
      // B channel
    for (i=0;i<sy-1;i+=2)  { //every two lines
      for (j=0;j<sx-1;j+=2) {
        outB[(i*sx+j)*3]=outB[((i+1)*sx+j)*3];
        outB[(i*sx+j+1)*3]=outB[((i+1)*sx+j)*3];
        outB[((i+1)*sx+j+1)*3]=outB[((i+1)*sx+j)*3];
      }
    }
    // using lower direction for G channel
      
    // G channel
    for (i=0;i<sy-1;i+=2)//every two lines
      for (j=1;j<sx;j+=2)
        outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
      
    for (i=1;i<sy-2;i+=2)//every two lines
      for (j=0;j<sx-1;j+=2)
        outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
    
    // copy it for the next line
    for (j=0;j<sx-1;j+=2)
      outG[((sy-1)*sx+j)*3]=outG[((sy-2)*sx+j)*3];
    
    break;
  case BAYER_PATTERN_BGGR: //-------------------------------------------
  case BAYER_PATTERN_RGGB:
    // copy original data
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        outB[(i*sx+j)*3]=src[i*sx+j];
        outR[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
        outG[(i*sx+j+1)*3]=src[i*sx+j+1];
        outG[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
      }
    }
    // R channel
    for (i=0;i<sy;i+=2){
      for (j=0;j<sx-1;j+=2) {
        outR[(i*sx+j)*3]=outR[((i+1)*sx+j+1)*3];
        outR[(i*sx+j+1)*3]=outR[((i+1)*sx+j+1)*3];
        outR[((i+1)*sx+j)*3]=outR[((i+1)*sx+j+1)*3];
      }
    }
    // B channel
    for (i=0;i<sy-1;i+=2) { //every two lines
      for (j=0;j<sx-1;j+=2) {
        outB[((i+1)*sx+j)*3]=outB[(i*sx+j)*3];
        outB[(i*sx+j+1)*3]=outB[(i*sx+j)*3];
        outB[((i+1)*sx+j+1)*3]=outB[(i*sx+j)*3];
      }
    }
    // using lower direction for G channel
    
    // G channel
    for (i=0;i<sy-1;i+=2)//every two lines
      for (j=0;j<sx-1;j+=2)
        outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
    
    for (i=1;i<sy-2;i+=2)//every two lines
      for (j=0;j<sx-1;j+=2)
        outG[(i*sx+j+1)*3]=outG[((i+1)*sx+j+1)*3];
    
    // copy it for the next line
    for (j=0;j<sx-1;j+=2)
      outG[((sy-1)*sx+j+1)*3]=outG[((sy-2)*sx+j+1)*3];
    
    break;
    
  default:  //-------------------------------------------
    break;
  }
}


void
BayerEdgeSense(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;
  int dh, dv;
  int tmp;

  // sx and sy should be even
  switch (type) {
  case BAYER_PATTERN_GRBG:
  case BAYER_PATTERN_BGGR:
    outR=&dest[0];
    outG=&dest[1];
    outB=&dest[2];
    break;
  case BAYER_PATTERN_GBRG:
  case BAYER_PATTERN_RGGB:
    outR=&dest[2];
    outG=&dest[1];
    outB=&dest[0];
    break;
  default:
    outR=NULL;outG=NULL;outB=NULL;
    break;
  }

  switch (type) {
  case BAYER_PATTERN_GRBG://---------------------------------------------------------
  case BAYER_PATTERN_GBRG:
    // copy original RGB data to output images
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        outG[(i*sx+j)*3]=src[i*sx+j];
        outG[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
        outR[(i*sx+j+1)*3]=src[i*sx+j+1];
        outB[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
      }
    }
    // process GREEN channel
    for (i=3;i<sy-2;i+=2) {
      for (j=2;j<sx-3;j+=2) {
        dh=abs((outB[(i*sx+j-2)*3]+outB[(i*sx+j+2)*3])/2-outB[(i*sx+j)*3]);
        dv=abs((outB[((i-2)*sx+j)*3]+outB[((i+2)*sx+j)*3])/2-outB[(i*sx+j)*3]);
        if (dh<dv)
          tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
        else {
          if (dh>dv)
            tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
          else
            tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
        }
        CLIP(tmp,outG[(i*sx+j)*3]);
      }
    }

    for (i=2;i<sy-3;i+=2) {
      for (j=3;j<sx-2;j+=2) {
        dh=abs((outR[(i*sx+j-2)*3]+outR[(i*sx+j+2)*3])/2-outR[(i*sx+j)*3]);
        dv=abs((outR[((i-2)*sx+j)*3]+outR[((i+2)*sx+j)*3])/2-outR[(i*sx+j)*3]);
        if (dh<dv)
          tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
        else {
          if (dh>dv)
            tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
          else
            tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
        } 
        CLIP(tmp,outG[(i*sx+j)*3]);
      }
    }
    // process RED channel
    for (i=0;i<sy-1;i+=2) {
      for (j=2;j<sx-1;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outR[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
                              outR[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
    }
    for (i=1;i<sy-2;i+=2) {
      for (j=1;j<sx;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
                              outR[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
      for (j=2;j<sx-1;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
                              outR[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
                              outR[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
                              outR[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
    }
      
    // process BLUE channel
    for (i=1;i<sy;i+=2) {
      for (j=1;j<sx-2;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
                              outB[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
    }
    for (i=2;i<sy-1;i+=2) {
      for (j=0;j<sx-1;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
                              outB[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
      for (j=1;j<sx-2;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
                              outB[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
                              outB[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
                              outB[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
    }
      break;

  case BAYER_PATTERN_BGGR: //---------------------------------------------------------
  case BAYER_PATTERN_RGGB:
    // copy original RGB data to output images
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        outB[(i*sx+j)*3]=src[i*sx+j];
        outR[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
        outG[(i*sx+j+1)*3]=src[i*sx+j+1];
        outG[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
      }
    }
    // process GREEN channel
    for (i=2;i<sy-2;i+=2) {
      for (j=2;j<sx-3;j+=2) {
        dh=abs((outB[(i*sx+j-2)*3]+outB[(i*sx+j+2)*3])/2-outB[(i*sx+j)*3]);
        dv=abs((outB[((i-2)*sx+j)*3]+outB[((i+2)*sx+j)*3])/2-outB[(i*sx+j)*3]);
        if (dh<dv)
          tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
        else {
          if (dh>dv)
            tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
          else
            tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
        }
        CLIP(tmp,outG[(i*sx+j)*3]);
      }
    }
    for (i=3;i<sy-3;i+=2) {
      for (j=3;j<sx-2;j+=2) {
        dh=abs((outR[(i*sx+j-2)*3]+outR[(i*sx+j+2)*3])/2-outR[(i*sx+j)*3]);
        dv=abs((outR[((i-2)*sx+j)*3]+outR[((i+2)*sx+j)*3])/2-outR[(i*sx+j)*3]);
        if (dh<dv)
          tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
        else {
          if (dh>dv)
            tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
          else
            tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
        }
        CLIP(tmp,outG[(i*sx+j)*3]);
      }
    }
    // process RED channel
    for (i=1;i<sy-1;i+=2) { // G-points (1/2)
      for (j=2;j<sx-1;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outR[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
                              outR[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
    }
    for (i=2;i<sy-2;i+=2)  {
      for (j=1;j<sx;j+=2) { // G-points (2/2)
        tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
                              outR[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
      for (j=2;j<sx-1;j+=2) { // B-points
        tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
                              outR[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
                              outR[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
                              outR[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
        CLIP(tmp,outR[(i*sx+j)*3]);
      }
    }
    
      // process BLUE channel
    for (i=0;i<sy;i+=2) {
      for (j=1;j<sx-2;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
                              outB[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
    }
    for (i=1;i<sy-1;i+=2) {
      for (j=0;j<sx-1;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
                              outB[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
      for (j=1;j<sx-2;j+=2) {
        tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
                              outB[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
                              outB[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
                              outB[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
        CLIP(tmp,outB[(i*sx+j)*3]);
      }
    }
    break;
  default: //---------------------------------------------------------
    fprintf(stderr,"Bad bayer pattern ID\n");
    break;
  }
}

void
BayerDownsample(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;
  int tmp;

  sx*=2;
  sy*=2;

  switch (type) {
  case BAYER_PATTERN_GRBG:
  case BAYER_PATTERN_BGGR:
    outR=&dest[0];
    outG=&dest[1];
    outB=&dest[2];
    break;
  case BAYER_PATTERN_GBRG:
  case BAYER_PATTERN_RGGB:
    outR=&dest[2];
    outG=&dest[1];
    outB=&dest[0];
    break;
  default:
    outR=NULL;outG=NULL;outB=NULL;
    break;
  }
  
  switch (type) {
  case BAYER_PATTERN_GRBG://---------------------------------------------------------
  case BAYER_PATTERN_GBRG:
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        tmp=((src[i*sx+j]+src[(i+1)*sx+(j+1)])>>1);
        CLIP(tmp,outG[(((i*sx)>>2)+(j>>1))*3]);
        tmp=src[i*sx+j+1];
        CLIP(tmp,outR[(((i*sx)>>2)+(j>>1))*3]);
        tmp=src[(i+1)*sx+j];
        CLIP(tmp,outB[(((i*sx)>>2)+(j>>1))*3]);
      }
    }
    break;
  case BAYER_PATTERN_BGGR://---------------------------------------------------------
  case BAYER_PATTERN_RGGB:
    for (i=0;i<sy;i+=2) {
      for (j=0;j<sx;j+=2) {
        tmp=((src[(i+1)*sx+j]+src[i*sx+(j+1)])>>1);
        CLIP(tmp,outG[(((i*sx)>>2)+(j>>1))*3]);
        tmp=src[(i+1)*sx+j+1];
        CLIP(tmp,outR[(((i*sx)>>2)+(j>>1))*3]);
        tmp=src[i*sx+j];
        CLIP(tmp,outB[(((i*sx)>>2)+(j>>1))*3]);
      }
    }
    break;
  default: //---------------------------------------------------------
    fprintf(stderr,"Bad bayer pattern ID\n");
    break;
  }
  
}

// change a 16bit stereo image (8bit/channel) into two 8bit images on top
// of each other
void
StereoDecode(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels)
{
  register int i = NumPixels-1;
  register int j = (NumPixels>>1)-1;
  register int k = NumPixels-1;
  //fprintf(stderr,"NumPixels: %d\n",NumPixels);
  while (i > 0) {
    dest[k--] = src[i--];
    dest[j--] = src[i--];
  }
}