Mercurial > hg > aim92
diff tools/sigproc.c @ 0:5242703e91d3 tip
Initial checkin for AIM92 aimR8.2 (last updated May 1997).
author | tomwalters |
---|---|
date | Fri, 20 May 2011 15:19:45 +0100 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tools/sigproc.c Fri May 20 15:19:45 2011 +0100 @@ -0,0 +1,830 @@ +#include <stdio.h> +#include <math.h> +#include "sigproc.h" + + +/* + C is n complex numbers: (real,imag), (ie 2n floats). + Overwrite the first n floats with the modulus of the n complex numbers. +*/ + +Mod( C, n ) +complex *C ; +int n ; +{ + register complex *eptr = C+n ; + register float *V = (float *)C ; + + for ( ; C < eptr ; C++ ) + *V++ = mod(C) ; +} + +/* + Return the modulus of the given complex number +*/ + +float mod( C ) +complex *C ; +{ + return ( sqrt( sq(Re(C)) + sq(Im(C)) ) ) ; +} + + + +/* + C is n complex numbers: (real,imag), (ie 2n floats). + Overwrite the first n floats with the argument of the n complex numbers. +*/ + +Arg( C, n ) +complex *C ; +int n ; +{ + register complex *eptr = C+n ; + register float *V = (float *)C ; + + for ( ; C < eptr ; C++ ) + *V++ = arg(C) ; +} + +/* + Return the argument of the given complex number +*/ + +float arg( C ) +complex *C; +{ + if (Im(C) >= 0) { + if (Re(C) >= 0) return ( atan(Im(C)/Re(C)) ); /* 1st quad */ + else return ( PI + atan(Im(C)/Re(C)) ); /* 2nd quad */ + } + else { + if (Re(C) < 0) return ( PI + atan(Im(C)/Re(C)) ); /* 1st quad */ + else return ( TWOPI + atan(Im(C)/Re(C)) ); /* 2nd quad */ + } +} + + +/* + C1 and C2 are both n complex numbers: (real,imag), (ie each 2n floats). + Overwrite the C1 with product of C1 and the complex conjugate of C2. + (When C1==C2, result is squared mod in the real parts, and 0 imag parts). +*/ + +conj( C1, C2, n ) +complex *C1, *C2 ; +int n ; +{ + register complex *eptr = C1+n ; + float ReC1, ReC2 ; + + for ( ; C1 < eptr ; C1++, C2++ ) { + Re(C1) = ( ReC1=Re(C1) ) * ( ReC2=Re(C2) ) + Im(C1) * Im(C2) ; + Im(C1) = ReC2 * Im(C1) - ReC1 * Im(C2) ; + } +} + + +/* + Autocorrelation function via fft. + A complex fft (of real data) is multiplied by its conjugate, and also scaled + prior to the inverse fft. +*/ + + + +acf( V, m ) +float *V ; +int m ; +{ + register complex *C, *eptr ; + register int n = m>>1 ; + float tmp ; + + fft( V, m, 1 ) ; + tmp = V[1] ; V[1] = 0 ; + + for ( C=(complex *)V, eptr=C+n ; C<eptr ; C++ ) { + Re(C) = ( sq(Re(C)) + sq(Im(C)) ) / n ; + Im(C) = 0 ; + } + V[1] = sq( tmp ) / n ; + + fft( V, m, -1 ) ; +} + + +/* + Test x: return 1 if x is a power of 2, otherwise return 0. +*/ + +int ispower( x ) +int x ; +{ + return ( x > 1 && log2(x) == (int)log2(x) ) ; +} + + +/* + Return a power of 2, either equal to x (if x is already a power of 2), or + the next power of 2 larger than x. +*/ + +int getpower( x ) +int x ; +{ + if ( log2(x) > (int)log2(x) ) + x = 1 << ( 1 + (int)log2(x) ) ; + return ( x ) ; +} + + +/* +Copy float array x into short array y, both length n points. +Scale each point using the given scale factor. +Return 1 if no 16-bit over or underflow. +Otherwise return a scale factor to replace the given scale factor. + +*/ + +float ftos( x, y, n, scale ) +float *x ; +short *y ; +int n ; +float scale ; +{ + float p ; + float min = MAXSHORT, fmin ; + float max = MINSHORT, fmax ; + int i ; + + fmax = fmin = 1. ; + + for ( i = 0 ; i < n ; i++ ) { + p = x[i] * scale ; + + if ( p > max ) max = p ; + if ( p < min ) min = p ; + + y[i] = (short)p ; + } + + if ( max > MAXSHORT || min < MINSHORT ) { + + if ( max > MAXSHORT ) fmax = MAXSHORT / max ; + if ( min < MINSHORT ) fmin = MINSHORT / min ; + + if ( fmax < fmin ) return ( scale * fmax ) ; + else return ( scale * fmin ) ; + } + + return ( 1. ) ; +} + + + + +/*************************** convolution **********************************/ + +/* +Time-domain convolution. An input signal y of length n points is convolved +with a window x of length m points (where m is assumed odd for symmetry). +Return convolution signal z which must have allocated space of length n. + +Discrete convolution is defined: + z[i] = sum{j=-inf to inf} x[i-j].y[j] for i=-inf to inf +But the signal y and window x are assumed to be zero for all time outside +the given points so that: + z[i] = sum{j=-m/2 to m/2} x[i-j].y[j] for i=0,1,...,n-1 + +Since the response of a linear filter to an arbitiary input signal is the +convolution of the signal with the filter's impulse response, the window may +be seen as the impulse response characterising the filter, and the return +z is then the filter output in response to input y. + +Alternatively the convolution may be seen as a local averaging operation +on y with weights obtained by time-reversing and shifting x. +*/ + +convolve_time( y, n, x, m, z ) +short *y ; /* input signal */ +int n ; /* length of array y */ +float *x ; /* convolution window or filter impulse response */ +int m ; /* length of array x */ +float *z ; /* output signal of length n (same as input) */ +{ + register int i, j, k, lim1, lim2 ; + register float sum ; + + for ( i = 0 ; i < n ; i++ ) { + + k = m - 1 ; + if ( ( lim1 = i - (m-1)/2 ) < 0 ) { + k += lim1 ; + lim1 = 0 ; + } + if ( ( lim2 = i + (m-1)/2 ) > n ) + lim2 = n - 1 ; + + sum = 0 ; + for ( j = lim1 ; j <= lim2 ; j++, --k ) + sum += x[k] * y[j] ; + + z[i] = sum ; + } +} + + +/* +Frequency-domain convolution. +*/ + +convolve_freq( y, n, x, m, z ) +short *y ; /* input signal */ +int n ; /* length of array y */ +float *x ; /* convolution window or filter impulse response */ +int m ; /* length of array x */ +float *z ; /* output signal of length n (same as input) */ +{ + int i, j, n1 ; + float *data, *respns, *ans ; + + n1 = getpower( n + m ) ; /* padded length of input signal */ + + data = (float *)malloc( ( n1 + 1 ) * sizeof(float) ) ; + respns = (float *)malloc( ( n1 + 1 ) * sizeof(float) ) ; + ans = (float *)malloc( ( n1 * 2 + 1 ) * sizeof(float) ) ; + + /* copy and pad signal */ + + for ( i = 0 ; i < n ; i++ ) + data[i] = y[i] ; + for ( ; i < n1 ; i++ ) + data[i] = (float)0 ; + + /* copy window into wrap-around order */ + + for ( i = m/2, j = 0 ; i < m ; i++, j++ ) + respns[i] = x[j] ; + for ( i = 0 ; i < m/2 ; i++, j++ ) + respns[i] = x[j] ; + + /* Convolve and copy output */ + + convlv( data-1, n1, respns-1, m, 1, ans-1 ) ; + + for ( i = 0 ; i < n ; i++ ) + z[i] = ans[i] ; + + free( data ) ; + free( respns ) ; + free( ans ) ; +} + + +/* +Convolution of two functions. [See NR: pp407-413]. +Function 1 (the input signal) is in `data', length n, (n power of 2). +Function 2 (the response function) is in `respns', length m, (m<=n and odd). +(However respns must have n space available). +The response file should be stored in respns in "wrap-around order", ie with +its last half first and first half last in the array. +Return is the first n numbers in `ans', (though ans must have 2*n space). +Return convolution if isign==1, or deconvolution if isign==(-1). + +The data array should be padded with zeroes to a power of 2. +The recommended amount of padding [NR: p411] is at least m/2, but need not be +more than m. +*/ + +static float sqrarg; +#define SQR(a) (sqrarg=(a),sqrarg*sqrarg) + + +convlv( data, n, respns, m, isign, ans ) +float data[], respns[], ans[] ; +int n, m, isign ; +{ + int i, no2 ; + float dum, mag2, *fftbuf ; + + fftbuf = (float *)malloc( 2 * n * sizeof(float) ) ; + + for (i=1;i<=(m-1)/2;i++) + respns[n+1-i]=respns[m+1-i]; + for (i=(m+3)/2;i<=n-(m-1)/2;i++) + respns[i]=0.0; + + twofft(data,respns,fftbuf,ans,n); + no2=n/2; + for (i=2;i<=n+2;i+=2) { + if (isign == 1) { + ans[i-1]=(fftbuf[i-1]*(dum=ans[i-1])-fftbuf[i]*ans[i])/no2; + ans[i]=(fftbuf[i]*dum+fftbuf[i-1]*ans[i])/no2; + } else if (isign == -1) { + if ((mag2=SQR(ans[i-1])+SQR(ans[i])) == 0.0) + fprintf( stderr, "warning: deconvolving at response zero\n" ) ; + ans[i-1]=(fftbuf[i-1]*(dum=ans[i-1])+fftbuf[i]*ans[i])/mag2/no2; + ans[i]=(fftbuf[i]*dum-fftbuf[i-1]*ans[i])/mag2/no2; + } + else { + fprintf( stderr, "No meaning for ISIGN in CONVLV\n" ) ; + exit( 1 ) ; + } + } + ans[2]=ans[n+1]; + realft(ans,no2,-1); + + free( fftbuf ) ; +} + + + +/************************ fft ********************************************/ + +/* + In-place FFT/IFFT routine for real-valued data. + (Ref Numerical recipes, pp417). + + Arguments: + data = Input array of 2n floats, + also output data array of n complex numbers. + n = Number of data points, (must be a power of two). + Input data is 2n real numbers. + Output data is n complex numbers. + isign = FFT when 1; IFFT when -1. + + Each complex number occupies two consecutive locations: real and imag. + The output data is n real and imag parts which are the positive frequency + half of the symmetric Fourier transform. + + Indexing conversion: + + Both input and output data are indexed: 1,..,n. + where: Input data is 2n real numbers. + Output data is n complex numbers, (ie 2n floats). + To convert to conventional indexing: 0,..,m-1 + where: Input data is m real numbers (eg. input framesize). + Output data is m/2 complex numbers, (ie m floats). + + allocated space for "data" of: + + (m+1)*sizeof(float). + + call to routine "realft": + + realft( data-1, m/2, isign ) ; + + for example via the define: + +#define fft(data,m,isign) ( realft((float *)(data)-1, m>>1, isign) ) + + This works because the location (data-1) is never indexed in routine realft. + Output data will then be data[0] to data[m/2-1] complex numbers, + ie data[0] to data[m-1] floats, to be interpreted as m/2 (real,imag) pairs. + + The first of the m/2 points of the magnitude spectrum is then: + sqrt( sq( data[0] ) + sq( data[1] ) ) + + + The results of the IFFT are not normalized by multplication by 1/N. + The user should multiply each returned element by 1/n. + + Routine realft returns the first and last real components in data[1] and + data[2]. To make this correspond with the return from twofft, the mod below + sets data[2]=0. +*/ + + +realft(data,n,isign) +float data[]; +int n,isign; +{ + int i,i1,i2,i3,i4,n2p3; + float c1=0.5,c2,h1r,h1i,h2r,h2i; + double wr,wi,wpr,wpi,wtemp,theta; + + theta=3.141592653589793/(double) n; + if (isign == 1) { + c2 = -0.5; + four1(data,n,1); + } else { + c2=0.5; + theta = -theta; + } + wtemp=sin(0.5*theta); + wpr = -2.0*wtemp*wtemp; + wpi=sin(theta); + wr=1.0+wpr; + wi=wpi; + n2p3=2*n+3; + for (i=2;i<=n/2;i++) { + i4=1+(i3=n2p3-(i2=1+(i1=i+i-1))); + h1r=c1*(data[i1]+data[i3]); + h1i=c1*(data[i2]-data[i4]); + h2r = -c2*(data[i2]+data[i4]); + h2i=c2*(data[i1]-data[i3]); + data[i1]=h1r+wr*h2r-wi*h2i; + data[i2]=h1i+wr*h2i+wi*h2r; + data[i3]=h1r-wr*h2r+wi*h2i; + data[i4] = -h1i+wr*h2i+wi*h2r; + wr=(wtemp=wr)*wpr-wi*wpi+wr; + wi=wi*wpr+wtemp*wpi+wi; + } + if (isign == 1) { + data[1] = (h1r=data[1])+data[2]; + data[2] = h1r-data[2]; + } else { + data[1]=c1*((h1r=data[1])+data[2]); + data[2]=c1*(h1r-data[2]); + four1(data,n,-1); + } +} + + +#define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr + +four1(data,nn,isign) +float data[]; +int nn,isign; +{ + int n,mmax,m,j,istep,i; + double wtemp,wr,wpr,wpi,wi,theta; + float tempr,tempi; + + n=nn << 1; + j=1; + for (i=1;i<n;i+=2) { + if (j > i) { + SWAP(data[j],data[i]); + SWAP(data[j+1],data[i+1]); + } + m=n >> 1; + while (m >= 2 && j > m) { + j -= m; + m >>= 1; + } + j += m; + } + mmax=2; + while (n > mmax) { + istep=2*mmax; + theta=6.28318530717959/(isign*mmax); + wtemp=sin(0.5*theta); + wpr = -2.0*wtemp*wtemp; + wpi=sin(theta); + wr=1.0; + wi=0.0; + for (m=1;m<mmax;m+=2) { + for (i=m;i<=n;i+=istep) { + j=i+mmax; + tempr=wr*data[j]-wi*data[j+1]; + tempi=wr*data[j+1]+wi*data[j]; + data[j]=data[i]-tempr; + data[j+1]=data[i+1]-tempi; + data[i] += tempr; + data[i+1] += tempi; + } + wr=(wtemp=wr)*wpr-wi*wpi+wr; + wi=wi*wpr+wtemp*wpi+wi; + } + mmax=istep; + } +} + + +twofft(data1,data2,fft1,fft2,n) +float data1[],data2[],fft1[],fft2[]; +int n; +{ + int nn3,nn2,jj,j; + float rep,rem,aip,aim; + + nn3=1+(nn2=2+n+n); + for (j=1,jj=2;j<=n;j++,jj+=2) { + fft1[jj-1]=data1[j]; + fft1[jj]=data2[j]; + } + four1(fft1,n,1); + fft2[1]=fft1[2]; + fft1[2]=fft2[2]=0.0; + for (j=3;j<=n+1;j+=2) { + rep=0.5*(fft1[j]+fft1[nn2-j]); + rem=0.5*(fft1[j]-fft1[nn2-j]); + aip=0.5*(fft1[j+1]+fft1[nn3-j]); + aim=0.5*(fft1[j+1]-fft1[nn3-j]); + fft1[j]=rep; + fft1[j+1]=aim; + fft1[nn2-j]=rep; + fft1[nn3-j] = -aim; + fft2[j]=aip; + fft2[j+1] = -rem; + fft2[nn2-j]=aip; + fft2[nn3-j]=rem; + } +} + + +/*********************** windows *******************************************/ + +/* +Allocate space for and compute an n-point Hann or raised cosine window, +returning array. +*/ + +float *raised_cosine( n ) +int n ; +{ + float *W ; + register int i ; + register double k ; + + W = (float *)malloc( n * sizeof(float) ) ; + + k = TWOPI/(double)(n-1); + for (i=0 ; i<n ; i++) + W[i] = 0.5 * ( 1.0 - cos(k*i) ) ; + + return W ; +} + + +/* +Allocate space for and compute an n-point Hamming window, returning array. +*/ + +float *hamming( n ) +int n ; +{ + float *W ; + register int i ; + register double k ; + + W = (float *)malloc( n * sizeof(float) ) ; + + k = TWOPI/(double)(n-1); + for (i=0 ; i<n ; i++) + W[i] = 0.54 - 0.46*cos(k*i) ; + + return W ; +} + + +/* +Allocate space for and compute a Gaussian window with given standard deviation +`s' (samples) over a range of `n' standard deviations either side of a zero +mean. The size of the returned window (returned via variable `num') will be: + 2 * n * s + 1 [points] +(This is always odd as the window is symmetrical). +*/ + +float *gauss_window( s, n, num ) +float s, n ; +int *num ; +{ + float x, *y, var ; + int i, points ; + + points = 2 * n * s + 1 ; + y = (float *)malloc( points * sizeof(float) ) ; + + x = ( - n * s ) ; + var = s * s ; + for ( i = 0 ; i < points ; i++, x++ ) + y[i] = exp( -(double)( 0.5 * x * x / var ) ) ; + + *num = points ; + return y ; +} + + +/*********************** frames *******************************************/ + +/* +Return a ptr to the n'th frame (numbered 1,2,3,...,n), with given +framewidth (size) and frameshift (step). Return the null ptr if eof before +the n'th frame. Frame number n=0 refers to the last frame before eof. +Successive calls to getframe must request increasing frame numbers. + +Data (binary shorts) is buffered in an array of "blocksize" shorts, +allocated internally, where: + blocksize = size + ( frames_per_block - 1 ) * step +This means that the buffer can hold "frames_per_block" frames of the given +size and step, and ensures that no frame crosses the boundary between +successive buffers. At the buffer boundary, the overlap part +(where overlap = size - step) of the last frame in the buffer must +be shifted to the start of the buffer, and the step part of the next frame +must be read so as to follow it in the buffer, completing the first frame of +the next buffer. +To protect the last frame (in the event that the next step part is actually +the eof), the first frame of the next buffer must not corrupt the last frame +of the previous buffer. This is ensured by arranging that: + size <= ( frames_per_block - 1 ) * step +This constrains frames_per_block to be: + frames_per_block >= ( size / step ) + 1 +Therefore the minimum number of frames_per_block (when step = size, and there +is no overlap) is 2. +The actual value you give to frames_per_block is an efficiency issue, being +a trade-off between space usage and the number of times a shift is necessary +at a buffer boundary. A reasonable guess would be (see define below): + frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK + +An example of a shifting frame buffer, from frame a to b inclusive in steps +of 1 frame, would be: + + while ( ( buf = getframe( fp, a, size, step ) ) != (short *)0 && ( a<=b || b==0 ) ) { + process( buf, size ) ; + a++ ; + } + if ( a<=b && b>0 ) + fprintf(stderr,"warning: not enough frames for request\n"); + +*/ + +#define FRAMES_PER_BLOCK 10 + +short *getframe( fp, n, size, step ) +FILE *fp ; +int n, size, step ; +{ + static int first = 1 ; + static short *buf, *endptr ; + static short *bptr = (short *) 0 ; + static int blocksize, overlap, frames_per_block ; + static int m = 0 ; /* frame counter */ + int startsize, i ; + + if ( n < m ) /* catches invalid calls and also case of n==0 */ + return (short *) 0 ; + + if ( first ) { + first = 0 ; + frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK ; + blocksize = size + ( frames_per_block - 1 ) * step ; + overlap = size - step ; + bptr = buf = (short *)malloc( blocksize * sizeof(short) ) ; + endptr = buf + blocksize ; + + if ( n > 0 ) { + + /* seek the n'th frame in blocks of blocksize. */ + + startsize = ( n - 1 ) * step ; + for ( i = blocksize ; i < startsize ; i += blocksize ) + if ( fread( buf, sizeof(short), blocksize, fp ) < blocksize ) + return (short *) 0 ; + if ( ( startsize -= ( i - blocksize ) ) > 0 ) + if ( fread( buf, sizeof(short), startsize, fp ) < startsize ) + return (short *) 0 ; + + if ( ( fread( bptr, sizeof(short), size, fp ) ) < size ) + return (short *) 0 ; + m = n ; + } + else { + + /* seek the last frame in blocks of blocksize */ + + if ( fread( buf, sizeof(short), size, fp ) < size ) + return (short *) 0 ; + startsize = blocksize - size ; + for ( m = 1 ; ( i = fread( buf+size, sizeof(short), startsize, fp ) ) == startsize ; m += frames_per_block - 1 ) { + bptr = endptr-size ; + while ( bptr < endptr ) + *buf++ = *bptr++ ; + buf -= size ; + } + bptr = buf + ( i / step) * step ; + m += i / step ; + } + return ( bptr ) ; + } + + for ( ; m < n || n == 0 ; m++ ) { + + if ( bptr + size >= endptr ) { /* shift last overlap to buf start */ + while ( bptr < endptr ) + *buf++ = *bptr++ ; + buf -= overlap ; + bptr = buf - step ; + } + + if ( ( fread( bptr+size, sizeof(short), step, fp ) ) < step ) { + if ( n == 0 ) { + if ( bptr < buf ) return ( endptr - size ) ; + else return ( bptr ) ; + } + return (short *) 0 ; + } + bptr += step ; + } + return ( bptr ) ; +} + + +/*************************************************************************/ + +/* +Seek past n frames in blocks of blocksize. +Frames have width=size and shift=step. +When step == size, the seek reads ( n * size ) shorts from the i/p stream. +When step < size, the seek reads ( size + ( n - 1 ) * step ) shorts. +Return 1, or otherwise 0 for an improper seek (insufficient data in stream). +*/ + +seekframes( fp, n, size, step ) +FILE *fp ; +int n, size, step ; +{ + static short *buf ; + static int first = 1 ; + static int blocksize, frames_per_block ; + int startsize = size + ( n - 1 ) * step ; + int i ; + + if ( first ) { + first = 0 ; + frames_per_block = ( size / step ) + 1 + FRAMES_PER_BLOCK ; + blocksize = size + ( frames_per_block - 1 ) * step ; + buf = (short *)malloc( blocksize * sizeof(short) ) ; + } + for ( i = blocksize ; i < startsize ; i += blocksize ) + if ( fread( buf, sizeof(short), blocksize, fp ) < blocksize ) + return 0 ; + if ( (startsize -= (i - blocksize)) > 0 ) + if ( fread( buf, sizeof(short), startsize, fp ) < startsize ) + return 0 ; + + return 1 ; +} + +/*************************************************************************/ + +/* +Seek past n bytes from current position in stream. +Return 1, or otherwise 0 for an improper seek (insufficient data in stream). +*/ + +seekbytes( fp, n ) +FILE *fp ; +int n ; +{ + static char *buf ; + static int first = 1 ; + + if ( n > 0 ) { + if ( first ) { + first = 0 ; + buf = (char *)malloc( n ) ; + } + + if ( fread( buf, 1, n, fp ) < n ) + return 0 ; + } + return 1 ; +} + + + +/*********************** normalizing **************************************/ + +/* +Normalize the n-array y so that the sum of the points (ie the area) is unity. +*/ + +normalize_area( y, n ) +float *y ; +int n ; +{ + float sum = 0 ; + int i ; + + for ( i=0 ; i < n ; i++ ) + sum += y[i] ; + for ( i=0 ; i < n ; i++ ) + y[i] /= sum ; +} + + +/* +Normalize the n-array y so that the range of y[i] is [0,1]. +*/ + +normalize_range( y, n ) +float *y ; +int n ; +{ + int i ; + float max = ( -256000. ) ; + float min = 256000. ; + + for ( i = 0 ; i < n ; i++ ) { /* find max and min */ + if ( y[i] > max ) max = y[i] ; + if ( y[i] < min ) min = y[i] ; + } + + for ( i = 0 ; i < n ; i++ ) + y[i] = ( y[i] - min ) / ( max - min ) ; +} + +