1 /*
2 * Copyright (C) 2020 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #define LOG_TAG "EffectHG_Processors"
18 //#define LOG_NDEBUG 0
19 #include <utils/Log.h>
20
21 #include <assert.h>
22
23 #include <cmath>
24
25 #include "Processors.h"
26
27 #if defined(__aarch64__) || defined(__ARM_NEON__)
28 #ifndef USE_NEON
29 #define USE_NEON (true)
30 #endif
31 #else
32 #define USE_NEON (false)
33 #endif
34 #if USE_NEON
35 #include <arm_neon.h>
36 #endif
37
38 namespace android::audio_effect::haptic_generator {
39
getRealPoleZ(float cornerFrequency,float sampleRate)40 float getRealPoleZ(float cornerFrequency, float sampleRate) {
41 // This will be a pole of a first order filter.
42 float realPoleS = -2 * M_PI * cornerFrequency;
43 return exp(realPoleS / sampleRate); // zero-pole matching
44 }
45
getComplexPoleZ(float ringingFrequency,float q,float sampleRate)46 std::pair<float, float> getComplexPoleZ(float ringingFrequency, float q, float sampleRate) {
47 // This is the pole for 1/(s^2 + s/q + 1) in normalized frequency. The other pole is
48 // the complex conjugate of this.
49 float poleImagS = 2 * M_PI * ringingFrequency;
50 float poleRealS = -poleImagS / (2 * q);
51 float poleRadius = exp(poleRealS / sampleRate);
52 float poleImagZ = poleRadius * sin(poleImagS / sampleRate);
53 float poleRealZ = poleRadius * cos(poleImagS / sampleRate);
54 return {poleRealZ, poleImagZ};
55 }
56
57 // Implementation of Ramp
58
Ramp(size_t channelCount)59 Ramp::Ramp(size_t channelCount) : mChannelCount(channelCount) {}
60
process(float * out,const float * in,size_t frameCount)61 void Ramp::process(float *out, const float *in, size_t frameCount) {
62 size_t i = 0;
63 #if USE_NEON
64 size_t sampleCount = frameCount * mChannelCount;
65 float32x2_t allZero = vdup_n_f32(0.0f);
66 while (i + 1 < sampleCount) {
67 vst1_f32(out, vmax_f32(vld1_f32(in), allZero));
68 in += 2;
69 out += 2;
70 i += 2;
71 }
72 #endif // USE_NEON
73 for (; i < frameCount * mChannelCount; ++i) {
74 *out = *in >= 0.0f ? *in : 0.0f;
75 out++;
76 in++;
77 }
78 }
79
80 // Implementation of SlowEnvelope
81
SlowEnvelope(float cornerFrequency,float sampleRate,float normalizationPower,float envOffset,size_t channelCount)82 SlowEnvelope::SlowEnvelope(
83 float cornerFrequency,
84 float sampleRate,
85 float normalizationPower,
86 float envOffset,
87 size_t channelCount)
88 : mLpf(createLPF(cornerFrequency, sampleRate, channelCount)),
89 mNormalizationPower(normalizationPower),
90 mEnvOffset(envOffset),
91 mChannelCount(channelCount) {}
92
process(float * out,const float * in,size_t frameCount)93 void SlowEnvelope::process(float* out, const float* in, size_t frameCount) {
94 size_t sampleCount = frameCount * mChannelCount;
95 if (sampleCount > mLpfOutBuffer.size()) {
96 mLpfOutBuffer.resize(sampleCount);
97 mLpfInBuffer.resize(sampleCount);
98 }
99 for (size_t i = 0; i < sampleCount; ++i) {
100 mLpfInBuffer[i] = fabs(in[i]);
101 }
102 mLpf->process(mLpfOutBuffer.data(), mLpfInBuffer.data(), frameCount);
103 for (size_t i = 0; i < sampleCount; ++i) {
104 out[i] = in[i] * pow(mLpfOutBuffer[i] + mEnvOffset, mNormalizationPower);
105 }
106 }
107
setNormalizationPower(float normalizationPower)108 void SlowEnvelope::setNormalizationPower(float normalizationPower) {
109 mNormalizationPower = normalizationPower;
110 }
111
clear()112 void SlowEnvelope::clear() {
113 mLpf->clear();
114 }
115
116 // Implementation of distortion
117
Distortion(float cornerFrequency,float sampleRate,float inputGain,float cubeThreshold,float outputGain,size_t channelCount)118 Distortion::Distortion(
119 float cornerFrequency,
120 float sampleRate,
121 float inputGain,
122 float cubeThreshold,
123 float outputGain,
124 size_t channelCount)
125 : mLpf(createLPF2(cornerFrequency, sampleRate, channelCount)),
126 mSampleRate(sampleRate),
127 mCornerFrequency(cornerFrequency),
128 mInputGain(inputGain),
129 mCubeThreshold(cubeThreshold),
130 mOutputGain(outputGain),
131 mChannelCount(channelCount) {}
132
process(float * out,const float * in,size_t frameCount)133 void Distortion::process(float *out, const float *in, size_t frameCount) {
134 size_t sampleCount = frameCount * mChannelCount;
135 if (sampleCount > mLpfInBuffer.size()) {
136 mLpfInBuffer.resize(sampleCount);
137 }
138 for (size_t i = 0; i < sampleCount; ++i) {
139 const float x = mInputGain * in[i];
140 mLpfInBuffer[i] = x * x * x / (mCubeThreshold + x * x); // "Coring" nonlinearity.
141 }
142 mLpf->process(out, mLpfInBuffer.data(), frameCount); // Reduce 3*F components.
143 for (size_t i = 0; i < sampleCount; ++i) {
144 const float x = out[i];
145 out[i] = mOutputGain * x / (1.0f + fabs(x)); // Soft limiter.
146 }
147 }
148
setCornerFrequency(float cornerFrequency)149 void Distortion::setCornerFrequency(float cornerFrequency) {
150 mCornerFrequency = cornerFrequency;
151 BiquadFilterCoefficients coefficient = lpfCoefs(cornerFrequency, mSampleRate);
152 mLpf->setCoefficients(coefficient);
153 }
154
setInputGain(float inputGain)155 void Distortion::setInputGain(float inputGain) {
156 mInputGain = inputGain;
157 }
158
setCubeThrehold(float cubeThreshold)159 void Distortion::setCubeThrehold(float cubeThreshold) {
160 mCubeThreshold = cubeThreshold;
161 }
162
setOutputGain(float outputGain)163 void Distortion::setOutputGain(float outputGain) {
164 mOutputGain = outputGain;
165 }
166
clear()167 void Distortion::clear() {
168 mLpf->clear();
169 }
170
171
172 // Implementation of helper functions
173
cascadeFirstOrderFilters(const BiquadFilterCoefficients & coefs1,const BiquadFilterCoefficients & coefs2)174 BiquadFilterCoefficients cascadeFirstOrderFilters(const BiquadFilterCoefficients &coefs1,
175 const BiquadFilterCoefficients &coefs2) {
176 assert(coefs1[2] == 0.0f);
177 assert(coefs2[2] == 0.0f);
178 assert(coefs1[4] == 0.0f);
179 assert(coefs2[4] == 0.0f);
180 return {coefs1[0] * coefs2[0],
181 coefs1[0] * coefs2[1] + coefs1[1] * coefs2[0],
182 coefs1[1] * coefs2[1],
183 coefs1[3] + coefs2[3],
184 coefs1[3] * coefs2[3]};
185 }
186
lpfCoefs(const float cornerFrequency,const float sampleRate)187 BiquadFilterCoefficients lpfCoefs(const float cornerFrequency, const float sampleRate) {
188 BiquadFilterCoefficients coefficient;
189 float realPoleZ = getRealPoleZ(cornerFrequency, sampleRate);
190 // This is a zero at nyquist
191 coefficient[0] = 0.5f * (1 - realPoleZ);
192 coefficient[1] = coefficient[0];
193 coefficient[2] = 0.0f;
194 coefficient[3] = -realPoleZ; // This is traditional 1/(s+1) filter
195 coefficient[4] = 0.0f;
196 return coefficient;
197 }
198
bpfCoefs(const float ringingFrequency,const float q,const float sampleRate)199 BiquadFilterCoefficients bpfCoefs(const float ringingFrequency,
200 const float q,
201 const float sampleRate) {
202 BiquadFilterCoefficients coefficient;
203 const auto [real, img] = getComplexPoleZ(ringingFrequency, q, sampleRate);
204 // Note: this is not a standard cookbook BPF, but a low pass filter with zero at DC
205 coefficient[0] = 1.0f;
206 coefficient[1] = -1.0f;
207 coefficient[2] = 0.0f;
208 coefficient[3] = -2 * real;
209 coefficient[4] = real * real + img * img;
210 return coefficient;
211 }
212
bsfCoefs(const float ringingFrequency,const float zq,const float pq,const float sampleRate)213 BiquadFilterCoefficients bsfCoefs(const float ringingFrequency,
214 const float zq,
215 const float pq,
216 const float sampleRate) {
217 BiquadFilterCoefficients coefficient;
218 const auto [zeroReal, zeroImg] = getComplexPoleZ(ringingFrequency, zq, sampleRate);
219 float zeroCoeff1 = -2 * zeroReal;
220 float zeroCoeff2 = zeroReal* zeroReal + zeroImg * zeroImg;
221 const auto [poleReal, poleImg] = getComplexPoleZ(ringingFrequency, pq, sampleRate);
222 float poleCoeff1 = -2 * poleReal;
223 float poleCoeff2 = poleReal * poleReal + poleImg * poleImg;
224 const float norm = (1.0f + poleCoeff1 + poleCoeff2) / (1.0f + zeroCoeff1 + zeroCoeff2);
225 coefficient[0] = 1.0f * norm;
226 coefficient[1] = zeroCoeff1 * norm;
227 coefficient[2] = zeroCoeff2 * norm;
228 coefficient[3] = poleCoeff1;
229 coefficient[4] = poleCoeff2;
230 return coefficient;
231 }
232
createLPF(const float cornerFrequency,const float sampleRate,const size_t channelCount)233 std::shared_ptr<HapticBiquadFilter> createLPF(const float cornerFrequency,
234 const float sampleRate,
235 const size_t channelCount) {
236 BiquadFilterCoefficients coefficient = lpfCoefs(cornerFrequency, sampleRate);
237 return std::make_shared<HapticBiquadFilter>(channelCount, coefficient);
238 }
239
createLPF2(const float cornerFrequency,const float sampleRate,const size_t channelCount)240 std::shared_ptr<HapticBiquadFilter> createLPF2(const float cornerFrequency,
241 const float sampleRate,
242 const size_t channelCount) {
243 BiquadFilterCoefficients coefficient = lpfCoefs(cornerFrequency, sampleRate);
244 return std::make_shared<HapticBiquadFilter>(
245 channelCount, cascadeFirstOrderFilters(coefficient, coefficient));
246 }
247
createHPF2(const float cornerFrequency,const float sampleRate,const size_t channelCount)248 std::shared_ptr<HapticBiquadFilter> createHPF2(const float cornerFrequency,
249 const float sampleRate,
250 const size_t channelCount) {
251 BiquadFilterCoefficients coefficient;
252 // Note: this is valid only when corner frequency is less than nyquist / 2.
253 float realPoleZ = getRealPoleZ(cornerFrequency, sampleRate);
254
255 // Note: this is a zero at DC
256 coefficient[0] = 0.5f * (1 + realPoleZ);
257 coefficient[1] = -coefficient[0];
258 coefficient[2] = 0.0f;
259 coefficient[3] = -realPoleZ;
260 coefficient[4] = 0.0f;
261 return std::make_shared<HapticBiquadFilter>(
262 channelCount, cascadeFirstOrderFilters(coefficient, coefficient));
263 }
264
createBPF(const float ringingFrequency,const float q,const float sampleRate,const size_t channelCount)265 std::shared_ptr<HapticBiquadFilter> createBPF(const float ringingFrequency,
266 const float q,
267 const float sampleRate,
268 const size_t channelCount) {
269 BiquadFilterCoefficients coefficient = bpfCoefs(ringingFrequency, q, sampleRate);
270 return std::make_shared<HapticBiquadFilter>(channelCount, coefficient);
271 }
272
createBSF(const float ringingFrequency,const float zq,const float pq,const float sampleRate,const size_t channelCount)273 std::shared_ptr<HapticBiquadFilter> createBSF(const float ringingFrequency,
274 const float zq,
275 const float pq,
276 const float sampleRate,
277 const size_t channelCount) {
278 BiquadFilterCoefficients coefficient = bsfCoefs(ringingFrequency, zq, pq, sampleRate);
279 return std::make_shared<HapticBiquadFilter>(channelCount, coefficient);
280 }
281
282 } // namespace android::audio_effect::haptic_generator
283