snd_dsp.cpp

hl2 source code. Do not use it illegal.

#define FIX20_INTPART(a)	(((int)(a)) >> FIX20_BITS)			// get integer part of fixed point
#define FIX20_FRACPART(a)	((a) - (((a) >> FIX20_BITS) << FIX20_BITS))	// get fractional part of fixed point

#define FIX20_FRACTION(a,b)	(FIX(a)/(b))						// convert int a to fixed point, divide by b

typedef int fix20int;

/////////////////////////////////
// DSP processor parameter block
/////////////////////////////////

// NOTE: these prototypes must match the XXX_Params ( prc_t *pprc ) and XXX_GetNext ( XXX_t *p, int x ) functions

typedef void * (*prc_Param_t)( void *pprc );					// individual processor allocation functions
typedef int (*prc_GetNext_t) ( void *pdata, int x );			// get next function for processor
typedef int (*prc_GetNextN_t) ( void *pdata,  portable_samplepair_t *pbuffer, int SampleCount, int op);	// batch version of getnext
typedef void (*prc_Free_t) ( void *pdata );						// free function for processor
typedef void (*prc_Mod_t) (void *pdata, float v);				// modulation function for processor	

#define	OP_LEFT				0		// batch process left channel in place
#define OP_RIGHT			1		// batch process right channel in place
#define OP_LEFT_DUPLICATE	2		// batch process left channel in place, duplicate to right channel

#define PRC_NULL			0		// pass through - must be 0
#define PRC_DLY				1		// simple feedback reverb
#define PRC_RVA				2		// parallel reverbs
#define PRC_FLT				3		// lowpass or highpass filter
#define PRC_CRS				4		// chorus
#define	PRC_PTC				5		// pitch shifter
#define PRC_ENV				6		// adsr envelope
#define PRC_LFO				7		// lfo
#define PRC_EFO				8		// envelope follower
#define PRC_MDY				9		// mod delay
#define PRC_DFR				10		// diffusor - n series allpass delays
#define PRC_AMP				11		// amplifier with distortion

#define QUA_LO				0		// quality of filter or reverb.  Must be 0,1,2,3.
#define QUA_MED				1
#define QUA_HI				2
#define QUA_VHI				3
#define QUA_MAX				QUA_VHI

#define CPRCPARAMS			16		// up to 16 floating point params for each processor type

// processor definition - one for each running instance of a dsp processor

struct prc_t
{
	int type;					// PRC type

	float prm[CPRCPARAMS];		// dsp processor parameters - array of floats

	prc_Param_t pfnParam;		// allocation function - takes ptr to prc, returns ptr to specialized data struct for proc type
	prc_GetNext_t pfnGetNext;	// get next function
	prc_GetNextN_t pfnGetNextN;	// batch version of get next
	prc_Free_t pfnFree;			// free function
	prc_Mod_t pfnMod;			// modulation function

	void *pdata;				// processor state data - ie: pdly, pflt etc.
};

// processor parameter ranges - for validating parameters during allocation of new processor

typedef struct prm_rng_t
{
	int iprm;		// parameter index
	float lo;		// min value of parameter
	float hi;		// max value of parameter
} prm_rng_s;

void PRC_CheckParams ( prc_t *pprc, prm_rng_t *prng );

///////////
// Filters
///////////

#define CFLTS	64			// max number of filters simultaneously active
#define FLT_M	12			// max order of any filter

#define FLT_LP	0			// lowpass filter
#define FLT_HP	1			// highpass filter
#define FTR_MAX	FLT_HP

// flt parameters

struct flt_t
{
	bool fused;				// true if slot in use

	int b[FLT_M+1];			// filter numerator parameters  (convert 0.0-1.0 to 0-PMAX representation)
	int a[FLT_M+1];			// filter denominator parameters (convert 0.0-1.0 to 0-PMAX representation)
	int w[FLT_M+1];			// filter state - samples (dimension of max (M, L))
	int L;					// filter order numerator (dimension of a[M+1])
	int M;					// filter order denominator (dimension of b[L+1])
};

// flt flts

flt_t flts[CFLTS];

void FLT_Init ( flt_t *pf ) { if ( pf ) Q_memset ( pf, 0, sizeof (flt_t) ); }
void FLT_InitAll ( void ) {	for ( int i = 0 ; i < CFLTS; i++ ) FLT_Init ( &flts[i] ); }
void FLT_Free ( flt_t *pf ) { if ( pf )	Q_memset ( pf, 0, sizeof (flt_t) );	}
void FLT_FreeAll ( void ) {	for (int i = 0 ; i < CFLTS; i++) FLT_Free ( &flts[i] ); }


// find a free filter from the filter pool
// initialize filter numerator, denominator b[0..M], a[0..L]

flt_t * FLT_Alloc ( int M, int L, int *a, int *b )
{
	int i, j;
	flt_t *pf = NULL;
	
	for (i = 0; i < CFLTS; i++)
	{
		if ( !flts[i].fused )
			{
			pf = &flts[i];

			// transfer filter params into filter struct
			pf->M = M;
			pf->L = L;
			for (j = 0; j <= M; j++)
				pf->a[j] = a[j];

			for (j = 0; j <= L; j++)
				pf->b[j] = b[j];

			pf->fused = true;
			break;
			}
	}

	Assert(pf);	// make sure we're not trying to alloc more than CFLTS flts

	return pf;
}

// convert filter params cutoff and type into
// iir transfer function params M, L, a[], b[]

// iir filter, 1st order, transfer function is H(z) = b0 + b1 Z^-1  /  a0 + a1 Z^-1
// or H(z) = b0 - b1 Z^-1 / a0 + a1 Z^-1 for lowpass

// design cutoff filter at 3db (.5 gain) p579

void FLT_Design_3db_IIR ( float cutoff, float ftype, int *pM, int *pL, int *a, int *b )
{
	// ftype: FLT_LP, FLT_HP, FLT_BP
	
	double Wc = 2.0 * M_PI * cutoff / SOUND_DMA_SPEED;			// radians per sample
	double Oc;
	double fa;
	double fb; 

	// calculations:
	// Wc = 2pi * fc/44100								convert to radians
	// Oc = tan (Wc/2) * Gc / sqt ( 1 - Gc^2)			get analog version, low pass
	// Oc = tan (Wc/2) * (sqt (1 - Gc^2)) / Gc			analog version, high pass
	// Gc = 10 ^ (-Ac/20)								gain at cutoff.  Ac = 3db, so Gc^2 = 0.5
	// a = ( 1 - Oc ) / ( 1 + Oc )
	// b = ( 1 - a ) / 2

	Oc = tan ( Wc / 2.0 );

	fa = ( 1.0 - Oc ) / ( 1.0 + Oc );

	fb = ( 1.0 - fa ) / 2.0;

	if ( ftype == FLT_HP )
		fb = ( 1.0 + fa ) / 2.0;

	a[0] = 0;						// a0 always ignored
	a[1] = (int)( -fa * PMAX );		// quantize params down to 0-PMAX >> PBITS
	b[0] = (int)( fb * PMAX );
	b[1] = b[0];
	
	if ( ftype == FLT_HP )
		b[1] = -b[1];

	*pM = *pL = 1;

	return;
}


// convolution of x[n] with h[n], resulting in y[n]
// h, x, y filter, input and output arrays (double precision)
// M = filter order, L = input length
// h is M+1 dimensional
// x is L dimensional
// y is L+M dimensional

void conv ( int M, double *h, int L, double *x, double *y )
{
	int n, m;

	for ( n = 0; n < L+M; n++ )
	{
		for (y[n] = 0, m = max(0, n-L+1); m <= min(n, M); m++ )
		{
			y[n] += h[m] * x[n-m];
		}
	}
}

// cas2can - convert cascaded, second order section parameter arrays to 
// canonical numerator/denominator arrays.  Canonical implementations
// have half as many multiplies as cascaded implementations.

// K is number of cascaded sections
// A is Kx3 matrix of sos params A[K] = A[0]..A[K-1]
// a is (2K + 1) -dimensional output of canonical params

#define KMAX	32			// max # of sos sections - 8 is the most we should ever see at runtime

void cas2can ( int K, double A[KMAX+1][3], int *aout )
{
	int i, j;
	double d[2*KMAX + 1];
	double a[2*KMAX + 1];

	Assert ( K <= KMAX );
	
	Q_memset(d, 0, sizeof (double) * (2 * KMAX + 1));
	Q_memset(a, 0, sizeof (double) * (2 * KMAX + 1));

	a[0] = 1;

	for (i = 0; i < K; i++)
	{
		conv( 2, A[i], 2*i + 1, a, d );

		for ( j = 0; j < 2*i + 3; j++ )
			a[j] = d[j];
	}

	for (i = 0; i < (2*K + 1); i++)
		aout[i] = a[i] * PMAX;
}


// chebyshev IIR design, type 2, Lowpass or Highpass

#define lnf(e) (2.303 * log10 (e))

#define acosh(e)  ( lnf( (e) + sqrt((e)*(e) - 1) ) )
#define asinh(e)  ( lnf( (e) + sqrt((e)*(e) + 1) ) )


// returns a[], b[] which are Kx3 matrices of cascaded second-order sections
// these matrices may be passed directly to the iir_cas() routine for evaluation
// Nmax - maximum order of filter
// cutoff, ftype, qwidth - filter cutoff in hz, filter type FLT_LOWPASS/HIGHPASS, qwidth in hz
// pM - denominator order
// pL - numerator order
// a - array of canonical filter params
// b - array of canonical filter params

void FLT_Design_Cheb ( int Nmax, float cutoff, float ftype, float qwidth, int *pM, int *pL, int *a, int *b )
{
// p769 - converted from MATLAB

	double s =		(ftype == FLT_LP ? 1 : -1 );		// 1 for LP, -1 for HP
	double fs =		SOUND_DMA_SPEED;					// sampling frequency
	double fpass =	cutoff;								// cutoff frequency
	double fstop =	fpass + max (2000, qwidth);	// stop frequency
	double Apass =	0.5;								// max attenuation of pass band UNDONE: use Quality to select this
	double Astop =	10;									// max amplitude of stop band	UNDONE: use Quality to select this

	double Wpass, Wstop, epass, estop, Nex, aa, W3, f3, W0, G, Wi2, W02, a1, a2, th, Wi, D, b1;
	int K, r, N;
	double A[KMAX+1][3];								// denominator output matrices, second order sections
	double B[KMAX+1][3];								// numerator output matrices, second order sections

	Wpass = tan( M_PI * fpass / fs ); Wpass = powf (Wpass, s);
	Wstop = tan( M_PI * fstop / fs ); Wstop = powf (Wstop, s);

	epass = sqrt( pow( 10, Apass/10 ) - 1 );
	estop = sqrt( pow( 10, Astop/10 ) - 1 );

	// calculate filter order N

	Nex = acosh( estop/epass ) / acosh ( Wstop/Wpass );
	N = min ( ceil(Nex), Nmax );	// don't exceed Nmax for filter order
	r = ( (int)N & 1);				// r == 1 if N is odd
	K = (N - r ) / 2;

	aa = asinh ( estop ) / N;
	W3 = Wstop / cosh( acosh(estop)/N );
	f3 = (fs / M_PI) * atan( pow( W3, s ) );
	
	W0 = sinh( aa ) / Wstop;
	W02 = W0 * W0;

	// 1st order section for N odd

	if ( r == 1 )
	{
		G = 1 / (1 + W0);
		A[0][0] = 1; A[0][1] = s * (2*G-1); A[0][2] = 0;
		B[0][0] = G; B[0][1] = G*s;			B[0][2] = 0;
	}
	else
	{
		A[0][0] = 1; A[0][1] = 0; A[0][2] = 0;
		B[0][0] = 1; B[0][1] = 0; B[0][2] = 0;
	}

	for (int i = 1; i <= K ; i++ )
	{
		th = M_PI * (N - 1 + 2 * i) / (2 * N);
		Wi = sin(th) / Wstop;
		Wi2 = Wi * Wi;

		D = 1 - 2 * W0 * cos(th) + W02 + Wi2;
		G = ( 1 + Wi2 ) / D;

		b1 = 2 * ( 1 - Wi2 ) / ( 1 + Wi2 );
		a1 = 2 * ( 1 - W02 - Wi2) / D;
		a2 = ( 1 + 2 * W0 * cos(th) + W02 + Wi2) / D;

		A[i][0] = 1;
		A[i][1] = s * a1;
		A[i][2] = a2;
		
		B[i][0] = G;
		B[i][1] = G* s* b1;
		B[i][2] = G;
	}

	// convert cascade parameters to canonical parameters

	cas2can ( K, A, a );
	*pM = 2*K + 1;

	cas2can ( K, B, b );
	*pL = 2*K + 1;
}

// filter parameter order
	
typedef enum
{
	flt_iftype,				
	flt_icutoff,			
	flt_iqwidth,			
	flt_iquality,			

	flt_cparam				// # of params
} flt_e;

// filter parameter ranges

prm_rng_t flt_rng[] = {

	{flt_cparam,	0, 0},			// first entry is # of parameters

	{flt_iftype,	0, FTR_MAX},	// filter type FLT_LP, FLT_HP, FLT_BP (UNDONE: FLT_BP currently ignored)			
	{flt_icutoff,	10, 22050},		// cutoff frequency in hz at -3db gain
	{flt_iqwidth,	100, 11025},	// width of BP, or steepness of LP/HP (ie: fcutoff + qwidth = -60db gain point)
	{flt_iquality,	0, QUA_MAX},	// QUA_LO, _MED, _HI 0,1,2,3 
};


// convert prc float params to iir filter params, alloc filter and return ptr to it
// filter quality set by prc quality - 0,1,2

flt_t * FLT_Params ( prc_t *pprc )
{
	float qual		= pprc->prm[flt_iquality];
	float cutoff	= pprc->prm[flt_icutoff];
	float ftype		= pprc->prm[flt_iftype];
	float qwidth	= pprc->prm[flt_iqwidth];
	
	int L = 0;					// numerator order
	int M = 0;					// denominator order
	int b[FLT_M+1];				// numerator params	 0..PMAX
	int a[FLT_M+1];				// denominator params 0..PMAX
	
	// low pass and highpass filter design 
	
	if ( (int) qual == QUA_LO) qual = QUA_MED;	// disable lowest quality filter - check perf on lowend KDB

	switch ( (int)qual ) 
	{
	case QUA_LO:
		
		// lowpass averaging filter: perf KDB
		
		Assert ( ftype == FLT_LP );
		Assert ( cutoff <= SOUND_DMA_SPEED );
		M = 0;
		
		// L is # of samples to average

		L = 0;
		if ( cutoff <= SOUND_DMA_SPEED / 4) L = 1;		// 11k
		if ( cutoff <= SOUND_DMA_SPEED / 8) L = 2;		// 5.5k
		if ( cutoff <= SOUND_DMA_SPEED / 16) L = 4;		// 2.75k
		if ( cutoff <= SOUND_DMA_SPEED / 32) L = 8;		// 1.35k
		if ( cutoff <= SOUND_DMA_SPEED / 64) L = 12;	// 750hz
		
		break;
	case QUA_MED:
		//	1st order IIR filter, 3db cutoff at fc
		FLT_Design_3db_IIR ( cutoff, ftype, &M, &L, a, b );
			
		M = clamp (M, 1, FLT_M);
		L = clamp (L, 1, FLT_M);

		break;
	case QUA_HI:
		// type 2 chebyshev N = 4 IIR 
		FLT_Design_Cheb ( 4, cutoff, ftype, qwidth, &M, &L, a, b );
			
		M = clamp (M, 1, FLT_M);
		L = clamp (L, 1, FLT_M);

		break;
	case QUA_VHI:
	// type 2 chebyshev N = 7 IIR
		FLT_Design_Cheb ( 8, cutoff, ftype, qwidth, &M, &L, a, b );

			
		M = clamp (M, 1, FLT_M);
		L = clamp (L, 1, FLT_M);

		break;
	}

	return FLT_Alloc ( M, L, a, b );
}

inline void * FLT_VParams ( void *p ) 
{
	PRC_CheckParams( (prc_t *)p, flt_rng);
	return (void *) FLT_Params ((prc_t *)p); 
}

inline void FLT_Mod ( void *p, float v ) { return; }

// get next filter value for filter pf and input x

inline int FLT_GetNext ( flt_t *pf, int  x )
{
	return iir_filter (pf->M, pf->a, pf->L, pf->b, pf->w, x);
	// return iir_filter2 (pf->M, pf->a, pf->L, pf->b, pf->w, x);
}

// batch version for performance

inline void FLT_GetNextN( flt_t *pflt, portable_samplepair_t *pbuffer, int SampleCount, int op )
{
	int count = SampleCount;
	portable_samplepair_t *pb = pbuffer;
	
	switch (op)
	{
	default:
	case OP_LEFT:
		while (count--)
		{
			pb->left = FLT_GetNext( pflt, pb->left );
			pb++;
		}
		return;
	case OP_RIGHT:
		while (count--)
		{
			pb->right = FLT_GetNext( pflt, pb->right );
			pb++;
		}
		return;
	case OP_LEFT_DUPLICATE:
		while (count--)
		{
			pb->left = pb->right = FLT_GetNext( pflt, pb->left );
			pb++;
		}
		return;
	}
}

///////////////////////////////////////////////////////////////////////////
// Positional updaters for pitch shift etc
///////////////////////////////////////////////////////////////////////////

// looping position within a wav, with integer and fractional parts
// used for pitch shifting, upsampling/downsampling
// 20 bits of fraction, 8+ bits of integer

struct pos_t
{

	fix20int step;	// wave table whole and fractional step value
	fix20int cstep;	// current cummulative step value
	int pos;		// current position within wav table
	
	int D;			// max dimension of array w[0...D] ie: # of samples = D+1
};

// circular wrap of pointer p, relative to array w
// D max buffer index w[0...D] (count of samples in buffer is D+1)
// i circular index

inline void POS_Wrap ( int D, int *i )
{
	if ( *i > D )