41 PassType filterPassType,
44 this->resonance = resonance;
45 this->frequency = frequency;
46 this->sampleRate = sampleRate;
47 this->filterPassType = filterPassType;
48 switch (filterPassType)
51 c = 1.0 / tan(
M_PI * frequency / sampleRate);
52 a1 = 1.0 / (1.0 + resonance *
c +
c *
c);
55 b1 = 2.0 * (1.0 -
c *
c) * a1;
56 b2 = (1.0 - resonance *
c +
c *
c) * a1;
59 c = tan(
M_PI * frequency / sampleRate);
60 a1 = 1.0 / (1.0 + resonance *
c +
c *
c);
63 b1 = 2.0 * (
c *
c - 1.0) * a1;
64 b2 = (1.0 - resonance *
c +
c *
c) * a1;
73 armarx::PassType filterPassType,
76 reset(frequency, sampleRate, filterPassType, resonance);
82 for (
auto&
v : inputHistory)
86 for (
auto&
v : outputHistory)
92 armarx::ParameterTypeList
95 ParameterTypeList result;
110 if (!var->getInitialized())
115 double newInput = 0.0;
116 auto type = var->getType();
119 newInput =
static_cast<double>(var->getFloat());
123 newInput = var->getDouble();
127 newInput =
static_cast<double>(var->getInt());
131 newInput =
static_cast<double>(var->getLong());
139 std::unique_lock lock(historyMutex);
141 double newOutput = a1 * newInput + a2 * this->inputHistory[0] + a3 * this->inputHistory[1] -
142 b1 * this->outputHistory[0] - b2 * this->outputHistory[1];
144 this->inputHistory[1] = this->inputHistory[0];
145 this->inputHistory[0] = newInput;
147 this->outputHistory[2] = this->outputHistory[1];
148 this->outputHistory[1] = this->outputHistory[0];
149 this->outputHistory[0] = newOutput;
155 std::unique_lock lock(historyMutex);
157 return new Variant(outputHistory[0]);
163 std::unique_lock lock(historyMutex);
164 return outputHistory[0];
170 std::unique_lock lock(historyMutex);
172 return new Variant(outputHistory[0]);
175 StringFloatDictionary
178 StringFloatDictionary props;
179 props[
"resonance"] = resonance;
180 props[
"frequency"] = frequency;
181 props[
"sampleRate"] = sampleRate;
182 props[
"filterPassType"] = (int)(filterPassType);
190 auto it = newValues.find(
"resonance");
191 if (it != newValues.end())
193 resonance = it->second;
196 it = newValues.find(
"frequency");
197 if (it != newValues.end())
199 frequency = it->second;
202 it = newValues.find(
"sampleRate");
203 if (it != newValues.end())
205 sampleRate = it->second;
208 it = newValues.find(
"filterPassType");
209 if (it != newValues.end())
211 filterPassType =
static_cast<PassType
>(it->second);
213 reset(frequency, sampleRate, filterPassType, resonance);