pctolsf.c

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/**************************************************************************
*
* NAME	
*	PCtoLSF (formerly pctolsp2)
*
* FUNCTION
*
*	Compute LSF from predictor polynomial.
*
*	NOTE:  Much faster conversion can be performed
*	       if the LSP quantization is incorporated.
*
* SYNOPSIS
*
*	PCtoLSF(a,freq)
*
*   formal 
*                       data	I/O
*	name		type	type	function
*	-------------------------------------------------------------------
*	a		float	i	a-polynomial a(0)=1
*	freq		float	o	lsp frequencies
*
***************************************************************************
*	
* DESCRIPTION
*
*  	Compute line spectrum frequencies by disection method as described in:
*	
*	Line Spectrum Pair (LSP) and Speech Data Compression,
*	F.K. Soong and B-H Juang,
*       Proc. ICASSP 84, pp. 1.10.1-1.10.4
*
*	CELP's LPC predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
*	Peter uses n=128, eps=1.e-06, nb=15 (this appears to be overkill!)
*
***************************************************************************
*
* CALLED BY
*
*	LPC_Analysis
*
**************************************************************************/
#define N	128
#define NB	15
#define EPS	1.e-6
#define FALSE	0
#define TRUE	1

#include <math.h>
#include <stdio.h>
#include "main.h"
#include "pctolsf.h"


void PCtoLSF(
float	a[ORDER+1],
float 	freq[ORDER+1])
{
  static float lastfreq[ORDER];
  float p[ORDER], q[ORDER], pi, ang, fm, tempfreq;
  float fr, pxr, tpxr, tfr, pxm, pxl, fl, qxl, tqxr;
  float qxm, qxr, tqxl;
  int mp, mh, nf, mb, jc, i, j, lspflag;

  pi = atan(1.) * 4.0;
  mp = ORDER + 1;
  mh = ORDER / 2;

  /* *generate p and q polynomials	*/

  for (i = 0; i < mh; i++)
  {
    p[i] = a[i+1] + a[ORDER-i];
    q[i] = a[i+1] - a[ORDER-i];
  }
    
  /* *compute p at f=0.			*/

  fl = 0.;
  for (pxl = 1.0, j = 0; j < mh; j++)
    pxl += p[j];

  /* *search for zeros of p		*/

  nf = 0;
  for (i = 1; i <= N; pxl = tpxr, fl = tfr, i++)
  {
    mb = 0;
    fr = i * (0.5 / N);
    pxr = cos(mp * pi * fr);
    for (j = 0; j < mh; j++)
    {
      jc = mp - (j+1)*2;
      ang = jc * pi * fr;
      pxr += cos(ang) * p[j];
    }
    tpxr = pxr;
    tfr = fr;
    if (pxl * pxr > 0.0) continue;

    do
    {
      mb++;
      fm = fl + (fr-fl) / (pxl-pxr) * pxl;
      pxm = cos(mp * pi * fm);
    
      for (j = 0; j < mh; j++)
      {
        jc = mp - (j+1) * 2;
        ang = jc * pi * fm;
        pxm += cos(ang) * p[j];
      }
      (pxm*pxl > 0.0) ? (pxl = pxm, fl = fm) : (pxr = pxm, fr = fm);

    } while ((fabs(pxm) > EPS) && (mb < 4));

    if ((pxl-pxr) * pxl == 0) 
    {
      for (j = 0; j < ORDER; j++)
        freq[j] = (j+1) * 0.04545;
      printf("pctolsf: default lsps used, avoiding /0\n");
      return;
    }
    freq[nf] = fl + (fr-fl) / (pxl-pxr) * pxl;
    nf += 2;
    if (nf > ORDER-2) break;
  }


  /* *search for the zeros of q(z)	*/

  freq[ORDER] = 0.5;
  fl = freq[0];
  qxl = sin(mp * pi * fl);
  for (j = 0; j < mh; j++)
  {
    jc = mp - (j+1) * 2;
    ang = jc * pi * fl;
    qxl += sin(ang) * q[j];
  }

  for (i = 2; i < mp; qxl = tqxr, fl = tfr, i += 2)
  {
    mb = 0;
    fr = freq[i];
    qxr = sin(mp * pi * fr);
    for (j = 0; j < mh; j++)
    {
      jc = mp - (j+1) * 2;
      ang = jc * pi * fr;
      qxr += sin(ang) * q[j];
    }
    tqxl = qxl;
    tfr = fr;
    tqxr = qxr;
    
    do
    {
      mb++;
      fm = (fl+fr) * 0.5;
      qxm = sin(mp * pi * fm);
      

      for (j = 0; j < mh; j++)
      {
        jc = mp - (j+1) * 2;
        ang = jc * pi * fm;
        qxm += sin(ang) * q[j];
      }
      (qxm*qxl > 0.0) ? (qxl = qxm, fl = fm) : (qxr = qxm, fr = fm);

    } while ((fabs(qxm) > EPS*tqxl) && (mb < NB));

    if ((qxl-qxr) * qxl == 0)
    {
      for (j = 0; j < ORDER; j++)
        freq[j] = lastfreq[j];
      printf("pctolsf: last lsps used, avoiding /0\n");
      return;
    }
    freq[i-1] = fl + (fr-fl) / (qxl-qxr) * qxl;
  }

  /* *** ill-conditioned cases		*/

  lspflag = FALSE;
  if (freq[0] == 0.0 || freq[0] == 0.5) 
    lspflag = TRUE;
  for (i = 1; i < ORDER; i++)
  {
    if (freq[i] == 0.0 || freq[i] == 0.5) 
      lspflag = TRUE;

    /* *reorder lsps if non-monotonic	*/

    if (freq[i]  <  freq[i-1]) 
    {
      lspflag = TRUE;
      printf("pctolsf: non-monotonic lsps\n");
      tempfreq = freq[i];
      freq[i] = freq[i-1];
      freq[i-1] = tempfreq;
    }
  }

  /* *if non-monotonic after 1st pass, reset to last values	*/

  for (i = 1; i < ORDER; i++)
  {
    if (freq[i]  <  freq[i-1])
    {
      printf("pctolsf: Reset to previous lsp values\n");
      for (j = 0; j < ORDER; j++)
        freq[j] = lastfreq[j];
      break;
    }
  }
  for (i = 0; i < ORDER; i++) 
    lastfreq[i] = freq[i];
}