1/* This software has been licensed to the Centre of Speech Technology, KTH 2 * by AT&T Corp. and Microsoft Corp. with the terms in the accompanying 3 * file BSD.txt, which is a BSD style license. 4*/ 5/* Copyright (c) 1995 Entropic Research Laboratory, Inc. */ 6/* Copyright (c) 1987 AT&T */ 7/* All Rights Reserved */ 8 9#include <stdio.h> 10#include <math.h> 11#include <string.h> 12#include "snack.h" 13 14 15static double *pxl,*pa,*py,*pyl,*pa1,*px; 16void dlwrtrn(a,n,x,y) 17double *a,*x,*y; int *n; 18/* routine to solve ax=y with cholesky 19 a - nxn matrix 20 x,y -vectors 21 */ 22{ 23 double sm; 24 *x = *y / *a; 25 pxl = x + 1; 26 pyl = y + *n; 27 pa = a + *n; 28 for(py=y+1;py<pyl;py++,pxl++){ 29 sm = *py; 30 pa1 = pa; 31 for(px=x;px<pxl;px++) 32 sm = sm - *(pa1++) * *px; 33 pa += *n; 34 *px = sm / *pa1; 35 } 36} 37 38static double *pa1,*pa2,*pa3,*pa4,*pa5,*pc; 39void dreflpc(c,a,n) double *c,*a; int *n;{ 40double ta1; 41/* convert ref to lpc 42 c - ref 43 a - polyn 44 n - no of coef 45 */ 46*a = 1.; 47*(a+1) = *c; 48pc = c; pa2 = a+ *n; 49for(pa1=a+2;pa1<=pa2;pa1++) 50 { 51 pc++; 52 *pa1 = *pc; 53 pa5 = a + (pa1-a)/2; 54 pa4 = pa1 - 1; 55 for(pa3=a+1;pa3<=pa5;pa3++,pa4--) 56 { 57 ta1 = *pa3 + *pc * *pa4; 58 *pa4 = *pa4 + *pa3 * *pc; 59 *pa3 = ta1; 60 } 61 } 62} 63 64double *pa_1,*pa_2,*pa_3,*pa_4,*pa_5,*pal,*pt; 65int dchlsky(a,n,t,det) 66double *a,*t,*det; 67int *n; 68/* performs cholesky decomposition 69 a - l * l(transpose) 70 l - lower triangle 71 det det(a) 72 a - nxn matrix 73 return - no of reduced elements 74 results in lower half + diagonal 75 upper half undisturbed. 76 */ 77{ 78 double sm; 79 double sqrt(); 80 int m; 81 *det = 1.; 82 m = 0; 83 pal = a + *n * *n; 84 for(pa_1=a;pa_1<pal;pa_1+= *n){ 85 pa_3=pa_1; 86 pt = t; 87 for(pa_2=a;pa_2<=pa_1;pa_2+= *n){ 88 sm = *pa_3; /*a(i,j)*/ 89 pa_5 = pa_2; 90 for(pa_4=pa_1;pa_4<pa_3;pa_4++) 91 sm = sm - *pa_4 * *(pa_5++); 92 if(pa_1==pa_2){ 93 if(sm<=0.)return(m); 94 *pt = sqrt(sm); 95 *det = *det * *pt; 96 *(pa_3++) = *pt; 97 m++; 98 *pt = 1. / *pt; 99 pt++; 100 } 101 else 102 *(pa_3++) = sm * *(pt++); 103 } 104 } 105 return(m); 106} 107 108 109static double *pp,*ppl,*pa; 110int dcovlpc(p,s,a,n,c) 111double *p,*s,*a,*c; 112int *n; 113/* solve p*a=s using stabilized covariance method 114 p - cov nxn matrix 115 s - corrvec 116 a lpc coef *a = 1. 117 c - ref coefs 118 */ 119{ 120 double ee; 121 double sqrt(),ps,ps1,thres,d; 122 int m,n1; 123 m = dchlsky(p,n,c,&d); 124 dlwrtrn(p,n,c,s); 125 thres = 1.0e-31; 126 n1 = *n + 1; 127 ps = *(a + *n); 128 ps1 = 1.e-8*ps; 129 ppl = p + *n * m; 130 m = 0; 131 for(pp=p;pp<ppl;pp+=n1){ 132 if(*pp<thres)break; 133 m++; 134 } 135 ee = ps; 136 ppl = c + m; pa = a; 137 for(pp=c;pp<ppl;pp++){ 138 ee = ee - *pp * *pp; 139 if(ee<thres)break; 140 if(ee<ps1)fprintf(stderr,"*w* covlpc is losing accuracy\n"); 141 *(pa++) = sqrt(ee); 142 } 143 m = pa - a; 144 *c = - *c/sqrt(ps); 145 ppl = c + m; pa = a; 146 for(pp=c+1;pp<ppl;pp++) 147 *pp = - *pp / *(pa++); 148 dreflpc(c,a,&m); 149 ppl = a + *n; 150 for(pp=a+m+1;pp<=ppl;pp++)*pp=0.; 151 return(m); 152} 153 154/* cov mat for wtd lpc */ 155static double *pdl1,*pdl2,*pdl3,*pdl4,*pdl5,*pdl6,*pdll; 156void dcwmtrx(s,ni,nl,np,phi,shi,ps,w) 157double *s,*phi,*shi,*ps,*w; int *ni,*nl,*np; 158{ 159 double sm; 160 int i,j; 161 *ps = 0; 162 for(pdl1=s+*ni,pdl2=w,pdll=s+*nl;pdl1<pdll;pdl1++,pdl2++) 163 *ps += *pdl1 * *pdl1 * *pdl2; 164 165 for(pdl3=shi,pdl4=shi+*np,pdl5=s+*ni;pdl3<pdl4;pdl3++,pdl5--){ 166 *pdl3 = 0.; 167 for(pdl1=s+*ni,pdl2=w,pdll=s+*nl,pdl6=pdl5-1; 168 pdl1<pdll;pdl1++,pdl2++,pdl6++) 169 *pdl3 += *pdl1 * *pdl6 * *pdl2; 170 171 } 172 173 for(i=0;i<*np;i++) 174 for(j=0;j<=i;j++){ 175 sm = 0.; 176 for(pdl1=s+*ni-i-1,pdl2=s+*ni-j-1,pdl3=w,pdll=s+*nl-i-1; 177 pdl1<pdll;) 178 sm += *pdl1++ * *pdl2++ * *pdl3++; 179 180 *(phi + *np * i + j) = sm; 181 *(phi + *np * j + i) = sm; 182 } 183} 184 185double *psl,*pp2,*ppl2,*pc2,*pcl,*pph1,*pph2,*pph3,*pphl; 186int dlpcwtd(s,ls,p,np,c,phi,shi,xl,w) 187double *s,*p,*c,*phi,*shi,*xl,*w; 188int *ls,*np; 189/* pred anal subroutine with ridge reg 190 s - speech 191 ls - length of s 192 p - pred coefs 193 np - polyn order 194 c - ref coers 195 phi - cov matrix 196 shi - cov vect 197 */ 198{ 199int m,np1,mm; 200double d,pre,pre3,pre2,pre0,pss,pss7,thres; 201double ee; 202np1 = *np + 1; 203dcwmtrx(s,np,ls,np,phi,shi,&pss,w); 204if(*xl>=1.0e-4) 205 { 206 pph1 = phi; ppl2 = p + *np; 207 for(pp2=p;pp2<ppl2;pp2++){ 208 *pp2 = *pph1; 209 pph1 += np1; 210 } 211 *ppl2 = pss; 212 pss7 = .0000001 * pss; 213 mm = dchlsky(phi,np,c,&d); 214 if(mm< *np)fprintf(stderr,"LPCHFA error covariance matric rank %d \n",mm); 215 dlwrtrn(phi,np,c,shi); 216 ee = pss; 217 thres = 0.; 218 pph1 = phi; pcl = c + mm; 219 for(pc2=c;pc2<pcl;pc2++) 220 { 221 if(*pph1<thres)break; 222 ee = ee - *pc2 * *pc2; 223 if(ee<thres)break; 224 if(ee<pss7) 225 fprintf(stderr,"LPCHFA is losing accuracy\n"); 226 } 227 m = pc2 - c; 228 if(m != mm) 229 fprintf(stderr,"*W* LPCHFA error - inconsistent value of m %d \n",m); 230 pre = ee * *xl; 231 pphl = phi + *np * *np; 232 for(pph1=phi+1;pph1<pphl;pph1+=np1) 233 { 234 pph2 = pph1; 235 for(pph3=pph1+ *np-1;pph3<pphl;pph3+= *np) 236 { 237 *pph3 = *(pph2++); 238 } 239 } 240 pp2 = p; pre3 = .375 * pre; pre2 = .25 * pre; pre0 = .0625 * pre; 241 for(pph1=phi;pph1<pphl;pph1+=np1) 242 { 243 *pph1 = *(pp2++) + pre3; 244 if((pph2=pph1- *np)>phi) 245 *pph2 = *(pph1-1) = *pph2 - pre2; 246 if((pph3=pph2- *np)>phi) 247 *pph3 = *(pph1-2) = *pph3 + pre0; 248 } 249 *shi -= pre2; 250 *(shi+1) += pre0; 251 *(p+ *np) = pss + pre3; 252 } 253m = dcovlpc(phi,shi,p,np,c); 254return(m); 255} 256 257/* 258 * 259 * An implementation of the Le Roux - Gueguen PARCOR computation. 260 * See: IEEE/ASSP June, 1977 pp 257-259. 261 * 262 * Author: David Talkin Jan., 1984 263 * 264 */ 265 266#include <stdio.h> 267#include <stdlib.h> 268#include <math.h> 269#include <fcntl.h> 270#ifndef DBL_MAX 271#define DBL_MAX 1.7976931348623157E+308 272#endif 273/*#include <esps/limits.h>*/ 274 /* for definition of DBL_MAX */ 275 276#define MAXORDER 60 /* maximum permissible LPC order */ 277#define FALSE 0 278#define TRUE 1 279 280#ifndef M_PI 281#define M_PI 3.14159265358979323846 282#define M_PI_2 1.57079632679489661923 283#define M_PI_4 0.78539816339744830962 284#endif 285 286 287void lgsol(p, r, k, ex) 288/* p = The order of the LPC analysis. 289 * r = The array of p+1 autocorrelation coefficients. 290 * k = The array of PARCOR coefficients returned by lgsol. 291 * ex = The normalized prediction residual or "gain." 292 * (Errors are flagged by ex < 0.) 293 * All coefficients are returned in "normal" signed format, 294 * i.e. a[0] is assumed to be = 1. 295 */ 296 297register int p; 298register double *r, *k, *ex; 299 300{ 301register int m, h; 302double rl[MAXORDER+1], ep[MAXORDER], en[MAXORDER]; 303register double *q, *s, temp; 304 305 if( p > MAXORDER ){ 306 printf("\n Specified lpc order to large in lgsol.\n"); 307 *ex = -1.; /* Errors flagged by "ex" less than 0. */ 308 return; 309 } 310 if( *r <= 0.){ 311 printf("\n Bad autocorelation coefficients in lgsol.\n"); 312 *ex = -2.; 313 return; 314 } 315 if( *r != 1.){ /* Normalize the autocorrelation coeffs. */ 316 for( q = rl+1, s = r+1, m = 0; m < p; m++, q++, s++){ 317 *q = *s / *r; 318 } 319 *rl = 1.; 320 q = rl; /* point to local array of normalized coeffs. */ 321 322 } 323 else /* Point to normalized remote array. */ 324 q = r; 325 326 327/* Begin the L-G recursion. */ 328 for( s = q, m = 0; m < p; m++, s++){ 329 ep[m] = *(s+1); 330 en[m] = *s; 331 } 332 for( h = 0; h < p; h++){ 333 k[h] = -ep[h]/en[0]; 334 *en += k[h]*ep[h]; 335 if(h == p-1) break; 336 337 ep[p-1] += k[h]*en[p-h-1]; 338 for( m = h+1; m < p-1; m++){ 339 temp = ep[m] + k[h]*en[m-h]; 340 en[m-h] += k[h]*ep[m]; 341 ep[m] = temp; 342 } 343 } 344 *ex = *en; 345} 346 347/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 348void rwindow(din, dout, n, preemp) 349 register short *din; 350 register double *dout, preemp; 351 register int n; 352{ 353 register short *p; 354 355/* If preemphasis is to be performed, this assumes that there are n+1 valid 356 samples in the input buffer (din). */ 357 if(preemp != 0.0) { 358 for( p=din+1; n-- > 0; ) 359 *dout++ = (double)(*p++) - (preemp * *din++); 360 } else { 361 for( ; n-- > 0; ) 362 *dout++ = *din++; 363 } 364} 365 366 367/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 368void cwindow(din, dout, n, preemp) 369 register short *din; 370 register double *dout, preemp; 371 register int n; 372{ 373 register int i; 374 register short *p; 375 static int wsize = 0; 376 static double *wind=NULL; 377 register double *q, co; 378 379 if(wsize != n) { /* Need to create a new cos**4 window? */ 380 register double arg, half=0.5; 381 382 if(wind) wind = (double*)ckrealloc((void *)wind,n*sizeof(double)); 383 else wind = (double*)ckalloc(n*sizeof(double)); 384 wsize = n; 385 for(i=0, arg=3.1415927*2.0/(wsize), q=wind; i < n; ) { 386 co = half*(1.0 - cos((half + (double)i++) * arg)); 387 *q++ = co * co * co * co; 388 } 389 } 390/* If preemphasis is to be performed, this assumes that there are n+1 valid 391 samples in the input buffer (din). */ 392 if(preemp != 0.0) { 393 for(i=n, p=din+1, q=wind; i-- > 0; ) 394 *dout++ = *q++ * ((double)(*p++) - (preemp * *din++)); 395 } else { 396 for(i=n, q=wind; i-- > 0; ) 397 *dout++ = *q++ * *din++; 398 } 399} 400 401 402/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 403void hwindow(din, dout, n, preemp) 404 register short *din; 405 register double *dout, preemp; 406 register int n; 407{ 408 register int i; 409 register short *p; 410 static int wsize = 0; 411 static double *wind=NULL; 412 register double *q; 413 414 if(wsize != n) { /* Need to create a new Hamming window? */ 415 register double arg, half=0.5; 416 417 if(wind) wind = (double*)ckrealloc((void *)wind,n*sizeof(double)); 418 else wind = (double*)ckalloc(n*sizeof(double)); 419 wsize = n; 420 for(i=0, arg=3.1415927*2.0/(wsize), q=wind; i < n; ) 421 *q++ = (.54 - .46 * cos((half + (double)i++) * arg)); 422 } 423/* If preemphasis is to be performed, this assumes that there are n+1 valid 424 samples in the input buffer (din). */ 425 if(preemp != 0.0) { 426 for(i=n, p=din+1, q=wind; i-- > 0; ) 427 *dout++ = *q++ * ((double)(*p++) - (preemp * *din++)); 428 } else { 429 for(i=n, q=wind; i-- > 0; ) 430 *dout++ = *q++ * *din++; 431 } 432} 433 434 435/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 436void hnwindow(din, dout, n, preemp) 437 register short *din; 438 register double *dout, preemp; 439 register int n; 440{ 441 register int i; 442 register short *p; 443 static int wsize = 0; 444 static double *wind=NULL; 445 register double *q; 446 447 if(wsize != n) { /* Need to create a new Hamming window? */ 448 register double arg, half=0.5; 449 450 if(wind) wind = (double*)ckrealloc((void *)wind,n*sizeof(double)); 451 else wind = (double*)ckalloc(n*sizeof(double)); 452 wsize = n; 453 for(i=0, arg=3.1415927*2.0/(wsize), q=wind; i < n; ) 454 *q++ = (half - half * cos((half + (double)i++) * arg)); 455 } 456/* If preemphasis is to be performed, this assumes that there are n+1 valid 457 samples in the input buffer (din). */ 458 if(preemp != 0.0) { 459 for(i=n, p=din+1, q=wind; i-- > 0; ) 460 *dout++ = *q++ * ((double)(*p++) - (preemp * *din++)); 461 } else { 462 for(i=n, q=wind; i-- > 0; ) 463 *dout++ = *q++ * *din++; 464 } 465} 466 467/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 468int get_window(dout, n, type) 469 register double *dout; 470 register int n; 471{ 472 static short *din = NULL; 473 static int n0 = 0; 474 double preemp = 0.0; 475 476 if(n > n0) { 477 register short *p; 478 register int i; 479 480 if(din) ckfree((void *)din); 481 din = NULL; 482 if(!(din = (short*)ckalloc(sizeof(short)*n))) { 483 printf("Allocation problems in get_window()\n"); 484 return(FALSE); 485 } 486 for(i=0, p=din; i++ < n; ) *p++ = 1; 487 n0 = n; 488 } 489 switch(type) { 490 case 0: 491 rwindow(din, dout, n, preemp); 492 break; 493 case 1: 494 hwindow(din, dout, n, preemp); 495 break; 496 case 2: 497 cwindow(din, dout, n, preemp); 498 break; 499 case 3: 500 hnwindow(din, dout, n, preemp); 501 break; 502 default: 503 printf("Unknown window type (%d) requested in get_window()\n",type); 504 } 505 return(TRUE); 506} 507 508/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 509int get_float_window(fout, n, type) 510 register float *fout; 511 register int n; 512{ 513 static int n0 = 0; 514 static double *dout = NULL; 515 516 if(n > n0) { 517 if(dout)ckfree((void *)dout); 518 dout = NULL; 519 if(!(dout = (double*)ckalloc(sizeof(double)*n))) { 520 printf("Allocation problems in get_window()\n"); 521 return(FALSE); 522 } 523 n0 = n; 524 } 525 if(get_window(dout, n, type)) { 526 register int i; 527 register double *d; 528 529 for(i=0, d = dout; i++ < n; ) *fout++ = (float) *d++; 530 return(TRUE); 531 } 532 return(FALSE); 533} 534 535/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 536int fwindow(din, dout, n, preemp, type) 537 register short *din; 538 register float *dout, preemp; 539 register int n; 540{ 541 static float *fwind=NULL; 542 static int size=0, otype= (-100); 543 register int i; 544 register float *q; 545 register short *p; 546 547 if(size != n) { 548 if(fwind) fwind = (float*)ckrealloc((void *)fwind,sizeof(float)*(n+1)); 549 else fwind = (float*)ckalloc(sizeof(float)*(n+1)); 550 if(!fwind) { 551 printf("Allocation problems in fwindow\n"); 552 return(FALSE); 553 } 554 otype = -100; 555 size = n; 556 } 557 if(type != otype) { 558 get_float_window(fwind, n, type); 559 otype = type; 560 } 561/* If preemphasis is to be performed, this assumes that there are n+1 valid 562 samples in the input buffer (din). */ 563 if(preemp != 0.0) { 564 for(i=n, p=din+1, q=fwind; i-- > 0; ) 565 *dout++ = (float) (*q++ * ((float)(*p++) - (preemp * *din++))); 566 } else { 567 for(i=n, q=fwind; i-- > 0; ) 568 *dout++ = *q++ * *din++; 569 } 570 return(TRUE); 571} 572 573/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 574/* same as fwindow() but input is float */ 575int fwindow_f(din, dout, n, preemp, type) 576 register float *din; 577 register float *dout, preemp; 578 register int n; 579{ 580 static float *fwind=NULL; 581 static int size=0, otype= (-100); 582 register int i; 583 register float *q; 584 register float *p; 585 586 if(size != n) { 587 if(fwind) fwind = (float*)ckrealloc((void *)fwind,sizeof(float)*(n+1)); 588 else fwind = (float*)ckalloc(sizeof(float)*(n+1)); 589 if(!fwind) { 590 printf("Allocation problems in fwindow\n"); 591 return(FALSE); 592 } 593 otype = -100; 594 size = n; 595 } 596 if(type != otype) { 597 get_float_window(fwind, n, type); 598 otype = type; 599 } 600/* If preemphasis is to be performed, this assumes that there are n+1 valid 601 samples in the input buffer (din). */ 602 if(preemp != 0.0) { 603 for(i=n, p=din+1, q=fwind; i-- > 0; ) 604 *dout++ = (float) (*q++ * ((*p++) - (preemp * *din++))); 605 } else { 606 for(i=n, q=fwind; i-- > 0; ) 607 *dout++ = *q++ * *din++; 608 } 609 return(TRUE); 610} 611 612/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 613/* same as fwindow() but I/O is double */ 614int fwindow_d(din, dout, n, preemp, type) 615 register double *din; 616 register double *dout, preemp; 617 register int n; 618{ 619 static float *fwind=NULL; 620 static int size=0, otype= (-100); 621 register int i; 622 register float *q; 623 register double *p; 624 625 if(size != n) { 626 if(fwind) fwind = (float*)ckrealloc((void *)fwind,sizeof(float)*(n+1)); 627 else fwind = (float*)ckalloc(sizeof(float)*(n+1)); 628 if(!fwind) { 629 printf("Allocation problems in fwindow\n"); 630 return(FALSE); 631 } 632 otype = -100; 633 size = n; 634 } 635 if(type != otype) { 636 get_float_window(fwind, n, type); 637 otype = type; 638 } 639/* If preemphasis is to be performed, this assumes that there are n+1 valid 640 samples in the input buffer (din). */ 641 if(preemp != 0.0) { 642 for(i=n, p=din+1, q=fwind; i-- > 0; ) 643 *dout++ = *q++ * ((*p++) - (preemp * *din++)); 644 } else { 645 for(i=n, q=fwind; i-- > 0; ) 646 *dout++ = *q++ * *din++; 647 } 648 return(TRUE); 649} 650 651 652 653/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 654void w_window(din, dout, n, preemp, type) 655 register short *din; 656 register double *dout, preemp; 657 register int n; 658{ 659 switch(type) { 660 case 0: 661 rwindow(din, dout, n, preemp); 662 return; 663 case 1: 664 hwindow(din, dout, n, preemp); 665 return; 666 case 2: 667 cwindow(din, dout, n, preemp); 668 return; 669 case 3: 670 hnwindow(din, dout, n, preemp); 671 return; 672 default: 673 printf("Unknown window type (%d) requested in w_window()\n",type); 674 } 675} 676 677/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ 678void autoc( windowsize, s, p, r, e ) 679register int windowsize, p; 680register double *s, *r, *e; 681/* 682 * Compute the pp+1 autocorrelation lags of the windowsize samples in s. 683 * Return the normalized autocorrelation coefficients in r. 684 * The rms is returned in e. 685 */ 686{ 687 register int i, j; 688 register double *q, *t, sum, sum0; 689 690 for ( i=0, q=s, sum0=0.; i< windowsize; q++, i++){ 691 sum0 += (*q) * (*q); 692 } 693 *r = 1.; /* r[0] will always =1. */ 694 if ( sum0 == 0.){ /* No energy: fake low-energy white noise. */ 695 *e = 1.; /* Arbitrarily assign 1 to rms. */ 696 /* Now fake autocorrelation of white noise. */ 697 for ( i=1; i<=p; i++){ 698 r[i] = 0.; 699 } 700 return; 701 } 702 for( i=1; i <= p; i++){ 703 for( sum=0., j=0, q=s, t=s+i; j < (windowsize)-i; j++, q++, t++){ 704 sum += (*q) * (*t); 705 } 706 *(++r) = sum/sum0; 707 } 708 if(sum0 < 0.0) printf("lpcfloat:autoc(): sum0 = %f\n",sum0); 709 *e = sqrt(sum0/windowsize); 710} 711 712 713void k_to_a ( k, a, p ) 714register int p; 715register double *k, *a; 716/* 717 * Convert the p PARCOR parameters in k to LPC (AR) coefficients in a. 718 */ 719{ 720 int i,j; 721 double b[MAXORDER]; 722 723 *a = *k; 724 for ( i=1; i < p; i++ ){ 725 a[i] = k[i]; 726 for ( j=0; j<=i; j++ ){ 727 b[j] = a[j]; 728 } 729 for ( j=0; j<i; j++ ){ 730 a[j] += k[i]*b[i-j-1]; 731 } 732 } 733} 734 735void durbin ( r, k, a, p, ex) 736register int p; 737register double *r, *k, *a, *ex; 738/* 739* Compute the AR and PARCOR coefficients using Durbin's recursion. 740* Note: Durbin returns the coefficients in normal sign format. 741* (i.e. a[0] is assumed to be = +1.) 742*/ 743{ 744 double b[MAXORDER]; 745 register int i, j; 746 register double e, s; 747 748 e = *r; 749 *k = -r[1]/e; 750 *a = *k; 751 e *= (1. - (*k) * (*k)); 752 for ( i=1; i < p; i++){ 753 s = 0; 754 for ( j=0; j<i; j++){ 755 s -= a[j] * r[i-j]; 756 } 757 k[i] = ( s - r[i+1] )/e; 758 a[i] = k[i]; 759 for ( j=0; j<=i; j++){ 760 b[j] = a[j]; 761 } 762 for ( j=0; j<i; j++){ 763 a[j] += k[i] * b[i-j-1]; 764 } 765 e *= ( 1. - (k[i] * k[i]) ); 766 } 767 *ex = e; 768} 769 770void a_to_aca ( a, b, c, p ) 771double *a, *b, *c; 772register int p; 773/* Compute the autocorrelations of the p LP coefficients in a. 774 * (a[0] is assumed to be = 1 and not explicitely accessed.) 775 * The magnitude of a is returned in c. 776 * 2* the other autocorrelation coefficients are returned in b. 777 */ 778{ 779 double s; 780 register short i, j, pm; 781 for ( s=1., i = 0; i < p; i++ ){ 782 s += (a[i] * a[i]); 783 } 784 *c = s; 785 pm = p-1; 786 for ( i = 0; i < p; i++){ 787 s = a[i]; 788 for ( j = 0; j < (pm-i); j++){ 789 s += (a[j] * a[j+i+1]); 790 } 791 b[i] = 2. * s; 792 } 793 794} 795 796double itakura ( p, b, c, r, gain ) 797register double *b, *c, *r, *gain; 798register int p; 799/* Compute the Itakura LPC distance between the model represented 800 * by the signal autocorrelation (r) and its residual (gain) and 801 * the model represented by an LPC autocorrelation (c, b). 802 * Both models are of order p. 803 * r is assumed normalized and r[0]=1 is not explicitely accessed. 804 * Values returned by the function are >= 1. 805 */ 806 { 807 double s; 808 register int i; 809 for( s= *c, i=0; i< p; i++){ 810 s += r[i] * b[i]; 811 } 812 return (s/ *gain); 813} 814 815int lpc(lpc_ord,lpc_stabl,wsize,data,lpca,ar,lpck,normerr,rms,preemp,type) 816 int lpc_ord, wsize, type; 817 double lpc_stabl, *lpca, *ar, *lpck, *normerr, *rms, preemp; 818 short *data; 819{ 820 static double *dwind=NULL; 821 static int nwind=0; 822 double rho[MAXORDER+1], k[MAXORDER], a[MAXORDER+1],*r,*kp,*ap,en,er; 823 double wfact = 1.0; 824 825 if((wsize <= 0) || (!data) || (lpc_ord > MAXORDER)) return(FALSE); 826 827 if(nwind != wsize) { 828 if(dwind) dwind = (double*)ckrealloc((void *)dwind,wsize*sizeof(double)); 829 else dwind = (double*)ckalloc(wsize*sizeof(double)); 830 if(!dwind) { 831 printf("Can't allocate scratch memory in lpc()\n"); 832 return(FALSE); 833 } 834 nwind = wsize; 835 } 836 837 w_window(data, dwind, wsize, preemp, type); 838 if(!(r = ar)) r = rho; 839 if(!(kp = lpck)) kp = k; 840 if(!(ap = lpca)) ap = a; 841 autoc( wsize, dwind, lpc_ord, r, &en ); 842 if(lpc_stabl > 1.0) { /* add a little to the diagonal for stability */ 843 int i; 844 double ffact; 845 ffact =1.0/(1.0 + exp((-lpc_stabl/20.0) * log(10.0))); 846 for(i=1; i <= lpc_ord; i++) rho[i] = ffact * r[i]; 847 *rho = *r; 848 r = rho; 849 if(ar) 850 for(i=0;i<=lpc_ord; i++) ar[i] = r[i]; /* copy out for possible use later */ 851 } 852 durbin ( r, kp, &ap[1], lpc_ord, &er); 853 854/* After talkin with David T., I decided to take the following correction 855out -- if people window, the resulting output (spectrum and power) should 856correspond to the windowed data, and we shouldn't try to back-correct 857to un-windowed data. */ 858 859/* switch(type) { / rms correction for window */ 860/* case 0: 861 wfact = 1.0; / rectangular */ 862/* break; 863 case 1: 864 wfact = .630397; / Hamming */ 865/* break; 866 case 2: 867 wfact = .443149; / (.5 - .5*cos)^4 */ 868/* break; 869 case 3: 870 wfact = .612372; / Hanning */ 871/* break; 872 } 873*/ 874 875 *ap = 1.0; 876 if(rms) *rms = en/wfact; 877 if(normerr) *normerr = er; 878 return(TRUE); 879} 880 881/* convert reflection (PARCOR) coefficients to areas */ 882void dreflar(c,a,n) double *c,*a; int n;{ 883/* refl to area */ 884 register double *pa,*pa1,*pc,*pcl; 885 pa = a + 1; pa1 = a; 886 *a = 1.; 887 for(pc=c,pcl=c+ n;pc<pcl;pc++) 888 *pa++ = *pa1++ * (1+*pc)/(1-*pc); 889 } 890 891/* covariance LPC analysis; originally from Markel and Gray */ 892/* (a translation from the fortran) */ 893 894int w_covar(xx,m,n,istrt,y,alpha,r0,preemp,w_type) 895 double *y, *alpha, *r0, preemp; 896 short *xx; 897 int *m,n,istrt; 898 int w_type; 899{ 900 static double *x=NULL; 901 static int nold = 0; 902 static int mold = 0; 903 static double *b = NULL, *beta = NULL, *grc = NULL, *cc = NULL, gam,s; 904 int ibeg, ibeg1, ibeg2, ibegmp, np0, ibegm1, msq, np, np1, mf, jp, ip, 905 mp, i, j, minc, n1, n2, n3, npb, msub, mm1, isub, m2; 906 int mnew = 0; 907 908 if((n+1) > nold) { 909 if(x) ckfree((void *)x); 910 x = NULL; 911 if(!(x = (double*)ckalloc((n+1)*sizeof(double)))) { 912 printf("Allocation failure in w_covar()\n"); 913 return(FALSE); 914 } 915 memset(x, 0, (n+1) * sizeof(double)); 916 nold = n+1; 917 } 918 919 if(*m > mold) { 920 if(b) ckfree((void *)b); if(beta) ckfree((void *)beta); if (grc) ckfree((void *)grc); if (cc) ckfree((void *)cc); 921 b = beta = grc = cc = NULL; 922 mnew = *m; 923 924 if(!((b = (double*)ckalloc(sizeof(double)*((mnew+1)*(mnew+1)/2))) && 925 (beta = (double*)ckalloc(sizeof(double)*(mnew+3))) && 926 (grc = (double*)ckalloc(sizeof(double)*(mnew+3))) && 927 (cc = (double*)ckalloc(sizeof(double)*(mnew+3))))) { 928 printf("Allocation failure in w_covar()\n"); 929 return(FALSE); 930 } 931 mold = mnew; 932 } 933 934 w_window(xx, x, n, preemp, w_type); 935 936 ibeg = istrt - 1; 937 ibeg1 = ibeg + 1; 938 mp = *m + 1; 939 ibegm1 = ibeg - 1; 940 ibeg2 = ibeg + 2; 941 ibegmp = ibeg + mp; 942 i = *m; 943 msq = ( i + i*i)/2; 944 for (i=1; i <= msq; i++) b[i] = 0.0; 945 *alpha = 0.0; 946 cc[1] = 0.0; 947 cc[2] = 0.0; 948 for(np=mp; np <= n; np++) { 949 np1 = np + ibegm1; 950 np0 = np + ibeg; 951 *alpha += x[np0] * x[np0]; 952 cc[1] += x[np0] * x[np1]; 953 cc[2] += x[np1] * x[np1]; 954 } 955 *r0 = *alpha; 956 b[1] = 1.0; 957 beta[1] = cc[2]; 958 grc[1] = -cc[1]/cc[2]; 959 y[0] = 1.0; 960 y[1] = grc[1]; 961 *alpha += grc[1]*cc[1]; 962 if( *m <= 1) return(FALSE); /* need to correct indices?? */ 963 mf = *m; 964 for( minc = 2; minc <= mf; minc++) { 965 for(j=1; j <= minc; j++) { 966 jp = minc + 2 - j; 967 n1 = ibeg1 + mp - jp; 968 n2 = ibeg1 + n - minc; 969 n3 = ibeg2 + n - jp; 970 cc[jp] = cc[jp - 1] + x[ibegmp-minc]*x[n1] - x[n2]*x[n3]; 971 } 972 cc[1] = 0.0; 973 for(np = mp; np <= n; np++) { 974 npb = np + ibeg; 975 cc[1] += x[npb-minc]*x[npb]; 976 } 977 msub = (minc*minc - minc)/2; 978 mm1 = minc - 1; 979 b[msub+minc] = 1.0; 980 for(ip=1; ip <= mm1; ip++) { 981 isub = (ip*ip - ip)/2; 982 if(beta[ip] <= 0.0) { 983 *m = minc-1; 984 return(TRUE); 985 } 986 gam = 0.0; 987 for(j=1; j <= ip; j++) 988 gam += cc[j+1]*b[isub+j]; 989 gam /= beta[ip]; 990 for(jp=1; jp <= ip; jp++) 991 b[msub+jp] -= gam*b[isub+jp]; 992 } 993 beta[minc] = 0.0; 994 for(j=1; j <= minc; j++) 995 beta[minc] += cc[j+1]*b[msub+j]; 996 if(beta[minc] <= 0.0) { 997 *m = minc-1; 998 return(TRUE); 999 } 1000 s = 0.0; 1001 for(ip=1; ip <= minc; ip++) 1002 s += cc[ip]*y[ip-1]; 1003 grc[minc] = -s/beta[minc]; 1004 for(ip=1; ip < minc; ip++) { 1005 m2 = msub+ip; 1006 y[ip] += grc[minc]*b[m2]; 1007 } 1008 y[minc] = grc[minc]; 1009 s = grc[minc]*grc[minc]*beta[minc]; 1010 *alpha -= s; 1011 if(*alpha <= 0.0) { 1012 if(minc < *m) *m = minc; 1013 return(TRUE); 1014 } 1015 } 1016 return(TRUE); 1017} 1018 1019/* Same as above, but returns alpha as a function of order. */ 1020int covar2(xx,m,n,istrt,y,alpha,r0,preemp) 1021 double *y, *alpha, *r0, preemp; 1022 short *xx; 1023 int *m,n,istrt; 1024{ 1025 static double *x=NULL; 1026 static int nold = 0; 1027 double b[513],beta[33],grc[33],cc[33],gam,s; 1028 int ibeg, ibeg1, ibeg2, ibegmp, np0, ibegm1, msq, np, np1, mf, jp, ip, 1029 mp, i, j, minc, n1, n2, n3, npb, msub, mm1, isub, m2; 1030 1031 if((n+1) > nold) { 1032 if(x) ckfree((void*)x); 1033 x = NULL; 1034 if(!(x = (double*)ckalloc(sizeof(double)*(n+1)))) { 1035 printf("Allocation failure in covar2()\n"); 1036 return(FALSE); 1037 } 1038 nold = n+1; 1039 } 1040 1041 for(i=1; i <= n; i++) x[i] = (double)xx[i] - preemp * xx[i-1]; 1042 ibeg = istrt - 1; 1043 ibeg1 = ibeg + 1; 1044 mp = *m + 1; 1045 ibegm1 = ibeg - 1; 1046 ibeg2 = ibeg + 2; 1047 ibegmp = ibeg + mp; 1048 i = *m; 1049 msq = ( i + i*i)/2; 1050 for (i=1; i <= msq; i++) b[i] = 0.0; 1051 *alpha = 0.0; 1052 cc[1] = 0.0; 1053 cc[2] = 0.0; 1054 for(np=mp; np <= n; np++) { 1055 np1 = np + ibegm1; 1056 np0 = np + ibeg; 1057 *alpha += x[np0] * x[np0]; 1058 cc[1] += x[np0] * x[np1]; 1059 cc[2] += x[np1] * x[np1]; 1060 } 1061 *r0 = *alpha; 1062 b[1] = 1.0; 1063 beta[1] = cc[2]; 1064 grc[1] = -cc[1]/cc[2]; 1065 y[0] = 1.0; 1066 y[1] = grc[1]; 1067 *alpha += grc[1]*cc[1]; 1068 if( *m <= 1) return(TRUE); /* need to correct indices?? */ 1069 mf = *m; 1070 for( minc = 2; minc <= mf; minc++) { 1071 for(j=1; j <= minc; j++) { 1072 jp = minc + 2 - j; 1073 n1 = ibeg1 + mp - jp; 1074 n2 = ibeg1 + n - minc; 1075 n3 = ibeg2 + n - jp; 1076 cc[jp] = cc[jp - 1] + x[ibegmp-minc]*x[n1] - x[n2]*x[n3]; 1077 } 1078 cc[1] = 0.0; 1079 for(np = mp; np <= n; np++) { 1080 npb = np + ibeg; 1081 cc[1] += x[npb-minc]*x[npb]; 1082 } 1083 msub = (minc*minc - minc)/2; 1084 mm1 = minc - 1; 1085 b[msub+minc] = 1.0; 1086 for(ip=1; ip <= mm1; ip++) { 1087 isub = (ip*ip - ip)/2; 1088 if(beta[ip] <= 0.0) { 1089 *m = minc-1; 1090 return(TRUE); 1091 } 1092 gam = 0.0; 1093 for(j=1; j <= ip; j++) 1094 gam += cc[j+1]*b[isub+j]; 1095 gam /= beta[ip]; 1096 for(jp=1; jp <= ip; jp++) 1097 b[msub+jp] -= gam*b[isub+jp]; 1098 } 1099 beta[minc] = 0.0; 1100 for(j=1; j <= minc; j++) 1101 beta[minc] += cc[j+1]*b[msub+j]; 1102 if(beta[minc] <= 0.0) { 1103 *m = minc-1; 1104 return(TRUE); 1105 } 1106 s = 0.0; 1107 for(ip=1; ip <= minc; ip++) 1108 s += cc[ip]*y[ip-1]; 1109 grc[minc] = -s/beta[minc]; 1110 for(ip=1; ip < minc; ip++) { 1111 m2 = msub+ip; 1112 y[ip] += grc[minc]*b[m2]; 1113 } 1114 y[minc] = grc[minc]; 1115 s = grc[minc]*grc[minc]*beta[minc]; 1116 alpha[minc-1] = alpha[minc-2] - s; 1117 if(alpha[minc-1] <= 0.0) { 1118 if(minc < *m) *m = minc; 1119 return(TRUE); 1120 } 1121 } 1122 return(TRUE); 1123} 1124 1125int lbpoly(); 1126 1127/* ---------------------------------------------------------- */ 1128/* Find the roots of the LPC denominator polynomial and convert the z-plane 1129 zeros to equivalent resonant frequencies and bandwidths. */ 1130/* The complex poles are then ordered by frequency. */ 1131int formant(lpc_order,s_freq,lpca,n_form,freq,band,init) 1132int lpc_order, /* order of the LP model */ 1133 *n_form, /* number of COMPLEX roots of the LPC polynomial */ 1134 init; /* preset to true if no root candidates are available */ 1135double s_freq, /* the sampling frequency of the speech waveform data */ 1136 *lpca, /* linear predictor coefficients */ 1137 *freq, /* returned array of candidate formant frequencies */ 1138 *band; /* returned array of candidate formant bandwidths */ 1139{ 1140 double x, flo, pi2t, theta; 1141 static double rr[MAXORDER], ri[MAXORDER]; 1142 int i,ii,iscomp1,iscomp2,fc,swit; 1143 1144 if(init){ /* set up starting points for the root search near unit circle */ 1145 x = M_PI/(lpc_order + 1); 1146 for(i=0;i<=lpc_order;i++){ 1147 flo = lpc_order - i; 1148 rr[i] = 2.0 * cos((flo + 0.5) * x); 1149 ri[i] = 2.0 * sin((flo + 0.5) * x); 1150 } 1151 } 1152 if(! lbpoly(lpca,lpc_order,rr,ri)){ /* find the roots of the LPC polynomial */ 1153 *n_form = 0; /* was there a problem in the root finder? */ 1154 return(FALSE); 1155 } 1156 1157 pi2t = M_PI * 2.0 /s_freq; 1158 1159 /* convert the z-plane locations to frequencies and bandwidths */ 1160 for(fc=0, ii=0; ii < lpc_order; ii++){ 1161 if((rr[ii] != 0.0)||(ri[ii] != 0.0)){ 1162 theta = atan2(ri[ii],rr[ii]); 1163 freq[fc] = fabs(theta / pi2t); 1164 if((band[fc] = 0.5 * s_freq * 1165 log(((rr[ii] * rr[ii]) + (ri[ii] * ri[ii])))/M_PI) < 0.0) 1166 band[fc] = -band[fc]; 1167 fc++; /* Count the number of real and complex poles. */ 1168 1169 if((rr[ii] == rr[ii+1])&&(ri[ii] == -ri[ii+1]) /* complex pole? */ 1170 && (ri[ii] != 0.0)) ii++; /* if so, don't duplicate */ 1171 } 1172 } 1173 1174 1175 /* Now order the complex poles by frequency. Always place the (uninteresting) 1176 real poles at the end of the arrays. */ 1177 theta = s_freq/2.0; /* temporarily hold the folding frequency. */ 1178 for(i=0; i < fc -1; i++){ /* order the poles by frequency (bubble) */ 1179 for(ii=0; ii < fc -1 -i; ii++){ 1180 /* Force the real poles to the end of the list. */ 1181 iscomp1 = (freq[ii] > 1.0) && (freq[ii] < theta); 1182 iscomp2 = (freq[ii+1] > 1.0) && (freq[ii+1] < theta); 1183 swit = (freq[ii] > freq[ii+1]) && iscomp2 ; 1184 if(swit || (iscomp2 && ! iscomp1)){ 1185 flo = band[ii+1]; 1186 band[ii+1] = band[ii]; 1187 band[ii] = flo; 1188 flo = freq[ii+1]; 1189 freq[ii+1] = freq[ii]; 1190 freq[ii] = flo; 1191 } 1192 } 1193 } 1194 /* Now count the complex poles as formant candidates. */ 1195 for(i=0, theta = theta - 1.0, ii=0 ; i < fc; i++) 1196 if( (freq[i] > 1.0) && (freq[i] < theta) ) ii++; 1197 *n_form = ii; 1198 1199 return(TRUE); 1200} 1201 1202 1203 1204/*-----------------------------------------------------------------------*/ 1205int log_mag(x,y,z,n) 1206 /* z <- (10 * log10(x^2 + y^2)) for n elements */ 1207double *x, *y, *z; 1208int n; 1209{ 1210register double *xp, *yp, *zp, t1, t2, ssq; 1211 1212 if(x && y && z && n) { 1213 for(xp=x+n, yp=y+n, zp=z+n; zp > z;) { 1214 t1 = *--xp; 1215 t2 = *--yp; 1216 ssq = (t1*t1)+(t2*t2); 1217 *--zp = (ssq > 0.0)? 10.0 * log10(ssq) : -200.0; 1218 } 1219 return(TRUE); 1220 } else { 1221 return(FALSE); 1222 } 1223} 1224 1225/*-----------------------------------------------------------------------*/ 1226int flog_mag(x,y,z,n) 1227 /* z <- (10 * log10(x^2 + y^2)) for n elements */ 1228float *x, *y, *z; 1229int n; 1230{ 1231register float *xp, *yp, *zp, t1, t2, ssq; 1232 1233 if(x && y && z && n) { 1234 for(xp=x+n, yp=y+n, zp=z+n; zp > z;) { 1235 t1 = *--xp; 1236 t2 = *--yp; 1237 ssq = (t1*t1)+(t2*t2); 1238 *--zp = (float) ((ssq > 0.0)? 10.0 * log10((double)ssq) : -200.0); 1239 } 1240 return(TRUE); 1241 } else { 1242 return(FALSE); 1243 } 1244} 1245 1246#ifdef USE_OLD_FFT 1247#include "thetable.c" /* table of sines and cosines */ 1248/*-----------------------------------------------------------------------*/ 1249fft ( l, x, y ) 1250int l; 1251double *x, *y; 1252/* Compute the discrete Fourier transform of the 2**l complex sequence 1253 * in x (real) and y (imaginary). The DFT is computed in place and the 1254 * Fourier coefficients are returned in x and y. 1255 */ 1256{ 1257register double c, s, t1, t2; 1258register int j1, j2, li, lix, i; 1259int np, lmx, lo, lixnp, lm, j, nv2, k, im, jm; 1260 1261 for ( np=1, i=1; i <= l; i++) np *= 2; 1262 1263 if(fft_tablesize < (np/2)) { 1264 printf("\nPrecomputed trig tables are not big enough in fft().\n"); 1265 return(FALSE); 1266 } 1267 k = (fft_tablesize * 2)/np; 1268 1269 for (lmx=np, lo=0; lo < l; lo++, k *= 2) { 1270 lix = lmx; 1271 lmx /= 2; 1272 lixnp = np - lix; 1273 for (i=0, lm=0; lm<lmx; lm++, i += k ) { 1274 c = cosine[i]; 1275 s = sine[i]; 1276 for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li; 1277 j1+=lix, j2+=lix ) { 1278 t1 = x[j1] - x[j2]; 1279 t2 = y[j1] - y[j2]; 1280 x[j1] += x[j2]; 1281 y[j1] += y[j2]; 1282 x[j2] = (c * t1) + (s * t2); 1283 y[j2] = (c * t2) - (s * t1); 1284 } 1285 } 1286 } 1287 1288 /* Now perform the bit reversal. */ 1289 1290 j = 1; 1291 nv2 = np/2; 1292 for ( i=1; i < np; i++ ) { 1293 if ( j < i ) { 1294 jm = j-1; 1295 im = i-1; 1296 t1 = x[jm]; 1297 t2 = y[jm]; 1298 x[jm] = x[im]; 1299 y[jm] = y[im]; 1300 x[im] = t1; 1301 y[im] = t2; 1302 } 1303 k = nv2; 1304 while ( j > k ) { 1305 j -= k; 1306 k /= 2; 1307 } 1308 j += k; 1309 } 1310 return(TRUE); 1311} 1312 1313/*-----------------------------------------------------------------------*/ 1314ifft ( l, x, y ) 1315int l; 1316double *x, *y; 1317/* Compute the discrete inverse Fourier transform of the 2**l complex sequence 1318 * in x (real) and y (imaginary). The DFT is computed in place and the 1319 * Fourier coefficients are returned in x and y. 1320 */ 1321{ 1322register double c, s, t1, t2; 1323register int j1, j2, li, lix, i; 1324int np, lmx, lo, lixnp, lm, j, nv2, k, im, jm; 1325 1326 for ( np=1, i=1; i <= l; i++) np *= 2; 1327 1328 if(fft_tablesize < (np/2)) { 1329 printf("\nPrecomputed trig tables are not big enough in ifft().\n"); 1330 return(FALSE); 1331 } 1332 k = (fft_tablesize * 2)/np; 1333 1334 for (lmx=np, lo=0; lo < l; lo++, k *= 2) { 1335 lix = lmx; 1336 lmx /= 2; 1337 lixnp = np - lix; 1338 for (i=0, lm=0; lm<lmx; lm++, i += k ) { 1339 c = cosine[i]; 1340 s = - sine[i]; 1341 for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li; 1342 j1+=lix, j2+=lix ) { 1343 t1 = x[j1] - x[j2]; 1344 t2 = y[j1] - y[j2]; 1345 x[j1] += x[j2]; 1346 y[j1] += y[j2]; 1347 x[j2] = (c * t1) + (s * t2); 1348 y[j2] = (c * t2) - (s * t1); 1349 } 1350 } 1351 } 1352 1353 /* Now perform the bit reversal. */ 1354 1355 j = 1; 1356 nv2 = np/2; 1357 for ( i=1; i < np; i++ ) { 1358 if ( j < i ) { 1359 jm = j-1; 1360 im = i-1; 1361 t1 = x[jm]; 1362 t2 = y[jm]; 1363 x[jm] = x[im]; 1364 y[jm] = y[im]; 1365 x[im] = t1; 1366 y[im] = t2; 1367 } 1368 k = nv2; 1369 while ( j > k ) { 1370 j -= k; 1371 k /= 2; 1372 } 1373 j += k; 1374 } 1375 return(TRUE); 1376} 1377 1378#include "theftable.c" /* floating table of sines and cosines */ 1379/*-----------------------------------------------------------------------*/ 1380ffft ( l, x, y ) 1381int l; 1382float *x, *y; 1383/* Compute the discrete Fourier transform of the 2**l complex sequence 1384 * in x (real) and y (imaginary). The DFT is computed in place and the 1385 * Fourier coefficients are returned in x and y. 1386 */ 1387{ 1388register float c, s, t1, t2; 1389register int j1, j2, li, lix, i; 1390int np, lmx, lo, lixnp, lm, j, nv2, k, im, jm; 1391 1392 for ( np=1, i=1; i <= l; i++) np *= 2; 1393 1394 if(fft_ftablesize < (np/2)) { 1395 printf("\nPrecomputed trig tables are not big enough in fft().\n"); 1396 return(FALSE); 1397 } 1398 k = (fft_ftablesize * 2)/np; 1399 1400 for (lmx=np, lo=0; lo < l; lo++, k *= 2) { 1401 lix = lmx; 1402 lmx /= 2; 1403 lixnp = np - lix; 1404 for (i=0, lm=0; lm<lmx; lm++, i += k ) { 1405 c = fcosine[i]; 1406 s = fsine[i]; 1407 for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li; 1408 j1+=lix, j2+=lix ) { 1409 t1 = x[j1] - x[j2]; 1410 t2 = y[j1] - y[j2]; 1411 x[j1] += x[j2]; 1412 y[j1] += y[j2]; 1413 x[j2] = (c * t1) + (s * t2); 1414 y[j2] = (c * t2) - (s * t1); 1415 } 1416 } 1417 } 1418 1419 /* Now perform the bit reversal. */ 1420 1421 j = 1; 1422 nv2 = np/2; 1423 for ( i=1; i < np; i++ ) { 1424 if ( j < i ) { 1425 jm = j-1; 1426 im = i-1; 1427 t1 = x[jm]; 1428 t2 = y[jm]; 1429 x[jm] = x[im]; 1430 y[jm] = y[im]; 1431 x[im] = t1; 1432 y[im] = t2; 1433 } 1434 k = nv2; 1435 while ( j > k ) { 1436 j -= k; 1437 k /= 2; 1438 } 1439 j += k; 1440 } 1441 return(TRUE); 1442} 1443 1444/*-----------------------------------------------------------------------*/ 1445fifft ( l, x, y ) 1446int l; 1447float *x, *y; 1448/* Compute the discrete inverse Fourier transform of the 2**l complex sequence 1449 * in x (real) and y (imaginary). The DFT is computed in place and the 1450 * Fourier coefficients are returned in x and y. 1451 */ 1452{ 1453register float c, s, t1, t2; 1454register int j1, j2, li, lix, i; 1455int np, lmx, lo, lixnp, lm, j, nv2, k, im, jm; 1456 1457 for ( np=1, i=1; i <= l; i++) np *= 2; 1458 1459 if(fft_ftablesize < (np/2)) { 1460 printf("\nPrecomputed trig tables are not big enough in ifft().\n"); 1461 return(FALSE); 1462 } 1463 k = (fft_ftablesize * 2)/np; 1464 1465 for (lmx=np, lo=0; lo < l; lo++, k *= 2) { 1466 lix = lmx; 1467 lmx /= 2; 1468 lixnp = np - lix; 1469 for (i=0, lm=0; lm<lmx; lm++, i += k ) { 1470 c = fcosine[i]; 1471 s = - fsine[i]; 1472 for ( li = lixnp+lm, j1 = lm, j2 = lm+lmx; j1<=li; 1473 j1+=lix, j2+=lix ) { 1474 t1 = x[j1] - x[j2]; 1475 t2 = y[j1] - y[j2]; 1476 x[j1] += x[j2]; 1477 y[j1] += y[j2]; 1478 x[j2] = (c * t1) + (s * t2); 1479 y[j2] = (c * t2) - (s * t1); 1480 } 1481 } 1482 } 1483 1484 /* Now perform the bit reversal. */ 1485 1486 j = 1; 1487 nv2 = np/2; 1488 for ( i=1; i < np; i++ ) { 1489 if ( j < i ) { 1490 jm = j-1; 1491 im = i-1; 1492 t1 = x[jm]; 1493 t2 = y[jm]; 1494 x[jm] = x[im]; 1495 y[jm] = y[im]; 1496 x[im] = t1; 1497 y[im] = t2; 1498 } 1499 k = nv2; 1500 while ( j > k ) { 1501 j -= k; 1502 k /= 2; 1503 } 1504 j += k; 1505 } 1506 return(TRUE); 1507} 1508#endif 1509 1510/*********************************************************************/ 1511/* Simple-minded real DFT (slooooowww) */ 1512void dft(n,x,re,im) 1513 register int n; 1514 double *x, *re, *im; 1515{ 1516 register int m = n/2, i, j; 1517 register double arg, sr, si, a, *rp; 1518 1519 for(i=0; i <= m; i++) { 1520 arg = i * 3.1415927/m; 1521 for(rp=x, j=0, sr=0.0, si=0.0; j < n; j++) { 1522 sr += cos((a = j*arg))* *rp; 1523 si += sin(a) * *rp++; 1524 } 1525 *re++ = sr; 1526 *im++ = si; 1527 } 1528} 1529 1530/* lbpoly.c */ 1531/* */ 1532/* A polynomial root finder using the Lin-Bairstow method (outlined 1533 in R.W. Hamming, "Numerical Methods for Scientists and 1534 Engineers," McGraw-Hill, 1962, pp 356-359.) */ 1535 1536 1537#define MAX_ITS 100 /* Max iterations before trying new starts */ 1538#define MAX_TRYS 100 /* Max number of times to try new starts */ 1539#define MAX_ERR 1.e-6 /* Max acceptable error in quad factor */ 1540 1541int qquad(a,b,c,r1r,r1i,r2r,r2i) /* find x, where a*x**2 + b*x + c = 0 */ 1542double a, b, c; 1543double *r1r, *r2r, *r1i, *r2i; /* return real and imag. parts of roots */ 1544{ 1545double sqrt(), numi; 1546double den, y; 1547 1548 if(a == 0.0){ 1549 if(b == 0){ 1550 printf("Bad coefficients to _quad().\n"); 1551 return(FALSE); 1552 } 1553 *r1r = -c/b; 1554 *r1i = *r2r = *r2i = 0; 1555 return(TRUE); 1556 } 1557 numi = b*b - (4.0 * a * c); 1558 if(numi >= 0.0) { 1559 /* 1560 * Two forms of the quadratic formula: 1561 * -b + sqrt(b^2 - 4ac) 2c 1562 * ------------------- = -------------------- 1563 * 2a -b - sqrt(b^2 - 4ac) 1564 * The r.h.s. is numerically more accurate when 1565 * b and the square root have the same sign and 1566 * similar magnitudes. 1567 */ 1568 *r1i = *r2i = 0.0; 1569 if(b < 0.0) { 1570 y = -b + sqrt(numi); 1571 *r1r = y / (2.0 * a); 1572 *r2r = (2.0 * c) / y; 1573 } 1574 else { 1575 y = -b - sqrt(numi); 1576 *r1r = (2.0 * c) / y; 1577 *r2r = y / (2.0 * a); 1578 } 1579 return(TRUE); 1580 } 1581 else { 1582 den = 2.0 * a; 1583 *r1i = sqrt( -numi )/den; 1584 *r2i = -*r1i; 1585 *r2r = *r1r = -b/den; 1586 return(TRUE); 1587 } 1588} 1589 1590int lbpoly(a, order, rootr, rooti) /* return FALSE on error */ 1591 double *a; /* coeffs. of the polynomial (increasing order) */ 1592 int order; /* the order of the polynomial */ 1593 double *rootr, *rooti; /* the real and imag. roots of the polynomial */ 1594 /* Rootr and rooti are assumed to contain starting points for the root 1595 search on entry to lbpoly(). */ 1596{ 1597 int ord, ordm1, ordm2, itcnt, i, k, mmk, mmkp2, mmkp1, ntrys; 1598 double err, p, q, delp, delq, b[MAXORDER], c[MAXORDER], den; 1599 double lim0 = 0.5*sqrt(DBL_MAX); 1600 1601 for(ord = order; ord > 2; ord -= 2){ 1602 ordm1 = ord-1; 1603 ordm2 = ord-2; 1604 /* Here is a kluge to prevent UNDERFLOW! (Sometimes the near-zero 1605 roots left in rootr and/or rooti cause underflow here... */ 1606 if(fabs(rootr[ordm1]) < 1.0e-10) rootr[ordm1] = 0.0; 1607 if(fabs(rooti[ordm1]) < 1.0e-10) rooti[ordm1] = 0.0; 1608 p = -2.0 * rootr[ordm1]; /* set initial guesses for quad factor */ 1609 q = (rootr[ordm1] * rootr[ordm1]) + (rooti[ordm1] * rooti[ordm1]); 1610 for(ntrys = 0; ntrys < MAX_TRYS; ntrys++) 1611 { 1612 int found = FALSE; 1613 1614 for(itcnt = 0; itcnt < MAX_ITS; itcnt++) 1615 { 1616 double lim = lim0 / (1 + fabs(p) + fabs(q)); 1617 1618 b[ord] = a[ord]; 1619 b[ordm1] = a[ordm1] - (p * b[ord]); 1620 c[ord] = b[ord]; 1621 c[ordm1] = b[ordm1] - (p * c[ord]); 1622 for(k = 2; k <= ordm1; k++){ 1623 mmk = ord - k; 1624 mmkp2 = mmk+2; 1625 mmkp1 = mmk+1; 1626 b[mmk] = a[mmk] - (p* b[mmkp1]) - (q* b[mmkp2]); 1627 c[mmk] = b[mmk] - (p* c[mmkp1]) - (q* c[mmkp2]); 1628 if (b[mmk] > lim || c[mmk] > lim) 1629 break; 1630 } 1631 if (k > ordm1) { /* normal exit from for(k ... */ 1632 /* ???? b[0] = a[0] - q * b[2]; */ 1633 b[0] = a[0] - p * b[1] - q * b[2]; 1634 if (b[0] <= lim) k++; 1635 } 1636 if (k <= ord) /* Some coefficient exceeded lim; */ 1637 break; /* potential overflow below. */ 1638 1639 err = fabs(b[0]) + fabs(b[1]); 1640 1641 if(err <= MAX_ERR) { 1642 found = TRUE; 1643 break; 1644 } 1645 1646 den = (c[2] * c[2]) - (c[3] * (c[1] - b[1])); 1647 if(den == 0.0) 1648 break; 1649 1650 delp = ((c[2] * b[1]) - (c[3] * b[0]))/den; 1651 delq = ((c[2] * b[0]) - (b[1] * (c[1] - b[1])))/den; 1652 1653 /* printf("\nerr=%f delp=%f delq=%f p=%f q=%f", 1654 err,delp,delq,p,q); */ 1655 1656 p += delp; 1657 q += delq; 1658 1659 } /* for(itcnt... */ 1660 1661 if (found) /* we finally found the root! */ 1662 break; 1663 else { /* try some new starting values */ 1664 p = ((double)rand() - 0.5*RAND_MAX)/(double)RAND_MAX; 1665 q = ((double)rand() - 0.5*RAND_MAX)/(double)RAND_MAX; 1666 /* fprintf(stderr, "\nTried new values: p=%f q=%f\n",p,q); */ 1667 } 1668 1669 } /* for(ntrys... */ 1670 if((itcnt >= MAX_ITS) && (ntrys >= MAX_TRYS)){ 1671 /* printf("Exceeded maximum trial count in _lbpoly.\n");*/ 1672 return(FALSE); 1673 } 1674 1675 if(!qquad(1.0, p, q, 1676 &rootr[ordm1], &rooti[ordm1], &rootr[ordm2], &rooti[ordm2])) 1677 return(FALSE); 1678 1679 /* Update the coefficient array with the coeffs. of the 1680 reduced polynomial. */ 1681 for( i = 0; i <= ordm2; i++) a[i] = b[i+2]; 1682 } 1683 1684 if(ord == 2){ /* Is the last factor a quadratic? */ 1685 if(!qquad(a[2], a[1], a[0], 1686 &rootr[1], &rooti[1], &rootr[0], &rooti[0])) 1687 return(FALSE); 1688 return(TRUE); 1689 } 1690 if(ord < 1) { 1691 printf("Bad ORDER parameter in _lbpoly()\n"); 1692 return(FALSE); 1693 } 1694 1695 if( a[1] != 0.0) rootr[0] = -a[0]/a[1]; 1696 else { 1697 rootr[0] = 100.0; /* arbitrary recovery value */ 1698 printf("Numerical problems in lbpoly()\n"); 1699 } 1700 rooti[0] = 0.0; 1701 1702 return(TRUE); 1703} 1704 1705