view tools/sigproc.c @ 0:5242703e91d3 tip

Initial checkin for AIM92 aimR8.2 (last updated May 1997).

#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 ) ;
}