lpcfloat.c

speech signal process tools

    for ( np=1, i=1; i <= l; i++) np *= 2;

    if(fft_tablesize < (np/2)) {
	printf("\nPrecomputed trig tables are not big enough in fft().\n");
	return(FALSE);
    }
    k = (fft_tablesize * 2)/np;
        
    for (lmx=np, lo=0; lo < l; lo++, k *= 2) {
	lix = lmx;
	lmx /= 2;
	lixnp = np - lix;
	for (i=0, lm=0; lm<lmx; lm++, i += k ) {
	    c = cosine[i];
	    s = sine[i];
	    for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li;
	    				j1+=lix, j2+=lix ) {
		t1 = x[j1] - x[j2];
		t2 = y[j1] - y[j2];
		x[j1] += x[j2];
		y[j1] += y[j2];
		x[j2] = (c * t1) + (s * t2);
		y[j2] = (c * t2) - (s * t1);
	    }
	}
    }

	/* Now perform the bit reversal. */

    j = 1;
    nv2 = np/2;
    for ( i=1; i < np; i++ ) {
	if ( j < i ) {
	    jm = j-1;
	    im = i-1;
	    t1 = x[jm];
	    t2 = y[jm];
	    x[jm] = x[im];
	    y[jm] = y[im];
	    x[im] = t1;
	    y[im] = t2;
	}
	k = nv2;
	while ( j > k ) {
	    j -= k;
	    k /= 2;
	}
	j += k;
    }
    return(TRUE);
}

/*-----------------------------------------------------------------------*/
ifft ( l, x, y )
int l;
double *x, *y;
/* Compute the discrete inverse Fourier transform of the 2**l complex sequence
 * in x (real) and y (imaginary).  The DFT is computed in place and the
 * Fourier coefficients are returned in x and y.
 */
{
register double	c, s,  t1, t2;
register int	j1, j2, li, lix, i;
int	np, lmx, lo, lixnp, lm, j, nv2, k, im, jm;

    for ( np=1, i=1; i <= l; i++) np *= 2;

    if(fft_tablesize < (np/2)) {
	printf("\nPrecomputed trig tables are not big enough in ifft().\n");
	return(FALSE);
    }
    k = (fft_tablesize * 2)/np;
        
    for (lmx=np, lo=0; lo < l; lo++, k *= 2) {
	lix = lmx;
	lmx /= 2;
	lixnp = np - lix;
	for (i=0, lm=0; lm<lmx; lm++, i += k ) {
	    c = cosine[i];
	    s = - sine[i];
	    for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li;
	    				j1+=lix, j2+=lix ) {
		t1 = x[j1] - x[j2];
		t2 = y[j1] - y[j2];
		x[j1] += x[j2];
		y[j1] += y[j2];
		x[j2] = (c * t1) + (s * t2);
		y[j2] = (c * t2) - (s * t1);
	    }
	}
    }

	/* Now perform the bit reversal. */

    j = 1;
    nv2 = np/2;
    for ( i=1; i < np; i++ ) {
	if ( j < i ) {
	    jm = j-1;
	    im = i-1;
	    t1 = x[jm];
	    t2 = y[jm];
	    x[jm] = x[im];
	    y[jm] = y[im];
	    x[im] = t1;
	    y[im] = t2;
	}
	k = nv2;
	while ( j > k ) {
	    j -= k;
	    k /= 2;
	}
	j += k;
    }
    return(TRUE);
}

#include	"theftable.c"	/*  floating table of sines and cosines */
/*-----------------------------------------------------------------------*/
ffft ( l, x, y )
int l;
float *x, *y;
/* Compute the discrete Fourier transform of the 2**l complex sequence
 * in x (real) and y (imaginary).  The DFT is computed in place and the
 * Fourier coefficients are returned in x and y.
 */
{
register float	c, s,  t1, t2;
register int	j1, j2, li, lix, i;
int	np, lmx, lo, lixnp, lm, j, nv2, k, im, jm;

    for ( np=1, i=1; i <= l; i++) np *= 2;

    if(fft_ftablesize < (np/2)) {
	printf("\nPrecomputed trig tables are not big enough in fft().\n");
	return(FALSE);
    }
    k = (fft_ftablesize * 2)/np;
        
    for (lmx=np, lo=0; lo < l; lo++, k *= 2) {
	lix = lmx;
	lmx /= 2;
	lixnp = np - lix;
	for (i=0, lm=0; lm<lmx; lm++, i += k ) {
	    c = fcosine[i];
	    s = fsine[i];
	    for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li;
	    				j1+=lix, j2+=lix ) {
		t1 = x[j1] - x[j2];
		t2 = y[j1] - y[j2];
		x[j1] += x[j2];
		y[j1] += y[j2];
		x[j2] = (c * t1) + (s * t2);
		y[j2] = (c * t2) - (s * t1);
	    }
	}
    }

	/* Now perform the bit reversal. */

    j = 1;
    nv2 = np/2;
    for ( i=1; i < np; i++ ) {
	if ( j < i ) {
	    jm = j-1;
	    im = i-1;
	    t1 = x[jm];
	    t2 = y[jm];
	    x[jm] = x[im];
	    y[jm] = y[im];
	    x[im] = t1;
	    y[im] = t2;
	}
	k = nv2;
	while ( j > k ) {
	    j -= k;
	    k /= 2;
	}
	j += k;
    }
    return(TRUE);
}

/*-----------------------------------------------------------------------*/
fifft ( l, x, y )
int l;
float *x, *y;
/* Compute the discrete inverse Fourier transform of the 2**l complex sequence
 * in x (real) and y (imaginary).  The DFT is computed in place and the
 * Fourier coefficients are returned in x and y.
 */
{
register float	c, s,  t1, t2;
register int	j1, j2, li, lix, i;
int	np, lmx, lo, lixnp, lm, j, nv2, k, im, jm;

    for ( np=1, i=1; i <= l; i++) np *= 2;

    if(fft_ftablesize < (np/2)) {
	printf("\nPrecomputed trig tables are not big enough in ifft().\n");
	return(FALSE);
    }
    k = (fft_ftablesize * 2)/np;
        
    for (lmx=np, lo=0; lo < l; lo++, k *= 2) {
	lix = lmx;
	lmx /= 2;
	lixnp = np - lix;
	for (i=0, lm=0; lm<lmx; lm++, i += k ) {
	    c = fcosine[i];
	    s = - fsine[i];
	    for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li;
	    				j1+=lix, j2+=lix ) {
		t1 = x[j1] - x[j2];
		t2 = y[j1] - y[j2];
		x[j1] += x[j2];
		y[j1] += y[j2];
		x[j2] = (c * t1) + (s * t2);
		y[j2] = (c * t2) - (s * t1);
	    }
	}
    }

	/* Now perform the bit reversal. */

    j = 1;
    nv2 = np/2;
    for ( i=1; i < np; i++ ) {
	if ( j < i ) {
	    jm = j-1;
	    im = i-1;
	    t1 = x[jm];
	    t2 = y[jm];
	    x[jm] = x[im];
	    y[jm] = y[im];
	    x[im] = t1;
	    y[im] = t2;
	}
	k = nv2;
	while ( j > k ) {
	    j -= k;
	    k /= 2;
	}
	j += k;
    }
    return(TRUE);
}
#endif

/*********************************************************************/
/* Simple-minded real DFT (slooooowww) */
dft(n,x,re,im)
     register int n;
     double *x, *re, *im;
{
  register int m = n/2, i, j, k;
  register double arg, sr, si, a, *rp;

  for(i=0; i <= m; i++) {
    arg = i * 3.1415927/m;
    for(rp=x, j=0, sr=0.0, si=0.0; j < n; j++) {
      sr += cos((a = j*arg))* *rp;
      si += sin(a) * *rp++;
    }
    *re++ = sr;
    *im++ = si;
  }
}

/*		lbpoly.c		*/
/*					*/
/* A polynomial root finder using the Lin-Bairstow method (outlined
	in R.W. Hamming, "Numerical Methods for Scientists and
	Engineers," McGraw-Hill, 1962, pp 356-359.)		*/


#define MAX_ITS	100	/* Max iterations before trying new starts */
#define MAX_TRYS	100	/* Max number of times to try new starts */
#define MAX_ERR		1.e-6	/* Max acceptable error in quad factor */

qquad(a,b,c,r1r,r1i,r2r,r2i) /* find x, where a*x**2 + b*x + c = 0 	*/
double	a, b, c;
double *r1r, *r2r, *r1i, *r2i; /* return real and imag. parts of roots */
{
double  sqrt(), numi;
double  den, y;

	if(a == 0.0){
		if(b == 0){
		   printf("Bad coefficients to _quad().\n");
		   return(FALSE);
		}
		*r1r = -c/b;
		*r1i = *r2r = *r2i = 0;
		return(TRUE);
	}
	numi = b*b - (4.0 * a * c);
	if(numi >= 0.0) {
		/*
		 * Two forms of the quadratic formula:
		 *  -b + sqrt(b^2 - 4ac)           2c
		 *  ------------------- = --------------------
		 *           2a           -b - sqrt(b^2 - 4ac)
		 * The r.h.s. is numerically more accurate when
		 * b and the square root have the same sign and
		 * similar magnitudes.
		 */
		*r1i = *r2i = 0.0;
		if(b < 0.0) {
			y = -b + sqrt(numi);
			*r1r = y / (2.0 * a);
			*r2r = (2.0 * c) / y;
		}
		else {
			y = -b - sqrt(numi);
			*r1r = (2.0 * c) / y;
			*r2r = y / (2.0 * a);
		}
		return(TRUE);
	}
	else {
		den = 2.0 * a;
		*r1i = sqrt( -numi )/den;
		*r2i = -*r1i;
		*r2r = *r1r = -b/den;
		return(TRUE);
	}
}

lbpoly(a, order, rootr, rooti) /* return FALSE on error */
    double  *a;		    /* coeffs. of the polynomial (increasing order) */
    int	    order;	    /* the order of the polynomial */
    double  *rootr, *rooti; /* the real and imag. roots of the polynomial */
    /* Rootr and rooti are assumed to contain starting points for the root
       search on entry to lbpoly(). */
{
    int	    ord, ordm1, ordm2, itcnt, i, j, k, mmk, mmkp2, mmkp1, ntrys;
    double  fabs(), err, p, q, delp, delq, b[MAXORDER], c[MAXORDER], den;
    double  lim0 = 0.5*sqrt(DBL_MAX);

    for(ord = order; ord > 2; ord -= 2){
	ordm1 = ord-1;
	ordm2 = ord-2;
	/* Here is a kluge to prevent UNDERFLOW! (Sometimes the near-zero
	   roots left in rootr and/or rooti cause underflow here...	*/
	if(fabs(rootr[ordm1]) < 1.0e-10) rootr[ordm1] = 0.0;
	if(fabs(rooti[ordm1]) < 1.0e-10) rooti[ordm1] = 0.0;
	p = -2.0 * rootr[ordm1]; /* set initial guesses for quad factor */
	q = (rootr[ordm1] * rootr[ordm1]) + (rooti[ordm1] * rooti[ordm1]);
	for(ntrys = 0; ntrys < MAX_TRYS; ntrys++)
	{
	    int	found = FALSE;

	    for(itcnt = 0; itcnt < MAX_ITS; itcnt++)
	    {
		double	lim = lim0 / (1 + fabs(p) + fabs(q));

		b[ord] = a[ord];
		b[ordm1] = a[ordm1] - (p * b[ord]);
		c[ord] = b[ord];
		c[ordm1] = b[ordm1] - (p * c[ord]);
		for(k = 2; k <= ordm1; k++){
		    mmk = ord - k;
		    mmkp2 = mmk+2;
		    mmkp1 = mmk+1;
		    b[mmk] = a[mmk] - (p* b[mmkp1]) - (q* b[mmkp2]);
		    c[mmk] = b[mmk] - (p* c[mmkp1]) - (q* c[mmkp2]);
		    if (b[mmk] > lim || c[mmk] > lim)
			break;
		}
		if (k > ordm1) { /* normal exit from for(k ... */
		    /* ????	b[0] = a[0] - q * b[2];	*/
		    b[0] = a[0] - p * b[1] - q * b[2];
		    if (b[0] <= lim) k++;
		}
		if (k <= ord)	/* Some coefficient exceeded lim; */
		    break;	/* potential overflow below. */

		err = fabs(b[0]) + fabs(b[1]);

		if(err <= MAX_ERR) {
		    found = TRUE;
		    break;
		}

		den = (c[2] * c[2]) - (c[3] * (c[1] - b[1]));  
		if(den == 0.0)
		    break;

		delp = ((c[2] * b[1]) - (c[3] * b[0]))/den;
		delq = ((c[2] * b[0]) - (b[1] * (c[1] - b[1])))/den;  

		/* printf("\nerr=%f  delp=%f  delq=%f  p=%f  q=%f",
		   err,delp,delq,p,q); */

		p += delp;
		q += delq;

	    } /* for(itcnt... */

	    if (found)		/* we finally found the root! */
		break;
	    else { /* try some new starting values */
		p = ((double)rand() - (1<<30))/(1<<31);
		q = ((double)rand() - (1<<30))/(1<<31);
		/* fprintf(stderr, "\nTried new values: p=%f  q=%f\n",p,q); */
	    }

	} /* for(ntrys... */
	if((itcnt >= MAX_ITS) && (ntrys >= MAX_TRYS)){
	    printf("Exceeded maximum trial count in _lbpoly.\n");
	    return(FALSE);
	}

	if(!qquad(1.0, p, q,
		  &rootr[ordm1], &rooti[ordm1], &rootr[ordm2], &rooti[ordm2]))
	    return(FALSE);

	/* Update the coefficient array with the coeffs. of the
	   reduced polynomial. */
	for( i = 0; i <= ordm2; i++) a[i] = b[i+2];
    }

    if(ord == 2){		/* Is the last factor a quadratic? */
	if(!qquad(a[2], a[1], a[0],
		  &rootr[1], &rooti[1], &rootr[0], &rooti[0]))
	    return(FALSE);
	return(TRUE);
    }
    if(ord < 1) {
	printf("Bad ORDER parameter in _lbpoly()\n");
	return(FALSE);
    }

    if( a[1] != 0.0) rootr[0] = -a[0]/a[1];
    else {
	rootr[0] = 100.0;	/* arbitrary recovery value */
	printf("Numerical problems in lbpoly()\n");
    }
    rooti[0] = 0.0;

    return(TRUE);
}

/*
main()
{
  int i,n;
  int j;
  double a[MAXORDER],rootr[MAXORDER],rooti[MAXORDER];

  while(1){
    printf("\nOrder of the polynomial to be solved: ");
    scanf("%d",&j);
    n = j;
    printf("\nEnter the coefficients of the polynomial in increasing order.\n");
    for(i=0;i <= n;i++){
      printf("a\(%2d\) : ",i);
      scanf("%lf",&a[i]);
      rootr[i] = 22.0; 
      rooti[i] = -10.0;
    }
    if(! lbpoly(a,n,rootr,rooti)){
      printf("\nProblem in root solver.\n");
    } else {
      for(i=0;i<n;i++)printf("\n%d  re %f    im%f",i,rootr[i],rooti[i]);
    }
  }
}
*/


#ifdef STANDALONE
main(ac, av)
     int ac;
     char **av;
{
  int i,j,k, nfft=9;
  FILE *fd, *fopen();
  double x[4096], y[4096];
  char line[500];

  if((fd = fopen(av[1],"r"))) {
    i=0;
    while((i < 512) && fgets(line,500,fd)) {
      sscanf(line,"%lf",&x[i]);
      y[i++] = 0.0;
    }
    for( ; i < 512; i++) {
      x[i] = y[i] = 0.0;
    }
    fft(nfft,x,y);
    log_mag(x,y,x,257);
    for(i=0; i<257; i++) {
      printf("%4d %7.3lf\n",i,x[i]);
    }
  }
}
#endif