motion_est.c

用MPEG-4对YUV视频文件编码压缩成divx视频文件

		xin1 = x - 1;
		yin1 = y;
		vec1 = 3;
		xin2 = x;
		yin2 = y;
		vec2 = 0;
		xin3 = x;
		yin3 = y;
		vec3 = 1;
		break;
	default:
		xin1 = x;
		yin1 = y;
		vec1 = 2;
		xin2 = x;
		yin2 = y;
		vec2 = 0;
		xin3 = x;
		yin3 = y;
		vec3 = 1;
	}


	if (xin1 < 0 || xin1 >= (int32_t) x_dim) {
		pmv[1] = zeroMV;
	} else {
		pmv[1] = pMBs[xin1 + yin1 * x_dim].mvs[vec1];
	}

	if (xin2 < 0 || xin2 >= (int32_t) x_dim) {
		pmv[2] = zeroMV;
	} else {
		pmv[2] = pMBs[xin2 + yin2 * x_dim].mvs[vec2];
	}

	if (xin3 < 0 || xin3 >= (int32_t) x_dim) {
		pmv[3] = zeroMV;
	} else {
		pmv[3] = pMBs[xin3 + yin3 * x_dim].mvs[vec3];
	}

	if ((MVequal(pmv[1], pmv[2])) && (MVequal(pmv[1], pmv[3]))) {
		pmv[0] = pmv[1];
		return 1;
	}

	/* median,minimum */

	pmv[0].x =
		min(max(pmv[1].x, pmv[2].x),
			min(max(pmv[2].x, pmv[3].x), max(pmv[1].x, pmv[3].x)));
	pmv[0].y =
		min(max(pmv[1].y, pmv[2].y),
			min(max(pmv[2].y, pmv[3].y), max(pmv[1].y, pmv[3].y)));

	return 0;
}

/*static int prev_mv_available=0;*/

/*#   define  abs(x)                (((x) < 0) ? -(x) : (x))*/
int SAD16_count=0;
int  SAD8_count=0;
float factor1,factor2; 
int first_P_seq;
int first_P_gop;

/* add predict median value point check and early quit */


static int amvfastSearch16(
				   const uint8_t * const pRef,
                   const uint8_t * const pRefH,
				   const uint8_t * const pRefV,
                   const uint8_t * const pRefHV,
                   const IMAGE * const pCur,
				   const int x,/* macroblock coordinate */
				   const int y,
				   const uint32_t MotionFlags,
				   const int32_t iQuant,
				   const uint32_t iFcode,
				   const MBParam * const pParam,
				   const MACROBLOCK * const pMBs,
				   const MACROBLOCK * const prevMBs,
				   VECTOR * const currMV,
				   VECTOR * const currPMV)
{
	/*int blocksize=16;*//* block size:16 or 8 */
    const uint32_t iWcount = pParam->mb_width;
	const int32_t iWidth = pParam->width;
	const int32_t iHeight = pParam->height;
	const int32_t iEdgedWidth = pParam->edged_width;
	const uint8_t *cur = pCur->y + x * 16 + y * 16 * iEdgedWidth;
	/*const uint8_t *cur = pCur->y + x * blocksize + y * blocksize * iEdgedWidth;*/
    VECTOR pmv[4];/* half pixel offset */
	VECTOR mvs[5];
	int pmv_x,pmv_y;
	int cand_x,cand_y;

	const MACROBLOCK * const pMB = pMBs + x + y * iWcount;  
	const MACROBLOCK *const prevMB = prevMBs + x + y * iWcount;

/*  int     i,j;*/
  int     sad, sad_min;
  int     mv_x;
  int     mv_y;

  int     min_dx, max_dx,
          min_dy, max_dy;/* half pixel motion vector range */

/*  int     pos16;*/
  /*int     mvm_width   = iWidth/blocksize;*/
/*    int     mvm_width   = iWidth/16;*/

     int pass_flag; /* SAD calculated flag for point in prediction */
     int pass_x,pass_y; /* SAD calculated point in prediction*/
/*	 int SAD_th1=500;
	 int SAD_th2=750;*/
     int SAD_th1;
	 int SAD_th2;
	 int V_count;
	 int ii;
	 int skip_flag;
	 int same_mv;
/*     float factor1=(float)1.05,factor2=(float)1.5; */
	 int more_loop=0,new_min,eightORfour=8;
/* ------------------------------DS-----------t-----------------------*/
       typedef struct
       {
		int x;
		int y;
		int start_nmbr;	
       } DPoint;

       typedef struct
       {
		DPoint point[8];
       } Diamond;

       int d_type=1,stop_flag=0,pt_nmbr=0,check_pts,total_check_pts=8,mot_dirn=0;
       int d_centre_x,d_centre_y,check_pt_x,check_pt_y;/* integer pixel */

#ifdef HalfPixel_Inter
//       Diamond diamond[2]={{{{0,1,3},{1,0,0},{0,-1,1},{-1,0,2}}},{{{0,2,6},{1,1,0},{2,0,0},{1,-1,2},{0,-2,2},{-1,-1,4},{-2,0,4},{-1,1,6}}}};
//	   Diamond diamond[2]={{{{0,1,3},{1,0,0},{0,-1,1},{-1,0,2}}},{{{0,4,6},{2,2,0},{4,0,0},{2,-2,2},{0,-4,2},{-2,-2,4},{-4,0,4},{-2,2,6}}}};
	   Diamond diamond[2]={{{{0,1,3},{1,0,0},{0,-1,1},{-1,0,2}}},{{{0,2,6},{2,2,0},{2,0,0},{2,-2,2},{0,-2,2},{-2,-2,4},{-2,0,4},{-2,2,6}}}};
#endif

#ifndef HalfPixel_Inter
	   Diamond diamond[2]={{{{0,2,3},{2,0,0},{0,-2,1},{-2,0,2}}},{{{0,4,6},{2,2,0},{4,0,0},{2,-2,2},{0,-4,2},{-2,-2,4},{-4,0,4},{-2,2,6}}}};
#endif

/*----------------------------------DS-----------b-------------------------*/
	   /* fyh add below code(2002.12.22):
          Saved vector and sad for the block of the same position 
		  in previous frame.
       */
	   int prev_sad;
	   int prev_mvx,prev_mvy;

       if(first_P_seq) 	     /* first P frame in sequence */
	   {
		 prev_sad=512;
		 prev_mvx=0;
		 prev_mvy=0;
	   }
	   else if(first_P_gop)  /* first P frame in gop */
	   {
         prev_sad=pMB->sad16; 
		 prev_mvx=pMB->mv16.x;
		 prev_mvy=pMB->mv16.y;
	   }
	   else                  /* other P frame */
	   {
         prev_sad=prevMB->sad16;
 		 prev_mvx=prevMB->mv16.x;
		 prev_mvy=prevMB->mv16.y;
	   }

  /* Get maximum range */
  get_range(&min_dx, &max_dx, &min_dy, &max_dy, x, y, 16, iWidth, iHeight,iFcode);
//	   min_dx=-32;
//	   max_dx=32;
//	   min_dy=-32;
//	   max_dy=32;

  /* 获得中值法的运动矢量 */ 
  /*pmv=get_pmv(pMBs, x, y, iWcount, 0);*/
  same_mv=get_pmvdata_amv(pMBs, x, y, iWcount, 0, pmv);
  
  cand_x=pmv_x=pmv[0].x;
  cand_y=pmv_y=pmv[0].y;
  /* Calculate SAD around the Median prediction. 
     If Motion Vector equal to Previous frame motion vector 
     and SAD<PrevFrmSAD goto half pixel refine. 
     If SAD<=256 goto Step half pixel refine. 
  */
  	if (cand_x > max_dx) {
		cand_x = max_dx;
	}
	if (cand_x < min_dx) {
		cand_x = min_dx;
	}
	if (cand_y > max_dy) {
		cand_y = max_dy;
	}
	if (cand_y < min_dy) {
		cand_y = min_dy;
	}

    check_pt_x=mv_x=pass_x=cand_x;
    check_pt_y=mv_y=pass_y=cand_y;
    sad_min =
		sad16(cur,get_ref(pRef, pRefH, pRefV, pRefHV, x, y, 16,cand_x,cand_y,
						 iEdgedWidth), iEdgedWidth, MV_MAX_ERROR);
    
	SAD16_count++;
	sad_min +=
		calc_delta_16(cand_x - pmv_x, cand_y - pmv_y,iFcode, iQuant);

	
/*    if ((sad_min < 256) || ((cand_x == prevMB->mv16.x && cand_y == prevMB->mv16.y)
								&& ( sad_min <prevMB->sad16)))*/
    if ((sad_min < 256) || ((cand_x == prev_mvx && cand_y == prev_mvy)
								&& ( sad_min <prev_sad)))
	{
		if(sad_min < 2 * iQuant)	/* high chances for SKIP-mode */
		{
            if(cand_x || cand_y) 
			{
				sad = sad16( cur, get_ref(pRef, pRefH, pRefV, pRefHV, x, y, 16, 0, 0 , iEdgedWidth), iEdgedWidth, MV_MAX_ERROR); 
				SAD16_count++;
				sad += calc_delta_16(-pmv_x, -pmv_y, (uint8_t)iFcode, iQuant);
				if (sad <= iQuant * 96) 	sad -= MV16_00_BIAS;
				if (sad <sad_min) 
				{
					sad_min=sad;
					mv_x=0;
					mv_y=0;	
					
				}
			}
		}
		/*  currMV->x=mv_x;
        currMV->y=mv_y;*/
		goto Amvfast16_Refine;
	}
/*
  if(!x && !y)
  {
	prev_mv_available++;
	if(prev_mv_available>1)
	{
	   prev_mv_available=2;
	}
  }
*/
	/* Resize SAD_th */
  /*current frame don't first P frame*/
	/*if(prev_mv_available>1)
	{*/
/*      sad=prevMB->sad16; */
	  /*if(sad<250)
	  {
		SAD_th1=300;
		SAD_th2=450;
	  }*/
/*	  if(sad<512)
	  {
		  SAD_th1=512;
		  SAD_th2=768;
	  }
	  else
	  {
		SAD_th1=(int) (sad * factor1);
		SAD_th2=(int) (sad * factor2);
	  }*/

	/*}*/
	  if(prev_sad<512)
	  {
		  SAD_th1=512;
		  SAD_th2=768;
	  }
	  else
	  {
		SAD_th1=(int) (prev_sad * factor1);
		SAD_th2=(int) (prev_sad * factor2);
	  }

  /* save (0,0) point */
  mvs[0].x=0;
  mvs[0].y=0;
  V_count=1;

  /* save left block motion vector */
  cand_x=pmv[1].x;
  cand_y=pmv[1].y;
  if(cand_x || cand_y)
  {
    mvs[1].x=cand_x;
    mvs[1].y=cand_y;
    V_count=2;
  }

  if(same_mv)
  {
	  /* left,top,righttop block motion vector equal */
	  /*current frame don't first P frame*/
	  /*if(prev_mv_available>1)
	  {*/
		  /*cand_x=prevMB->mv16.x;
		  cand_y=prevMB->mv16.y;*/
	      cand_x=prev_mvx;
		  cand_y=prev_mvy;
	      if(cand_x==pmv[1].x && cand_y==pmv[1].y)
		     same_mv=1;/* left top righttop previous_frame block motion vector equal */
    	  else
		  {
      	     /* save previous frame block motion vetor */
		     same_mv=0;
			 if(cand_x || cand_y)
			 {
	           mvs[V_count].x=cand_x;
	           mvs[V_count].y=cand_y;
               V_count++;
			 }
		  }
	  /*}*/
  }
  else
  {
	  /* save top block motion vector */
   	  skip_flag=0;
      cand_x=pmv[2].x;
	  cand_y=pmv[2].y;
	  for(ii=0;ii<V_count;ii++)
		if(cand_x==mvs[ii].x &&cand_y==mvs[ii].y)
		{
			skip_flag=1;
			break;
		}
	  if(!skip_flag)
	  {
			mvs[V_count].x= cand_x;
			mvs[V_count].y= cand_y;
			V_count++;
	  }

	  /* save righttop block motion vector */
   	  skip_flag=0;
      cand_x=pmv[3].x;
	  cand_y=pmv[3].y;
	  for(ii=0;ii<V_count;ii++)
		if(cand_x==mvs[ii].x && cand_y==mvs[ii].y)
		{
			skip_flag=1;
			break;
		}
	  if(!skip_flag)
	  {
			mvs[V_count].x= cand_x;
			mvs[V_count].y= cand_y;
			V_count++;
	  }
	  
	  /* save previous frame block motion vetor */
   	  skip_flag=0;
/*	  cand_x=prevMB->mv16.x;
	  cand_y=prevMB->mv16.y;*/
      cand_x=prev_mvx;
	  cand_y=prev_mvy;

	  for(ii=0;ii<V_count;ii++)
		if(cand_x ==mvs[ii].x &&cand_y ==mvs[ii].y)
		{
			skip_flag=1;
			break;
		}
	  if(!skip_flag)
	  {
			mvs[V_count].x= cand_x;
			mvs[V_count].y= cand_y;
			V_count++;
	  }      
  
  }/* same_mv */

  /* decide initial search mode */
  if(same_mv)/*SDS MODE*/
  {
	    d_type=0;
    	total_check_pts=4;
	    more_loop=1;
  }

  /*decide the starting point of searching */
  pass_flag=0;
  for(ii=0;ii<V_count;ii++)
  {
    cand_x=mvs[ii].x;
	cand_y=mvs[ii].y;
	
	if(!(cand_x==check_pt_x && cand_y== check_pt_y ))
	{
	   if (cand_x < min_dx || cand_x > max_dx || cand_y < min_dy || cand_y > max_dy)
		 continue;
	   else
	   {
		   pass_flag=1;
		   pass_x=cand_x;
		   pass_y=cand_y;
         
		   sad =
/*		      sad16(cur,get_ref_mv(pRef, pRefH, pRefV, pRefHV, x, y, 16, currMV,
					  iEdgedWidth), iEdgedWidth, sad_min);*/
		      sad16(cur,get_ref(pRef, pRefH, pRefV, pRefHV, x, y, 16, cand_x,cand_y,
					  iEdgedWidth), iEdgedWidth, sad_min);

	       SAD16_count++;

		   if(sad<sad_min)
		   {
              sad +=
		      calc_delta_16(cand_x - pmv_x, cand_y - pmv_y,iFcode, iQuant);

		     if(sad<sad_min)
			 {
			    sad_min=sad;
			    mv_x=cand_x;
			    mv_y=cand_y;
			 }
			 else if ( sad==sad_min && (abs(cand_x)+abs(cand_y))<(abs(mv_x)+abs(mv_y)) ) 
             {
             	sad_min=sad;
             	mv_x=cand_x;
             	mv_y=cand_y;
             }
            
		   }
	   }/* End of else(....) */
   }/* End of if(..) */
  }/* end of for */

  /* convert half pixel to interge pixel */
  /*mv_x>>=1;
  mv_y>>=1;
  pass_x>>=1;
  pass_y>>=1;*/

  if(mv_x || mv_y)
  {
	 pass_x=0;
	 pass_y=0;
  }
  else/* best candidate is (0,0) point */
  {
	if(sad_min <= iQuant * 96)	
       sad_min -= MV16_00_BIAS;
  }

/* ------------------------------DS t----------------------------------*/
	d_centre_x=mv_x;
	d_centre_y=mv_y;

	do
	{
		check_pts=total_check_pts;
		new_min=0;
		do
		{
			/* TYM add 2000.04.27   half-stop by using SAD threshold */
			if(sad_min<SAD_th1)
			{
				d_type=0;
				break;
			}
			else
			{
				if(sad_min<SAD_th2 && d_type!=0)/*again sds*/
				{
					more_loop=1;
					d_centre_x=mv_x;
					d_centre_y=mv_y;
					break;
				}
			}
			check_pt_x = diamond[d_type].point[pt_nmbr].x + d_centre_x;
			check_pt_y = diamond[d_type].point[pt_nmbr].y + d_centre_y;
            /*cand_x=check_pt_x<<1;
            cand_y=check_pt_y<<1;	*/
			if( check_pt_x < min_dx || check_pt_x > max_dx || check_pt_y < min_dy || check_pt_y > max_dy)
			{
				sad = MV_MAX_ERROR;
			}
            else
				if(!(pass_flag && check_pt_x==pass_x && check_pt_y==pass_y ))
				{
					
					/*sad = dist1(ref+(i0+check_pt_x)+lx*(j0+check_pt_y),blk,lx,h,sad_min);*/
					sad =
						sad16(cur,
						/*			  get_ref_mv(pRef, pRefH, pRefV, pRefHV, x, y, 16, currMV,
						iEdgedWidth), iEdgedWidth, sad_min);*/
						get_ref(pRef, pRefH, pRefV, pRefHV, x, y, 16, check_pt_x,check_pt_y,
						iEdgedWidth), iEdgedWidth, sad_min);
					
					SAD16_count++;
					if(sad<sad_min)
					{
						sad +=
						/*		      calc_delta_16(currMV->x - pmv[0].x, currMV->y - pmv[0].y,
						(uint8_t) iFcode, iQuant);*/
						calc_delta_16(check_pt_x - pmv_x, check_pt_y - pmv_y,iFcode, iQuant);
						
						if (sad<sad_min)
						{
							sad_min=sad;
							mv_x=check_pt_x;
							mv_y=check_pt_y;
							
							mot_dirn=pt_nmbr;
							new_min=1;
						}
						/* fyh comment code by 2002.12.16*/
						else if (sad==sad_min)
							if((abs(check_pt_x)+abs(check_pt_y)) < (abs(mv_x)+abs(mv_y)))
							{
								sad_min=sad;
								mv_x=check_pt_x;
								mv_y=check_pt_y;
								mot_dirn=pt_nmbr;
							}
					}
				}
				pt_nmbr+=1;
				if((pt_nmbr)>= eightORfour) pt_nmbr-=eightORfour;
				check_pts-=1;
	      	}
	      	while(check_pts>0);/*decide one pass diamond search stop or not*/
			/* check one pass search result	*/
			if( d_type == 0)/*sds*/
			{
				if(more_loop==0 || new_min==0)
					stop_flag = 1;
				else
				{
					total_check_pts=3;
					eightORfour=4;
					pt_nmbr=diamond[d_type].point[mot_dirn].start_nmbr;
					d_centre_x = mv_x;
					d_centre_y = mv_y;
				}
				
			}