spectrum.c

IP网络语音通讯软件源代码. 不可多得的语音源代码

#include "netfone.h"

/*
** Instructions:
**
**	   First call buildtable to create the sin/cos tables.
**		  buildtable(ln)  where ln is the log base 2 of the number of
**						  samples.
**	   Next call bitreverse to bitreverse the input data.
**		  bitreverse(x,ln) where ln is the log base 2 of the number of
**						   samples and x is a complex array which holds
**						   the samples.
**	   Finally, call fft to perform a forward fft, or call ifft to
**						   perform an inverse fft.
**		  fft(x,ln)
**		  ifft(x,ln)	   where ln is the log base 2 of the number of
**						   samples and x is a complex array which holds
**						   the samples.
**	   If you are performing several fft/iffts of the same size, you only
**						   need to build the tables once
*/

typedef struct {
	double r, i;
} complex;

static complex *c = NULL;			// Sine and cosine table

#define PI2 6.283185307179586476925287

//	Table of powers of two, used to obtain array size from ln_2 size

static int pwr_two[] = {
            1,           2,            4,           8,
           16,          32,           64,         128,
          256,         512,         1024,        2048,
	     4096,        8192,        16384,       32768,
	    65536,      131072,       262144,      524288,
	  1048576,     2097152,      4194304,	  8388608,
	 16777216,	  33554432,     67108864,	134217728,
	268435456,	 536870912,   1073741824
};

#define MAXSAMPS 8192

#define SAMPLE short

static int fftsize = -1;				// ln_2 of current sine/cosine tables

static BOOL buildtable(int ln)
{
	int n, halfn, i;
	double theta;

	n = pwr_two[ln];
	halfn = pwr_two[ln - 1];
	if (c != NULL) {
		free(c);
	}
	c =(complex *) malloc(halfn * sizeof(complex));
	if (c == 0) {
		return FALSE;
	}
	for (i = 0; i < halfn; i++) {
		theta = (PI2 * i) / n;
		c[i].r =  cos(theta);
		c[i].i = -sin(theta);
	}
	return TRUE;
}

static void bitreverse(complex *x, int ln)
{
	int i, j, k, n;
	complex hold;

	n = pwr_two[ln];
	k = 0;
	j = 0;
	while (TRUE) {
		if (k > j) {
			hold.r = x[j].r;
			hold.i = x[j].i;

			x[j].r = x[k].r;
			x[j].i = x[k].i;

			x[k].r = hold.r;
			x[k].i = hold.i;
		}
		j++;
		if (j == n) {
			break;
		}

		/* k = k + 1, (k is n bits long and bit reversed). */

		i = ln - 1;
		while (i >= 0) {
			if (k & pwr_two[i]) {
				k = k - pwr_two[i];
			} else {
				break;
			}
			i--;
		}
		k = k + pwr_two[i];
	}
}

static void fft(complex *x, int ln)
{
	unsigned int stuckbit, stuckbitcomplement, halfn, t, b;
	unsigned int shft, msk, wt;
	unsigned int c0, c1;
	unsigned int n;
	double qr, qi, xtr, xti, xbr, xbi, cwtr, cwti;

	n = pwr_two[ln];
	msk = n - 1;
	halfn = pwr_two[ln - 1];
	shft = ln - 1;

	stuckbit = 1;
	c0 = ln;
	while (c0--) {
		t = 0;
		stuckbitcomplement = ~stuckbit;
		c1 = halfn;
		while (c1--) {
			wt = (t << shft) & msk;
			b = t + stuckbit;

			xbr = x[b].r;
			xbi = x[b].i;
			cwtr = c[wt].r;
			cwti = c[wt].i;

			qr = xbr * cwtr - xbi * cwti;
			qi = xbr * cwti + xbi * cwtr;

			xtr = x[t].r;
			xti = x[t].i;

			x[b].r = xtr-qr;
			x[b].i = xti-qi;

			x[t].r = xtr + qr;
			x[t].i = xti + qi;

			t = (b + 1) & stuckbitcomplement;
		}
		stuckbit <<= 1;
		shft -= 1;
	}
}

#ifdef NEEDED
void ifft(complex *x, int ln)
{
	unsigned int stuckbit, stuckbitcomplement, halfn, t, b;
	unsigned int shft, msk, wt;
	unsigned int c0, c1;
	unsigned int n;
	double qr, qi;

	n = pwr_two[ln];
	msk = n - 1;
	halfn = pwr_two[ln - 1];
	shft = ln - 1;

	stuckbit = 1;
	c0 = ln;
	while (c0--) {
		t = 0;
		stuckbitcomplement = ~stuckbit;
		c1 = halfn;
		while (c1--){
			b = t + stuckbit;
			wt = (t << shft) & msk;

			qr =  x[b].r * c[wt].r  + x[b].i * c[wt].i;
			qi = -(x[b].r * c[wt].i) + x[b].i * c[wt].r;

			x[b].r = x[t].r-qr;
			x[b].i = x[t].i-qi;

			x[t].r += qr;
			x[t].i += qi;

			t = (b + 1) & stuckbitcomplement;
		}
		stuckbit <<= 1;
		shft -= 1;
	}
}
#endif

double spectrum(SAMPLE *samples, int nsamples, double *ps, int nfreqs, double maxpwr)
{
	static complex tmpc[MAXSAMPS];
	int i, lognsamples, nfft;
	SAMPLE *pb;
	double pwr, nfftsqd;
	complex *pc;

	nfft = 1;
	lognsamples = 0;

	while (nfft < nsamples) {
		nfft <<= 1;
		lognsamples++;
	}

	if (fftsize != lognsamples) {
		buildtable(lognsamples);
		fftsize = lognsamples;
	}

	for (pb = samples, pc = tmpc, i = 0; i < nsamples; i+=1) {
		pc->r = (double) *pb++;
		pc->i = 0.0;
		pc++;
	}

	/*	If size of FFT (which must be a power of two) exceeds the number
		of samples, zero-fill the array.  */

	if (nfft > nsamples) {
		for (i = 0; i < nfft-nsamples ; i++) {
			pc->r = 0.0;
			pc->i = 0.0;
			pc++;
		}
	}
	pc = tmpc;

	bitreverse(tmpc, lognsamples);
	fft(tmpc, lognsamples);
	nfftsqd = ((double) nfft) * nfft;		//	Scale factor for FFT results

	for (i = 1; i < nfreqs; i++) {
		pwr = tmpc[i].r * tmpc[i].r;
		pwr += tmpc[i].i * tmpc[i].i;
		pwr = sqrt(pwr / nfftsqd);
		ps[i - 1] = pwr;
		if (pwr > maxpwr) {
			maxpwr = pwr;
		}
	}
	return maxpwr;
}

//	SPECTRUMEND  --  Release resources allocated by spectrum computation

void spectrumEnd(void)
{
	if (c != NULL) {
		free(c);
		c = NULL;
	}
	fftsize = -1;
}