filtbank.c

《Visual C++小波变换技术与工程实践》作者：靳济芳。书上的代码。第3章：语音的去噪处理

	case SHORT_LONG_WINDOW :
		memcpy(transf_buf, p_in_data,BLOCK_LEN_LONG*sizeof(double));
		IMDCT( transf_buf, 2*BLOCK_LEN_LONG );
		for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++)
			transf_buf[i+NFLAT_LS] *= first_window[i];
		if (overlap_select != MNON_OVERLAPPED) {
			for ( i = 0 ; i < BLOCK_LEN_SHORT; i++ )
				o_buf[i+NFLAT_LS] += transf_buf[i+NFLAT_LS];
			memcpy(o_buf+BLOCK_LEN_SHORT+NFLAT_LS,transf_buf+BLOCK_LEN_SHORT+NFLAT_LS,NFLAT_LS*sizeof(double));
			for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
				o_buf[i+BLOCK_LEN_LONG] = transf_buf[i+BLOCK_LEN_LONG] * second_window[BLOCK_LEN_LONG-i-1];
		} else { /* overlap_select == NON_OVERLAPPED */
			SetMemory(transf_buf,0,NFLAT_LS*sizeof(double));
			for ( i = 0 ; i < BLOCK_LEN_LONG ; i++)
				transf_buf[i+BLOCK_LEN_LONG] *= second_window[BLOCK_LEN_LONG-i-1];
		}
		break;

	case ONLY_SHORT_WINDOW :
		if (overlap_select != MNON_OVERLAPPED) {
			fp = o_buf + NFLAT_LS;
		} else { /* overlap_select == NON_OVERLAPPED */
			fp = transf_buf;
		}
		for ( k=0; k < MAX_SHORT_WINDOWS; k++ ) {
			memcpy(transf_buf,p_in_data,BLOCK_LEN_SHORT*sizeof(double));
			IMDCT( transf_buf, 2*BLOCK_LEN_SHORT );
			p_in_data += BLOCK_LEN_SHORT;
			if (overlap_select != MNON_OVERLAPPED) {
				for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++){
					transf_buf[i] *= first_window[i];
					fp[i] += transf_buf[i];
					fp[i+BLOCK_LEN_SHORT] = transf_buf[i+BLOCK_LEN_SHORT] * second_window[BLOCK_LEN_SHORT-i-1];
				}
				fp += BLOCK_LEN_SHORT;
			} else { /* overlap_select == NON_OVERLAPPED */
				for ( i = 0 ; i < BLOCK_LEN_SHORT ; i++){
					fp[i] *= first_window[i];
					fp[i+BLOCK_LEN_SHORT] *= second_window[BLOCK_LEN_SHORT-i-1];
				}
				fp += 2*BLOCK_LEN_SHORT;
			}
			first_window = second_window;
		}
		SetMemory(o_buf+BLOCK_LEN_LONG+NFLAT_LS+BLOCK_LEN_SHORT,0,NFLAT_LS*sizeof(double));
		break;
	}

	if (overlap_select != MNON_OVERLAPPED)
		memcpy(p_out_data,o_buf,BLOCK_LEN_LONG*sizeof(double));
	else  /* overlap_select == NON_OVERLAPPED */
		memcpy(p_out_data,transf_buf,2*BLOCK_LEN_LONG*sizeof(double));

	/* save unused output data */
	memcpy(p_overlap,o_buf+BLOCK_LEN_LONG,BLOCK_LEN_LONG*sizeof(double));

	if (overlap_buf) FreeMemory(overlap_buf);
	if (transf_buf) FreeMemory(transf_buf);
}

void specFilter(double *freqBuff,
				int sampleRate,
				int lowpassFreq,
				int specLen
				)
{
	int lowpass,xlowpass;

	/* calculate the last line which is not zero */
	lowpass = (lowpassFreq * specLen) / (sampleRate>>1) + 1;
	xlowpass = (lowpass < specLen) ? lowpass : specLen ;

	SetMemory(freqBuff+xlowpass,0,(specLen-xlowpass)*sizeof(double));
}

static double Izero(double x)
{
	const double IzeroEPSILON = 1E-41;  /* Max error acceptable in Izero */
	double sum, u, halfx, temp;
	int n;

	sum = u = n = 1;
	halfx = x/2.0;
	do {
		temp = halfx/(double)n;
		n += 1;
		temp *= temp;
		u *= temp;
		sum += u;
	} while (u >= IzeroEPSILON*sum);

	return(sum);
}

static void CalculateKBDWindow(double* win, double alpha, int length)
{
	int i;
	double IBeta;
	double tmp;
	double sum = 0.0;

	alpha *= M_PI;
	IBeta = 1.0/Izero(alpha);
	
	/* calculate lower half of Kaiser Bessel window */
	for(i=0; i<(length>>1); i++) {
		tmp = 4.0*(double)i/(double)length - 1.0;
		win[i] = Izero(alpha*sqrt(1.0-tmp*tmp))*IBeta;
		sum += win[i];
	}

	sum = 1.0/sum;
	tmp = 0.0;

	/* calculate lower half of window */
	for(i=0; i<(length>>1); i++) {
		tmp += win[i];
		win[i] = sqrt(tmp*sum);
	}
}

static void MDCT(double *data, int N)
{
	double *xi, *xr;
	double tempr, tempi, c, s, cold, cfreq, sfreq; /* temps for pre and post twiddle */
	double freq = TWOPI / N;
	double cosfreq8, sinfreq8;
	int i, n;

	xi = (double*)AllocMemory((N >> 2)*sizeof(double));
	xr = (double*)AllocMemory((N >> 2)*sizeof(double));

	/* prepare for recurrence relation in pre-twiddle */
	cfreq = cos (freq);
	sfreq = sin (freq);
	cosfreq8 = cos (freq * 0.125);
	sinfreq8 = sin (freq * 0.125);
	c = cosfreq8;
	s = sinfreq8;

	for (i = 0; i < (N >> 2); i++) {
		/* calculate real and imaginary parts of g(n) or G(p) */
		n = (N >> 1) - 1 - 2 * i;

		if (i < (N >> 3))
			tempr = data [(N >> 2) + n] + data [N + (N >> 2) - 1 - n]; /* use second form of e(n) for n = N / 2 - 1 - 2i */
		else
			tempr = data [(N >> 2) + n] - data [(N >> 2) - 1 - n]; /* use first form of e(n) for n = N / 2 - 1 - 2i */

		n = 2 * i;
		if (i < (N >> 3))
			tempi = data [(N >> 2) + n] - data [(N >> 2) - 1 - n]; /* use first form of e(n) for n=2i */
		else
			tempi = data [(N >> 2) + n] + data [N + (N >> 2) - 1 - n]; /* use second form of e(n) for n=2i*/

		/* calculate pre-twiddled FFT input */
		xr[i] = tempr * c + tempi * s;
		xi[i] = tempi * c - tempr * s;

		/* use recurrence to prepare cosine and sine for next value of i */
		cold = c;
		c = c * cfreq - s * sfreq;
		s = s * cfreq + cold * sfreq;
	}

	/* Perform in-place complex FFT of length N/4 */
	switch (N) {
    case 256:
		srfft(xr, xi, 6);
		break;
    case 2048:
		srfft(xr, xi, 9);
	}

	/* prepare for recurrence relations in post-twiddle */
	c = cosfreq8;
	s = sinfreq8;

	/* post-twiddle FFT output and then get output data */
	for (i = 0; i < (N >> 2); i++) {
		/* get post-twiddled FFT output  */
		tempr = 2. * (xr[i] * c + xi[i] * s);
		tempi = 2. * (xi[i] * c - xr[i] * s);

		/* fill in output values */
		data [2 * i] = -tempr;   /* first half even */
		data [(N >> 1) - 1 - 2 * i] = tempi;  /* first half odd */
		data [(N >> 1) + 2 * i] = -tempi;  /* second half even */
		data [N - 1 - 2 * i] = tempr;  /* second half odd */

		/* use recurrence to prepare cosine and sine for next value of i */
		cold = c;
		c = c * cfreq - s * sfreq;
		s = s * cfreq + cold * sfreq;
	}

	if (xr) FreeMemory(xr);
	if (xi) FreeMemory(xi);
}

static void IMDCT(double *data, int N)
{
	double *xi, *xr;
	double tempr, tempi, c, s, cold, cfreq, sfreq; /* temps for pre and post twiddle */
	double freq = 2.0 * M_PI / N;
	double fac, cosfreq8, sinfreq8;
	int i;

	xi = (double*)AllocMemory((N >> 2)*sizeof(double));
	xr = (double*)AllocMemory((N >> 2)*sizeof(double));

	/* Choosing to allocate 2/N factor to Inverse Xform! */
	fac = 2. / N; /* remaining 2/N from 4/N IFFT factor */

	/* prepare for recurrence relation in pre-twiddle */
	cfreq = cos (freq);
	sfreq = sin (freq);
	cosfreq8 = cos (freq * 0.125);
	sinfreq8 = sin (freq * 0.125);
	c = cosfreq8;
	s = sinfreq8;

	for (i = 0; i < (N >> 2); i++) {
		/* calculate real and imaginary parts of g(n) or G(p) */
		tempr = -data[2 * i];
		tempi = data[(N >> 1) - 1 - 2 * i];

		/* calculate pre-twiddled FFT input */
		xr[i] = tempr * c - tempi * s;
		xi[i] = tempi * c + tempr * s;

		/* use recurrence to prepare cosine and sine for next value of i */
		cold = c;
		c = c * cfreq - s * sfreq;
		s = s * cfreq + cold * sfreq;
	}

    /* Perform in-place complex IFFT of length N/4 */
	switch (N) {
    case 256:
		srifft(xr, xi, 6);
		break;
    case 2048:
		srifft(xr, xi, 9);
	}

    /* prepare for recurrence relations in post-twiddle */
    c = cosfreq8;
    s = sinfreq8;

    /* post-twiddle FFT output and then get output data */
    for (i = 0; i < (N >> 2); i++) {

		/* get post-twiddled FFT output  */
		tempr = fac * (xr[i] * c - xi[i] * s);
		tempi = fac * (xi[i] * c + xr[i] * s);

		/* fill in output values */
		data [(N >> 1) + (N >> 2) - 1 - 2 * i] = tempr;
		if (i < (N >> 3))
			data [(N >> 1) + (N >> 2) + 2 * i] = tempr;
		else
			data [2 * i - (N >> 2)] = -tempr;

		data [(N >> 2) + 2 * i] = tempi;
		if (i < (N >> 3))
			data [(N >> 2) - 1 - 2 * i] = -tempi;
		else
			data [(N >> 2) + N - 1 - 2*i] = tempi;

		/* use recurrence to prepare cosine and sine for next value of i */
		cold = c;
		c = c * cfreq - s * sfreq;
		s = s * cfreq + cold * sfreq;
	}

	if (xr) FreeMemory(xr);
	if (xi) FreeMemory(xi);
}