1 /*
2 * Copyright 2012, Gerasim Troeglazov (3dEyes**), 3dEyes@gmail.com.
3 * All rights reserved.
4 * Distributed under the terms of the MIT License.
5 */
6
7 #include <stdlib.h>
8 #include <math.h>
9 #include <string.h>
10
11 #include "Equalizer.h"
12
Equalizer()13 Equalizer::Equalizer()
14 {
15 fActivated = false;
16 CleanUp();
17 }
18
~Equalizer()19 Equalizer::~Equalizer()
20 {
21 }
22
23 void
SetFormat(int channels,float framerate)24 Equalizer::SetFormat(int channels, float framerate)
25 {
26 fChannels = channels;
27 fRate = framerate;
28
29 memset(fWDataVector[0][0], 0, sizeof(fWDataVector));
30
31 fActiveBands = BandCount();
32 while(fFrequency[fActiveBands-1] * 2.0 > fRate) {
33 fActiveBands--;
34 }
35
36 for(int i = 0; i < fActiveBands; i++) {
37 BandPassFilterCalcs(fAWeights[i], fBWeights[i],
38 fFrequency[i] / (float)fRate);
39 }
40 }
41
42 void
SetPreAmp(double value)43 Equalizer::SetPreAmp(double value)
44 {
45 fPreAmp = value;
46 RecalcGains();
47 }
48
49 double
PreAmp(void)50 Equalizer::PreAmp(void)
51 {
52 return fPreAmp;
53 }
54
55 void
SetBand(int band,double value)56 Equalizer::SetBand(int band, double value)
57 {
58 if (band < 0 || band >= BandCount())
59 return;
60
61 fBands[band] = value;
62 RecalcGains();
63 }
64
65 double
Band(int band)66 Equalizer::Band(int band)
67 {
68 if (band < 0 || band >= BandCount())
69 return 0.0;
70
71 return fBands[band];
72 }
73
74 int
BandCount(void)75 Equalizer::BandCount(void)
76 {
77 return EQ_BANDS;
78 }
79
80 float
BandFrequency(int band)81 Equalizer::BandFrequency(int band)
82 {
83 if (band < 0 || band >= BandCount())
84 return 0.0;
85
86 return fFrequency[band];
87 }
88
89 void
ProcessBuffer(float * buffer,int samples)90 Equalizer::ProcessBuffer(float* buffer, int samples)
91 {
92 if (!fActivated || samples <= 0)
93 return;
94
95 for(int i = 0; i < fChannels; i++) {
96 float *g = fGainVector[i];
97 float *end = buffer + samples;
98 for(float *fptr = buffer + i; fptr < end; fptr += fChannels) {
99 float yt = *fptr;
100 for (int k = 0; k < fActiveBands; k ++) {
101 float *Wq = fWDataVector[i][k];
102 float w = yt * fBWeights[k][0] +
103 Wq[0] * fAWeights[k][0] +
104 Wq[1] * fAWeights[k][1];
105 yt += (w + Wq[1] * fBWeights[k][1]) * g[k];
106 Wq[1] = Wq[0];
107 Wq[0] = w;
108 }
109 *fptr = yt;
110 }
111 }
112 }
113
114 void
CleanUp()115 Equalizer::CleanUp()
116 {
117 float freq[EQ_BANDS] = {31.25, 62.5, 125, 250, 500, 1000, 2000, 4000, 8000, 16000};
118
119 for(int i = 0; i < BandCount(); i++) {
120 fFrequency[i] = freq[i];
121 fBands[i] = 0.0;
122 }
123
124 fPreAmp = 0.0;
125 RecalcGains();
126 }
127
128 void
RecalcGains(void)129 Equalizer::RecalcGains(void)
130 {
131 float adjust[EQ_BANDS];
132
133 for(int i = 0; i < BandCount(); i++)
134 adjust[i] = fPreAmp + fBands[i];
135
136 for(int c = 0; c < MAX_CHANNELS; c++)
137 for(int i = 0; i<BandCount(); i++)
138 fGainVector[c][i] = pow(10, adjust[i] / 20) - 1;
139
140 fActivated = true;
141 }
142
143 void
BandPassFilterCalcs(float * a,float * b,float f)144 Equalizer::BandPassFilterCalcs(float *a, float *b, float f)
145 {
146 float q = 1.2247449;
147 float theta = 2.0 * M_PI * f;
148 float c_value = (1 - tanf(theta * q / 2.0)) / (1 + tanf(theta * q / 2.0));
149
150 a[0] = (1 + c_value) * cosf(theta);
151 a[1] = -c_value;
152 b[0] = (1 - c_value) / 2.0;
153 b[1] = -1.005;
154 }
155