vconvert.cxx

sloedgy open sip stack source code

    else {

      // dest is bigger than the source. No subsampling.
      // Place the src in the middle of the destination.
      unsigned int yOffset = (dstFrameHeight - srcFrameHeight)/2;
      unsigned int xOffset = (dstFrameWidth - srcFrameWidth)/2;

      d = dest;
      for (i=0; i < npixels; i++) 
        *d++ = BLACK_Y;
      for (i=0; i < npixels/4; i++)
        *d++ = BLACK_U;
      for (i=0; i < npixels/4; i++)
        *d++ = BLACK_V;

      // Copy plane Y
      d = dest + yOffset * dstFrameWidth + xOffset;
      s = src;
      for (y = 0; y < srcFrameHeight; y++) {
        memcpy(d, s, srcFrameWidth);
        s += srcFrameWidth;
        d += dstFrameWidth;
      }

      // Copy plane U
      d = dest + npixels + (yOffset*dstFrameWidth/4) + xOffset/2;
      for (y = 0; y < srcFrameHeight/2; y++) {
        memcpy(d, s, srcFrameWidth/2);
        s += srcFrameWidth/2;
        d += dstFrameWidth/2;
      }

      // Copy plane V
      d = dest + npixels + npixels/4 + (yOffset*dstFrameWidth/4) + xOffset/2;
      for (y = 0; y < srcFrameHeight/2; y++) {
        memcpy(d, s, srcFrameWidth/2);
        s += srcFrameWidth/2;
        d += dstFrameWidth/2;
      }
    }

  } else {  
    // source is bigger than the destination.
    //
#define FIX_FLOAT	12
    unsigned int dx = (srcFrameWidth<<FIX_FLOAT)/dstFrameWidth;
    unsigned int dy = (srcFrameHeight<<FIX_FLOAT)/dstFrameHeight;
    unsigned int fy, fx;

    s = src;
    d = dest;

    /* Copy Plane Y */
    for (fy=0, y=0; y<dstFrameHeight; y++, fy+=dy) {
       s = src + (fy>>FIX_FLOAT) * srcFrameWidth;
       for (fx=0, x=0; x<dstFrameWidth; x++, fx+=dx) {
	  *d++ = s[fx>>FIX_FLOAT];
       }
    }

    /* Copy Plane U */
    src += srcFrameWidth*srcFrameHeight;
    for (fy=0, y=0; y<dstFrameHeight/2; y++, fy+=dy) {
       s = src + (fy>>FIX_FLOAT) * srcFrameWidth/2;
       for (fx=0, x=0; x<dstFrameWidth/2; x++, fx+=dx) {
	  *d++ = s[fx>>FIX_FLOAT];
       }
    }

    /* Copy Plane V */
    src += srcFrameWidth*srcFrameHeight/4;
    for (fy=0, y=0; y<dstFrameHeight/2; y++, fy+=dy) {
       s = src + (fy>>FIX_FLOAT) * srcFrameWidth/2;
       for (fx=0, x=0; x<dstFrameWidth/2; x++, fx+=dx) {
	  *d++ = s[fx>>FIX_FLOAT];
       }
    }

  }

}

PSTANDARD_COLOUR_CONVERTER(YUV420P,YUV420P)
{
  if (bytesReturned != NULL)
    *bytesReturned = dstFrameBytes;
  
  if (srcFrameBuffer == dstFrameBuffer)
    return TRUE;

  if ((srcFrameWidth == dstFrameWidth) && (srcFrameHeight == dstFrameHeight)) 
    memcpy(dstFrameBuffer,srcFrameBuffer,srcFrameWidth*srcFrameHeight*3/2);
  else
    ResizeYUV420P(srcFrameBuffer, dstFrameBuffer);

  return TRUE;
}

/*
 * Format YUY2 or YUV422(non planar):
 *
 * off: 0  Y00 U00 Y01 V00 Y02 U01 Y03 V01
 * off: 8  Y10 U10 Y11 V10 Y12 U11 Y13 V11
 * off:16  Y20 U20 Y21 V20 Y22 U21 Y23 V21
 * off:24  Y30 U30 Y31 V30 Y32 U31 Y33 V31
 * length:32 bytes
 *
 * Format YUV420P:
 * off: 00  Y00 Y01 Y02 Y03
 * off: 04  Y10 Y11 Y12 Y13
 * off: 08  Y20 Y21 Y22 Y23
 * off: 12  Y30 Y31 Y32 Y33
 * off: 16  U00 U02 U20 U22
 * off: 20  V00 V02 V20 V22
 * 
 * So, we loose some bit of information when converting YUY2 to YUV420 
 *
 */
PSTANDARD_COLOUR_CONVERTER(YUV422,YUV420P)
{
  if ((srcFrameWidth | dstFrameWidth | srcFrameHeight | dstFrameHeight) & 1) {
    PTRACE(2,"PColCnv\tError in YUV422 to YUV420P converter, All size need to be pair.");
    return FALSE;
  }

  if ((srcFrameWidth==dstFrameWidth) && (srcFrameHeight==dstFrameHeight))
    YUY2toYUV420PSameSize(srcFrameBuffer, dstFrameBuffer);
  else
    YUY2toYUV420PWithResize(srcFrameBuffer, dstFrameBuffer);

  if (bytesReturned != NULL)
    *bytesReturned = dstFrameBytes;

  return TRUE;
}


#define LIMIT(x) (unsigned char) ((x > 255) ? 255 : ((x < 0) ? 0 : x ))
static inline int clip(int a, int limit) {
  return a<limit?a:limit;
}

BOOL PStandardColourConverter::SBGGR8toYUV420P(const BYTE * src, BYTE * dst, PINDEX * bytesReturned) const
{
#define USE_SBGGR8_NATIVE 1 // set to 0 to use the double conversion algorithm (Bayer->RGB->YUV420P)
  
#if USE_SBGGR8_NATIVE

  // kernels for Y conversion, normalised by 2^16
  const int kR[]={1802,9667,1802,9667,19661,9667,1802,9667,1802}; 
  const int kG1[]={7733,9830,7733,3604,7733,3604,7733,9830,7733};
  const int kG2[]={7733,3604,7733,9830,7733,9830,7733,3604,7733};
  const int kB[]={4915,9667,4915,9667,7209,9667,4915,9667,4915};
  //  const int kID[]={0,0,0,0,65536,0,0,0,0}; identity kernel, use to test

  int B, G, G1, G2, R;
  const int stride = srcFrameWidth;
  unsigned const int hSize =srcFrameHeight/2;
  unsigned const int vSize =srcFrameWidth/2;
  unsigned const int lastRow=srcFrameHeight-1;
  unsigned const int lastCol=srcFrameWidth-1;
  unsigned int i,j;
  const BYTE *sBayer = src;

  //  Y = round( 0.256788 * R + 0.504129 * G + 0.097906 * B) +  16;
  //  Y = round( 0.30 * R + 0.59 * G + 0.11 * B ) use this!
  //  U = round(-0.148223 * R - 0.290993 * G + 0.439216 * B) + 128;
  //  V = round( 0.439216 * R - 0.367788 * G - 0.071427 * B) + 128;

  // Compute U and V planes using EXACT values, reading 2x2 pixels at a time
  BYTE *dU = dst+srcFrameHeight*srcFrameWidth;
  BYTE *dV = dU+hSize*vSize;
  for (i=0; i<hSize; i++) {      
    for (j=0; j<vSize; j++) {
      B=sBayer[0];
      G1=sBayer[1];
      G2=sBayer[stride];
      R=sBayer[stride+1];
      G=G1+G2;
      *dU = (BYTE)( ( (-19428 * R -19071*G +57569 * B) >> 17) + 128 );
      *dV = (BYTE)( ( ( 57569 * R -24103*G -9362 * B) >> 17) + 128 );
      sBayer+=2;
      dU++;
      dV++;
    }
    sBayer+=stride; // skip odd lines
  }
  // Compute Y plane
  BYTE *dY = dst;
  sBayer=src;
  const int * k; // kernel pointer
  int dxLeft, dxRight; // precalculated offsets, needed for first and last column
  const BYTE *sBayerTop, *sBayerBottom;
  for (i=0; i<srcFrameHeight; i++) {
    // Pointer to previous row, to the next if we are on the first one
    sBayerTop=sBayer+(i?(-stride):stride);
    // Pointer to next row, to the previous one if we are on the last
    sBayerBottom=sBayer+((i<lastRow)?stride:(-stride));
    // offset to previous column, to the next if we are on the first col
    dxLeft=1;
    for (j=0; j<srcFrameWidth; j++) {
      // offset to next column, to previous if we are on the last one
      dxRight=j<lastCol?1:(-1);
      // find the proper kernel according to the current pixel color
      if ( (i ^ j) & 1)  k=(j&1)?kG1:kG2; // green 1 or green 2
      else if (!(i & 1))  k=kB; // blue
      else /* if (!(j & 1)) */ k=kR; // red
      
      // apply the proper kernel to this pixel and surrounding ones
      *dY= (BYTE)(clip( (k[0])*(int)sBayerTop[dxLeft]+
      (k[1])*(int)(*sBayerTop)+
      (k[2])*(int)sBayerTop[dxRight]+
      (k[3])*(int)sBayer[dxLeft]+
      (k[4])*(int)(*sBayer)+
      (k[5])*(int)sBayer[dxRight]+
      (k[6])*(int)sBayerBottom[dxLeft]+
      (k[7])*(int)(*sBayerBottom)+
      (k[8])*(int)sBayerBottom[dxRight], (1<<24)) >> 16);
      dY++;
      sBayer++;
      sBayerTop++;
      sBayerBottom++;
      dxLeft=-1;
    }
  }

  if (bytesReturned)
    *bytesReturned = srcFrameHeight*srcFrameWidth+2*hSize*vSize;

  return true;

#else //USE_SBGGR8_NATIVE

  // shortest but less efficient (one malloc per conversion!)
  BYTE * tempDest=(BYTE*)malloc(3*srcFrameWidth*srcFrameHeight);
  SBGGR8toRGB(src, tempDest, NULL);
  BOOL r = RGBtoYUV420P(tempDest, dst, bytesReturned, 3, 2, 0);
  free(tempDest);
  return r;

#endif //USE_SBGGR8_NATIVE
}

BOOL PStandardColourConverter::SBGGR8toRGB(const BYTE * src,
                                           BYTE       * dst,
                                           PINDEX     * bytesReturned) const
{
  if (src == dst || verticalFlip)
    return FALSE;

  long int i;
  const BYTE *rawpt;
  BYTE *scanpt;
  long int size;

  rawpt = src;
  scanpt = dst;
  long int WIDTH = srcFrameWidth, HEIGHT = srcFrameHeight;
  size = WIDTH*HEIGHT;

  for ( i = 0; i < size; i++ ) {
    if ( (i/WIDTH) % 2 == 0 ) {
      if ( (i % 2) == 0 ) {
        /* B */
        if ( (i > WIDTH) && ((i % WIDTH) > 0) ) {
          *scanpt++ = (BYTE) ((*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+ *(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4);  /* R */
          *scanpt++ = (BYTE) ((*(rawpt-1)+*(rawpt+1)+ *(rawpt+WIDTH)+*(rawpt-WIDTH))/4);  /* G */
          *scanpt++ = *rawpt;         /* B */
        } else {
          /* first line or left column */
          *scanpt++ = *(rawpt+WIDTH+1);   /* R */
          *scanpt++ = (BYTE) ((*(rawpt+1)+*(rawpt+WIDTH))/2); /* G */
          *scanpt++ = *rawpt;       /* B */
        }
      } else {
        /* (B)G */
        if ( (i > WIDTH) && ((i % WIDTH) < (WIDTH-1)) ) {
          *scanpt++ = (BYTE) ((*(rawpt+WIDTH)+*(rawpt-WIDTH))/2); /* R */
          *scanpt++ = *rawpt;         /* G */
          *scanpt++ = (BYTE) ((*(rawpt-1)+*(rawpt+1))/2);   /* B */
        } else {
          /* first line or right column */
          *scanpt++ = *(rawpt+WIDTH); /* R */
          *scanpt++ = *rawpt;   /* G */
          *scanpt++ = *(rawpt-1); /* B */
        }
      }
    } else {
      if ( (i % 2) == 0 ) {
        /* G(R) */
        if ( (i < (WIDTH*(HEIGHT-1))) && ((i % WIDTH) > 0) ) {
          *scanpt++ = (BYTE) ((*(rawpt-1)+*(rawpt+1))/2);   /* R */
          *scanpt++ = *rawpt;         /* G */
          *scanpt++ = (BYTE) ((*(rawpt+WIDTH)+*(rawpt-WIDTH))/2); /* B */
        } else {
          /* bottom line or left column */
          *scanpt++ = *(rawpt+1);   /* R */
          *scanpt++ = *rawpt;         /* G */
          *scanpt++ = *(rawpt-WIDTH);   /* B */
        }
      } else {
        /* R */
        if ( i < (WIDTH*(HEIGHT-1)) && ((i % WIDTH) < (WIDTH-1)) ) {
          *scanpt++ = *rawpt;         /* R */
          *scanpt++ = (BYTE) ((*(rawpt-1)+*(rawpt+1)+*(rawpt-WIDTH)+*(rawpt+WIDTH))/4);  /* G */
          *scanpt++ = (BYTE) ((*(rawpt-WIDTH-1)+*(rawpt-WIDTH+1)+*(rawpt+WIDTH-1)+*(rawpt+WIDTH+1))/4);  /* B */
        } else {
          /* bottom line or right column */
          *scanpt++ = *rawpt;        /* R */
          *scanpt++ = (BYTE) ((*(rawpt-1)+*(rawpt-WIDTH))/2);  /* G */
          *scanpt++ = *(rawpt-WIDTH-1);    /* B */
        }
      }
    }
    rawpt++;
  }

  if (bytesReturned)
    *bytesReturned = scanpt - dst;

  return TRUE;
}

#define SCALEBITS 12
#define ONE_HALF  (1UL << (SCALEBITS - 1))
#define FIX(x)    ((int) ((x) * (1UL<<SCALEBITS) + 0.5))

/* 
 * Please note when converting colorspace from YUV to RGB.
 * Not all YUV have the same colorspace. 
 *
 * For instance Jpeg use this formula
 * YCbCr is defined per CCIR 601-1, except that Cb and Cr are
 * normalized to the range 0..MAXJSAMPLE rather than -0.5 .. 0.5.
 * The conversion equations to be implemented are therefore
 *      Y  =  0.29900 * R + 0.58700 * G + 0.11400 * B
 *      Cb = -0.16874 * R - 0.33126 * G + 0.50000 * B  + CENTERJSAMPLE
 *      Cr =  0.50000 * R - 0.41869 * G - 0.08131 * B  + CENTERJSAMPLE
 * (These numbers are derived from TIFF 6.0 section 21, dated 3-June-92.)
 * So
 * R = Y + 1.402 (Cr-128)
 * G = Y - 0.34414 (Cb-128) - 0.71414 (Cr-128)
 * B = Y + 1.772 (Cb-128)
 * 
 */
BOOL PStandardColourConverter::YUV420PtoRGB(const BYTE * srcFrameBuffer,
                                            BYTE * dstFrameBuffer,
                                            PINDEX * bytesReturned,
                                            unsigned rgbIncrement,
                                            unsigned redOffset,
                                            unsigned blueOffset) const
{
  if (srcFrameBuffer == dstFrameBuffer)
    return FALSE;

  BYTE          *dstImageFrame;
  unsigned int   nbytes    = srcFrameWidth*srcFrameHeight;
  const BYTE    *yplane    = srcFrameBuffer;               // 1 byte Y (luminance) for each pixel
  const BYTE    *uplane    = yplane+nbytes;                // 1 byte U for a block of 4 pixels
  const BYTE    *vplane    = uplane+(nbytes/4);            // 1 byte V for a block of 4 pixels

  unsigned int   pixpos[4] = {0, 1, srcFrameWidth, srcFrameWidth + 1};
  unsigned int   originalPixpos[4] = {0, 1, srcFrameWidth, srcFrameWidth + 1};

  unsigned int   x, y, p;

  long     int   yvalue;
  long     int   l, r, g, b;

  if (verticalFlip) {
    dstImageFrame = dstFrameBuffer + ((srcFrameHeight - 2) * srcFrameWidth * rgbIncrement);
    pixpos[0] = srcFrameWidth;
    pixpos[1] = srcFrameWidth +1;
    pixpos[2] = 0;
    pixpos[3] = 1;
  }
  else
    dstImageFrame = dstFrameBuffer;

  for (y = 0; y < srcFrameHeight; y += 2)
  {
    for (x = 0; x < srcFrameWidth; x += 2)
    {
      // The RGB value without luminance
      long cb = *uplane-128;
      long cr = *vplane-128;
      long rd = FIX(1.40200) * cr + ONE_HALF;
      long gd = -FIX(0.34414) * cb -FIX(0.71414) * cr + ONE_HALF;
      long bd = FIX(1.77200) * cb + ONE_HALF;

      // Add luminance to each of the 4 pixels

      for (p = 0; p < 4; p++)
      {
        yvalue = *(yplane + originalPixpos[p]);

        l = yvalue << SCALEBITS;

        r = (l+rd)>>SCALEBITS;
        g = (l+gd)>>SCALEBITS;
        b = (l+bd)>>SCALEBITS;

        BYTE * rgpPtr = dstImageFrame + rgbIncrement*pixpos[p];
        rgpPtr[redOffset ] = LIMIT(r);
        rgpPtr[1         ] = LIMIT(g);
        rgpPtr[blueOffset] = LIMIT(b);
        if (rgbIncrement == 4)
          rgpPtr[3] = 0;
      }

      yplane += 2;
      dstImageFrame += rgbIncrement*2;

      uplane++;
      vplane++;
    }
 
    yplane += srcFrameWidth;
    if (verticalFlip)
      dstImageFrame -= 3*rgbIncrement*srcFrameWidth;
    else
      dstImageFrame += rgbIncrement*srcFrameWidth;  
  }

  if (bytesReturned != NULL)
    *bytesReturned = dstFrameBytes;