Exemplo n.º 1
0
n0Texture::n0Texture(u32 texID, u32 texWidth, u32 texHeight, u32 bpp, u32 colorspace)
	:width(texWidth), height(texHeight), textureID(texID), bpp(bpp), colorspace(colorspace)
{
	//glEnable(GL_TEXTURE_2D);
	while( glGetError());
	glGenTextures(1,&glID);
	glBindTexture(GL_TEXTURE_2D, glID);
	Pow2(texWidth);
	Pow2(texHeight);
	u32 val = 0xff00ff00;
	u32 val2 = 0xff0000FF;
	u32 * fakeData = (u32*)malloc(texWidth*texHeight * sizeof(u32));

	for(u32 y = 0; y < texHeight ; y++)
	{
		if(y%16 == 0)
		{
			SWAP(val,val2);
		}
		for(u32 x = 0; x < texWidth ; x++)
		{
			if(x%16 == 0)
			{
				SWAP(val,val2);
			}
			fakeData[y*texWidth + x] = val;
	
		}

	}

	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST); // Linear Filtering
	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST); // Linear Filtering
	
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, fakeData);


	//n0_FlushGLError();
	 u32 err; 
	 while(err = glGetError()) 
	 { 
		 printf("GL ERROR #%d - %s:%d",err,__FILE__,__LINE__);
	 };
//	 glDisable(GL_TEXTURE_2D);
}
Exemplo n.º 2
0
double Track_RMSE(Wml::Vector2d &point_2d, Wml::Matrix4d &P, 
				  Wml::Matrix3d &K,
				  Wml::Vector3d &v_3d)
{
	Wml::Vector3d pt3d;

	pt3d[0] = P(0,0)*v_3d[0]+P(0,1)*v_3d[1]+P(0,2)*v_3d[2]+P(0,3);
	pt3d[1] = P(1,0)*v_3d[0]+P(1,1)*v_3d[1]+P(1,2)*v_3d[2]+P(1,3);
	pt3d[2] = P(2,0)*v_3d[0]+P(2,1)*v_3d[1]+P(2,2)*v_3d[2]+P(2,3);
	pt3d[0] /= pt3d[2];
	pt3d[1] /= pt3d[2];
	pt3d[2] = 1.0;


	double RMSE = Pow2(K(0, 0)*pt3d[0] + K(0, 2) - point_2d.X()) + Pow2(K(1, 1) * pt3d[1] + K(1, 2) - point_2d.Y());
	

	RMSE = sqrt(RMSE);

	return RMSE;
}
Exemplo n.º 3
0
ID opzMiniIdMgr::GetId()
{
	ID i, count;
	if(_binTree[1] >= Pow2(_level - 1))
	{
		DoubleGrow();
	}
	
	//본격적으로 빈 id를 찾습니다.
	for(i = 1u, count = 0u; count < _level - 1; count++)
	{
		_binTree[i]++;
		if(_binTree[2u * i] < Pow2(_level - count - 2U))
		{	//왼쪽
			i *= 2u;
		}
		else
		{	//오른쪽
			i = 2u * i + 1u;
		}
	}
	_binTree[i] = 1u;
	return i - Pow2(_level - 1u) + 1u;
}
int main () {
    while(1){
             gets(In);
             if(In[0] == '0') break;
             int len = strlen(In);
             int Tmp = len;
             Ans = 0;
             for(int i = 0; i < len; i++){
                     int t = In[i] - '0';
                     Ans = Ans + (t * (Pow2(Tmp) - 1));
                     Tmp--;
                     }
             printf("%d\n",Ans);
             }
    return 0;
    }
Exemplo n.º 5
0
void opzMiniIdMgr::SetId(ID id)
{
	while(Pow2(_level - 1) < id)
	{
		//level이 충분하지 못하다.
		DoubleGrow();
	}

	ID index = GetTreeIndex(id);
	if(index == 0u) return;
	if(_binTree[index] >= 1u)
	{
//		assert(L"이미 할당 된 ID를 또 Set했습니다.");
		return;
	}
	else
	{
		while(index > 0u)
		{
			++_binTree[index];
			index /= 2u;
		}
	}
}
Exemplo n.º 6
0
//=============================================================================
//函数名称:Dec_gain
//函数功能:解码的音调和码书增益
//=============================================================================
void Dec_gain(
    gc_predState *pred_state, /* i/o: MA predictor state           */
    enum Mode mode,           /* i  : AMR mode                     */
    Word16 index,             /* i  : index of quantization.       */
    Word16 code[],            /* i  : Innovative vector.           */
    Word16 evenSubfr,         /* i  : Flag for even subframes      */
    Word16 * gain_pit,        /* o  : Pitch gain.                  */
    Word16 * gain_cod,        /* o  : Code gain.                   */
    Flag   * pOverflow
)
{
    const Word16 *p;
    Word16 frac;
    Word16 gcode0;
    Word16 exp;
    Word16 qua_ener;
    Word16 qua_ener_MR122;
    Word16 g_code;
    Word32 L_tmp;
    Word16 temp1;
    Word16 temp2;

    /* Read the quantized gains (table depends on mode) */
    //阅读量化收益(表取决于模式)
    index = shl(index, 2, pOverflow);

    if (mode == MR102 || mode == MR74 || mode == MR67)
    {
        p = &table_gain_highrates[index];

        *gain_pit = *p++;
        g_code = *p++;
        qua_ener_MR122 = *p++;
        qua_ener = *p;
    }
    else
    {
        if (mode == MR475)
        {
            index += (1 ^ evenSubfr) << 1; /* evenSubfr is 0 or 1 */

            if (index > (MR475_VQ_SIZE*4 - 2))
            {
                index = (MR475_VQ_SIZE * 4 - 2); /* avoid possible buffer overflow */
            }

            p = &table_gain_MR475[index];

            *gain_pit = *p++;
            g_code = *p++;

            /*---------------------------------------------------------*
             *  calculate predictor update values (not stored in 4.75  *
             *  quantizer table to save space):                        *
             *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~  *
             *                                                         *
             *   qua_ener       = log2(g)                              *
             *   qua_ener_MR122 = 20*log10(g)                          *
             *---------------------------------------------------------*/
            //计算预测更新值(不存储在4.75量化表,以节省空间)

            /* Log2(x Q12) = log2(x) + 12 */
            temp1 = (Word16) L_deposit_l(g_code);
            Log2(temp1, &exp, &frac, pOverflow);
            exp = sub(exp, 12, pOverflow);

            temp1 = shr_r(frac, 5, pOverflow);
            temp2 = shl(exp, 10, pOverflow);
            qua_ener_MR122 = add(temp1, temp2, pOverflow);

            /* 24660 Q12 ~= 6.0206 = 20*log10(2) */
            L_tmp = Mpy_32_16(exp, frac, 24660, pOverflow);
            L_tmp = L_shl(L_tmp, 13, pOverflow);
            qua_ener = pv_round(L_tmp, pOverflow);
            /* Q12 * Q0 = Q13 -> Q10 */
        }
        else
        {
            p = &table_gain_lowrates[index];

            *gain_pit = *p++;
            g_code = *p++;
            qua_ener_MR122 = *p++;
            qua_ener = *p;
        }
    }

    /*-------------------------------------------------------------------*
     *  predict codebook gain                                            *
     *  ~~~~~~~~~~~~~~~~~~~~~                                            *
     *  gc0     = Pow2(int(d)+frac(d))                                   *
     *          = 2^exp + 2^frac                                         *
     *                                                                   *
     *  gcode0 (Q14) = 2^14*2^frac = gc0 * 2^(14-exp)                    *
     *-------------------------------------------------------------------*/

    gc_pred(pred_state, mode, code, &exp, &frac, NULL, NULL, pOverflow);

    gcode0 = (Word16) Pow2(14, frac, pOverflow);

    /*------------------------------------------------------------------*
     *  read quantized gains, update table of past quantized energies   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *  st->past_qua_en(Q10) = 20 * Log10(g_fac) / constant             *
     *                       = Log2(g_fac)                              *
     *                       = qua_ener                                 *
     *                                           constant = 20*Log10(2) *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0, pOverflow);
    temp1 = sub(10, exp, pOverflow);
    L_tmp = L_shr(L_tmp, temp1, pOverflow);
    *gain_cod = extract_h(L_tmp);

    /* update table of past quantized energies */
    //过去的量化能量表更新

    gc_pred_update(pred_state, qua_ener_MR122, qua_ener);

    return;
}
Exemplo n.º 7
0
	void solve(void) {
		Input();
		GetPrime();
		Work();
		Pow2();
	}
Exemplo n.º 8
0
GrGeomExtentArray  *GrGeomCreateExtentArray(
                                            SubgridArray *subgrids,
                                            int           xl_ghost,
                                            int           xu_ghost,
                                            int           yl_ghost,
                                            int           yu_ghost,
                                            int           zl_ghost,
                                            int           zu_ghost)
{
  Background         *bg = GlobalsBackground;

  GrGeomExtentArray  *extent_array;
  GrGeomExtents      *extents;
  int size;

  Subgrid            *subgrid;

  int ref;
  int bg_ix, bg_iy, bg_iz;
  int bg_nx, bg_ny, bg_nz;
  int is;


  size = SubgridArraySize(subgrids);
  extents = ctalloc(GrGeomExtents, size);

  ForSubgridI(is, subgrids)
  {
    subgrid = SubgridArraySubgrid(subgrids, is);

    /* compute background grid extents on MaxRefLevel index space */
    ref = (int)pow(2.0, GlobalsMaxRefLevel);
    bg_ix = BackgroundIX(bg) * ref;
    bg_iy = BackgroundIY(bg) * ref;
    bg_iz = BackgroundIZ(bg) * ref;
    bg_nx = BackgroundNX(bg) * ref;
    bg_ny = BackgroundNY(bg) * ref;
    bg_nz = BackgroundNZ(bg) * ref;

    ref = (int)Pow2(GlobalsMaxRefLevel);

    /*------------------------------------------
     * set the lower extent values
     *------------------------------------------*/

    if (xl_ghost > -1)
    {
      xl_ghost = pfmax(xl_ghost, 1);
      GrGeomExtentsIXLower(extents[is]) =
        (SubgridIX(subgrid) - xl_ghost) * ref;
    }
    else
    {
      GrGeomExtentsIXLower(extents[is]) = bg_ix;
    }

    if (yl_ghost > -1)
    {
      yl_ghost = pfmax(yl_ghost, 1);
      GrGeomExtentsIYLower(extents[is]) =
        (SubgridIY(subgrid) - yl_ghost) * ref;
    }
    else
    {
      GrGeomExtentsIYLower(extents[is]) = bg_iy;
    }

    if (zl_ghost > -1)
    {
      zl_ghost = pfmax(zl_ghost, 1);
      GrGeomExtentsIZLower(extents[is]) =
        (SubgridIZ(subgrid) - zl_ghost) * ref;
    }
    else
    {
      GrGeomExtentsIZLower(extents[is]) = bg_iz;
    }

    /*------------------------------------------
     * set the upper extent values
     *------------------------------------------*/

    if (xu_ghost > -1)
    {
      xu_ghost = pfmax(xu_ghost, 1);
      GrGeomExtentsIXUpper(extents[is]) =
        (SubgridIX(subgrid) + SubgridNX(subgrid) + xu_ghost) * ref - 1;
    }
    else
    {
      GrGeomExtentsIXUpper(extents[is]) = bg_ix + bg_nx - 1;
    }

    if (yu_ghost > -1)
    {
      yu_ghost = pfmax(yu_ghost, 1);
      GrGeomExtentsIYUpper(extents[is]) =
        (SubgridIY(subgrid) + SubgridNY(subgrid) + yu_ghost) * ref - 1;
    }
    else
    {
      GrGeomExtentsIYUpper(extents[is]) = bg_iy + bg_ny - 1;
    }

    if (zu_ghost > -1)
    {
      zu_ghost = pfmax(zu_ghost, 1);
      GrGeomExtentsIZUpper(extents[is]) =
        (SubgridIZ(subgrid) + SubgridNZ(subgrid) + zu_ghost) * ref - 1;
    }
    else
    {
      GrGeomExtentsIZUpper(extents[is]) = bg_iz + bg_nz - 1;
    }

    /*------------------------------------------
     * convert to "octree coordinates"
     *------------------------------------------*/

    /* Moved into the loop by SGS 7/8/98, was lying outside the is
     * loop which was an error (accessing invalid array elements)
     */

    GrGeomExtentsIXLower(extents[is]) -= bg_ix;
    GrGeomExtentsIYLower(extents[is]) -= bg_iy;
    GrGeomExtentsIZLower(extents[is]) -= bg_iz;
    GrGeomExtentsIXUpper(extents[is]) -= bg_ix;
    GrGeomExtentsIYUpper(extents[is]) -= bg_iy;
    GrGeomExtentsIZUpper(extents[is]) -= bg_iz;
  }
Exemplo n.º 9
0
/*************************************************************************
 *
 *   FUNCTION:  Dec_gain()
 *
 *   PURPOSE: Decode the pitch and codebook gains
 *
 ************************************************************************/
void Dec_gain(
    gc_predState *pred_state, /* i/o: MA predictor state           */
    enum Mode mode,           /* i  : AMR mode                     */
    Word16 index,             /* i  : index of quantization.       */
    Word16 code[],            /* i  : Innovative vector.           */
    Word16 evenSubfr,         /* i  : Flag for even subframes      */     
    Word16 * gain_pit,        /* o  : Pitch gain.                  */
    Word16 * gain_cod         /* o  : Code gain.                   */
)
{
    const Word16 *p;
    Word16 frac, gcode0, exp, qua_ener, qua_ener_MR122;
    Word16 g_code;
    Word32 L_tmp;
    
    /* Read the quantized gains (table depends on mode) */
    index = shl (index, 2);
    
    test(); test(); test();
    if (    sub (mode, MR102) == 0
         || sub (mode, MR74) == 0
         || sub (mode, MR67) == 0)
    {
        p = &table_gain_highrates[index];                  move16 ();
        
        *gain_pit = *p++;                                  move16 ();
        g_code = *p++;                                     move16 ();
        qua_ener_MR122 = *p++;                             move16 ();
        qua_ener = *p;                                     move16 ();
    }
    else
    {
        test();
        if (sub (mode, MR475) == 0)
        {
            index = add (index, shl(sub(1, evenSubfr), 1));
            p = &table_gain_MR475[index];                  move16 ();
            
            *gain_pit = *p++;                              move16 ();
            g_code = *p++;                                 move16 ();
            
            /*---------------------------------------------------------*
             *  calculate predictor update values (not stored in 4.75  *
             *  quantizer table to save space):                        *
             *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~  *
             *                                                         *
             *   qua_ener       = log2(g)                              *
             *   qua_ener_MR122 = 20*log10(g)                          *
             *---------------------------------------------------------*/

            /* Log2(x Q12) = log2(x) + 12 */
            Log2 (L_deposit_l (g_code), &exp, &frac); 
            exp = sub(exp, 12);
    
            qua_ener_MR122 = add (shr_r (frac, 5), shl (exp, 10));
    
            /* 24660 Q12 ~= 6.0206 = 20*log10(2) */
            L_tmp = Mpy_32_16(exp, frac, 24660);
            qua_ener = round (L_shl (L_tmp, 13)); /* Q12 * Q0 = Q13 -> Q10 */
        }
        else
        {
            p = &table_gain_lowrates[index];                move16 ();
            
            *gain_pit = *p++;                               move16 ();
            g_code = *p++;                                  move16 ();
            qua_ener_MR122 = *p++;                          move16 ();
            qua_ener = *p;                                  move16 ();
        }
    }
    
    /*-------------------------------------------------------------------*
     *  predict codebook gain                                            *
     *  ~~~~~~~~~~~~~~~~~~~~~                                            *
     *  gc0     = Pow2(int(d)+frac(d))                                   *
     *          = 2^exp + 2^frac                                         *
     *                                                                   *
     *  gcode0 (Q14) = 2^14*2^frac = gc0 * 2^(14-exp)                    *
     *-------------------------------------------------------------------*/

    gc_pred(pred_state, mode, code, &exp, &frac, NULL, NULL);

    gcode0 = extract_l(Pow2(14, frac));

    /*------------------------------------------------------------------*
     *  read quantized gains, update table of past quantized energies   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *  st->past_qua_en(Q10) = 20 * Log10(g_fac) / constant             *
     *                       = Log2(g_fac)                              *
     *                       = qua_ener                                 *
     *                                           constant = 20*Log10(2) *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0);
    L_tmp = L_shr(L_tmp, sub(10, exp));
    *gain_cod = extract_h(L_tmp);

    /* update table of past quantized energies */

    gc_pred_update(pred_state, qua_ener_MR122, qua_ener);

    return;
}
Exemplo n.º 10
0
Word16
Qua_gain(                   /* o  : index of quantization.                 */
    enum Mode mode,         /* i  : AMR mode                               */
    Word16 exp_gcode0,      /* i  : predicted CB gain (exponent),      Q0  */
    Word16 frac_gcode0,     /* i  : predicted CB gain (fraction),      Q15 */
    Word16 frac_coeff[],    /* i  : energy coeff. (5), fraction part,  Q15 */
    Word16 exp_coeff[],     /* i  : energy coeff. (5), exponent part,  Q0  */
    /*      (frac_coeff and exp_coeff computed in  */
    /*       calc_filt_energies())                 */
    Word16 gp_limit,        /* i  : pitch gain limit                       */
    Word16 *gain_pit,       /* o  : Pitch gain,                        Q14 */
    Word16 *gain_cod,       /* o  : Code gain,                         Q1  */
    Word16 *qua_ener_MR122, /* o  : quantized energy error,            Q10 */
    /*      (for MR122 MA predictor update)        */
    Word16 *qua_ener,       /* o  : quantized energy error,            Q10 */
    /*      (for other MA predictor update)        */
    CommonAmrTbls* common_amr_tbls, /* i : ptr to struct of tables ptrs    */
    Flag   *pOverflow       /* o  : overflow indicator                     */
)
{
    const Word16 *p;
    Word16 i;
    Word16 j;
    Word16 index = 0;
    Word16 gcode0;
    Word16 e_max;
    Word16 temp;
    Word16 exp_code;
    Word16 g_pitch;
    Word16 g2_pitch;
    Word16 g_code;
    Word16 g2_code;
    Word16 g_pit_cod;
    Word16 coeff[5];
    Word16 coeff_lo[5];
    Word16 exp_max[5];
    Word32 L_tmp;
    Word32 L_tmp2;
    Word32 dist_min;
    const Word16 *table_gain;
    Word16 table_len;

    if (mode == MR102 || mode == MR74 || mode == MR67)
    {
        table_len = VQ_SIZE_HIGHRATES;
        table_gain = common_amr_tbls->table_gain_highrates_ptr;
    }
    else
    {
        table_len = VQ_SIZE_LOWRATES;
        table_gain = common_amr_tbls->table_gain_lowrates_ptr;
    }

    /*-------------------------------------------------------------------*
     *  predicted codebook gain                                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~                                          *
     *  gc0     = 2^exp_gcode0 + 2^frac_gcode0                           *
     *                                                                   *
     *  gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0)      *
     *-------------------------------------------------------------------*/

    gcode0 = (Word16)(Pow2(14, frac_gcode0, pOverflow));

    /*-------------------------------------------------------------------*
     *  Scaling considerations:                                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~                                          *
     *-------------------------------------------------------------------*/

    /*
     * The error energy (sum) to be minimized consists of five terms, t[0..4].
     *
     *                      t[0] =    gp^2  * <y1 y1>
     *                      t[1] = -2*gp    * <xn y1>
     *                      t[2] =    gc^2  * <y2 y2>
     *                      t[3] = -2*gc    * <xn y2>
     *                      t[4] =  2*gp*gc * <y1 y2>
     *
     */

    /* determine the scaling exponent for g_code: ec = ec0 - 11 */
    exp_code = exp_gcode0 - 11;

    /* calculate exp_max[i] = s[i]-1 */
    exp_max[0] = exp_coeff[0] - 13;
    exp_max[1] = exp_coeff[1] - 14;

    temp = shl(exp_code, 1, pOverflow);
    temp += 15;
    exp_max[2] = add_16(exp_coeff[2], temp, pOverflow);

    exp_max[3] = add_16(exp_coeff[3], exp_code, pOverflow);

    temp = exp_code + 1;
    exp_max[4] = add_16(exp_coeff[4], temp, pOverflow);


    /*-------------------------------------------------------------------*
     *  Find maximum exponent:                                           *
     *  ~~~~~~~~~~~~~~~~~~~~~~                                           *
     *                                                                   *
     *  For the sum operation, all terms must have the same scaling;     *
     *  that scaling should be low enough to prevent overflow. There-    *
     *  fore, the maximum scale is determined and all coefficients are   *
     *  re-scaled:                                                       *
     *                                                                   *
     *    e_max = max(exp_max[i]) + 1;                                   *
     *    e = exp_max[i]-e_max;         e <= 0!                          *
     *    c[i] = c[i]*2^e                                                *
     *-------------------------------------------------------------------*/

    e_max = exp_max[0];
    for (i = 1; i < 5; i++)
    {
        if (exp_max[i] > e_max)
        {
            e_max = exp_max[i];
        }
    }

    e_max++;

    for (i = 0; i < 5; i++)
    {
        j = e_max - exp_max[i];
        L_tmp = ((Word32)frac_coeff[i] << 16);
        L_tmp = L_shr(L_tmp, j, pOverflow);
        L_Extract(L_tmp, &coeff[i], &coeff_lo[i], pOverflow);
    }


    /*-------------------------------------------------------------------*
     *  Codebook search:                                                 *
     *  ~~~~~~~~~~~~~~~~                                                 *
     *                                                                   *
     *  For each pair (g_pitch, g_fac) in the table calculate the        *
     *  terms t[0..4] and sum them up; the result is the mean squared    *
     *  error for the quantized gains from the table. The index for the  *
     *  minimum MSE is stored and finally used to retrieve the quantized *
     *  gains                                                            *
     *-------------------------------------------------------------------*/

    /* start with "infinite" MSE */
    dist_min = MAX_32;

    p = &table_gain[0];

    for (i = 0; i < table_len; i++)
    {
        g_pitch = *p++;
        g_code = *p++;                   /* this is g_fac        */
        p++;                             /* skip log2(g_fac)     */
        p++;                             /* skip 20*log10(g_fac) */

        if (g_pitch <= gp_limit)
        {
            g_code = mult(g_code, gcode0, pOverflow);
            g2_pitch = mult(g_pitch, g_pitch, pOverflow);
            g2_code = mult(g_code, g_code, pOverflow);
            g_pit_cod = mult(g_code, g_pitch, pOverflow);

            L_tmp = Mpy_32_16(coeff[0], coeff_lo[0], g2_pitch, pOverflow);
            L_tmp2 = Mpy_32_16(coeff[1], coeff_lo[1], g_pitch, pOverflow);
            L_tmp = L_add(L_tmp, L_tmp2, pOverflow);

            L_tmp2 = Mpy_32_16(coeff[2], coeff_lo[2], g2_code, pOverflow);
            L_tmp = L_add(L_tmp, L_tmp2, pOverflow);

            L_tmp2 =  Mpy_32_16(coeff[3], coeff_lo[3], g_code, pOverflow);
            L_tmp = L_add(L_tmp, L_tmp2, pOverflow);

            L_tmp2 = Mpy_32_16(coeff[4], coeff_lo[4], g_pit_cod, pOverflow);
            L_tmp = L_add(L_tmp, L_tmp2, pOverflow);

            /* store table index if MSE for this index is lower
               than the minimum MSE seen so far */
            if (L_tmp < dist_min)
            {
                dist_min = L_tmp;
                index = i;
            }
        }
    }

    /*------------------------------------------------------------------*
     *  read quantized gains and new values for MA predictor memories   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *------------------------------------------------------------------*/

    /* Read the quantized gains */
    p = &table_gain[shl(index, 2, pOverflow)];
    *gain_pit = *p++;
    g_code = *p++;
    *qua_ener_MR122 = *p++;
    *qua_ener = *p;

    /*------------------------------------------------------------------*
     *  calculate final fixed codebook gain:                            *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                            *
     *                                                                  *
     *   gc = gc0 * g                                                   *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0, pOverflow);
    temp  = 10 - exp_gcode0;
    L_tmp = L_shr(L_tmp, temp, pOverflow);

    *gain_cod = (Word16)(L_tmp >> 16);

    return index;
}
Exemplo n.º 11
0
/*************************************************************************
 *
 * FUNCTION:  Qua_gain()
 *
 * PURPOSE: Quantization of pitch and codebook gains.
 *          (using predicted codebook gain)
 *
 *************************************************************************/
Word16
Qua_gain(                   /* o  : index of quantization.                 */   
    enum Mode mode,         /* i  : AMR mode                               */
    Word16 exp_gcode0,      /* i  : predicted CB gain (exponent),      Q0  */
    Word16 frac_gcode0,     /* i  : predicted CB gain (fraction),      Q15 */
    Word16 frac_coeff[],    /* i  : energy coeff. (5), fraction part,  Q15 */
    Word16 exp_coeff[],     /* i  : energy coeff. (5), exponent part,  Q0  */
                            /*      (frac_coeff and exp_coeff computed in  */
                            /*       calc_filt_energies())                 */
    Word16 gp_limit,        /* i  : pitch gain limit                       */
    Word16 *gain_pit,       /* o  : Pitch gain,                        Q14 */
    Word16 *gain_cod,       /* o  : Code gain,                         Q1  */
    Word16 *qua_ener_MR122, /* o  : quantized energy error,            Q10 */
                            /*      (for MR122 MA predictor update)        */
    Word16 *qua_ener        /* o  : quantized energy error,            Q10 */
                            /*      (for other MA predictor update)        */
)
{
    const Word16 *p;
    Word16 i, j, index = 0;
    Word16 gcode0, e_max, exp_code;
    Word16 g_pitch, g2_pitch, g_code, g2_code, g_pit_cod;
    Word16 coeff[5], coeff_lo[5];
    Word16 exp_max[5];
    Word32 L_tmp, dist_min;
    const Word16 *table_gain;
    Word16 table_len;
    

    if ( sub (mode, MR102) == 0 || sub (mode, MR74) == 0 || sub (mode, MR67) == 0)
    {
       table_len = VQ_SIZE_HIGHRATES;
       table_gain = table_gain_highrates;
    }
    else
    {
       table_len = VQ_SIZE_LOWRATES;
       table_gain = table_gain_lowrates;
    }
    
    /*-------------------------------------------------------------------*
     *  predicted codebook gain                                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~                                          *
     *  gc0     = 2^exp_gcode0 + 2^frac_gcode0                           *
     *                                                                   *
     *  gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0)      *
     *-------------------------------------------------------------------*/

    gcode0 = extract_l(Pow2(14, frac_gcode0));

    /*-------------------------------------------------------------------*
     *  Scaling considerations:                                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~                                          *
     *-------------------------------------------------------------------*/

    /*
     * The error energy (sum) to be minimized consists of five terms, t[0..4].
     *
     *                      t[0] =    gp^2  * <y1 y1>
     *                      t[1] = -2*gp    * <xn y1>
     *                      t[2] =    gc^2  * <y2 y2>
     *                      t[3] = -2*gc    * <xn y2>
     *                      t[4] =  2*gp*gc * <y1 y2>
     *
     */

    /* determine the scaling exponent for g_code: ec = ec0 - 11 */
    exp_code = sub(exp_gcode0, 11);

    /* calculate exp_max[i] = s[i]-1 */
    exp_max[0] = sub(exp_coeff[0], 13);
    exp_max[1] = sub(exp_coeff[1], 14);
    exp_max[2] = add(exp_coeff[2], add(15, shl(exp_code, 1)));
    exp_max[3] = add(exp_coeff[3], exp_code);
    exp_max[4] = add(exp_coeff[4], add(1, exp_code));


    /*-------------------------------------------------------------------*
     *  Find maximum exponent:                                           *
     *  ~~~~~~~~~~~~~~~~~~~~~~                                           *
     *                                                                   *
     *  For the sum operation, all terms must have the same scaling;     *
     *  that scaling should be low enough to prevent overflow. There-    *
     *  fore, the maximum scale is determined and all coefficients are   *
     *  re-scaled:                                                       *
     *                                                                   *
     *    e_max = max(exp_max[i]) + 1;                                   *
     *    e = exp_max[i]-e_max;         e <= 0!                          *
     *    c[i] = c[i]*2^e                                                *
     *-------------------------------------------------------------------*/

    e_max = exp_max[0];
    for (i = 1; i < 5; i++)
    {

        if (sub(exp_max[i], e_max) > 0)
        {
            e_max = exp_max[i];
        }
    }

    e_max = add(e_max, 1);      /* To avoid overflow */

    for (i = 0; i < 5; i++) {
        j = sub(e_max, exp_max[i]);
        L_tmp = L_deposit_h(frac_coeff[i]);
        L_tmp = L_shr(L_tmp, j);
        L_Extract(L_tmp, &coeff[i], &coeff_lo[i]);
    }


    /*-------------------------------------------------------------------*
     *  Codebook search:                                                 *
     *  ~~~~~~~~~~~~~~~~                                                 *
     *                                                                   *
     *  For each pair (g_pitch, g_fac) in the table calculate the        *
     *  terms t[0..4] and sum them up; the result is the mean squared    *
     *  error for the quantized gains from the table. The index for the  *
     *  minimum MSE is stored and finally used to retrieve the quantized *
     *  gains                                                            *
     *-------------------------------------------------------------------*/

    /* start with "infinite" MSE */
    dist_min = MAX_32;

    p = &table_gain[0];

    for (i = 0; i < table_len; i++)
    {
        g_pitch = *p++;
        g_code = *p++;         /* this is g_fac        */
        p++;                             /* skip log2(g_fac)     */
        p++;                             /* skip 20*log10(g_fac) */
            

        if (sub(g_pitch, gp_limit) <= 0)
        {
            g_code = mult(g_code, gcode0);
            g2_pitch = mult(g_pitch, g_pitch);
            g2_code = mult(g_code, g_code);
            g_pit_cod = mult(g_code, g_pitch);

            L_tmp = Mpy_32_16(coeff[0], coeff_lo[0], g2_pitch);
            L_tmp = L_add(L_tmp, Mpy_32_16(coeff[1], coeff_lo[1], g_pitch));
            L_tmp = L_add(L_tmp, Mpy_32_16(coeff[2], coeff_lo[2], g2_code));
            L_tmp = L_add(L_tmp, Mpy_32_16(coeff[3], coeff_lo[3], g_code));
            L_tmp = L_add(L_tmp, Mpy_32_16(coeff[4], coeff_lo[4], g_pit_cod));

            /* store table index if MSE for this index is lower
               than the minimum MSE seen so far */

            if (L_sub(L_tmp, dist_min) < (Word32) 0)
            {
                dist_min = L_tmp;
                index = i;
            }
        }
    }

    /*------------------------------------------------------------------*
     *  read quantized gains and new values for MA predictor memories   *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~   *
     *------------------------------------------------------------------*/

    /* Read the quantized gains */
    p = &table_gain[shl (index, 2)];
    *gain_pit = *p++;
    g_code = *p++;
    *qua_ener_MR122 = *p++;
    *qua_ener = *p;

    /*------------------------------------------------------------------*
     *  calculate final fixed codebook gain:                            *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                            *
     *                                                                  *
     *   gc = gc0 * g                                                   *
     *------------------------------------------------------------------*/

    L_tmp = L_mult(g_code, gcode0);
    L_tmp = L_shr(L_tmp, sub(10, exp_gcode0));
    *gain_cod = extract_h(L_tmp);

    return index;
}
Exemplo n.º 12
0
//! Returns the distance between the two given 2D points.
template <typename T> force_inline T getDistance(const point2d<T> &PosA, const point2d<T> &PosB)
{
    return Sqrt(Pow2(PosB.X - PosA.X) + Pow2(PosB.Y - PosA.Y));
}
Exemplo n.º 13
0
/*
**************************************************************************
*
*  Function    : dtx_dec
*                
**************************************************************************
*/
int dtx_dec(
   dtx_decState *st,                /* i/o : State struct                    */
   Word16 mem_syn[],                /* i/o : AMR decoder state               */
   D_plsfState* lsfState,           /* i/o : decoder lsf states              */
   gc_predState* predState,         /* i/o : prediction states               */
   Cb_gain_averageState* averState, /* i/o : CB gain average states          */
   enum DTXStateType new_state,     /* i   : new DTX state                   */
   enum Mode mode,                  /* i   : AMR mode                        */
   Word16 parm[],                   /* i   : Vector of synthesis parameters  */
   Word16 synth[],                  /* o   : synthesised speech              */
   Word16 A_t[]                     /* o   : decoded LP filter in 4 subframes*/
   )
{
   Word16 log_en_index;
   Word16 i, j;
   Word16 int_fac;
   Word32 L_log_en_int;
   Word16 lsp_int[M];
   Word16 log_en_int_e;
   Word16 log_en_int_m;
   Word16 level;
   Word16 acoeff[M + 1];
   Word16 refl[M];
   Word16 pred_err;
   Word16 ex[L_SUBFR];
   Word16 ma_pred_init;
   Word16 log_pg_e, log_pg_m;
   Word16 log_pg;
   Flag negative;
   Word16 lsf_mean;
   Word32 L_lsf_mean;
   Word16 lsf_variab_index;
   Word16 lsf_variab_factor;
   Word16 lsf_int[M];
   Word16 lsf_int_variab[M];
   Word16 lsp_int_variab[M];
   Word16 acoeff_variab[M + 1];

   Word16 lsf[M];
   Word32 L_lsf[M];
   Word16 ptr;
   Word16 tmp_int_length;


   /*  This function is called if synthesis state is not SPEECH 
    *  the globally passed  inputs to this function are 
    * st->sid_frame 
    * st->valid_data 
    * st->dtxHangoverAdded
    * new_state  (SPEECH, DTX, DTX_MUTE)
    */

   test(); test();
   if ((st->dtxHangoverAdded != 0) && 
       (st->sid_frame != 0))
   {
      /* sid_first after dtx hangover period */
      /* or sid_upd after dtxhangover        */

      /* set log_en_adjust to correct value */
      st->log_en_adjust = dtx_log_en_adjust[mode];
          
      ptr = add(st->lsf_hist_ptr, M);                               move16(); 
      test();
      if (sub(ptr, 80) == 0)
      {
         ptr = 0;                                                   move16();
      }
      Copy( &st->lsf_hist[st->lsf_hist_ptr],&st->lsf_hist[ptr],M); 
      
      ptr = add(st->log_en_hist_ptr,1);                             move16();
      test();
      if (sub(ptr, DTX_HIST_SIZE) == 0)
      {
         ptr = 0;                                                   move16();
      }
      move16();
      st->log_en_hist[ptr] = st->log_en_hist[st->log_en_hist_ptr]; /* Q11 */
      
      /* compute mean log energy and lsp *
       * from decoded signal (SID_FIRST) */         
      st->log_en = 0;                                               move16();
      for (i = 0; i < M; i++)
      {
         L_lsf[i] = 0;                                              move16();
      }
      
      /* average energy and lsp */
      for (i = 0; i < DTX_HIST_SIZE; i++)
      {
         st->log_en = add(st->log_en,
                          shr(st->log_en_hist[i],3));
         for (j = 0; j < M; j++)
         {
            L_lsf[j] = L_add(L_lsf[j],
                             L_deposit_l(st->lsf_hist[i * M + j]));
         }
      }
       
      for (j = 0; j < M; j++)
      {
         lsf[j] = extract_l(L_shr(L_lsf[j],3)); /* divide by 8 */  move16();
      }
      
      Lsf_lsp(lsf, st->lsp, M); 

      /* make log_en speech coder mode independent */
      /* added again later before synthesis        */
      st->log_en = sub(st->log_en, st->log_en_adjust);

      /* compute lsf variability vector */
      Copy(st->lsf_hist, st->lsf_hist_mean, 80);

      for (i = 0; i < M; i++)
      {
         L_lsf_mean = 0;                                           move32();
         /* compute mean lsf */
         for (j = 0; j < 8; j++)
         {
            L_lsf_mean = L_add(L_lsf_mean, 
                               L_deposit_l(st->lsf_hist_mean[i+j*M]));
         }
         
         lsf_mean = extract_l(L_shr(L_lsf_mean, 3));               move16();
         /* subtract mean and limit to within reasonable limits  *
          * moreover the upper lsf's are attenuated              */
         for (j = 0; j < 8; j++)
         {
            /* subtract mean */ 
            st->lsf_hist_mean[i+j*M] = 
               sub(st->lsf_hist_mean[i+j*M], lsf_mean);

            /* attenuate deviation from mean, especially for upper lsf's */
            st->lsf_hist_mean[i+j*M] = 
               mult(st->lsf_hist_mean[i+j*M], lsf_hist_mean_scale[i]);

            /* limit the deviation */
            test();
            if (st->lsf_hist_mean[i+j*M] < 0)
            {
               negative = 1;                                        move16();
            }
            else
            {
               negative = 0;                                        move16();
            }
            st->lsf_hist_mean[i+j*M] = abs_s(st->lsf_hist_mean[i+j*M]);

            /* apply soft limit */
            test();
            if (sub(st->lsf_hist_mean[i+j*M], 655) > 0)
            {
               st->lsf_hist_mean[i+j*M] = 
                  add(655, shr(sub(st->lsf_hist_mean[i+j*M], 655), 2));
            }
            
            /* apply hard limit */
            test();
            if (sub(st->lsf_hist_mean[i+j*M], 1310) > 0)
            {
               st->lsf_hist_mean[i+j*M] = 1310;                     move16();
            }
            test();
            if (negative != 0) 
            {
               st->lsf_hist_mean[i+j*M] = -st->lsf_hist_mean[i+j*M];move16();
            }
            
         }
      }
   }
   
   test();
   if (st->sid_frame != 0 )
   {
      /* Set old SID parameters, always shift */
      /* even if there is no new valid_data   */
      Copy(st->lsp, st->lsp_old, M);
      st->old_log_en = st->log_en;                                  move16();

      test();
      if (st->valid_data != 0 )  /* new data available (no CRC) */
      {
         /* Compute interpolation factor, since the division only works *
          * for values of since_last_sid < 32 we have to limit the      *
          * interpolation to 32 frames                                  */
         tmp_int_length = st->since_last_sid;                       move16();
         st->since_last_sid = 0;                                    move16();

         test();
         if (sub(tmp_int_length, 32) > 0)
         {
            tmp_int_length = 32;                                    move16();
         }
         test();
         if (sub(tmp_int_length, 2) >= 0)
         {
            move16();
            st->true_sid_period_inv = div_s(1 << 10, shl(tmp_int_length, 10)); 
         }
         else
         {
            st->true_sid_period_inv = 1 << 14; /* 0.5 it Q15 */     move16();
         }
         
         Init_D_plsf_3(lsfState, parm[0]);  /* temporay initialization */ 
         D_plsf_3(lsfState, MRDTX, 0, &parm[1], st->lsp);
         Set_zero(lsfState->past_r_q, M);   /* reset for next speech frame */ 

         log_en_index = parm[4];                                    move16();
         /* Q11 and divide by 4 */
         st->log_en = shl(log_en_index, (11 - 2));                  move16();
         
         /* Subtract 2.5 in Q11 */
         st->log_en = sub(st->log_en, (2560 * 2));
         
         /* Index 0 is reserved for silence */
         test();
         if (log_en_index == 0)
         {
            st->log_en = MIN_16;                                    move16();
         }
         
         /* no interpolation at startup after coder reset        */
         /* or when SID_UPD has been received right after SPEECH */
         test(); test();
         if ((st->data_updated == 0) ||
             (sub(st->dtxGlobalState, SPEECH) == 0)
             ) 
         {
            Copy(st->lsp, st->lsp_old, M);
            st->old_log_en = st->log_en;                            move16();
         }         
      } /* endif valid_data */

      /* initialize gain predictor memory of other modes */       
      ma_pred_init = sub(shr(st->log_en,1), 9000);                  move16();
      test();
      if (ma_pred_init > 0)
      {                   
         ma_pred_init = 0;                                          move16();  
      }      
      test();
      if (sub(ma_pred_init, -14436) < 0)
      {
         ma_pred_init = -14436;                                     move16();
      }
      
      predState->past_qua_en[0] = ma_pred_init;                     move16();
      predState->past_qua_en[1] = ma_pred_init;                     move16();
      predState->past_qua_en[2] = ma_pred_init;                     move16();
      predState->past_qua_en[3] = ma_pred_init;                     move16();

      /* past_qua_en for other modes than MR122 */      
      ma_pred_init = mult(5443, ma_pred_init); 
      /* scale down by factor 20*log10(2) in Q15 */
      predState->past_qua_en_MR122[0] = ma_pred_init;               move16();
      predState->past_qua_en_MR122[1] = ma_pred_init;               move16();
      predState->past_qua_en_MR122[2] = ma_pred_init;               move16();
      predState->past_qua_en_MR122[3] = ma_pred_init;               move16();
   } /* endif sid_frame */
   
   /* CN generation */
   /* recompute level adjustment factor Q11             *
    * st->log_en_adjust = 0.9*st->log_en_adjust +       *
    *                     0.1*dtx_log_en_adjust[mode]); */
   move16();
   st->log_en_adjust = add(mult(st->log_en_adjust, 29491),
                           shr(mult(shl(dtx_log_en_adjust[mode],5),3277),5));

   /* Interpolate SID info */
   int_fac = shl(add(1,st->since_last_sid), 10); /* Q10 */                 move16();
   int_fac = mult(int_fac, st->true_sid_period_inv); /* Q10 * Q15 -> Q10 */
   
   /* Maximize to 1.0 in Q10 */
   test();
   if (sub(int_fac, 1024) > 0)
   {
      int_fac = 1024;                                               move16();
   }
   int_fac = shl(int_fac, 4); /* Q10 -> Q14 */
   
   L_log_en_int = L_mult(int_fac, st->log_en); /* Q14 * Q11->Q26 */ move32();
   for(i = 0; i < M; i++)
   {
      lsp_int[i] = mult(int_fac, st->lsp[i]);/* Q14 * Q15 -> Q14 */ move16();
   }
   
   int_fac = sub(16384, int_fac); /* 1-k in Q14 */                  move16();

   /* (Q14 * Q11 -> Q26) + Q26 -> Q26 */
   L_log_en_int = L_mac(L_log_en_int, int_fac, st->old_log_en);
   for(i = 0; i < M; i++)
   {
      /* Q14 + (Q14 * Q15 -> Q14) -> Q14 */
      lsp_int[i] = add(lsp_int[i], mult(int_fac, st->lsp_old[i]));  move16();
      lsp_int[i] = shl(lsp_int[i], 1); /* Q14 -> Q15 */             move16();
   }
   
   /* compute the amount of lsf variability */
   lsf_variab_factor = sub(st->log_pg_mean,2457); /* -0.6 in Q12 */ move16();
   /* *0.3 Q12*Q15 -> Q12 */
   lsf_variab_factor = sub(4096, mult(lsf_variab_factor, 9830)); 

   /* limit to values between 0..1 in Q12 */ 
   test();
   if (sub(lsf_variab_factor, 4096) > 0)
   {
      lsf_variab_factor = 4096;                                     move16();
   }
   test();
   if (lsf_variab_factor < 0)
   {
      lsf_variab_factor = 0;                                        move16(); 
   }
   lsf_variab_factor = shl(lsf_variab_factor, 3); /* -> Q15 */      move16();

   /* get index of vector to do variability with */
   lsf_variab_index = pseudonoise(&st->L_pn_seed_rx, 3);            move16();

   /* convert to lsf */
   Lsp_lsf(lsp_int, lsf_int, M);

   /* apply lsf variability */
   Copy(lsf_int, lsf_int_variab, M);
   for(i = 0; i < M; i++)
   {
      move16();
      lsf_int_variab[i] = add(lsf_int_variab[i], 
                              mult(lsf_variab_factor,
                                   st->lsf_hist_mean[i+lsf_variab_index*M]));
   }

   /* make sure that LSP's are ordered */
   Reorder_lsf(lsf_int, LSF_GAP, M);
   Reorder_lsf(lsf_int_variab, LSF_GAP, M);

   /* copy lsf to speech decoders lsf state */
   Copy(lsf_int, lsfState->past_lsf_q, M);

   /* convert to lsp */
   Lsf_lsp(lsf_int, lsp_int, M);
   Lsf_lsp(lsf_int_variab, lsp_int_variab, M);

   /* Compute acoeffs Q12 acoeff is used for level    * 
    * normalization and postfilter, acoeff_variab is  *
    * used for synthesis filter                       *
    * by doing this we make sure that the level       *
    * in high frequenncies does not jump up and down  */

   Lsp_Az(lsp_int, acoeff);
   Lsp_Az(lsp_int_variab, acoeff_variab);
   
   /* For use in postfilter */
   Copy(acoeff, &A_t[0],           M + 1);
   Copy(acoeff, &A_t[M + 1],       M + 1);
   Copy(acoeff, &A_t[2 * (M + 1)], M + 1);
   Copy(acoeff, &A_t[3 * (M + 1)], M + 1);
   
   /* Compute reflection coefficients Q15 */
   A_Refl(&acoeff[1], refl);
   
   /* Compute prediction error in Q15 */
   pred_err = MAX_16; /* 0.99997 in Q15 */                          move16();
   for (i = 0; i < M; i++)
   { 
      pred_err = mult(pred_err, sub(MAX_16, mult(refl[i], refl[i])));
   }

   /* compute logarithm of prediction gain */   
   Log2(L_deposit_l(pred_err), &log_pg_e, &log_pg_m);
   
   /* convert exponent and mantissa to Word16 Q12 */
   log_pg = shl(sub(log_pg_e,15), 12);  /* Q12 */                   move16();
   log_pg = shr(sub(0,add(log_pg, shr(log_pg_m, 15-12))), 1);       move16();
   st->log_pg_mean = add(mult(29491,st->log_pg_mean),
                         mult(3277, log_pg));                       move16();

   /* Compute interpolated log energy */
   L_log_en_int = L_shr(L_log_en_int, 10); /* Q26 -> Q16 */         move32();

   /* Add 4 in Q16 */
   L_log_en_int = L_add(L_log_en_int, 4 * 65536L);                  move32();

   /* subtract prediction gain */
   L_log_en_int = L_sub(L_log_en_int, L_shl(L_deposit_l(log_pg), 4));move32();

   /* adjust level to speech coder mode */
   L_log_en_int = L_add(L_log_en_int, 
                        L_shl(L_deposit_l(st->log_en_adjust), 5));  move32();
       
   log_en_int_e = extract_h(L_log_en_int);                    move16();
   move16();
   log_en_int_m = extract_l(L_shr(L_sub(L_log_en_int, 
                                        L_deposit_h(log_en_int_e)), 1));
   level = extract_l(Pow2(log_en_int_e, log_en_int_m)); /* Q4 */ move16();
   
   for (i = 0; i < 4; i++)
   {             
      /* Compute innovation vector */
      build_CN_code(&st->L_pn_seed_rx, ex);
      for (j = 0; j < L_SUBFR; j++)
      {
         ex[j] = mult(level, ex[j]);                                move16();
      }
      /* Synthesize */
      Syn_filt(acoeff_variab, ex, &synth[i * L_SUBFR], L_SUBFR, 
               mem_syn, 1);
      
   } /* next i */
   
   /* reset codebook averaging variables */ 
   averState->hangVar = 20;                                         move16();
   averState->hangCount = 0;                                        move16();
    
   test();
   if (sub(new_state, DTX_MUTE) == 0)
   {
      /* mute comfort noise as it has been quite a long time since  
       * last SID update  was performed                            */
      
      tmp_int_length = st->since_last_sid;                          move16();
      test();
      if (sub(tmp_int_length, 32) > 0)
      {
         tmp_int_length = 32;                                       move16();
      }
      
      /* safety guard against division by zero */
      test();
      if(tmp_int_length <= 0) {
         tmp_int_length = 8;                                       move16();
      }      
      
      move16();
      st->true_sid_period_inv = div_s(1 << 10, shl(tmp_int_length, 10)); 

      st->since_last_sid = 0;                                       move16();
      Copy(st->lsp, st->lsp_old, M);
      st->old_log_en = st->log_en;                                  move16();
      /* subtract 1/8 in Q11 i.e -6/8 dB */
      st->log_en = sub(st->log_en, 256);                            move16();  
   }

   /* reset interpolation length timer 
    * if data has been updated.        */
   test(); test(); test(); test();
   if ((st->sid_frame != 0) && 
       ((st->valid_data != 0) || 
        ((st->valid_data == 0) &&  (st->dtxHangoverAdded) != 0))) 
   {
      st->since_last_sid =  0;                                      move16();
      st->data_updated = 1;                                         move16();
   }
         
   return 0;
}
Exemplo n.º 14
0
void CIVLiveViewer::PaintArrowLine( 
    const HDC dc, 
    const RECT& rect, 
    WPG_TripwireEventDescription& Line,
    DWORD dwShowCount1,
    DWORD dwShowCount2 )
{
    int x[2], y[2];
    ViewHelper::TranslateWPGPoint(rect, Line.startPoint, x[0], y[0]);
    ViewHelper::TranslateWPGPoint(rect, Line.endPoint, x[1], y[1]);

    ::MoveToEx(dc, x[0], y[0], NULL);  
    ::LineTo(dc, x[1], y[1]);

    // 利用垂直和两点的距离算出两个点的坐标
    POINT MedPoint;
    MedPoint.x = (x[0] + x[1])/2;
    MedPoint.y = (y[0] + y[1])/2;
    double m = sqrt( 
        Pow2(double(MedPoint.x-x[0])) + Pow2(double(MedPoint.y-y[0])) );
    if ( m < 0.0001 )
    {
        return;
    }

    POINT A[2];
    A[0].x = MedPoint.x + long(ArrowLineLen/m*(y[0]-MedPoint.y));
    A[0].y = MedPoint.y - long(ArrowLineLen/m*(x[0]-MedPoint.x));
    A[1].x = MedPoint.x - long(ArrowLineLen/m*(y[0]-MedPoint.y));
    A[1].y = MedPoint.y + long(ArrowLineLen/m*(x[0]-MedPoint.x));

    // dc.p

    double o;
    long nXoffset = A[0].x-A[1].x;
    long nYoffset = A[0].y-A[1].y;
    if (nXoffset == 0)
    {
        o = nYoffset > 0 ? M_PI_2 : (-M_PI_2);
    }
    else
    {
        double tanValue = 1.0*(nYoffset)/nXoffset;
        o = atan(tanValue);
    }

    // 修正两种特殊情况,因为我把它的角度转换为[0, pi]区间的,
    // atan取值范围为(-pi/2, pi/2)
    if ( o < 0 )
    {
        o += M_PI;
    }
    else if ( o == 0 && nXoffset > 0 )
    {
        o = M_PI;
    }

    int nOldMode = ::SetBkMode(dc, TRANSPARENT);
    
    COLORREF nOldCol = ::SetTextColor(dc, Font_Color);
    //HGDIOBJ hOldFont = ::SelectObject(dc, m_hFont);
    bool bUp = (A[0].y > A[1].y);
    //bool bUp = (A[0].x > A[1].x) ^ (A[0].y < A[1].y);
    //bool bUp = GetPointRLineState(BeginPoint, EndPoint, A[1]) > 0; 
    //TRACE("%d\n", bUp);
    if ( Line.direction == ANY_DIRECTION || 
         Line.direction == RIGHT_TO_LEFT )
    {
        ::MoveToEx(dc, MedPoint.x, MedPoint.y, NULL);  
        ::LineTo(dc, A[0].x, A[0].y);
        DrawArrow(dc, A[0], ArrowHeadLen, o, bUp, dwShowCount1);
    }
    if (  Line.direction == ANY_DIRECTION || 
          Line.direction == LEFT_TO_RIGHT ) // Line_Show_Left
    {
        ::MoveToEx(dc, MedPoint.x, MedPoint.y, NULL);  
        ::LineTo(dc, A[1].x, A[1].y);
        DrawArrow(dc, A[1], ArrowHeadLen, o, !bUp, dwShowCount2);
    }
    //SelectObject(dc, hOldFont);
    ::SetTextColor(dc, nOldCol);
    ::SetBkMode(dc, nOldMode);
}
Exemplo n.º 15
0
/*************************************************************************
 *
 * FUNCTION:  MR795_gain_quant
 *
 * PURPOSE:   pitch and codebook quantization for MR795
 *
 *************************************************************************/
void
MR795_gain_quant(
    GainAdaptState *adapt_st, /* i/o: gain adapter state structure       */
    Word16 res[],             /* i  : LP residual,                  Q0   */
    Word16 exc[],             /* i  : LTP excitation (unfiltered),  Q0   */
    Word16 code[],            /* i  : CB innovation (unfiltered),   Q13  */
    Word16 frac_coeff[],      /* i  : coefficients (5),             Q15  */
    Word16 exp_coeff[],       /* i  : energy coefficients (5),      Q0   */
                              /*      coefficients from calc_filt_ener() */
    Word16 exp_code_en,       /* i  : innovation energy (exponent), Q0   */
    Word16 frac_code_en,      /* i  : innovation energy (fraction), Q15  */
    Word16 exp_gcode0,        /* i  : predicted CB gain (exponent), Q0   */
    Word16 frac_gcode0,       /* i  : predicted CB gain (fraction), Q15  */
    Word16 L_subfr,           /* i  : Subframe length                    */
    Word16 cod_gain_frac,     /* i  : opt. codebook gain (fraction),Q15  */
    Word16 cod_gain_exp,      /* i  : opt. codebook gain (exponent), Q0  */
    Word16 gp_limit,          /* i  : pitch gain limit                   */
    Word16 *gain_pit,         /* i/o: Pitch gain,                   Q14  */
    Word16 *gain_cod,         /* o  : Code gain,                    Q1   */
    Word16 *qua_ener_MR122,   /* o  : quantized energy error,       Q10  */
                              /*      (for MR122 MA predictor update)    */
    Word16 *qua_ener,         /* o  : quantized energy error,       Q10  */
                              /*      (for other MA predictor update)    */
    Word16 **anap             /* o  : Index of quantization              */
                              /*      (first gain pitch, then code pitch)*/
)
{
    Word16 frac_en[4];
    Word16 exp_en[4];
    Word16 ltpg, alpha, gcode0;
    Word16 g_pitch_cand[3];      /* pitch gain candidates   Q14 */
    Word16 g_pitch_cind[3];      /* pitch gain indices      Q0  */
    Word16 gain_pit_index;
    Word16 gain_cod_index;
    Word16 exp;
    Word16 gain_cod_unq;         /* code gain (unq.) Q(10-exp_gcode0)  */


    /* get list of candidate quantized pitch gain values
     * and corresponding quantization indices
     */
    gain_pit_index = q_gain_pitch (MR795, gp_limit, gain_pit,
                                   g_pitch_cand, g_pitch_cind); 
                      /* function result */

    /*-------------------------------------------------------------------*
     *  predicted codebook gain                                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~                                          *
     *  gc0     = 2^exp_gcode0 + 2^frac_gcode0                           *
     *                                                                   *
     *  gcode0 (Q14) = 2^14*2^frac_gcode0 = gc0 * 2^(14-exp_gcode0)      *
     *-------------------------------------------------------------------*/
    gcode0 = extract_l(Pow2(14, frac_gcode0));          /* Q14 */

    /* pre-quantization of codebook gain
     * (using three pitch gain candidates);
     * result: best guess of pitch gain and code gain
     */
    MR795_gain_code_quant3(
        exp_gcode0, gcode0, g_pitch_cand, g_pitch_cind,
        frac_coeff, exp_coeff,
        gain_pit, &gain_pit_index, gain_cod, &gain_cod_index,
        qua_ener_MR122, qua_ener);

    /* calculation of energy coefficients and LTP coding gain */
    calc_unfilt_energies(res, exc, code, *gain_pit, L_subfr,
                         frac_en, exp_en, &ltpg);

    /* run gain adaptor, calculate alpha factor to balance LTP/CB gain
     * (this includes the gain adaptor update)
     * Note: ltpg = 0 if frac_en[0] == 0, so the update is OK in that case
     */
    gain_adapt(adapt_st, ltpg, *gain_cod, &alpha);

    /* if this is a very low energy signal (threshold: see
     * calc_unfilt_energies) or alpha <= 0 then don't run the modified quantizer
     */
      
    if (frac_en[0] != 0 && alpha > 0)
    {
        /* innovation energy <cod cod> was already computed in gc_pred() */
        /* (this overwrites the LtpResEn which is no longer needed)      */
        frac_en[3] = frac_code_en;          
        exp_en[3] = exp_code_en;            
        
        /* store optimum codebook gain in Q(10-exp_gcode0) */
        exp = add (sub (cod_gain_exp, exp_gcode0), 10);
        gain_cod_unq = shl (cod_gain_frac, exp);
        
        /* run quantization with modified criterion */
        gain_cod_index = MR795_gain_code_quant_mod(
            *gain_pit, exp_gcode0, gcode0,
            frac_en, exp_en, alpha, gain_cod_unq,
            gain_cod, qua_ener_MR122, qua_ener);  /* function result */
    }

    *(*anap)++ = gain_pit_index;        
    *(*anap)++ = gain_cod_index;        
}
Exemplo n.º 16
0
/*----------------------------------------------------------------------------
; FUNCTION CODE
----------------------------------------------------------------------------*/
void d_gain_code(
    gc_predState *pred_state, /* i/o : MA predictor state               */
    enum Mode mode,           /* i   : AMR mode (MR795 or MR122)        */
    Word16 index,             /* i   : received quantization index      */
    Word16 code[],            /* i   : innovation codevector            */
    const Word16* qua_gain_code_ptr, /* i : Pointer to read-only table      */
    Word16 *gain_code,        /* o   : decoded innovation gain          */
    Flag   *pOverflow
)
{
    Word16 gcode0, exp, frac;
    const Word16 *p;
    Word16 qua_ener_MR122, qua_ener;
    Word16 exp_inn_en;
    Word16 frac_inn_en;
    Word32 L_tmp;
    Word16 tbl_tmp;
    Word16 temp;
    /*-------------- Decode codebook gain ---------------*/

    /*-------------------------------------------------------------------*
     *  predict codebook gain                                            *
     *  ~~~~~~~~~~~~~~~~~~~~~                                            *
     *  gc0     = Pow2(int(d)+frac(d))                                   *
     *          = 2^exp + 2^frac                                         *
     *                                                                   *
     *-------------------------------------------------------------------*/

    gc_pred(pred_state, mode, code, &exp, &frac,
            &exp_inn_en, &frac_inn_en, pOverflow);

    index &= 31;                    /* index < 32, to avoid buffer overflow */
    tbl_tmp = index + (index << 1);

    p = &qua_gain_code_ptr[tbl_tmp];

    /* Different scalings between MR122 and the other modes */
    temp = sub((Word16)mode, (Word16)MR122, pOverflow);
    if (temp == 0)
    {
        gcode0 = (Word16)(Pow2(exp, frac, pOverflow));    /* predicted gain */
        gcode0 = shl(gcode0, 4, pOverflow);
        *gain_code = shl(mult(gcode0, *p++, pOverflow), 1, pOverflow);
    }
    else
    {
        gcode0 = (Word16)(Pow2(14, frac, pOverflow));
        L_tmp = L_mult(*p++, gcode0, pOverflow);
        L_tmp = L_shr(L_tmp, sub(9, exp, pOverflow), pOverflow);
        *gain_code = (Word16)(L_tmp >> 16);        /* Q1 */
    }

    /*-------------------------------------------------------------------*
     *  update table of past quantized energies                          *
     *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                          *
     *-------------------------------------------------------------------*/
    qua_ener_MR122 = *p++;
    qua_ener = *p++;
    gc_pred_update(pred_state, qua_ener_MR122, qua_ener);

    return;
}
Exemplo n.º 17
0
Word16 vad2 (Word16 * farray_ptr, vadState2 * st)
{

	/*
	 * The channel table is defined below.  In this table, the
	 * lower and higher frequency coefficients for each of the 16
	 * channels are specified.  The table excludes the coefficients
	 * with numbers 0 (DC), 1, and 64 (Foldover frequency).
	 */

	const static Word16 ch_tbl[NUM_CHAN][2] =
	{

		{2, 3},
		{4, 5},
		{6, 7},
		{8, 9},
		{10, 11},
		{12, 13},
		{14, 16},
		{17, 19},
		{20, 22},
		{23, 26},
		{27, 30},
		{31, 35},
		{36, 41},
		{42, 48},
		{49, 55},
		{56, 63}

	};

	/* channel energy scaling table - allows efficient division by number
         * of DFT bins in the channel: 1/2, 1/3, 1/4, etc.
	 */

	const static Word16 ch_tbl_sh[NUM_CHAN] =
	{
		16384, 16384, 16384, 16384, 16384, 16384, 10923, 10923,
		10923, 8192, 8192, 6554, 5461, 4681, 4681, 4096
	};

	/*
	 * The voice metric table is defined below.  It is a non-
	 * linear table with a deadband near zero.  It maps the SNR
	 * index (quantized SNR value) to a number that is a measure
	 * of voice quality.
	 */

	const static Word16 vm_tbl[90] =
	{
		2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
		3, 3, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 7, 7, 7,
		8, 8, 9, 9, 10, 10, 11, 12, 12, 13, 13, 14, 15,
		15, 16, 17, 17, 18, 19, 20, 20, 21, 22, 23, 24,
		24, 25, 26, 27, 28, 28, 29, 30, 31, 32, 33, 34,
		35, 36, 37, 37, 38, 39, 40, 41, 42, 43, 44, 45,
		46, 47, 48, 49, 50, 50, 50, 50, 50, 50, 50, 50,
		50, 50
	};

	/* hangover as a function of peak SNR (3 dB steps) */
	const static Word16 hangover_table[20] =
	{
		30, 30, 30, 30, 30, 30, 28, 26, 24, 22, 20, 18, 16, 14, 12, 10, 8, 8, 8, 8
	};

	/* burst sensitivity as a function of peak SNR (3 dB steps) */
	const static Word16 burstcount_table[20] =
	{
		8, 8, 8, 8, 8, 8, 8, 8, 7, 6, 5, 4, 4, 4, 4, 4, 4, 4, 4, 4
	};

	/* voice metric sensitivity as a function of peak SNR (3 dB steps) */
	const static Word16 vm_threshold_table[20] =
	{
                34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 34, 40, 51, 71, 100, 139, 191, 257, 337, 432
	};


	/* State tables that use 22,9 or 27,4 scaling for ch_enrg[] */

   const static Word16 noise_floor_chan[2] =	{NOISE_FLOOR_CHAN_0, NOISE_FLOOR_CHAN_1};
	const  static Word16 min_chan_enrg[2] =	{MIN_CHAN_ENRG_0, MIN_CHAN_ENRG_1};
	const static Word16 ine_noise[2] = 		{INE_NOISE_0, INE_NOISE_1};
	const static Word16 fbits[2] = 		{FRACTIONAL_BITS_0, FRACTIONAL_BITS_1};
	const static Word16 state_change_shift_r[2] = {STATE_1_TO_0_SHIFT_R, STATE_0_TO_1_SHIFT_R};

	/* Energy scale table given 30,1 input scaling (also account for -6 dB shift on input) */
	const static Word16 enrg_norm_shift[2] = 	{(FRACTIONAL_BITS_0-1+2), (FRACTIONAL_BITS_1-1+2)};


	/* Automatic variables */

	Word32 Lenrg;				/* scaled as 30,1 */
	Word32 Ltne;				/* scaled as 22,9 */
	Word32 Ltce;				/* scaled as 22,9 or 27,4 */

	Word16 tne_db;				/* scaled as 7,8 */
	Word16 tce_db;				/* scaled as 7,8 */

	Word16 input_buffer[FRM_LEN];		/* used for block normalising input data */
	Word16 data_buffer[FFT_LEN];		/* used for in-place FFT */

	Word16 ch_snr[NUM_CHAN];		/* scaled as 7,8 */
	Word16 ch_snrq;				/* scaled as 15,0 (in 0.375 dB steps) */
	Word16 vm_sum;				/* scaled as 15,0 */
	Word16 ch_enrg_dev;			/* scaled as 7,8 */

	Word32 Lpeak;				/* maximum channel energy */
	Word16 p2a_flag;			/* flag to indicate spectral peak-to-average ratio > 10 dB */

	Word16 ch_enrg_db[NUM_CHAN];		/* scaled as 7,8 */
	Word16 ch_noise_db;			/* scaled as 7,8 */

	Word16 alpha;				/* scaled as 0,15 */
	Word16 one_m_alpha;			/* scaled as 0,15 */
	Word16 update_flag;			/* set to indicate a background noise estimate update */

	Word16 i, j, j1, j2;			/* Scratch variables */
	Word16 hi1, lo1;

	Word32 Ltmp, Ltmp1, Ltmp2;
	Word16 tmp;

	Word16 normb_shift;		/* block norm shift count */

	Word16 ivad;			/* intermediate VAD decision (return value) */
	Word16 tsnrq;			/* total signal-to-noise ratio (quantized 3 dB steps) scaled as 15,0 */
	Word16 xt;			/* instantaneous frame SNR in dB, scaled as 7,8 */

	Word16 state_change;


	/* Increment frame counter */
	st->Lframe_cnt = L_add(st->Lframe_cnt, 1);

	/* Block normalize the input */
	normb_shift = block_norm(farray_ptr, input_buffer, FRM_LEN, FFT_HEADROOM);

	/* Pre-emphasize the input data and store in the data buffer with the appropriate offset */
	for (i = 0; i < DELAY; i++)
	{
		data_buffer[i] = 0;									move16();
	}

	st->pre_emp_mem = shr_r(st->pre_emp_mem, sub(st->last_normb_shift, normb_shift));
	st->last_normb_shift = normb_shift;								move16();

	data_buffer[DELAY] = add(input_buffer[0], mult(PRE_EMP_FAC, st->pre_emp_mem));			move16();

	for (i = DELAY + 1, j = 1; i < DELAY + FRM_LEN; i++, j++)
	{
		data_buffer[i] = add(input_buffer[j], mult(PRE_EMP_FAC, input_buffer[j-1]));		move16();
	}
	st->pre_emp_mem = input_buffer[FRM_LEN-1];							move16();

	for (i = DELAY + FRM_LEN; i < FFT_LEN; i++)
	{
		data_buffer[i] = 0;									move16();
	}


	/* Perform FFT on the data buffer */
	r_fft(data_buffer);


	/* Use normb_shift factor to determine the scaling of the energy estimates */
	state_change = 0;										move16();
													test();
	if (st->shift_state == 0)
	{												test();
		if (sub(normb_shift, -FFT_HEADROOM+2) <= 0)
		{
			state_change = 1;								move16();
			st->shift_state = 1;								move16();
		}
	}
	else
	{												test();
		if (sub(normb_shift, -FFT_HEADROOM+5) >= 0)
		{
			state_change = 1;								move16();
			st->shift_state = 0;								move16();
		}
	}

	/* Scale channel energy estimate */								test();
	if (state_change)
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			st->Lch_enrg[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[st->shift_state]);	move32();
		}
	}


	/* Estimate the energy in each channel */
													test();
	if (L_sub(st->Lframe_cnt, 1) == 0)
	{
		alpha = 32767;										move16();
		one_m_alpha = 0;									move16();
	}
	else
	{
		alpha = CEE_SM_FAC;									move16();
		one_m_alpha = ONE_MINUS_CEE_SM_FAC;							move16();
	}

	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Lenrg = 0;										move16();
		j1 = ch_tbl[i][0];									move16();
		j2 = ch_tbl[i][1];									move16();

		for (j = j1; j <= j2; j++)
		{
			Lenrg = L_mac(Lenrg, data_buffer[2 * j], data_buffer[2 * j]);
			Lenrg = L_mac(Lenrg, data_buffer[2 * j + 1], data_buffer[2 * j + 1]);
		}

		/* Denorm energy & scale 30,1 according to the state */
		Lenrg = L_shr_r(Lenrg, sub(shl(normb_shift, 1), enrg_norm_shift[st->shift_state]));

		/* integrate over time: e[i] = (1-alpha)*e[i] + alpha*enrg/num_bins_in_chan */
		tmp = mult(alpha, ch_tbl_sh[i]);
		L_Extract (Lenrg, &hi1, &lo1);
		Ltmp = Mpy_32_16(hi1, lo1, tmp);

		L_Extract (st->Lch_enrg[i], &hi1, &lo1);
		st->Lch_enrg[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, one_m_alpha));			move32();
													test();
		if (L_sub(st->Lch_enrg[i], min_chan_enrg[st->shift_state]) < 0)
		{
			st->Lch_enrg[i] = min_chan_enrg[st->shift_state];				move32();
		}

	}


	/* Compute the total channel energy estimate (Ltce) */
	Ltce = 0;											move16();
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Ltce = L_add(Ltce, st->Lch_enrg[i]);
	}


	/* Calculate spectral peak-to-average ratio, set flag if p2a > 10 dB */
	Lpeak = 0;											move32();
	for (i = LO_CHAN+2; i <= HI_CHAN; i++)	/* Sine waves not valid for low frequencies */
	{												test();
		if (L_sub(st->Lch_enrg [i], Lpeak) > 0)
		{
			Lpeak = st->Lch_enrg [i];							move32();
		}
	}

	/* Set p2a_flag if peak (dB) > average channel energy (dB) + 10 dB */
	/*   Lpeak > Ltce/num_channels * 10^(10/10)                        */
	/*   Lpeak > (10/16)*Ltce                                          */

	L_Extract (Ltce, &hi1, &lo1);
	Ltmp = Mpy_32_16(hi1, lo1, 20480);
													test();
	if (L_sub(Lpeak, Ltmp) > 0)
	{
		p2a_flag = TRUE;									move16();
	}
	else
	{
		p2a_flag = FALSE;									move16();
	}


	/* Initialize channel noise estimate to either the channel energy or fixed level  */
	/*   Scale the energy appropriately to yield state 0 (22,9) scaling for noise */
													test();
	if (L_sub(st->Lframe_cnt, 4) <= 0)
	{												test();
		if (p2a_flag == TRUE)
		{
			for (i = LO_CHAN; i <= HI_CHAN; i++)
			{
				st->Lch_noise[i] = INE_NOISE_0;						move32();
			}
		}
		else
		{
			for (i = LO_CHAN; i <= HI_CHAN; i++)
			{										test();
				if (L_sub(st->Lch_enrg[i], ine_noise[st->shift_state]) < 0)
				{
					st->Lch_noise[i] = INE_NOISE_0;					move32();
				}
				else
				{									test();
					if (st->shift_state == 1)
					{
						st->Lch_noise[i] = L_shr(st->Lch_enrg[i], state_change_shift_r[0]);
													move32();
					}
					else
					{
						st->Lch_noise[i] = st->Lch_enrg[i];			move32();
					}
				}
			}
		}
	}


	/* Compute the channel energy (in dB), the channel SNRs, and the sum of voice metrics */
	vm_sum = 0;											move16();
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		ch_enrg_db[i] = fn10Log10(st->Lch_enrg[i], fbits[st->shift_state]);			move16();
		ch_noise_db = fn10Log10(st->Lch_noise[i], FRACTIONAL_BITS_0);

		ch_snr[i] = sub(ch_enrg_db[i], ch_noise_db);						move16();

		/* quantize channel SNR in 3/8 dB steps (scaled 7,8 => 15,0) */
		/*   ch_snr = round((snr/(3/8))>>8)                          */
		/*          = round(((0.6667*snr)<<2)>>8)                    */
		/*          = round((0.6667*snr)>>6)                         */

		ch_snrq = shr_r(mult(21845, ch_snr[i]), 6);

		/* Accumulate the sum of voice metrics	*/						test();
		if (sub(ch_snrq, 89) < 0)
		{											test();
			if (ch_snrq > 0)
			{
				j = ch_snrq;								move16();
			}
			else
			{
				j = 0;									move16();
			}
		}
		else
		{
			j = 89;										move16();
		}
		vm_sum = add(vm_sum, vm_tbl[j]);
	}


	/* Initialize NOMINAL peak voice energy and average noise energy, calculate instantaneous SNR */ 
												test(),test(),logic16();
	if (L_sub(st->Lframe_cnt, 4) <= 0 || st->fupdate_flag == TRUE)
	{
		/* tce_db = (96 - 22 - 10*log10(64) (due to FFT)) scaled as 7,8 */
		tce_db = 14320;										move16();
		st->negSNRvar = 0;									move16();
		st->negSNRbias = 0;									move16();

		/* Compute the total noise estimate (Ltne) */
		Ltne = 0;										move32();
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			Ltne = L_add(Ltne, st->Lch_noise[i]);
		}

		/* Get total noise in dB */
		tne_db = fn10Log10(Ltne, FRACTIONAL_BITS_0);

		/* Initialise instantaneous and long-term peak signal-to-noise ratios */
		xt = sub(tce_db, tne_db);
		st->tsnr = xt;										move16();
	}
	else
	{
		/* Calculate instantaneous frame signal-to-noise ratio */
		/* xt = 10*log10( sum(2.^(ch_snr*0.1*log2(10)))/length(ch_snr) ) */
		Ltmp1 = 0;										move32();
		for (i=LO_CHAN; i<=HI_CHAN; i++) {
			/* Ltmp2 = ch_snr[i] * 0.1 * log2(10); (ch_snr scaled as 7,8) */
			Ltmp2 = L_shr(L_mult(ch_snr[i], 10885), 8);
			L_Extract(Ltmp2, &hi1, &lo1);
			hi1 = add(hi1, 3);			/* 2^3 to compensate for negative SNR */
			Ltmp1 = L_add(Ltmp1, Pow2(hi1, lo1));
		}
		xt = fn10Log10(Ltmp1, 4+3);			/* average by 16, inverse compensation 2^3 */

		/* Estimate long-term "peak" SNR */							test(),test();
		if (sub(xt, st->tsnr) > 0)
		{
			/* tsnr = 0.9*tsnr + 0.1*xt; */
			st->tsnr = round(L_add(L_mult(29491, st->tsnr), L_mult(3277, xt)));
		}
		/* else if (xt > 0.625*tsnr) */	
		else if (sub(xt, mult(20480, st->tsnr)) > 0)
		{
			/* tsnr = 0.998*tsnr + 0.002*xt; */
			st->tsnr = round(L_add(L_mult(32702, st->tsnr), L_mult(66, xt)));
		}
	}

	/* Quantize the long-term SNR in 3 dB steps, limit to 0 <= tsnrq <= 19 */
	tsnrq = shr(mult(st->tsnr, 10923), 8);

	/* tsnrq = min(19, max(0, tsnrq)); */								test(),test();
	if (sub(tsnrq, 19) > 0)
	{
		tsnrq = 19;										move16();
	}
	else if (tsnrq < 0)
	{
		tsnrq = 0;										move16();
	}

	/* Calculate the negative SNR sensitivity bias */
													test();
	if (xt < 0)
	{
		/* negSNRvar = 0.99*negSNRvar + 0.01*xt*xt; */
		/*   xt scaled as 7,8 => xt*xt scaled as 14,17, shift to 7,8 and round */
		tmp = round(L_shl(L_mult(xt, xt), 7));
		st->negSNRvar = round(L_add(L_mult(32440, st->negSNRvar), L_mult(328, tmp)));

		/* if (negSNRvar > 4.0) negSNRvar = 4.0;  */						test();
		if (sub(st->negSNRvar, 1024) > 0)
		{
			st->negSNRvar = 1024;								move16();
		}

		/* negSNRbias = max(12.0*(negSNRvar - 0.65), 0.0); */
		tmp = mult_r(shl(sub(st->negSNRvar, 166), 4), 24576);					test();

		if (tmp < 0)
		{
			st->negSNRbias = 0;								move16();
		}
		else
		{
			st->negSNRbias = shr(tmp, 8);
		}
	}


	/* Determine VAD as a function of the voice metric sum and quantized SNR */

	tmp = add(vm_threshold_table[tsnrq], st->negSNRbias);						test();
	if (sub(vm_sum, tmp) > 0)
	{
		ivad = 1;										move16();
		st->burstcount = add(st->burstcount, 1);						test();
		if (sub(st->burstcount, burstcount_table[tsnrq]) > 0)
		{
			st->hangover = hangover_table[tsnrq];						move16();
		}
	}
	else
	{
		st->burstcount = 0;									move16();
		st->hangover = sub(st->hangover, 1);							test();
		if (st->hangover <= 0)
		{
			ivad = 0;									move16();
			st->hangover = 0;								move16();
		}
		else
		{
			ivad = 1;									move16();
		}
	}


	/* Calculate log spectral deviation */
	ch_enrg_dev = 0;										move16();
													test();
	if (L_sub(st->Lframe_cnt, 1) == 0)
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			st->ch_enrg_long_db[i] = ch_enrg_db[i];						move16();
		}
	}
	else
	{
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{
			tmp = abs_s(sub(st->ch_enrg_long_db[i], ch_enrg_db[i]));
			ch_enrg_dev = add(ch_enrg_dev, tmp);
		}
	}

	/*
	 * Calculate long term integration constant as a function of instantaneous SNR
	 * (i.e., high SNR (tsnr dB) -> slower integration (alpha = HIGH_ALPHA),
	 *         low SNR (0 dB) -> faster integration (alpha = LOW_ALPHA)
	 */

	/* alpha = HIGH_ALPHA - ALPHA_RANGE * (tsnr - xt) / tsnr, low <= alpha <= high */
	tmp = sub(st->tsnr, xt);						test(),logic16(),test(),test();
	if (tmp <= 0 || st->tsnr <= 0)
	{
		alpha = HIGH_ALPHA;								move16();
		one_m_alpha = 32768L-HIGH_ALPHA;						move16();
	}
	else if (sub(tmp, st->tsnr) > 0)
	{
		alpha = LOW_ALPHA;								move16();
		one_m_alpha = 32768L-LOW_ALPHA;							move16();
	}
	else
	{
		tmp = div_s(tmp, st->tsnr);
		alpha = sub(HIGH_ALPHA, mult(ALPHA_RANGE, tmp));
		one_m_alpha = sub(32767, alpha);
	}

	/* Calc long term log spectral energy */
	for (i = LO_CHAN; i <= HI_CHAN; i++)
	{
		Ltmp1 = L_mult(one_m_alpha, ch_enrg_db[i]);
		Ltmp2 = L_mult(alpha, st->ch_enrg_long_db[i]);
		st->ch_enrg_long_db[i] = round(L_add(Ltmp1, Ltmp2));
	}


	/* Set or clear the noise update flags */
	update_flag = FALSE;										move16();
	st->fupdate_flag = FALSE;									move16();
													test(),test();
	if (sub(vm_sum, UPDATE_THLD) <= 0)
	{												test();
		if (st->burstcount == 0)
		{
			update_flag = TRUE;								move16();
			st->update_cnt = 0;								move16();
		}
	}
	else if (L_sub(Ltce, noise_floor_chan[st->shift_state]) > 0)
	{												test();
		if (sub(ch_enrg_dev, DEV_THLD) < 0)
		{											test();
			if (p2a_flag == FALSE)
			{										test();
				if (st->LTP_flag == FALSE)
				{
					st->update_cnt = add(st->update_cnt, 1);			test();
					if (sub(st->update_cnt, UPDATE_CNT_THLD) >= 0)
					{
						update_flag = TRUE;					move16();
						st->fupdate_flag = TRUE;				move16();
					}
				}
			}
		}
	}
													test();
	if (sub(st->update_cnt, st->last_update_cnt) == 0)
	{
		st->hyster_cnt = add(st->hyster_cnt, 1);
	}
	else
	{
		st->hyster_cnt = 0;									move16();
	}

	st->last_update_cnt = st->update_cnt;								move16();
													test();
	if (sub(st->hyster_cnt, HYSTER_CNT_THLD) > 0)
	{
		st->update_cnt = 0;									move16();
	}


	/* Conditionally update the channel noise estimates */
													test();
	if (update_flag == TRUE)
	{
		/* Check shift state */									test();
		if (st->shift_state == 1)
		{
			/* get factor to shift ch_enrg[] from state 1 to 0 (noise always state 0) */
			tmp = state_change_shift_r[0];							move16();
		}
		else
		{
			/* No shift if already state 0 */
			tmp = 0;									move16();
		}

		/* Update noise energy estimate */
		for (i = LO_CHAN; i <= HI_CHAN; i++)
		{											test();
			/* integrate over time: en[i] = (1-alpha)*en[i] + alpha*e[n] */
			/* (extract with shift compensation for state 1) */
			L_Extract (L_shr(st->Lch_enrg[i], tmp), &hi1, &lo1);
			Ltmp = Mpy_32_16(hi1, lo1, CNE_SM_FAC);

			L_Extract (st->Lch_noise[i], &hi1, &lo1);
			st->Lch_noise[i] = L_add(Ltmp, Mpy_32_16(hi1, lo1, ONE_MINUS_CNE_SM_FAC));	move32();

			/* Limit low level noise */							test();
			if (L_sub(st->Lch_noise[i], MIN_NOISE_ENRG_0) < 0)
			{
				st->Lch_noise[i] = MIN_NOISE_ENRG_0;					move32();
			}
		}
	}

	return(ivad);
}								/* end of vad2 () */
Exemplo n.º 18
0
/*---------------------------------------------------------------------------*
 * Function  Gain_predict                                                    *
 * ~~~~~~~~~~~~~~~~~~~~~~                                                    *
 * MA prediction is performed on the innovation energy (in dB with mean      *
 * removed).                                                                 *
 *---------------------------------------------------------------------------*/
void Gain_predict(
   Word16 past_qua_en[], /* (i) Q10 :Past quantized energies        */
   Word16 code[],        /* (i) Q13 :Innovative vector.             */
   Word16 L_subfr,       /* (i)     :Subframe length.               */
   Word16 *gcode0,       /* (o) Qxx :Predicted codebook gain        */
   Word16 *exp_gcode0    /* (o)     :Q-Format(gcode0)               */
)
{
   Word16  i, exp, frac;
   Word32  L_tmp;

  /*-------------------------------*
   * Energy coming from code       *
   *-------------------------------*/

   L_tmp = 0;
   for(i=0; i<L_subfr; i++)
     L_tmp = L_mac(L_tmp, code[i], code[i]);

  /*-----------------------------------------------------------------*
   *  Compute: means_ener - 10log10(ener_code/ L_sufr)               *
   *  Note: mean_ener change from 36 dB to 30 dB because input/2     *
   *                                                                 *
   * = 30.0 - 10 log10( ener_code / lcode)  + 10log10(2^27)          *
   *                                          !!ener_code in Q27!!   *
   * = 30.0 - 3.0103 * log2(ener_code) + 10log10(40) + 10log10(2^27) *
   * = 30.0 - 3.0103 * log2(ener_code) + 16.02  + 81.278             *
   * = 127.298 - 3.0103 * log2(ener_code)                            *
   *-----------------------------------------------------------------*/

   Log2(L_tmp, &exp, &frac);               /* Q27->Q0 ^Q0 ^Q15       */
   L_tmp = Mpy_32_16(exp, frac, -24660);   /* Q0 Q15 Q13 -> ^Q14     */
                                           /* hi:Q0+Q13+1            */
                                           /* lo:Q15+Q13-15+1        */
                                           /* -24660[Q13]=-3.0103    */
   L_tmp = L_mac(L_tmp, 32588, 32);        /* 32588*32[Q14]=127.298  */

  /*-----------------------------------------------------------------*
   * Compute gcode0.                                                 *
   *  = Sum(i=0,3) pred[i]*past_qua_en[i] - ener_code + mean_ener    *
   *-----------------------------------------------------------------*/

   L_tmp = L_shl(L_tmp, 10);                      /* From Q14 to Q24 */
   for(i=0; i<4; i++)
     L_tmp = L_mac(L_tmp, pred[i], past_qua_en[i]); /* Q13*Q10 ->Q24 */

   *gcode0 = extract_h(L_tmp);                    /* From Q24 to Q8  */

  /*-----------------------------------------------------------------*
   * gcode0 = pow(10.0, gcode0/20)                                   *
   *        = pow(2, 3.3219*gcode0/20)                               *
   *        = pow(2, 0.166*gcode0)                                   *
   *-----------------------------------------------------------------*/

   L_tmp = L_mult(*gcode0, 5439);       /* *0.166 in Q15, result in Q24*/
   L_tmp = L_shr(L_tmp, 8);             /* From Q24 to Q16             */
   L_Extract(L_tmp, &exp, &frac);       /* Extract exponent of gcode0  */

   *gcode0 = extract_l(Pow2(14, frac)); /* Put 14 as exponent so that  */
                                        /* output of Pow2() will be:   */
                                        /* 16768 < Pow2() <= 32767     */
   *exp_gcode0 = sub(14,exp);
}
Exemplo n.º 19
0
INT16 q_gain_code (    /* Return quantization index                  */
    INT16 code[],      /* (i)      : fixed codebook excitation       */
    INT16 lcode,       /* (i)      : codevector size                 */
    INT16 *gain,       /* (i/o)    : quantized fixed codebook gain   */
    INT16 txdtx_ctrl,
    INT16 i_subfr
)
{
    INT16 i, index=0;
    INT16 gcode0, err, err_min, exp, frac;
    INT32 ener, ener_code;
    register INT32 ener_code_hi=0;
    register UINT32 ener_code_lo=0;
    INT16 aver_gain;
    static INT16 gcode0_CN;

    VPP_EFR_PROFILE_FUNCTION_ENTER(q_gain_code);



    if ((txdtx_ctrl & TX_SP_FLAG) != 0)
    {

        /*-------------------------------------------------------------------*
         *  energy of code:                                                   *
         *  ~~~~~~~~~~~~~~~                                                   *
         *  ener_code(Q17) = 10 * Log10(energy/lcode) / constant              *
         *                 = 1/2 * Log2(energy/lcode)                         *
         *                                           constant = 20*Log10(2)   *
         *-------------------------------------------------------------------*/

        /* ener_code = log10(ener_code/lcode) / (20*log10(2))       */
        //ener_code = 0;
        ener_code_lo = 0;
        for (i = 0; i < lcode; i++)
        {
            //ener_code = L_mac (ener_code, code[i], code[i]);
			//ener_code = L_MAC(ener_code, code[i], code[i]);
			VPP_MLA16(ener_code_hi,ener_code_lo,code[i], code[i]);

        }
        /* ener_code = ener_code / lcode */
        ener_code = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);

		//ener_code = L_mult (round(ener_code), 26214);
        ener_code = L_MULT(ROUND(ener_code), 26214);

        /* ener_code = 1/2 * Log2(ener_code) */
        Log2 (ener_code, &exp, &frac);
        //ener_code = L_Comp (sub (exp, 30), frac);
		ener_code = L_Comp (SUB (exp, 30), frac);

        /* predicted energy */

        //ener = MEAN_ENER;
        ener_code_lo = MEAN_ENER>>1;
        ener_code_hi =0;

        for (i = 0; i < 4; i++)
        {
            //ener = L_mac (ener, past_qua_en[i], pred[i]);
			//ener = L_MAC(ener, past_qua_en[i], pred[i]);
			VPP_MLA16(ener_code_hi,ener_code_lo, past_qua_en[i], pred[i]);
        }

        ener = VPP_SCALE64_TO_16(ener_code_hi,ener_code_lo);

        /*-------------------------------------------------------------------*
         *  predicted codebook gain                                           *
         *  ~~~~~~~~~~~~~~~~~~~~~~~                                           *
         *  gcode0(Qx) = Pow10( (ener*constant - ener_code*constant) / 20 )   *
         *             = Pow2(ener-ener_code)                                 *
         *                                           constant = 20*Log10(2)   *
         *-------------------------------------------------------------------*/

        //ener = L_shr (L_SUB(ener, ener_code), 1);
        ener = L_SHR_D(L_SUB(ener, ener_code), 1);


        L_Extract (ener, &exp, &frac);

        //gcode0 = extract_l (Pow2 (exp, frac));  /* predicted gain */
		gcode0 = EXTRACT_L(Pow2 (exp, frac));

        //gcode0 = shl (gcode0, 4);
		gcode0 = SHL(gcode0, 4);

        /*-------------------------------------------------------------------*
         *                   Search for best quantizer                        *
         *-------------------------------------------------------------------*/

        //err_min = abs_s (sub (*gain, mult (gcode0, qua_gain_code[0])));
		err_min = ABS_S(SUB (*gain, MULT(gcode0, qua_gain_code[0])));
        index = 0;

        for (i = 1; i < NB_QUA_CODE; i++)
        {
            //err = abs_s (sub (*gain, mult (gcode0, qua_gain_code[i])));
			err = ABS_S(SUB (*gain, MULT (gcode0, qua_gain_code[i])));


            //if (sub (err, err_min) < 0)
			if (SUB (err, err_min) < 0)
            {
                err_min = err;
                index = i;
            }
        }

         //*gain = mult (gcode0, qua_gain_code[index]);
		 *gain = MULT(gcode0, qua_gain_code[index]);


        /*------------------------------------------------------------------*
         *  update table of past quantized energies                         *
         *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                         *
         *  past_qua_en(Q12) = 20 * Log10(qua_gain_code) / constant         *
         *                   = Log2(qua_gain_code)                          *
         *                                           constant = 20*Log10(2) *
         *------------------------------------------------------------------*/

        for (i = 3; i > 0; i--)
        {
            past_qua_en[i] = past_qua_en[i - 1];
        }
        //Log2 (L_deposit_l (qua_gain_code[index]), &exp, &frac);
		Log2 (L_DEPOSIT_L(qua_gain_code[index]), &exp, &frac);

        //past_qua_en[0] = shr (frac, 5);
		past_qua_en[0] = SHR_D(frac, 5);

        //past_qua_en[0] = add (past_qua_en[0], shl (sub (exp, 11), 10));
        past_qua_en[0] = ADD (past_qua_en[0], SHL(SUB (exp, 11), 10));


        update_gain_code_history_tx (*gain, gain_code_old_tx);
    }