mcomp.c

mediastreamer2是开源的网络传输媒体流的库

/********************************************************************
 *                                                                  *
 * THIS FILE IS PART OF THE OggTheora SOFTWARE CODEC SOURCE CODE.   *
 * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
 * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
 * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
 *                                                                  *
 * THE Theora SOURCE CODE IS COPYRIGHT (C) 2002-2003                *
 * by the Xiph.Org Foundation http://www.xiph.org/                  *
 *                                                                  *
 ********************************************************************

  function:
  last mod: $Id: mcomp.c 11442 2006-05-27 17:28:08Z giles $

 ********************************************************************/

#include <stdlib.h>
#include <stdio.h>
#include "dsp.h"
#include "codec_internal.h"

/* Initialises motion compentsation. */
void InitMotionCompensation ( CP_INSTANCE *cpi ){
  int i;
  int SearchSite=0;
  int Len;
  int LineStepY = (ogg_int32_t)cpi->pb.YStride;

  Len=((MAX_MV_EXTENT/2)+1)/2;


  /* How many search stages are there. */
  cpi->MVSearchSteps = 0;

  /* Set up offsets arrays used in half pixel correction. */
  cpi->HalfPixelRef2Offset[0] = -LineStepY - 1;
  cpi->HalfPixelRef2Offset[1] = -LineStepY;
  cpi->HalfPixelRef2Offset[2] = -LineStepY + 1;
  cpi->HalfPixelRef2Offset[3] = - 1;
  cpi->HalfPixelRef2Offset[4] = 0;
  cpi->HalfPixelRef2Offset[5] = 1;
  cpi->HalfPixelRef2Offset[6] = LineStepY - 1;
  cpi->HalfPixelRef2Offset[7] = LineStepY;
  cpi->HalfPixelRef2Offset[8] = LineStepY + 1;

  cpi->HalfPixelXOffset[0] = -1;
  cpi->HalfPixelXOffset[1] = 0;
  cpi->HalfPixelXOffset[2] = 1;
  cpi->HalfPixelXOffset[3] = -1;
  cpi->HalfPixelXOffset[4] = 0;
  cpi->HalfPixelXOffset[5] = 1;
  cpi->HalfPixelXOffset[6] = -1;
  cpi->HalfPixelXOffset[7] = 0;
  cpi->HalfPixelXOffset[8] = 1;

  cpi->HalfPixelYOffset[0] = -1;
  cpi->HalfPixelYOffset[1] = -1;
  cpi->HalfPixelYOffset[2] = -1;
  cpi->HalfPixelYOffset[3] = 0;
  cpi->HalfPixelYOffset[4] = 0;
  cpi->HalfPixelYOffset[5] = 0;
  cpi->HalfPixelYOffset[6] = 1;
  cpi->HalfPixelYOffset[7] = 1;
  cpi->HalfPixelYOffset[8] = 1;


  /* Generate offsets for 8 search sites per step. */
  while ( Len>0 ) {
    /* Another step. */
    cpi->MVSearchSteps += 1;

    /* Compute offsets for search sites. */
    cpi->MVOffsetX[SearchSite] = -Len;
    cpi->MVOffsetY[SearchSite++] = -Len;
    cpi->MVOffsetX[SearchSite] = 0;
    cpi->MVOffsetY[SearchSite++] = -Len;
    cpi->MVOffsetX[SearchSite] = Len;
    cpi->MVOffsetY[SearchSite++] = -Len;
    cpi->MVOffsetX[SearchSite] = -Len;
    cpi->MVOffsetY[SearchSite++] = 0;
    cpi->MVOffsetX[SearchSite] = Len;
    cpi->MVOffsetY[SearchSite++] = 0;
    cpi->MVOffsetX[SearchSite] = -Len;
    cpi->MVOffsetY[SearchSite++] = Len;
    cpi->MVOffsetX[SearchSite] = 0;
    cpi->MVOffsetY[SearchSite++] = Len;
    cpi->MVOffsetX[SearchSite] = Len;
    cpi->MVOffsetY[SearchSite++] = Len;

    /* Contract. */
    Len /= 2;
  }

  /* Compute pixel index offsets. */
  for ( i=SearchSite-1; i>=0; i-- )
    cpi->MVPixelOffsetY[i] = (cpi->MVOffsetY[i]*LineStepY) + cpi->MVOffsetX[i];
}

static ogg_uint32_t GetInterErr (CP_INSTANCE *cpi, unsigned char * NewDataPtr,
                          unsigned char * RefDataPtr1,
                          unsigned char * RefDataPtr2,
                          ogg_uint32_t PixelsPerLine ) {
  ogg_int32_t   DiffVal;
  ogg_int32_t   RefOffset = (int)(RefDataPtr1 - RefDataPtr2);
  ogg_uint32_t  RefPixelsPerLine = PixelsPerLine + STRIDE_EXTRA;

  /* Mode of interpolation chosen based upon on the offset of the
     second reference pointer */
  if ( RefOffset == 0 ) {
    DiffVal = dsp_inter8x8_err (cpi->dsp, NewDataPtr, PixelsPerLine,
		          RefDataPtr1, RefPixelsPerLine);
  }else{
    DiffVal = dsp_inter8x8_err_xy2 (cpi->dsp, NewDataPtr, PixelsPerLine,
		          RefDataPtr1, 
		          RefDataPtr2, RefPixelsPerLine);
  }

  /* Compute and return population variance as mis-match metric. */
  return DiffVal;
}

static ogg_uint32_t GetHalfPixelSumAbsDiffs (CP_INSTANCE *cpi,
                                      unsigned char * SrcData,
                                      unsigned char * RefDataPtr1,
                                      unsigned char * RefDataPtr2,
                                      ogg_uint32_t PixelsPerLine,
                                      ogg_uint32_t ErrorSoFar,
                                      ogg_uint32_t BestSoFar ) {

  ogg_uint32_t  DiffVal = ErrorSoFar;
  ogg_int32_t   RefOffset = (int)(RefDataPtr1 - RefDataPtr2);
  ogg_uint32_t  RefPixelsPerLine = PixelsPerLine + STRIDE_EXTRA;

  if ( RefOffset == 0 ) {
    /* Simple case as for non 0.5 pixel */
    DiffVal += dsp_sad8x8 (cpi->dsp, SrcData, PixelsPerLine, 
		               RefDataPtr1, RefPixelsPerLine);
  } else  {
    DiffVal += dsp_sad8x8_xy2_thres (cpi->dsp, SrcData, PixelsPerLine, 
		               RefDataPtr1, 
		               RefDataPtr2, RefPixelsPerLine, BestSoFar);
  }

  return DiffVal;
}

ogg_uint32_t GetMBIntraError (CP_INSTANCE *cpi, ogg_uint32_t FragIndex,
                              ogg_uint32_t PixelsPerLine ) {
  ogg_uint32_t  LocalFragIndex = FragIndex;
  ogg_uint32_t  IntraError = 0;

  dsp_save_fpu (cpi->dsp);

  /* Add together the intra errors for those blocks in the macro block
     that are coded (Y only) */
  if ( cpi->pb.display_fragments[LocalFragIndex] )
    IntraError +=
      dsp_intra8x8_err (cpi->dsp, &cpi->
                    ConvDestBuffer[cpi->pb.pixel_index_table[LocalFragIndex]],
                    PixelsPerLine);

  LocalFragIndex++;
  if ( cpi->pb.display_fragments[LocalFragIndex] )
    IntraError +=
      dsp_intra8x8_err (cpi->dsp, &cpi->
                    ConvDestBuffer[cpi->pb.pixel_index_table[LocalFragIndex]],
                    PixelsPerLine);

  LocalFragIndex = FragIndex + cpi->pb.HFragments;
  if ( cpi->pb.display_fragments[LocalFragIndex] )
    IntraError +=
      dsp_intra8x8_err (cpi->dsp, &cpi->
                     ConvDestBuffer[cpi->pb.pixel_index_table[LocalFragIndex]],
                    PixelsPerLine);

  LocalFragIndex++;
  if ( cpi->pb.display_fragments[LocalFragIndex] )
    IntraError +=
      dsp_intra8x8_err (cpi->dsp, &cpi->
                    ConvDestBuffer[cpi->pb.pixel_index_table[LocalFragIndex]],
                    PixelsPerLine);

  dsp_restore_fpu (cpi->dsp);

  return IntraError;
}

ogg_uint32_t GetMBInterError (CP_INSTANCE *cpi,
                              unsigned char * SrcPtr,
                              unsigned char * RefPtr,
                              ogg_uint32_t FragIndex,
                              ogg_int32_t LastXMV,
                              ogg_int32_t LastYMV,
                              ogg_uint32_t PixelsPerLine ) {
  ogg_uint32_t  RefPixelsPerLine = cpi->pb.YStride;
  ogg_uint32_t  LocalFragIndex = FragIndex;
  ogg_int32_t   PixelIndex;
  ogg_int32_t   RefPixelIndex;
  ogg_int32_t   RefPixelOffset;
  ogg_int32_t   RefPtr2Offset;

  ogg_uint32_t  InterError = 0;

  unsigned char * SrcPtr1;
  unsigned char * RefPtr1;

  dsp_save_fpu (cpi->dsp);

  /* Work out pixel offset into source buffer. */
  PixelIndex = cpi->pb.pixel_index_table[LocalFragIndex];

  /* Work out the pixel offset in reference buffer for the default
     motion vector */
  RefPixelIndex = cpi->pb.recon_pixel_index_table[LocalFragIndex];
  RefPixelOffset = ((LastYMV/2) * RefPixelsPerLine) + (LastXMV/2);

  /* Work out the second reference pointer offset. */
  RefPtr2Offset = 0;
  if ( LastXMV % 2 ) {
    if ( LastXMV > 0 )
      RefPtr2Offset += 1;
    else
      RefPtr2Offset -= 1;
  }
  if ( LastYMV % 2 ) {
    if ( LastYMV > 0 )
      RefPtr2Offset += RefPixelsPerLine;
    else
      RefPtr2Offset -= RefPixelsPerLine;
  }

  /* Add together the errors for those blocks in the macro block that
     are coded (Y only) */
  if ( cpi->pb.display_fragments[LocalFragIndex] ) {
    SrcPtr1 = &SrcPtr[PixelIndex];
    RefPtr1 = &RefPtr[RefPixelIndex + RefPixelOffset];
    InterError += GetInterErr(cpi, SrcPtr1, RefPtr1,
                                 &RefPtr1[RefPtr2Offset], PixelsPerLine );
  }

  LocalFragIndex++;
  if ( cpi->pb.display_fragments[LocalFragIndex] ) {
    PixelIndex = cpi->pb.pixel_index_table[LocalFragIndex];
    RefPixelIndex = cpi->pb.recon_pixel_index_table[LocalFragIndex];
    SrcPtr1 = &SrcPtr[PixelIndex];
    RefPtr1 = &RefPtr[RefPixelIndex + RefPixelOffset];
    InterError += GetInterErr(cpi, SrcPtr1, RefPtr1,
                                 &RefPtr1[RefPtr2Offset], PixelsPerLine );

  }

  LocalFragIndex = FragIndex + cpi->pb.HFragments;
  if ( cpi->pb.display_fragments[LocalFragIndex] ) {
    PixelIndex = cpi->pb.pixel_index_table[LocalFragIndex];
    RefPixelIndex = cpi->pb.recon_pixel_index_table[LocalFragIndex];
    SrcPtr1 = &SrcPtr[PixelIndex];
    RefPtr1 = &RefPtr[RefPixelIndex + RefPixelOffset];
    InterError += GetInterErr(cpi, SrcPtr1, RefPtr1,
                                 &RefPtr1[RefPtr2Offset], PixelsPerLine );
  }

  LocalFragIndex++;
  if ( cpi->pb.display_fragments[LocalFragIndex] ) {
    PixelIndex = cpi->pb.pixel_index_table[LocalFragIndex];
    RefPixelIndex = cpi->pb.recon_pixel_index_table[LocalFragIndex];
    SrcPtr1 = &SrcPtr[PixelIndex];
    RefPtr1 = &RefPtr[RefPixelIndex + RefPixelOffset];
    InterError += GetInterErr(cpi, SrcPtr1, RefPtr1,
                                 &RefPtr1[RefPtr2Offset], PixelsPerLine );
  }

  dsp_restore_fpu (cpi->dsp);

  return InterError;
}

ogg_uint32_t GetMBMVInterError (CP_INSTANCE *cpi,
                                unsigned char * RefFramePtr,
                                ogg_uint32_t FragIndex,
                                ogg_uint32_t PixelsPerLine,
                                ogg_int32_t *MVPixelOffset,
                                MOTION_VECTOR *MV ) {
  ogg_uint32_t  Error = 0;
  ogg_uint32_t  MinError;
  ogg_uint32_t  InterMVError = 0;

  ogg_int32_t   i;
  ogg_int32_t   x=0, y=0;
  ogg_int32_t   step;
  ogg_int32_t   SearchSite=0;

  unsigned char *SrcPtr[4] = {NULL,NULL,NULL,NULL};
  unsigned char *RefPtr=NULL;
  unsigned char *CandidateBlockPtr=NULL;
  unsigned char *BestBlockPtr=NULL;

  ogg_uint32_t  RefRow2Offset = cpi->pb.YStride * 8;

  int    MBlockDispFrags[4];

  /* Half pixel variables */
  ogg_int32_t   HalfPixelError;
  ogg_int32_t   BestHalfPixelError;
  unsigned char   BestHalfOffset;
  unsigned char * RefDataPtr1;
  unsigned char * RefDataPtr2;

  dsp_save_fpu (cpi->dsp);

  /* Note which of the four blocks in the macro block are to be
     included in the search. */
  MBlockDispFrags[0] =
    cpi->pb.display_fragments[FragIndex];
  MBlockDispFrags[1] =
    cpi->pb.display_fragments[FragIndex + 1];
  MBlockDispFrags[2] =
    cpi->pb.display_fragments[FragIndex + cpi->pb.HFragments];
  MBlockDispFrags[3] =
    cpi->pb.display_fragments[FragIndex + cpi->pb.HFragments + 1];

  /* Set up the source pointers for the four source blocks.  */
  SrcPtr[0] = &cpi->ConvDestBuffer[cpi->pb.pixel_index_table[FragIndex]];
  SrcPtr[1] = SrcPtr[0] + 8;
  SrcPtr[2] = SrcPtr[0] + (PixelsPerLine * 8);
  SrcPtr[3] = SrcPtr[2] + 8;

  /* Set starting reference point for search. */
  RefPtr = &RefFramePtr[cpi->pb.recon_pixel_index_table[FragIndex]];

  /* Check the 0,0 candidate. */
  if ( MBlockDispFrags[0] ) {
    Error += dsp_sad8x8 (cpi->dsp, SrcPtr[0], PixelsPerLine, RefPtr,
                         PixelsPerLine + STRIDE_EXTRA);
  }
  if ( MBlockDispFrags[1] ) {
    Error += dsp_sad8x8 (cpi->dsp, SrcPtr[1], PixelsPerLine, RefPtr + 8,
                         PixelsPerLine + STRIDE_EXTRA);
  }
  if ( MBlockDispFrags[2] ) {
    Error += dsp_sad8x8 (cpi->dsp, SrcPtr[2], PixelsPerLine, RefPtr + RefRow2Offset,
                         PixelsPerLine + STRIDE_EXTRA);
  }
  if ( MBlockDispFrags[3] ) {
    Error += dsp_sad8x8 (cpi->dsp, SrcPtr[3], PixelsPerLine, RefPtr + RefRow2Offset + 8,
                         PixelsPerLine + STRIDE_EXTRA);
  }

  /* Set starting values to results of 0, 0 vector. */
  MinError = Error;
  BestBlockPtr = RefPtr;
  x = 0;
  y = 0;
  MV->x = 0;
  MV->y = 0;

  /* Proceed through N-steps. */
  for (  step=0; step<cpi->MVSearchSteps; step++ ) {
    /* Search the 8-neighbours at distance pertinent to current step.*/
    for ( i=0; i<8; i++ ) {
      /* Set pointer to next candidate matching block. */
      CandidateBlockPtr = RefPtr + MVPixelOffset[SearchSite];

      /* Reset error */
      Error = 0;

      /* Get the score for the current offset */
      if ( MBlockDispFrags[0] ) {
        Error += dsp_sad8x8 (cpi->dsp, SrcPtr[0], PixelsPerLine, CandidateBlockPtr,
                             PixelsPerLine + STRIDE_EXTRA);
      }

      if ( MBlockDispFrags[1] && (Error < MinError) ) {
        Error += dsp_sad8x8_thres (cpi->dsp, SrcPtr[1], PixelsPerLine, CandidateBlockPtr + 8,
                             PixelsPerLine + STRIDE_EXTRA, MinError);
      }

      if ( MBlockDispFrags[2] && (Error < MinError) ) {
        Error += dsp_sad8x8_thres (cpi->dsp, SrcPtr[2], PixelsPerLine, CandidateBlockPtr + RefRow2Offset,
                             PixelsPerLine + STRIDE_EXTRA, MinError);
      }

      if ( MBlockDispFrags[3] && (Error < MinError) ) {
        Error += dsp_sad8x8_thres (cpi->dsp, SrcPtr[3], PixelsPerLine, CandidateBlockPtr + RefRow2Offset + 8,
                             PixelsPerLine + STRIDE_EXTRA, MinError);