ITABiquad.cpp 3.35 KB
Newer Older
1
#include <ITABiquad.h>
2
#include <ITAException.h>
3
4
5

CITABiquad::CITABiquad()
{
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
6
	ClearAccumulators();
7
8
}

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
9
void CITABiquad::ClearAccumulators()
10
{
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
11
12
	m_vfAccumulators.push_back( 0.0f );
	m_vfAccumulators.push_back( 0.0f );
13
14
15
16
}

void CITABiquad::Process( const float* pfInputData, float* pfOutputData, const int iNumSamples )
{
17
18
	// Local accumulators
	float z0, z1, z2;
19

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
20
21
22
	// Restore accumulators from last process call
	z1 = m_vfAccumulators[ 0 ];
	z2 = m_vfAccumulators[ 1 ];
23
24
25
26

	for( int i = 0; i < iNumSamples; i++ )
	{
		// w[n] = x[n] - a_1*w[n-1] - a_2*w[n-2]
27
		z0 = oParams.g * pfInputData[ i ] - oParams.a1 * z1 - oParams.a2 * z2;
28
29

		// y[n] = b_0*w[n] + b_1*w[n-1] + b_2*w[n-2]
30
		pfOutputData[ i ] = oParams.b0 * z0 + oParams.b1 * z1 + oParams.b2 * z2;
31

32
		// Shift accumulators
33
34
35
36
		z2 = z1;
		z1 = z0;
	}

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
37
38
39
	// Store accumulators for next process call
	m_vfAccumulators[ 0 ] = z1;
	m_vfAccumulators[ 1 ] = z2;
40
41
42
43

	return;
}

44
void CITABiquad::Process( const float* pfInputData, float* pfOutputData, const int iNumSamples, const float fOutputGain, const int iOutputMode )
45
{
46
	// Local accumulators
47
48
	float z0, z1, z2;

Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
49
50
	z1 = m_vfAccumulators[ 0 ];
	z2 = m_vfAccumulators[ 1 ];
51
52

	if( iOutputMode == CITABiquad::OUTPUT_ADD )
53
	{
54
		for( int i = 0; i < iNumSamples; i++ )
55
		{
56
57
			z0 = oParams.g * pfInputData[ i ] - oParams.a1*z1 - oParams.a2*z2;
			pfOutputData[ i ] += ( oParams.b0*z0 + oParams.b1*z1 + oParams.b2*z2 ) * fOutputGain;
58

59
			// Shift accumulators
60
61
62
63
			z2 = z1;
			z1 = z0;
		}
	}
64
	else if( iOutputMode == CITABiquad::OUTPUT_OVERWRITE )
65
	{
66
		for( int i = 0; i < iNumSamples; i++ )
67
68
		{
			z0 = oParams.g*pfInputData[ i ] - oParams.a1*z1 - oParams.a2*z2;
69
			pfOutputData[ i ] = ( oParams.b0 * z0 + oParams.b1 * z1 + oParams.b2*z2 ) * fOutputGain;
70

71
			// Shift accumulators
72
73
74
75
			z2 = z1;
			z1 = z0;
		}
	}
76
77
78
79
	else
	{
		ITA_EXCEPT1( INVALID_PARAMETER, "Unrecognized output write mode in CITABiquad" );
	}
80

81
	// Store accumulators
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
82
83
	m_vfAccumulators[ 0 ] = z1;
	m_vfAccumulators[ 1 ] = z2;
84
85
86
87

	return;
}

88
void CITABiquad::Process( const float* pfInputData, float* out, const int iNumSamples, const float fOutputGain1, const float fOutputGain2, const int iOutputWriteMode )
89
{
90
	if( iNumSamples == 0 )
91
92
		return;

93
	// Local accumulators
94
	float z0, z1, z2;
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
95
96
	z1 = m_vfAccumulators[ 0 ];
	z2 = m_vfAccumulators[ 1 ];
97

98
99
	// Factor for linear gain
	const float fLinearGainFactor = ( fOutputGain2 - fOutputGain1 ) / float( iNumSamples );
100

101
	if( iOutputWriteMode == OUTPUT_ADD )
102
	{
103
		for( int i = 0; i < iNumSamples; i++ )
104
		{
105
			const float fSampleGain = fOutputGain1 + i * fLinearGainFactor;
106
107

			z0 = oParams.g*pfInputData[ i ] - oParams.a1*z1 - oParams.a2*z2;
108
			out[ i ] += ( oParams.b0*z0 + oParams.b1*z1 + oParams.b2*z2 ) * fSampleGain;
109

110
			// Shift accumulators
111
112
113
114
115
			z2 = z1;
			z1 = z0;
		}

	}
116
	else if( iOutputWriteMode == CITABiquad::OUTPUT_OVERWRITE )
117
	{
118
		for( int i = 0; i < iNumSamples; i++ )
119
		{
120
			const float fSampleGain = fOutputGain1 + i * fLinearGainFactor;
121
122

			z0 = oParams.g*pfInputData[ i ] - oParams.a1*z1 - oParams.a2*z2;
123
			out[ i ] = ( oParams.b0*z0 + oParams.b1*z1 + oParams.b2*z2 ) * fSampleGain;
124

125
			// Shift accumulators
126
127
128
129
			z2 = z1;
			z1 = z0;
		}
	}
130
131
132
133
	else
	{
		ITA_EXCEPT1( INVALID_PARAMETER, "Unrecognized output write mode in CITABiquad" );
	}
134

135
	// Store accumulators
Dipl.-Ing. Jonas Stienen's avatar
Dipl.-Ing. Jonas Stienen committed
136
137
	m_vfAccumulators[ 0 ] = z1;
	m_vfAccumulators[ 1 ] = z2;
138
139
140
141
142
143
144
145
146
147
148
}

CITABiquad::CParams::CParams()
	: g( 1 )
	, a1( 0 )
	, a2( 0 )
	, b0( 1 )
	, b1( 0 )
	, b2( 0 )
{
}