initial commit
This commit is contained in:
Christopher Herb
677
filter/aw_eq.h
Normal file
677
filter/aw_eq.h
Normal file
@@ -0,0 +1,677 @@
|
||||
#pragma once
|
||||
#include <cstdlib>
|
||||
#include <stdint.h>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
// 3 band equalizer with high/lowpass filters based on EQ by Chris Johnson.
|
||||
class aw_eq {
|
||||
public:
|
||||
aw_eq() {
|
||||
samplerate = 44100;
|
||||
|
||||
A = 0.5; //Treble -12 to 12
|
||||
B = 0.5; //Mid -12 to 12
|
||||
C = 0.5; //Bass -12 to 12
|
||||
D = 1.0; //Lowpass 16.0K log 1 to 16 defaulting to 16K
|
||||
E = 0.4; //TrebFrq 6.0 log 1 to 16 defaulting to 6K
|
||||
F = 0.4; //BassFrq 100.0 log 30 to 1600 defaulting to 100 hz
|
||||
G = 0.0; //Hipass 30.0 log 30 to 1600 defaulting to 30
|
||||
H = 0.5; //OutGain -18 to 18
|
||||
|
||||
lastSampleL = 0.0;
|
||||
last2SampleL = 0.0;
|
||||
lastSampleR = 0.0;
|
||||
last2SampleR = 0.0;
|
||||
|
||||
iirHighSampleLA = 0.0;
|
||||
iirHighSampleLB = 0.0;
|
||||
iirHighSampleLC = 0.0;
|
||||
iirHighSampleLD = 0.0;
|
||||
iirHighSampleLE = 0.0;
|
||||
iirLowSampleLA = 0.0;
|
||||
iirLowSampleLB = 0.0;
|
||||
iirLowSampleLC = 0.0;
|
||||
iirLowSampleLD = 0.0;
|
||||
iirLowSampleLE = 0.0;
|
||||
iirHighSampleL = 0.0;
|
||||
iirLowSampleL = 0.0;
|
||||
|
||||
iirHighSampleRA = 0.0;
|
||||
iirHighSampleRB = 0.0;
|
||||
iirHighSampleRC = 0.0;
|
||||
iirHighSampleRD = 0.0;
|
||||
iirHighSampleRE = 0.0;
|
||||
iirLowSampleRA = 0.0;
|
||||
iirLowSampleRB = 0.0;
|
||||
iirLowSampleRC = 0.0;
|
||||
iirLowSampleRD = 0.0;
|
||||
iirLowSampleRE = 0.0;
|
||||
iirHighSampleR = 0.0;
|
||||
iirLowSampleR = 0.0;
|
||||
|
||||
tripletLA = 0.0;
|
||||
tripletLB = 0.0;
|
||||
tripletLC = 0.0;
|
||||
tripletFactorL = 0.0;
|
||||
|
||||
tripletRA = 0.0;
|
||||
tripletRB = 0.0;
|
||||
tripletRC = 0.0;
|
||||
tripletFactorR = 0.0;
|
||||
|
||||
lowpassSampleLAA = 0.0;
|
||||
lowpassSampleLAB = 0.0;
|
||||
lowpassSampleLBA = 0.0;
|
||||
lowpassSampleLBB = 0.0;
|
||||
lowpassSampleLCA = 0.0;
|
||||
lowpassSampleLCB = 0.0;
|
||||
lowpassSampleLDA = 0.0;
|
||||
lowpassSampleLDB = 0.0;
|
||||
lowpassSampleLE = 0.0;
|
||||
lowpassSampleLF = 0.0;
|
||||
lowpassSampleLG = 0.0;
|
||||
|
||||
lowpassSampleRAA = 0.0;
|
||||
lowpassSampleRAB = 0.0;
|
||||
lowpassSampleRBA = 0.0;
|
||||
lowpassSampleRBB = 0.0;
|
||||
lowpassSampleRCA = 0.0;
|
||||
lowpassSampleRCB = 0.0;
|
||||
lowpassSampleRDA = 0.0;
|
||||
lowpassSampleRDB = 0.0;
|
||||
lowpassSampleRE = 0.0;
|
||||
lowpassSampleRF = 0.0;
|
||||
lowpassSampleRG = 0.0;
|
||||
|
||||
highpassSampleLAA = 0.0;
|
||||
highpassSampleLAB = 0.0;
|
||||
highpassSampleLBA = 0.0;
|
||||
highpassSampleLBB = 0.0;
|
||||
highpassSampleLCA = 0.0;
|
||||
highpassSampleLCB = 0.0;
|
||||
highpassSampleLDA = 0.0;
|
||||
highpassSampleLDB = 0.0;
|
||||
highpassSampleLE = 0.0;
|
||||
highpassSampleLF = 0.0;
|
||||
|
||||
highpassSampleRAA = 0.0;
|
||||
highpassSampleRAB = 0.0;
|
||||
highpassSampleRBA = 0.0;
|
||||
highpassSampleRBB = 0.0;
|
||||
highpassSampleRCA = 0.0;
|
||||
highpassSampleRCB = 0.0;
|
||||
highpassSampleRDA = 0.0;
|
||||
highpassSampleRDB = 0.0;
|
||||
highpassSampleRE = 0.0;
|
||||
highpassSampleRF = 0.0;
|
||||
|
||||
flip = false;
|
||||
flipthree = 0;
|
||||
|
||||
fpdL = 1.0; while (fpdL < 16386) fpdL = rand()*UINT32_MAX;
|
||||
fpdR = 1.0; while (fpdR < 16386) fpdR = rand()*UINT32_MAX;
|
||||
//this is reset: values being initialized only once. Startup values, whatever they are.
|
||||
}
|
||||
|
||||
void set_treble(double value) {
|
||||
A = clamp(value);
|
||||
}
|
||||
|
||||
void set_mid(double value) {
|
||||
B = clamp(value);
|
||||
}
|
||||
|
||||
void set_bass(double value) {
|
||||
C = clamp(value);
|
||||
}
|
||||
|
||||
void set_lowpass(double value) {
|
||||
D = clamp(value);
|
||||
}
|
||||
|
||||
void set_treble_frq(double value) {
|
||||
E = clamp(value);
|
||||
}
|
||||
|
||||
void set_bass_frq(double value) {
|
||||
F = clamp(value);
|
||||
}
|
||||
|
||||
void set_hipass(double value) {
|
||||
G = clamp(value);
|
||||
}
|
||||
|
||||
void set_out_gain(double value) {
|
||||
H = clamp(value);
|
||||
}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void process_block(double **inputs, double **outputs, long sampleframes) {
|
||||
|
||||
double* in1 = inputs[0];
|
||||
double* in2 = inputs[1];
|
||||
double* out1 = outputs[0];
|
||||
double* out2 = outputs[1];
|
||||
|
||||
double overallscale = 1.0;
|
||||
overallscale /= 44100.0;
|
||||
double compscale = overallscale;
|
||||
overallscale = samplerate;
|
||||
compscale = compscale * overallscale;
|
||||
//compscale is the one that's 1 or something like 2.2 for 96K rates
|
||||
|
||||
double inputSampleL;
|
||||
double inputSampleR;
|
||||
|
||||
double highSampleL = 0.0;
|
||||
double midSampleL = 0.0;
|
||||
double bassSampleL = 0.0;
|
||||
|
||||
double highSampleR = 0.0;
|
||||
double midSampleR = 0.0;
|
||||
double bassSampleR = 0.0;
|
||||
|
||||
double densityA = (A*12.0)-6.0;
|
||||
double densityB = (B*12.0)-6.0;
|
||||
double densityC = (C*12.0)-6.0;
|
||||
bool engageEQ = true;
|
||||
if ( (0.0 == densityA) && (0.0 == densityB) && (0.0 == densityC) ) engageEQ = false;
|
||||
|
||||
densityA = pow(10.0,densityA/20.0)-1.0;
|
||||
densityB = pow(10.0,densityB/20.0)-1.0;
|
||||
densityC = pow(10.0,densityC/20.0)-1.0;
|
||||
//convert to 0 to X multiplier with 1.0 being O db
|
||||
//minus one gives nearly -1 to ? (should top out at 1)
|
||||
//calibrate so that X db roughly equals X db with maximum topping out at 1 internally
|
||||
|
||||
double tripletIntensity = -densityA;
|
||||
|
||||
double iirAmountC = (((D*D*15.0)+1.0)*0.0188) + 0.7;
|
||||
if (iirAmountC > 1.0) iirAmountC = 1.0;
|
||||
bool engageLowpass = false;
|
||||
if (((D*D*15.0)+1.0) < 15.99) engageLowpass = true;
|
||||
|
||||
double iirAmountA = (((E*E*15.0)+1.0)*1000)/overallscale;
|
||||
double iirAmountB = (((F*F*1570.0)+30.0)*10)/overallscale;
|
||||
double iirAmountD = (((G*G*1570.0)+30.0)*1.0)/overallscale;
|
||||
bool engageHighpass = false;
|
||||
if (((G*G*1570.0)+30.0) > 30.01) engageHighpass = true;
|
||||
//bypass the highpass and lowpass if set to extremes
|
||||
double bridgerectifier;
|
||||
double outA = fabs(densityA);
|
||||
double outB = fabs(densityB);
|
||||
double outC = fabs(densityC);
|
||||
//end EQ
|
||||
double outputgain = pow(10.0,((H*36.0)-18.0)/20.0);
|
||||
|
||||
while (--sampleframes >= 0)
|
||||
{
|
||||
inputSampleL = *in1;
|
||||
inputSampleR = *in2;
|
||||
if (fabs(inputSampleL)<1.18e-23) inputSampleL = fpdL * 1.18e-17;
|
||||
if (fabs(inputSampleR)<1.18e-23) inputSampleR = fpdR * 1.18e-17;
|
||||
|
||||
last2SampleL = lastSampleL;
|
||||
lastSampleL = inputSampleL;
|
||||
|
||||
last2SampleR = lastSampleR;
|
||||
lastSampleR = inputSampleR;
|
||||
|
||||
flip = !flip;
|
||||
flipthree++;
|
||||
if (flipthree < 1 || flipthree > 3) flipthree = 1;
|
||||
//counters
|
||||
|
||||
//begin highpass
|
||||
if (engageHighpass)
|
||||
{
|
||||
if (flip)
|
||||
{
|
||||
highpassSampleLAA = (highpassSampleLAA * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLAA;
|
||||
highpassSampleLBA = (highpassSampleLBA * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLBA;
|
||||
highpassSampleLCA = (highpassSampleLCA * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLCA;
|
||||
highpassSampleLDA = (highpassSampleLDA * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLDA;
|
||||
}
|
||||
else
|
||||
{
|
||||
highpassSampleLAB = (highpassSampleLAB * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLAB;
|
||||
highpassSampleLBB = (highpassSampleLBB * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLBB;
|
||||
highpassSampleLCB = (highpassSampleLCB * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLCB;
|
||||
highpassSampleLDB = (highpassSampleLDB * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLDB;
|
||||
}
|
||||
highpassSampleLE = (highpassSampleLE * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLE;
|
||||
highpassSampleLF = (highpassSampleLF * (1.0 - iirAmountD)) + (inputSampleL * iirAmountD);
|
||||
inputSampleL -= highpassSampleLF;
|
||||
|
||||
if (flip)
|
||||
{
|
||||
highpassSampleRAA = (highpassSampleRAA * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRAA;
|
||||
highpassSampleRBA = (highpassSampleRBA * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRBA;
|
||||
highpassSampleRCA = (highpassSampleRCA * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRCA;
|
||||
highpassSampleRDA = (highpassSampleRDA * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRDA;
|
||||
}
|
||||
else
|
||||
{
|
||||
highpassSampleRAB = (highpassSampleRAB * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRAB;
|
||||
highpassSampleRBB = (highpassSampleRBB * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRBB;
|
||||
highpassSampleRCB = (highpassSampleRCB * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRCB;
|
||||
highpassSampleRDB = (highpassSampleRDB * (1.0 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRDB;
|
||||
}
|
||||
highpassSampleRE = (highpassSampleRE * (1 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRE;
|
||||
highpassSampleRF = (highpassSampleRF * (1 - iirAmountD)) + (inputSampleR * iirAmountD);
|
||||
inputSampleR -= highpassSampleRF;
|
||||
|
||||
}
|
||||
//end highpass
|
||||
|
||||
//begin EQ
|
||||
if (engageEQ)
|
||||
{
|
||||
switch (flipthree)
|
||||
{
|
||||
case 1:
|
||||
tripletFactorL = last2SampleL - inputSampleL;
|
||||
tripletLA += tripletFactorL;
|
||||
tripletLC -= tripletFactorL;
|
||||
tripletFactorL = tripletLA * tripletIntensity;
|
||||
iirHighSampleLC = (iirHighSampleLC * (1.0 - iirAmountA)) + (inputSampleL * iirAmountA);
|
||||
highSampleL = inputSampleL - iirHighSampleLC;
|
||||
iirLowSampleLC = (iirLowSampleLC * (1.0 - iirAmountB)) + (inputSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleLC;
|
||||
|
||||
tripletFactorR = last2SampleR - inputSampleR;
|
||||
tripletRA += tripletFactorR;
|
||||
tripletRC -= tripletFactorR;
|
||||
tripletFactorR = tripletRA * tripletIntensity;
|
||||
iirHighSampleRC = (iirHighSampleRC * (1.0 - iirAmountA)) + (inputSampleR * iirAmountA);
|
||||
highSampleR = inputSampleR - iirHighSampleRC;
|
||||
iirLowSampleRC = (iirLowSampleRC * (1.0 - iirAmountB)) + (inputSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleRC;
|
||||
break;
|
||||
case 2:
|
||||
tripletFactorL = last2SampleL - inputSampleL;
|
||||
tripletLB += tripletFactorL;
|
||||
tripletLA -= tripletFactorL;
|
||||
tripletFactorL = tripletLB * tripletIntensity;
|
||||
iirHighSampleLD = (iirHighSampleLD * (1.0 - iirAmountA)) + (inputSampleL * iirAmountA);
|
||||
highSampleL = inputSampleL - iirHighSampleLD;
|
||||
iirLowSampleLD = (iirLowSampleLD * (1.0 - iirAmountB)) + (inputSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleLD;
|
||||
|
||||
tripletFactorR = last2SampleR - inputSampleR;
|
||||
tripletRB += tripletFactorR;
|
||||
tripletRA -= tripletFactorR;
|
||||
tripletFactorR = tripletRB * tripletIntensity;
|
||||
iirHighSampleRD = (iirHighSampleRD * (1.0 - iirAmountA)) + (inputSampleR * iirAmountA);
|
||||
highSampleR = inputSampleR - iirHighSampleRD;
|
||||
iirLowSampleRD = (iirLowSampleRD * (1.0 - iirAmountB)) + (inputSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleRD;
|
||||
break;
|
||||
case 3:
|
||||
tripletFactorL = last2SampleL - inputSampleL;
|
||||
tripletLC += tripletFactorL;
|
||||
tripletLB -= tripletFactorL;
|
||||
tripletFactorL = tripletLC * tripletIntensity;
|
||||
iirHighSampleLE = (iirHighSampleLE * (1.0 - iirAmountA)) + (inputSampleL * iirAmountA);
|
||||
highSampleL = inputSampleL - iirHighSampleLE;
|
||||
iirLowSampleLE = (iirLowSampleLE * (1.0 - iirAmountB)) + (inputSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleLE;
|
||||
|
||||
tripletFactorR = last2SampleR - inputSampleR;
|
||||
tripletRC += tripletFactorR;
|
||||
tripletRB -= tripletFactorR;
|
||||
tripletFactorR = tripletRC * tripletIntensity;
|
||||
iirHighSampleRE = (iirHighSampleRE * (1.0 - iirAmountA)) + (inputSampleR * iirAmountA);
|
||||
highSampleR = inputSampleR - iirHighSampleRE;
|
||||
iirLowSampleRE = (iirLowSampleRE * (1.0 - iirAmountB)) + (inputSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleRE;
|
||||
break;
|
||||
}
|
||||
tripletLA /= 2.0;
|
||||
tripletLB /= 2.0;
|
||||
tripletLC /= 2.0;
|
||||
highSampleL = highSampleL + tripletFactorL;
|
||||
|
||||
tripletRA /= 2.0;
|
||||
tripletRB /= 2.0;
|
||||
tripletRC /= 2.0;
|
||||
highSampleR = highSampleR + tripletFactorR;
|
||||
|
||||
if (flip)
|
||||
{
|
||||
iirHighSampleLA = (iirHighSampleLA * (1.0 - iirAmountA)) + (highSampleL * iirAmountA);
|
||||
highSampleL -= iirHighSampleLA;
|
||||
iirLowSampleLA = (iirLowSampleLA * (1.0 - iirAmountB)) + (bassSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleLA;
|
||||
|
||||
iirHighSampleRA = (iirHighSampleRA * (1.0 - iirAmountA)) + (highSampleR * iirAmountA);
|
||||
highSampleR -= iirHighSampleRA;
|
||||
iirLowSampleRA = (iirLowSampleRA * (1.0 - iirAmountB)) + (bassSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleRA;
|
||||
}
|
||||
else
|
||||
{
|
||||
iirHighSampleLB = (iirHighSampleLB * (1.0 - iirAmountA)) + (highSampleL * iirAmountA);
|
||||
highSampleL -= iirHighSampleLB;
|
||||
iirLowSampleLB = (iirLowSampleLB * (1.0 - iirAmountB)) + (bassSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleLB;
|
||||
|
||||
iirHighSampleRB = (iirHighSampleRB * (1.0 - iirAmountA)) + (highSampleR * iirAmountA);
|
||||
highSampleR -= iirHighSampleRB;
|
||||
iirLowSampleRB = (iirLowSampleRB * (1.0 - iirAmountB)) + (bassSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleRB;
|
||||
}
|
||||
|
||||
iirHighSampleL = (iirHighSampleL * (1.0 - iirAmountA)) + (highSampleL * iirAmountA);
|
||||
highSampleL -= iirHighSampleL;
|
||||
iirLowSampleL = (iirLowSampleL * (1.0 - iirAmountB)) + (bassSampleL * iirAmountB);
|
||||
bassSampleL = iirLowSampleL;
|
||||
|
||||
iirHighSampleR = (iirHighSampleR * (1.0 - iirAmountA)) + (highSampleR * iirAmountA);
|
||||
highSampleR -= iirHighSampleR;
|
||||
iirLowSampleR = (iirLowSampleR * (1.0 - iirAmountB)) + (bassSampleR * iirAmountB);
|
||||
bassSampleR = iirLowSampleR;
|
||||
|
||||
midSampleL = (inputSampleL-bassSampleL)-highSampleL;
|
||||
midSampleR = (inputSampleR-bassSampleR)-highSampleR;
|
||||
|
||||
//drive section
|
||||
highSampleL *= (densityA+1.0);
|
||||
bridgerectifier = fabs(highSampleL)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityA > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (highSampleL > 0) highSampleL = (highSampleL*(1-outA))+(bridgerectifier*outA);
|
||||
else highSampleL = (highSampleL*(1-outA))-(bridgerectifier*outA);
|
||||
//blend according to densityA control
|
||||
|
||||
highSampleR *= (densityA+1.0);
|
||||
bridgerectifier = fabs(highSampleR)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityA > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (highSampleR > 0) highSampleR = (highSampleR*(1-outA))+(bridgerectifier*outA);
|
||||
else highSampleR = (highSampleR*(1-outA))-(bridgerectifier*outA);
|
||||
//blend according to densityA control
|
||||
|
||||
midSampleL *= (densityB+1.0);
|
||||
bridgerectifier = fabs(midSampleL)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityB > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (midSampleL > 0) midSampleL = (midSampleL*(1-outB))+(bridgerectifier*outB);
|
||||
else midSampleL = (midSampleL*(1-outB))-(bridgerectifier*outB);
|
||||
//blend according to densityB control
|
||||
|
||||
midSampleR *= (densityB+1.0);
|
||||
bridgerectifier = fabs(midSampleR)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityB > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (midSampleR > 0) midSampleR = (midSampleR*(1-outB))+(bridgerectifier*outB);
|
||||
else midSampleR = (midSampleR*(1-outB))-(bridgerectifier*outB);
|
||||
//blend according to densityB control
|
||||
|
||||
bassSampleL *= (densityC+1.0);
|
||||
bridgerectifier = fabs(bassSampleL)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityC > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (bassSampleL > 0) bassSampleL = (bassSampleL*(1-outC))+(bridgerectifier*outC);
|
||||
else bassSampleL = (bassSampleL*(1-outC))-(bridgerectifier*outC);
|
||||
//blend according to densityC control
|
||||
|
||||
bassSampleR *= (densityC+1.0);
|
||||
bridgerectifier = fabs(bassSampleR)*1.57079633;
|
||||
if (bridgerectifier > 1.57079633) bridgerectifier = 1.57079633;
|
||||
//max value for sine function
|
||||
if (densityC > 0) bridgerectifier = sin(bridgerectifier);
|
||||
else bridgerectifier = 1-cos(bridgerectifier);
|
||||
//produce either boosted or starved version
|
||||
if (bassSampleR > 0) bassSampleR = (bassSampleR*(1-outC))+(bridgerectifier*outC);
|
||||
else bassSampleR = (bassSampleR*(1-outC))-(bridgerectifier*outC);
|
||||
//blend according to densityC control
|
||||
|
||||
inputSampleL = midSampleL;
|
||||
inputSampleL += highSampleL;
|
||||
inputSampleL += bassSampleL;
|
||||
|
||||
inputSampleR = midSampleR;
|
||||
inputSampleR += highSampleR;
|
||||
inputSampleR += bassSampleR;
|
||||
}
|
||||
//end EQ
|
||||
|
||||
//EQ lowpass is after all processing like the compressor that might produce hash
|
||||
if (engageLowpass)
|
||||
{
|
||||
if (flip)
|
||||
{
|
||||
lowpassSampleLAA = (lowpassSampleLAA * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLAA;
|
||||
lowpassSampleLBA = (lowpassSampleLBA * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLBA;
|
||||
lowpassSampleLCA = (lowpassSampleLCA * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLCA;
|
||||
lowpassSampleLDA = (lowpassSampleLDA * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLDA;
|
||||
lowpassSampleLE = (lowpassSampleLE * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLE;
|
||||
|
||||
lowpassSampleRAA = (lowpassSampleRAA * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRAA;
|
||||
lowpassSampleRBA = (lowpassSampleRBA * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRBA;
|
||||
lowpassSampleRCA = (lowpassSampleRCA * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRCA;
|
||||
lowpassSampleRDA = (lowpassSampleRDA * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRDA;
|
||||
lowpassSampleRE = (lowpassSampleRE * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRE;
|
||||
}
|
||||
else
|
||||
{
|
||||
lowpassSampleLAB = (lowpassSampleLAB * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLAB;
|
||||
lowpassSampleLBB = (lowpassSampleLBB * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLBB;
|
||||
lowpassSampleLCB = (lowpassSampleLCB * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLCB;
|
||||
lowpassSampleLDB = (lowpassSampleLDB * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLDB;
|
||||
lowpassSampleLF = (lowpassSampleLF * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleL = lowpassSampleLF;
|
||||
|
||||
lowpassSampleRAB = (lowpassSampleRAB * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRAB;
|
||||
lowpassSampleRBB = (lowpassSampleRBB * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRBB;
|
||||
lowpassSampleRCB = (lowpassSampleRCB * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRCB;
|
||||
lowpassSampleRDB = (lowpassSampleRDB * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRDB;
|
||||
lowpassSampleRF = (lowpassSampleRF * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
inputSampleR = lowpassSampleRF;
|
||||
}
|
||||
lowpassSampleLG = (lowpassSampleLG * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
lowpassSampleRG = (lowpassSampleRG * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
|
||||
inputSampleL = (lowpassSampleLG * (1.0 - iirAmountC)) + (inputSampleL * iirAmountC);
|
||||
inputSampleR = (lowpassSampleRG * (1.0 - iirAmountC)) + (inputSampleR * iirAmountC);
|
||||
}
|
||||
|
||||
//built in output trim and dry/wet if desired
|
||||
if (outputgain != 1.0) {
|
||||
inputSampleL *= outputgain;
|
||||
inputSampleR *= outputgain;
|
||||
}
|
||||
|
||||
//begin 64 bit stereo floating point dither
|
||||
//int expon; frexp((double)inputSampleL, &expon);
|
||||
fpdL ^= fpdL << 13; fpdL ^= fpdL >> 17; fpdL ^= fpdL << 5;
|
||||
//inputSampleL += ((double(fpdL)-uint32_t(0x7fffffff)) * 1.1e-44l * pow(2,expon+62));
|
||||
//frexp((double)inputSampleR, &expon);
|
||||
fpdR ^= fpdR << 13; fpdR ^= fpdR >> 17; fpdR ^= fpdR << 5;
|
||||
//inputSampleR += ((double(fpdR)-uint32_t(0x7fffffff)) * 1.1e-44l * pow(2,expon+62));
|
||||
//end 64 bit stereo floating point dither
|
||||
|
||||
*out1 = inputSampleL;
|
||||
*out2 = inputSampleR;
|
||||
|
||||
*in1++;
|
||||
*in2++;
|
||||
*out1++;
|
||||
*out2++;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate;
|
||||
|
||||
uint32_t fpdL;
|
||||
uint32_t fpdR;
|
||||
//default stuff
|
||||
|
||||
double lastSampleL;
|
||||
double last2SampleL;
|
||||
double lastSampleR;
|
||||
double last2SampleR;
|
||||
|
||||
//begin EQ
|
||||
double iirHighSampleLA;
|
||||
double iirHighSampleLB;
|
||||
double iirHighSampleLC;
|
||||
double iirHighSampleLD;
|
||||
double iirHighSampleLE;
|
||||
double iirLowSampleLA;
|
||||
double iirLowSampleLB;
|
||||
double iirLowSampleLC;
|
||||
double iirLowSampleLD;
|
||||
double iirLowSampleLE;
|
||||
double iirHighSampleL;
|
||||
double iirLowSampleL;
|
||||
|
||||
double iirHighSampleRA;
|
||||
double iirHighSampleRB;
|
||||
double iirHighSampleRC;
|
||||
double iirHighSampleRD;
|
||||
double iirHighSampleRE;
|
||||
double iirLowSampleRA;
|
||||
double iirLowSampleRB;
|
||||
double iirLowSampleRC;
|
||||
double iirLowSampleRD;
|
||||
double iirLowSampleRE;
|
||||
double iirHighSampleR;
|
||||
double iirLowSampleR;
|
||||
|
||||
double tripletLA;
|
||||
double tripletLB;
|
||||
double tripletLC;
|
||||
double tripletFactorL;
|
||||
|
||||
double tripletRA;
|
||||
double tripletRB;
|
||||
double tripletRC;
|
||||
double tripletFactorR;
|
||||
|
||||
double lowpassSampleLAA;
|
||||
double lowpassSampleLAB;
|
||||
double lowpassSampleLBA;
|
||||
double lowpassSampleLBB;
|
||||
double lowpassSampleLCA;
|
||||
double lowpassSampleLCB;
|
||||
double lowpassSampleLDA;
|
||||
double lowpassSampleLDB;
|
||||
double lowpassSampleLE;
|
||||
double lowpassSampleLF;
|
||||
double lowpassSampleLG;
|
||||
|
||||
double lowpassSampleRAA;
|
||||
double lowpassSampleRAB;
|
||||
double lowpassSampleRBA;
|
||||
double lowpassSampleRBB;
|
||||
double lowpassSampleRCA;
|
||||
double lowpassSampleRCB;
|
||||
double lowpassSampleRDA;
|
||||
double lowpassSampleRDB;
|
||||
double lowpassSampleRE;
|
||||
double lowpassSampleRF;
|
||||
double lowpassSampleRG;
|
||||
|
||||
double highpassSampleLAA;
|
||||
double highpassSampleLAB;
|
||||
double highpassSampleLBA;
|
||||
double highpassSampleLBB;
|
||||
double highpassSampleLCA;
|
||||
double highpassSampleLCB;
|
||||
double highpassSampleLDA;
|
||||
double highpassSampleLDB;
|
||||
double highpassSampleLE;
|
||||
double highpassSampleLF;
|
||||
|
||||
double highpassSampleRAA;
|
||||
double highpassSampleRAB;
|
||||
double highpassSampleRBA;
|
||||
double highpassSampleRBB;
|
||||
double highpassSampleRCA;
|
||||
double highpassSampleRCB;
|
||||
double highpassSampleRDA;
|
||||
double highpassSampleRDB;
|
||||
double highpassSampleRE;
|
||||
double highpassSampleRF;
|
||||
|
||||
bool flip;
|
||||
int flipthree;
|
||||
//end EQ
|
||||
|
||||
|
||||
float A;
|
||||
float B;
|
||||
float C;
|
||||
float D;
|
||||
float E;
|
||||
float F;
|
||||
float G;
|
||||
float H;
|
||||
|
||||
double clamp(double& value) {
|
||||
if (value > 1) {
|
||||
value = 1;
|
||||
} else if (value < 0) {
|
||||
value = 0;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
};
|
||||
}
|
||||
80
filter/chebyshev.h
Normal file
80
filter/chebyshev.h
Normal file
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <array>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
class chebyshev {
|
||||
public:
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void process_sample(double& input, double frequency) {
|
||||
|
||||
if (frequency >= 20000.f) {
|
||||
frequency = 20000.f;
|
||||
}
|
||||
|
||||
// First calculate the prewarped digital frequency :
|
||||
auto K = tanf(M_PI * frequency / samplerate);
|
||||
|
||||
// Now we calc some Coefficients :
|
||||
auto sg = sinh(passband_ripple);
|
||||
auto cg = cosh(passband_ripple);
|
||||
cg *= cg;
|
||||
|
||||
std::array<double, 4> coeff;
|
||||
coeff[0] = 1 / (cg - 0.85355339059327376220042218105097);
|
||||
coeff[1] = K * coeff[0] * sg * 1.847759065022573512256366378792;
|
||||
coeff[2] = 1 / (cg - 0.14644660940672623779957781894758);
|
||||
coeff[3] = K * coeff[2] * sg * 0.76536686473017954345691996806;
|
||||
|
||||
K *= K; // (just to optimize it a little bit)
|
||||
|
||||
// Calculate the first biquad:
|
||||
a0 = 1 / (coeff[1] + K + coeff[0]);
|
||||
a1 = 2 * (coeff[0] - K) * a0;
|
||||
a2 = (coeff[1] - K - coeff[0]) * a0;
|
||||
b0 = a0 * K;
|
||||
b1 = 2 * b0;
|
||||
b2 = b0;
|
||||
|
||||
// Calculate the second biquad:
|
||||
a3 = 1 / (coeff[3] + K + coeff[2]);
|
||||
a4 = 2 * (coeff[2] - K) * a3;
|
||||
a5 = (coeff[3] - K - coeff[2]) * a3;
|
||||
b3 = a3 * K;
|
||||
b4 = 2 * b3;
|
||||
b5 = b3;
|
||||
|
||||
// Then calculate the output as follows:
|
||||
auto Stage1 = b0 * input + state0;
|
||||
state0 = b1 * input + a1 * Stage1 + state1;
|
||||
state1 = b2 * input + a2 * Stage1;
|
||||
input = b3 * Stage1 + state2;
|
||||
state2 = b4 * Stage1 + a4 * input + state3;
|
||||
state3 = b5 * Stage1 + a5 * input;
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate = 0;
|
||||
double a0 = 0;
|
||||
double a1 = 0;
|
||||
double a2 = 0;
|
||||
double a3 = 0;
|
||||
double a4 = 0;
|
||||
double a5 = 0;
|
||||
double b0 = 0;
|
||||
double b1 = 0;
|
||||
double b2 = 0;
|
||||
double b3 = 0;
|
||||
double b4 = 0;
|
||||
double b5 = 0;
|
||||
double state0 = 0;
|
||||
double state1 = 0;
|
||||
double state2 = 0;
|
||||
double state3 = 0;
|
||||
double passband_ripple = 1;
|
||||
};
|
||||
}
|
||||
313
filter/ybandpass.h
Normal file
313
filter/ybandpass.h
Normal file
@@ -0,0 +1,313 @@
|
||||
#pragma once
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
// Bandpass filter based on YBandpass by Chris Johnson
|
||||
class ybandpass {
|
||||
public:
|
||||
ybandpass(double _samplerate)
|
||||
: samplerate { _samplerate }
|
||||
, A { 0.1f }
|
||||
, B { 1.0f }
|
||||
, C { 0.0f }
|
||||
, D { 0.1f }
|
||||
, E { 0.9f }
|
||||
, F { 1.0f }
|
||||
, fpdL { 0 }
|
||||
, fpdR { 0 }
|
||||
, biquad { 0 }
|
||||
{
|
||||
for (int x = 0; x < biq_total; x++) {
|
||||
biquad[x] = 0.0;
|
||||
}
|
||||
powFactorA = 1.0;
|
||||
powFactorB = 1.0;
|
||||
inTrimA = 0.1;
|
||||
inTrimB = 0.1;
|
||||
outTrimA = 1.0;
|
||||
outTrimB = 1.0;
|
||||
for (int x = 0; x < fix_total; x++) {
|
||||
fixA[x] = 0.0;
|
||||
fixB[x] = 0.0;
|
||||
}
|
||||
|
||||
fpdL = 1.0;
|
||||
while (fpdL < 16386)
|
||||
fpdL = rand() * UINT32_MAX;
|
||||
fpdR = 1.0;
|
||||
while (fpdR < 16386)
|
||||
fpdR = rand() * UINT32_MAX;
|
||||
}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void set_drive(float value)
|
||||
{
|
||||
A = value * 0.9 + 0.1;
|
||||
}
|
||||
void set_frequency(float value)
|
||||
{
|
||||
B = value;
|
||||
}
|
||||
void set_resonance(float value)
|
||||
{
|
||||
C = value;
|
||||
}
|
||||
void set_edge(float value)
|
||||
{
|
||||
D = value;
|
||||
}
|
||||
void set_output(float value)
|
||||
{
|
||||
E = value;
|
||||
}
|
||||
void set_mix(float value)
|
||||
{
|
||||
F = value;
|
||||
}
|
||||
void processblock(double** inputs, double** outputs, int blockSize)
|
||||
{
|
||||
double* in1 = inputs[0];
|
||||
double* in2 = inputs[1];
|
||||
double* out1 = outputs[0];
|
||||
double* out2 = outputs[1];
|
||||
|
||||
int inFramesToProcess = blockSize;
|
||||
double overallscale = 1.0;
|
||||
overallscale /= 44100.0;
|
||||
overallscale *= samplerate;
|
||||
|
||||
inTrimA = inTrimB;
|
||||
inTrimB = A * 10.0;
|
||||
|
||||
biquad[biq_freq] = pow(B, 3) * 20000.0;
|
||||
if (biquad[biq_freq] < 15.0)
|
||||
biquad[biq_freq] = 15.0;
|
||||
biquad[biq_freq] /= samplerate;
|
||||
biquad[biq_reso] = (pow(C, 2) * 15.0) + 0.5571;
|
||||
biquad[biq_aA0] = biquad[biq_aB0];
|
||||
// biquad[biq_aA1] = biquad[biq_aB1];
|
||||
biquad[biq_aA2] = biquad[biq_aB2];
|
||||
biquad[biq_bA1] = biquad[biq_bB1];
|
||||
biquad[biq_bA2] = biquad[biq_bB2];
|
||||
// previous run through the buffer is still in the filter, so we move it
|
||||
// to the A section and now it's the new starting point.
|
||||
double K = tan(M_PI * biquad[biq_freq]);
|
||||
double norm = 1.0 / (1.0 + K / biquad[biq_reso] + K * K);
|
||||
biquad[biq_aB0] = K / biquad[biq_reso] * norm;
|
||||
// biquad[biq_aB1] = 0.0; //bandpass can simplify the biquad kernel: leave out this multiply
|
||||
biquad[biq_aB2] = -biquad[biq_aB0];
|
||||
biquad[biq_bB1] = 2.0 * (K * K - 1.0) * norm;
|
||||
biquad[biq_bB2] = (1.0 - K / biquad[biq_reso] + K * K) * norm;
|
||||
// for the coefficient-interpolated biquad filter
|
||||
|
||||
powFactorA = powFactorB;
|
||||
powFactorB = pow(D + 0.9, 4);
|
||||
|
||||
// 1.0 == target neutral
|
||||
|
||||
outTrimA = outTrimB;
|
||||
outTrimB = E;
|
||||
|
||||
double wet = F;
|
||||
|
||||
fixA[fix_freq] = fixB[fix_freq] = 20000.0 / samplerate;
|
||||
fixA[fix_reso] = fixB[fix_reso] = 0.7071; // butterworth Q
|
||||
|
||||
K = tan(M_PI * fixA[fix_freq]);
|
||||
norm = 1.0 / (1.0 + K / fixA[fix_reso] + K * K);
|
||||
fixA[fix_a0] = fixB[fix_a0] = K * K * norm;
|
||||
fixA[fix_a1] = fixB[fix_a1] = 2.0 * fixA[fix_a0];
|
||||
fixA[fix_a2] = fixB[fix_a2] = fixA[fix_a0];
|
||||
fixA[fix_b1] = fixB[fix_b1] = 2.0 * (K * K - 1.0) * norm;
|
||||
fixA[fix_b2] = fixB[fix_b2] = (1.0 - K / fixA[fix_reso] + K * K) * norm;
|
||||
// for the fixed-position biquad filter
|
||||
|
||||
for (int s = 0; s < blockSize; s++) {
|
||||
double inputSampleL = *in1;
|
||||
double inputSampleR = *in2;
|
||||
if (fabs(inputSampleL) < 1.18e-23)
|
||||
inputSampleL = fpdL * 1.18e-17;
|
||||
if (fabs(inputSampleR) < 1.18e-23)
|
||||
inputSampleR = fpdR * 1.18e-17;
|
||||
double drySampleL = inputSampleL;
|
||||
double drySampleR = inputSampleR;
|
||||
|
||||
double temp = (double)s / inFramesToProcess;
|
||||
biquad[biq_a0] = (biquad[biq_aA0] * temp) + (biquad[biq_aB0] * (1.0 - temp));
|
||||
// biquad[biq_a1] = (biquad[biq_aA1]*temp)+(biquad[biq_aB1]*(1.0-temp));
|
||||
biquad[biq_a2] = (biquad[biq_aA2] * temp) + (biquad[biq_aB2] * (1.0 - temp));
|
||||
biquad[biq_b1] = (biquad[biq_bA1] * temp) + (biquad[biq_bB1] * (1.0 - temp));
|
||||
biquad[biq_b2] = (biquad[biq_bA2] * temp) + (biquad[biq_bB2] * (1.0 - temp));
|
||||
// this is the interpolation code for the biquad
|
||||
double powFactor = (powFactorA * temp) + (powFactorB * (1.0 - temp));
|
||||
double inTrim = (inTrimA * temp) + (inTrimB * (1.0 - temp));
|
||||
double outTrim = (outTrimA * temp) + (outTrimB * (1.0 - temp));
|
||||
|
||||
inputSampleL *= inTrim;
|
||||
inputSampleR *= inTrim;
|
||||
|
||||
temp = (inputSampleL * fixA[fix_a0]) + fixA[fix_sL1];
|
||||
fixA[fix_sL1] = (inputSampleL * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sL2];
|
||||
fixA[fix_sL2] = (inputSampleL * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixA[fix_a0]) + fixA[fix_sR1];
|
||||
fixA[fix_sR1] = (inputSampleR * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sR2];
|
||||
fixA[fix_sR2] = (inputSampleR * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, powFactor);
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, powFactor);
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, powFactor);
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, powFactor);
|
||||
|
||||
temp = (inputSampleL * biquad[biq_a0]) + biquad[biq_sL1];
|
||||
biquad[biq_sL1] = -(temp * biquad[biq_b1]) + biquad[biq_sL2];
|
||||
biquad[biq_sL2] = (inputSampleL * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleL = temp; // coefficient interpolating biquad filter
|
||||
temp = (inputSampleR * biquad[biq_a0]) + biquad[biq_sR1];
|
||||
biquad[biq_sR1] = -(temp * biquad[biq_b1]) + biquad[biq_sR2];
|
||||
biquad[biq_sR2] = (inputSampleR * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleR = temp; // coefficient interpolating biquad filter
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, (1.0 / powFactor));
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, (1.0 / powFactor));
|
||||
|
||||
inputSampleL *= outTrim;
|
||||
inputSampleR *= outTrim;
|
||||
|
||||
temp = (inputSampleL * fixB[fix_a0]) + fixB[fix_sL1];
|
||||
fixB[fix_sL1] = (inputSampleL * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sL2];
|
||||
fixB[fix_sL2] = (inputSampleL * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixB[fix_a0]) + fixB[fix_sR1];
|
||||
fixB[fix_sR1] = (inputSampleR * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sR2];
|
||||
fixB[fix_sR2] = (inputSampleR * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
if (wet < 1.0) {
|
||||
inputSampleL = (inputSampleL * wet) + (drySampleL * (1.0 - wet));
|
||||
inputSampleR = (inputSampleR * wet) + (drySampleR * (1.0 - wet));
|
||||
}
|
||||
|
||||
// begin 32 bit stereo floating point dither
|
||||
int expon;
|
||||
frexpf((float)inputSampleL, &expon);
|
||||
fpdL ^= fpdL << 13;
|
||||
fpdL ^= fpdL >> 17;
|
||||
fpdL ^= fpdL << 5;
|
||||
inputSampleL += ((double(fpdL) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
frexpf((float)inputSampleR, &expon);
|
||||
fpdR ^= fpdR << 13;
|
||||
fpdR ^= fpdR >> 17;
|
||||
fpdR ^= fpdR << 5;
|
||||
inputSampleR += ((double(fpdR) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
// end 32 bit stereo floating point dither
|
||||
|
||||
*out1 = inputSampleL;
|
||||
*out2 = inputSampleR;
|
||||
|
||||
in1++;
|
||||
in2++;
|
||||
out1++;
|
||||
out2++;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate;
|
||||
enum {
|
||||
biq_freq,
|
||||
biq_reso,
|
||||
biq_a0,
|
||||
biq_a1,
|
||||
biq_a2,
|
||||
biq_b1,
|
||||
biq_b2,
|
||||
biq_aA0,
|
||||
biq_aA1,
|
||||
biq_aA2,
|
||||
biq_bA1,
|
||||
biq_bA2,
|
||||
biq_aB0,
|
||||
biq_aB1,
|
||||
biq_aB2,
|
||||
biq_bB1,
|
||||
biq_bB2,
|
||||
biq_sL1,
|
||||
biq_sL2,
|
||||
biq_sR1,
|
||||
biq_sR2,
|
||||
biq_total
|
||||
}; // coefficient interpolating biquad filter, stereo
|
||||
std::array<double, biq_total> biquad;
|
||||
|
||||
double powFactorA;
|
||||
double powFactorB;
|
||||
double inTrimA;
|
||||
double inTrimB;
|
||||
double outTrimA;
|
||||
double outTrimB;
|
||||
|
||||
enum {
|
||||
fix_freq,
|
||||
fix_reso,
|
||||
fix_a0,
|
||||
fix_a1,
|
||||
fix_a2,
|
||||
fix_b1,
|
||||
fix_b2,
|
||||
fix_sL1,
|
||||
fix_sL2,
|
||||
fix_sR1,
|
||||
fix_sR2,
|
||||
fix_total
|
||||
}; // fixed frequency biquad filter for ultrasonics, stereo
|
||||
std::array<double, fix_total> fixA;
|
||||
std::array<double, fix_total> fixB;
|
||||
|
||||
uint32_t fpdL;
|
||||
uint32_t fpdR;
|
||||
// default stuff
|
||||
|
||||
float A;
|
||||
float B;
|
||||
float C;
|
||||
float D;
|
||||
float E;
|
||||
float F; // parameters. Always 0-1, and we scale/alter them elsewhere.
|
||||
};
|
||||
}
|
||||
313
filter/yhighpass.h
Normal file
313
filter/yhighpass.h
Normal file
@@ -0,0 +1,313 @@
|
||||
#pragma once
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
// Highpass filter based on YHighpass by Chris Johnson
|
||||
class yhighpass {
|
||||
public:
|
||||
yhighpass(double _samplerate)
|
||||
: samplerate { _samplerate }
|
||||
, A { 0.1f }
|
||||
, B { 1.0f }
|
||||
, C { 0.0f }
|
||||
, D { 0.1f }
|
||||
, E { 0.9f }
|
||||
, F { 1.0f }
|
||||
, fpdL { 0 }
|
||||
, fpdR { 0 }
|
||||
, biquad { 0 }
|
||||
{
|
||||
for (int x = 0; x < biq_total; x++) {
|
||||
biquad[x] = 0.0;
|
||||
}
|
||||
powFactorA = 1.0;
|
||||
powFactorB = 1.0;
|
||||
inTrimA = 0.1;
|
||||
inTrimB = 0.1;
|
||||
outTrimA = 1.0;
|
||||
outTrimB = 1.0;
|
||||
for (int x = 0; x < fix_total; x++) {
|
||||
fixA[x] = 0.0;
|
||||
fixB[x] = 0.0;
|
||||
}
|
||||
|
||||
fpdL = 1.0;
|
||||
while (fpdL < 16386)
|
||||
fpdL = rand() * UINT32_MAX;
|
||||
fpdR = 1.0;
|
||||
while (fpdR < 16386)
|
||||
fpdR = rand() * UINT32_MAX;
|
||||
}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void set_drive(float value)
|
||||
{
|
||||
A = value * 0.9 + 0.1;
|
||||
}
|
||||
void set_frequency(float value)
|
||||
{
|
||||
B = value;
|
||||
}
|
||||
void set_resonance(float value)
|
||||
{
|
||||
C = value;
|
||||
}
|
||||
void set_edge(float value)
|
||||
{
|
||||
D = value;
|
||||
}
|
||||
void set_output(float value)
|
||||
{
|
||||
E = value;
|
||||
}
|
||||
void set_mix(float value)
|
||||
{
|
||||
F = value;
|
||||
}
|
||||
void processblock(double** inputs, double** outputs, int blockSize)
|
||||
{
|
||||
double* in1 = inputs[0];
|
||||
double* in2 = inputs[1];
|
||||
double* out1 = outputs[0];
|
||||
double* out2 = outputs[1];
|
||||
|
||||
int inFramesToProcess = blockSize;
|
||||
double overallscale = 1.0;
|
||||
overallscale /= 44100.0;
|
||||
overallscale *= samplerate;
|
||||
|
||||
inTrimA = inTrimB;
|
||||
inTrimB = A * 10.0;
|
||||
|
||||
biquad[biq_freq] = pow(B, 3) * 20000.0;
|
||||
if (biquad[biq_freq] < 15.0)
|
||||
biquad[biq_freq] = 15.0;
|
||||
biquad[biq_freq] /= samplerate;
|
||||
biquad[biq_reso] = (pow(C, 2) * 15.0) + 0.5571;
|
||||
biquad[biq_aA0] = biquad[biq_aB0];
|
||||
biquad[biq_aA1] = biquad[biq_aB1];
|
||||
biquad[biq_aA2] = biquad[biq_aB2];
|
||||
biquad[biq_bA1] = biquad[biq_bB1];
|
||||
biquad[biq_bA2] = biquad[biq_bB2];
|
||||
// previous run through the buffer is still in the filter, so we move it
|
||||
// to the A section and now it's the new starting point.
|
||||
double K = tan(M_PI * biquad[biq_freq]);
|
||||
double norm = 1.0 / (1.0 + K / biquad[biq_reso] + K * K);
|
||||
biquad[biq_aB0] = norm;
|
||||
biquad[biq_aB1] = -2.0 * biquad[biq_aB0];
|
||||
biquad[biq_aB2] = biquad[biq_aB0];
|
||||
biquad[biq_bB1] = 2.0 * (K * K - 1.0) * norm;
|
||||
biquad[biq_bB2] = (1.0 - K / biquad[biq_reso] + K * K) * norm;
|
||||
// for the coefficient-interpolated biquad filter
|
||||
|
||||
powFactorA = powFactorB;
|
||||
powFactorB = pow(D + 0.9, 4);
|
||||
|
||||
// 1.0 == target neutral
|
||||
|
||||
outTrimA = outTrimB;
|
||||
outTrimB = E;
|
||||
|
||||
double wet = F;
|
||||
|
||||
fixA[fix_freq] = fixB[fix_freq] = 20000.0 / samplerate;
|
||||
fixA[fix_reso] = fixB[fix_reso] = 0.7071; // butterworth Q
|
||||
|
||||
K = tan(M_PI * fixA[fix_freq]);
|
||||
norm = 1.0 / (1.0 + K / fixA[fix_reso] + K * K);
|
||||
fixA[fix_a0] = fixB[fix_a0] = K * K * norm;
|
||||
fixA[fix_a1] = fixB[fix_a1] = 2.0 * fixA[fix_a0];
|
||||
fixA[fix_a2] = fixB[fix_a2] = fixA[fix_a0];
|
||||
fixA[fix_b1] = fixB[fix_b1] = 2.0 * (K * K - 1.0) * norm;
|
||||
fixA[fix_b2] = fixB[fix_b2] = (1.0 - K / fixA[fix_reso] + K * K) * norm;
|
||||
// for the fixed-position biquad filter
|
||||
|
||||
for (int s = 0; s < blockSize; s++) {
|
||||
double inputSampleL = *in1;
|
||||
double inputSampleR = *in2;
|
||||
if (fabs(inputSampleL) < 1.18e-23)
|
||||
inputSampleL = fpdL * 1.18e-17;
|
||||
if (fabs(inputSampleR) < 1.18e-23)
|
||||
inputSampleR = fpdR * 1.18e-17;
|
||||
double drySampleL = inputSampleL;
|
||||
double drySampleR = inputSampleR;
|
||||
|
||||
double temp = (double)s / inFramesToProcess;
|
||||
biquad[biq_a0] = (biquad[biq_aA0] * temp) + (biquad[biq_aB0] * (1.0 - temp));
|
||||
biquad[biq_a1] = (biquad[biq_aA1] * temp) + (biquad[biq_aB1] * (1.0 - temp));
|
||||
biquad[biq_a2] = (biquad[biq_aA2] * temp) + (biquad[biq_aB2] * (1.0 - temp));
|
||||
biquad[biq_b1] = (biquad[biq_bA1] * temp) + (biquad[biq_bB1] * (1.0 - temp));
|
||||
biquad[biq_b2] = (biquad[biq_bA2] * temp) + (biquad[biq_bB2] * (1.0 - temp));
|
||||
// this is the interpolation code for the biquad
|
||||
double powFactor = (powFactorA * temp) + (powFactorB * (1.0 - temp));
|
||||
double inTrim = (inTrimA * temp) + (inTrimB * (1.0 - temp));
|
||||
double outTrim = (outTrimA * temp) + (outTrimB * (1.0 - temp));
|
||||
|
||||
inputSampleL *= inTrim;
|
||||
inputSampleR *= inTrim;
|
||||
|
||||
temp = (inputSampleL * fixA[fix_a0]) + fixA[fix_sL1];
|
||||
fixA[fix_sL1] = (inputSampleL * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sL2];
|
||||
fixA[fix_sL2] = (inputSampleL * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixA[fix_a0]) + fixA[fix_sR1];
|
||||
fixA[fix_sR1] = (inputSampleR * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sR2];
|
||||
fixA[fix_sR2] = (inputSampleR * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, powFactor);
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, powFactor);
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, powFactor);
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, powFactor);
|
||||
|
||||
temp = (inputSampleL * biquad[biq_a0]) + biquad[biq_sL1];
|
||||
biquad[biq_sL1] = (inputSampleL * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sL2];
|
||||
biquad[biq_sL2] = (inputSampleL * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleL = temp; // coefficient interpolating biquad filter
|
||||
temp = (inputSampleR * biquad[biq_a0]) + biquad[biq_sR1];
|
||||
biquad[biq_sR1] = (inputSampleR * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sR2];
|
||||
biquad[biq_sR2] = (inputSampleR * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleR = temp; // coefficient interpolating biquad filter
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, (1.0 / powFactor));
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, (1.0 / powFactor));
|
||||
|
||||
inputSampleL *= outTrim;
|
||||
inputSampleR *= outTrim;
|
||||
|
||||
temp = (inputSampleL * fixB[fix_a0]) + fixB[fix_sL1];
|
||||
fixB[fix_sL1] = (inputSampleL * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sL2];
|
||||
fixB[fix_sL2] = (inputSampleL * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixB[fix_a0]) + fixB[fix_sR1];
|
||||
fixB[fix_sR1] = (inputSampleR * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sR2];
|
||||
fixB[fix_sR2] = (inputSampleR * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
if (wet < 1.0) {
|
||||
inputSampleL = (inputSampleL * wet) + (drySampleL * (1.0 - wet));
|
||||
inputSampleR = (inputSampleR * wet) + (drySampleR * (1.0 - wet));
|
||||
}
|
||||
|
||||
// begin 32 bit stereo floating point dither
|
||||
int expon;
|
||||
frexpf((float)inputSampleL, &expon);
|
||||
fpdL ^= fpdL << 13;
|
||||
fpdL ^= fpdL >> 17;
|
||||
fpdL ^= fpdL << 5;
|
||||
inputSampleL += ((double(fpdL) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
frexpf((float)inputSampleR, &expon);
|
||||
fpdR ^= fpdR << 13;
|
||||
fpdR ^= fpdR >> 17;
|
||||
fpdR ^= fpdR << 5;
|
||||
inputSampleR += ((double(fpdR) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
// end 32 bit stereo floating point dither
|
||||
|
||||
*out1 = inputSampleL;
|
||||
*out2 = inputSampleR;
|
||||
|
||||
in1++;
|
||||
in2++;
|
||||
out1++;
|
||||
out2++;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate;
|
||||
enum {
|
||||
biq_freq,
|
||||
biq_reso,
|
||||
biq_a0,
|
||||
biq_a1,
|
||||
biq_a2,
|
||||
biq_b1,
|
||||
biq_b2,
|
||||
biq_aA0,
|
||||
biq_aA1,
|
||||
biq_aA2,
|
||||
biq_bA1,
|
||||
biq_bA2,
|
||||
biq_aB0,
|
||||
biq_aB1,
|
||||
biq_aB2,
|
||||
biq_bB1,
|
||||
biq_bB2,
|
||||
biq_sL1,
|
||||
biq_sL2,
|
||||
biq_sR1,
|
||||
biq_sR2,
|
||||
biq_total
|
||||
}; // coefficient interpolating biquad filter, stereo
|
||||
std::array<double, biq_total> biquad;
|
||||
|
||||
double powFactorA;
|
||||
double powFactorB;
|
||||
double inTrimA;
|
||||
double inTrimB;
|
||||
double outTrimA;
|
||||
double outTrimB;
|
||||
|
||||
enum {
|
||||
fix_freq,
|
||||
fix_reso,
|
||||
fix_a0,
|
||||
fix_a1,
|
||||
fix_a2,
|
||||
fix_b1,
|
||||
fix_b2,
|
||||
fix_sL1,
|
||||
fix_sL2,
|
||||
fix_sR1,
|
||||
fix_sR2,
|
||||
fix_total
|
||||
}; // fixed frequency biquad filter for ultrasonics, stereo
|
||||
std::array<double, fix_total> fixA;
|
||||
std::array<double, fix_total> fixB;
|
||||
|
||||
uint32_t fpdL;
|
||||
uint32_t fpdR;
|
||||
// default stuff
|
||||
|
||||
float A;
|
||||
float B;
|
||||
float C;
|
||||
float D;
|
||||
float E;
|
||||
float F; // parameters. Always 0-1, and we scale/alter them elsewhere.
|
||||
};
|
||||
}
|
||||
313
filter/ylowpass.h
Normal file
313
filter/ylowpass.h
Normal file
@@ -0,0 +1,313 @@
|
||||
#pragma once
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
// Lowpass filter based on YLowpass by Chris Johnson
|
||||
class ylowpass {
|
||||
public:
|
||||
ylowpass(double _samplerate)
|
||||
: samplerate { _samplerate }
|
||||
, A { 0.1f }
|
||||
, B { 1.0f }
|
||||
, C { 0.0f }
|
||||
, D { 0.1f }
|
||||
, E { 0.9f }
|
||||
, F { 1.0f }
|
||||
, fpdL { 0 }
|
||||
, fpdR { 0 }
|
||||
, biquad { 0 }
|
||||
{
|
||||
for (int x = 0; x < biq_total; x++) {
|
||||
biquad[x] = 0.0;
|
||||
}
|
||||
powFactorA = 1.0;
|
||||
powFactorB = 1.0;
|
||||
inTrimA = 0.1;
|
||||
inTrimB = 0.1;
|
||||
outTrimA = 1.0;
|
||||
outTrimB = 1.0;
|
||||
for (int x = 0; x < fix_total; x++) {
|
||||
fixA[x] = 0.0;
|
||||
fixB[x] = 0.0;
|
||||
}
|
||||
|
||||
fpdL = 1.0;
|
||||
while (fpdL < 16386)
|
||||
fpdL = rand() * UINT32_MAX;
|
||||
fpdR = 1.0;
|
||||
while (fpdR < 16386)
|
||||
fpdR = rand() * UINT32_MAX;
|
||||
}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void set_drive(float value)
|
||||
{
|
||||
A = value * 0.9 + 0.1;
|
||||
}
|
||||
void set_frequency(float value)
|
||||
{
|
||||
B = value;
|
||||
}
|
||||
void set_resonance(float value)
|
||||
{
|
||||
C = value;
|
||||
}
|
||||
void set_edge(float value)
|
||||
{
|
||||
D = value;
|
||||
}
|
||||
void set_output(float value)
|
||||
{
|
||||
E = value;
|
||||
}
|
||||
void set_mix(float value)
|
||||
{
|
||||
F = value;
|
||||
}
|
||||
void processblock(double** inputs, double** outputs, int blockSize)
|
||||
{
|
||||
double* in1 = inputs[0];
|
||||
double* in2 = inputs[1];
|
||||
double* out1 = outputs[0];
|
||||
double* out2 = outputs[1];
|
||||
|
||||
int inFramesToProcess = blockSize;
|
||||
double overallscale = 1.0;
|
||||
overallscale /= 44100.0;
|
||||
overallscale *= samplerate;
|
||||
|
||||
inTrimA = inTrimB;
|
||||
inTrimB = A * 10.0;
|
||||
|
||||
biquad[biq_freq] = pow(B, 3) * 20000.0;
|
||||
if (biquad[biq_freq] < 15.0)
|
||||
biquad[biq_freq] = 15.0;
|
||||
biquad[biq_freq] /= samplerate;
|
||||
biquad[biq_reso] = (pow(C, 2) * 15.0) + 0.5571;
|
||||
biquad[biq_aA0] = biquad[biq_aB0];
|
||||
biquad[biq_aA1] = biquad[biq_aB1];
|
||||
biquad[biq_aA2] = biquad[biq_aB2];
|
||||
biquad[biq_bA1] = biquad[biq_bB1];
|
||||
biquad[biq_bA2] = biquad[biq_bB2];
|
||||
// previous run through the buffer is still in the filter, so we move it
|
||||
// to the A section and now it's the new starting point.
|
||||
double K = tan(M_PI * biquad[biq_freq]);
|
||||
double norm = 1.0 / (1.0 + K / biquad[biq_reso] + K * K);
|
||||
biquad[biq_aB0] = K * K * norm;
|
||||
biquad[biq_aB1] = 2.0 * biquad[biq_aB0];
|
||||
biquad[biq_aB2] = biquad[biq_aB0];
|
||||
biquad[biq_bB1] = 2.0 * (K * K - 1.0) * norm;
|
||||
biquad[biq_bB2] = (1.0 - K / biquad[biq_reso] + K * K) * norm;
|
||||
// for the coefficient-interpolated biquad filter
|
||||
|
||||
powFactorA = powFactorB;
|
||||
powFactorB = pow(D + 0.9, 4);
|
||||
|
||||
// 1.0 == target neutral
|
||||
|
||||
outTrimA = outTrimB;
|
||||
outTrimB = E;
|
||||
|
||||
double wet = F;
|
||||
|
||||
fixA[fix_freq] = fixB[fix_freq] = 20000.0 / samplerate;
|
||||
fixA[fix_reso] = fixB[fix_reso] = 0.7071; // butterworth Q
|
||||
|
||||
K = tan(M_PI * fixA[fix_freq]);
|
||||
norm = 1.0 / (1.0 + K / fixA[fix_reso] + K * K);
|
||||
fixA[fix_a0] = fixB[fix_a0] = K * K * norm;
|
||||
fixA[fix_a1] = fixB[fix_a1] = 2.0 * fixA[fix_a0];
|
||||
fixA[fix_a2] = fixB[fix_a2] = fixA[fix_a0];
|
||||
fixA[fix_b1] = fixB[fix_b1] = 2.0 * (K * K - 1.0) * norm;
|
||||
fixA[fix_b2] = fixB[fix_b2] = (1.0 - K / fixA[fix_reso] + K * K) * norm;
|
||||
// for the fixed-position biquad filter
|
||||
|
||||
for (int s = 0; s < blockSize; s++) {
|
||||
double inputSampleL = *in1;
|
||||
double inputSampleR = *in2;
|
||||
if (fabs(inputSampleL) < 1.18e-23)
|
||||
inputSampleL = fpdL * 1.18e-17;
|
||||
if (fabs(inputSampleR) < 1.18e-23)
|
||||
inputSampleR = fpdR * 1.18e-17;
|
||||
double drySampleL = inputSampleL;
|
||||
double drySampleR = inputSampleR;
|
||||
|
||||
double temp = (double)s / inFramesToProcess;
|
||||
biquad[biq_a0] = (biquad[biq_aA0] * temp) + (biquad[biq_aB0] * (1.0 - temp));
|
||||
biquad[biq_a1] = (biquad[biq_aA1] * temp) + (biquad[biq_aB1] * (1.0 - temp));
|
||||
biquad[biq_a2] = (biquad[biq_aA2] * temp) + (biquad[biq_aB2] * (1.0 - temp));
|
||||
biquad[biq_b1] = (biquad[biq_bA1] * temp) + (biquad[biq_bB1] * (1.0 - temp));
|
||||
biquad[biq_b2] = (biquad[biq_bA2] * temp) + (biquad[biq_bB2] * (1.0 - temp));
|
||||
// this is the interpolation code for the biquad
|
||||
double powFactor = (powFactorA * temp) + (powFactorB * (1.0 - temp));
|
||||
double inTrim = (inTrimA * temp) + (inTrimB * (1.0 - temp));
|
||||
double outTrim = (outTrimA * temp) + (outTrimB * (1.0 - temp));
|
||||
|
||||
inputSampleL *= inTrim;
|
||||
inputSampleR *= inTrim;
|
||||
|
||||
temp = (inputSampleL * fixA[fix_a0]) + fixA[fix_sL1];
|
||||
fixA[fix_sL1] = (inputSampleL * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sL2];
|
||||
fixA[fix_sL2] = (inputSampleL * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixA[fix_a0]) + fixA[fix_sR1];
|
||||
fixA[fix_sR1] = (inputSampleR * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sR2];
|
||||
fixA[fix_sR2] = (inputSampleR * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, powFactor);
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, powFactor);
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, powFactor);
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, powFactor);
|
||||
|
||||
temp = (inputSampleL * biquad[biq_a0]) + biquad[biq_sL1];
|
||||
biquad[biq_sL1] = (inputSampleL * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sL2];
|
||||
biquad[biq_sL2] = (inputSampleL * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleL = temp; // coefficient interpolating biquad filter
|
||||
temp = (inputSampleR * biquad[biq_a0]) + biquad[biq_sR1];
|
||||
biquad[biq_sR1] = (inputSampleR * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sR2];
|
||||
biquad[biq_sR2] = (inputSampleR * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleR = temp; // coefficient interpolating biquad filter
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, (1.0 / powFactor));
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, (1.0 / powFactor));
|
||||
|
||||
inputSampleL *= outTrim;
|
||||
inputSampleR *= outTrim;
|
||||
|
||||
temp = (inputSampleL * fixB[fix_a0]) + fixB[fix_sL1];
|
||||
fixB[fix_sL1] = (inputSampleL * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sL2];
|
||||
fixB[fix_sL2] = (inputSampleL * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixB[fix_a0]) + fixB[fix_sR1];
|
||||
fixB[fix_sR1] = (inputSampleR * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sR2];
|
||||
fixB[fix_sR2] = (inputSampleR * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
if (wet < 1.0) {
|
||||
inputSampleL = (inputSampleL * wet) + (drySampleL * (1.0 - wet));
|
||||
inputSampleR = (inputSampleR * wet) + (drySampleR * (1.0 - wet));
|
||||
}
|
||||
|
||||
// begin 32 bit stereo floating point dither
|
||||
int expon;
|
||||
frexpf((float)inputSampleL, &expon);
|
||||
fpdL ^= fpdL << 13;
|
||||
fpdL ^= fpdL >> 17;
|
||||
fpdL ^= fpdL << 5;
|
||||
inputSampleL += ((double(fpdL) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
frexpf((float)inputSampleR, &expon);
|
||||
fpdR ^= fpdR << 13;
|
||||
fpdR ^= fpdR >> 17;
|
||||
fpdR ^= fpdR << 5;
|
||||
inputSampleR += ((double(fpdR) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
// end 32 bit stereo floating point dither
|
||||
|
||||
*out1 = inputSampleL;
|
||||
*out2 = inputSampleR;
|
||||
|
||||
in1++;
|
||||
in2++;
|
||||
out1++;
|
||||
out2++;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate;
|
||||
enum {
|
||||
biq_freq,
|
||||
biq_reso,
|
||||
biq_a0,
|
||||
biq_a1,
|
||||
biq_a2,
|
||||
biq_b1,
|
||||
biq_b2,
|
||||
biq_aA0,
|
||||
biq_aA1,
|
||||
biq_aA2,
|
||||
biq_bA1,
|
||||
biq_bA2,
|
||||
biq_aB0,
|
||||
biq_aB1,
|
||||
biq_aB2,
|
||||
biq_bB1,
|
||||
biq_bB2,
|
||||
biq_sL1,
|
||||
biq_sL2,
|
||||
biq_sR1,
|
||||
biq_sR2,
|
||||
biq_total
|
||||
}; // coefficient interpolating biquad filter, stereo
|
||||
std::array<double, biq_total> biquad;
|
||||
|
||||
double powFactorA;
|
||||
double powFactorB;
|
||||
double inTrimA;
|
||||
double inTrimB;
|
||||
double outTrimA;
|
||||
double outTrimB;
|
||||
|
||||
enum {
|
||||
fix_freq,
|
||||
fix_reso,
|
||||
fix_a0,
|
||||
fix_a1,
|
||||
fix_a2,
|
||||
fix_b1,
|
||||
fix_b2,
|
||||
fix_sL1,
|
||||
fix_sL2,
|
||||
fix_sR1,
|
||||
fix_sR2,
|
||||
fix_total
|
||||
}; // fixed frequency biquad filter for ultrasonics, stereo
|
||||
std::array<double, fix_total> fixA;
|
||||
std::array<double, fix_total> fixB;
|
||||
|
||||
uint32_t fpdL;
|
||||
uint32_t fpdR;
|
||||
// default stuff
|
||||
|
||||
float A;
|
||||
float B;
|
||||
float C;
|
||||
float D;
|
||||
float E;
|
||||
float F; // parameters. Always 0-1, and we scale/alter them elsewhere.
|
||||
};
|
||||
}
|
||||
313
filter/ynotch.h
Normal file
313
filter/ynotch.h
Normal file
@@ -0,0 +1,313 @@
|
||||
#pragma once
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <math.h>
|
||||
#include <array>
|
||||
#include <vector>
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
// Notch filter based on YNotch by Chris Johnson
|
||||
class ynotch {
|
||||
public:
|
||||
ynotch(double _samplerate)
|
||||
: samplerate { _samplerate }
|
||||
, A { 0.1f }
|
||||
, B { 1.0f }
|
||||
, C { 0.0f }
|
||||
, D { 0.1f }
|
||||
, E { 0.9f }
|
||||
, F { 1.0f }
|
||||
, fpdL { 0 }
|
||||
, fpdR { 0 }
|
||||
, biquad { 0 }
|
||||
{
|
||||
for (int x = 0; x < biq_total; x++) {
|
||||
biquad[x] = 0.0;
|
||||
}
|
||||
powFactorA = 1.0;
|
||||
powFactorB = 1.0;
|
||||
inTrimA = 0.1;
|
||||
inTrimB = 0.1;
|
||||
outTrimA = 1.0;
|
||||
outTrimB = 1.0;
|
||||
for (int x = 0; x < fix_total; x++) {
|
||||
fixA[x] = 0.0;
|
||||
fixB[x] = 0.0;
|
||||
}
|
||||
|
||||
fpdL = 1.0;
|
||||
while (fpdL < 16386)
|
||||
fpdL = rand() * UINT32_MAX;
|
||||
fpdR = 1.0;
|
||||
while (fpdR < 16386)
|
||||
fpdR = rand() * UINT32_MAX;
|
||||
}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
samplerate = _samplerate;
|
||||
}
|
||||
|
||||
void set_drive(float value)
|
||||
{
|
||||
A = value * 0.9 + 0.1;
|
||||
}
|
||||
void set_frequency(float value)
|
||||
{
|
||||
B = value;
|
||||
}
|
||||
void set_resonance(float value)
|
||||
{
|
||||
C = value;
|
||||
}
|
||||
void set_edge(float value)
|
||||
{
|
||||
D = value;
|
||||
}
|
||||
void set_output(float value)
|
||||
{
|
||||
E = value;
|
||||
}
|
||||
void set_mix(float value)
|
||||
{
|
||||
F = value;
|
||||
}
|
||||
void processblock(double** inputs, double** outputs, int blockSize)
|
||||
{
|
||||
double* in1 = inputs[0];
|
||||
double* in2 = inputs[1];
|
||||
double* out1 = outputs[0];
|
||||
double* out2 = outputs[1];
|
||||
|
||||
int inFramesToProcess = blockSize;
|
||||
double overallscale = 1.0;
|
||||
overallscale /= 44100.0;
|
||||
overallscale *= samplerate;
|
||||
|
||||
inTrimA = inTrimB;
|
||||
inTrimB = A * 10.0;
|
||||
|
||||
biquad[biq_freq] = pow(B, 3) * 20000.0;
|
||||
if (biquad[biq_freq] < 15.0)
|
||||
biquad[biq_freq] = 15.0;
|
||||
biquad[biq_freq] /= samplerate;
|
||||
biquad[biq_reso] = (pow(C, 2) * 15.0) + 0.0001;
|
||||
biquad[biq_aA0] = biquad[biq_aB0];
|
||||
biquad[biq_aA1] = biquad[biq_aB1];
|
||||
biquad[biq_aA2] = biquad[biq_aB2];
|
||||
biquad[biq_bA1] = biquad[biq_bB1];
|
||||
biquad[biq_bA2] = biquad[biq_bB2];
|
||||
// previous run through the buffer is still in the filter, so we move it
|
||||
// to the A section and now it's the new starting point.
|
||||
double K = tan(M_PI * biquad[biq_freq]);
|
||||
double norm = 1.0 / (1.0 + K / biquad[biq_reso] + K * K);
|
||||
biquad[biq_aB0] = (1.0 + K * K) * norm;
|
||||
biquad[biq_aB1] = 2.0 * (K * K - 1) * norm;
|
||||
biquad[biq_aB2] = biquad[biq_aB0];
|
||||
biquad[biq_bB1] = biquad[biq_aB1];
|
||||
biquad[biq_bB2] = (1.0 - K / biquad[biq_reso] + K * K) * norm;
|
||||
// for the coefficient-interpolated biquad filter
|
||||
|
||||
powFactorA = powFactorB;
|
||||
powFactorB = pow(D + 0.9, 4);
|
||||
|
||||
// 1.0 == target neutral
|
||||
|
||||
outTrimA = outTrimB;
|
||||
outTrimB = E;
|
||||
|
||||
double wet = F;
|
||||
|
||||
fixA[fix_freq] = fixB[fix_freq] = 20000.0 / samplerate;
|
||||
fixA[fix_reso] = fixB[fix_reso] = 0.7071; // butterworth Q
|
||||
|
||||
K = tan(M_PI * fixA[fix_freq]);
|
||||
norm = 1.0 / (1.0 + K / fixA[fix_reso] + K * K);
|
||||
fixA[fix_a0] = fixB[fix_a0] = K * K * norm;
|
||||
fixA[fix_a1] = fixB[fix_a1] = 2.0 * fixA[fix_a0];
|
||||
fixA[fix_a2] = fixB[fix_a2] = fixA[fix_a0];
|
||||
fixA[fix_b1] = fixB[fix_b1] = 2.0 * (K * K - 1.0) * norm;
|
||||
fixA[fix_b2] = fixB[fix_b2] = (1.0 - K / fixA[fix_reso] + K * K) * norm;
|
||||
// for the fixed-position biquad filter
|
||||
|
||||
for (int s = 0; s < blockSize; s++) {
|
||||
double inputSampleL = *in1;
|
||||
double inputSampleR = *in2;
|
||||
if (fabs(inputSampleL) < 1.18e-23)
|
||||
inputSampleL = fpdL * 1.18e-17;
|
||||
if (fabs(inputSampleR) < 1.18e-23)
|
||||
inputSampleR = fpdR * 1.18e-17;
|
||||
double drySampleL = inputSampleL;
|
||||
double drySampleR = inputSampleR;
|
||||
|
||||
double temp = (double)s / inFramesToProcess;
|
||||
biquad[biq_a0] = (biquad[biq_aA0] * temp) + (biquad[biq_aB0] * (1.0 - temp));
|
||||
biquad[biq_a1] = (biquad[biq_aA1] * temp) + (biquad[biq_aB1] * (1.0 - temp));
|
||||
biquad[biq_a2] = (biquad[biq_aA2] * temp) + (biquad[biq_aB2] * (1.0 - temp));
|
||||
biquad[biq_b1] = (biquad[biq_bA1] * temp) + (biquad[biq_bB1] * (1.0 - temp));
|
||||
biquad[biq_b2] = (biquad[biq_bA2] * temp) + (biquad[biq_bB2] * (1.0 - temp));
|
||||
// this is the interpolation code for the biquad
|
||||
double powFactor = (powFactorA * temp) + (powFactorB * (1.0 - temp));
|
||||
double inTrim = (inTrimA * temp) + (inTrimB * (1.0 - temp));
|
||||
double outTrim = (outTrimA * temp) + (outTrimB * (1.0 - temp));
|
||||
|
||||
inputSampleL *= inTrim;
|
||||
inputSampleR *= inTrim;
|
||||
|
||||
temp = (inputSampleL * fixA[fix_a0]) + fixA[fix_sL1];
|
||||
fixA[fix_sL1] = (inputSampleL * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sL2];
|
||||
fixA[fix_sL2] = (inputSampleL * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixA[fix_a0]) + fixA[fix_sR1];
|
||||
fixA[fix_sR1] = (inputSampleR * fixA[fix_a1]) - (temp * fixA[fix_b1]) + fixA[fix_sR2];
|
||||
fixA[fix_sR2] = (inputSampleR * fixA[fix_a2]) - (temp * fixA[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, powFactor);
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, powFactor);
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, powFactor);
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, powFactor);
|
||||
|
||||
temp = (inputSampleL * biquad[biq_a0]) + biquad[biq_sL1];
|
||||
biquad[biq_sL1] = (inputSampleL * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sL2];
|
||||
biquad[biq_sL2] = (inputSampleL * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleL = temp; // coefficient interpolating biquad filter
|
||||
temp = (inputSampleR * biquad[biq_a0]) + biquad[biq_sR1];
|
||||
biquad[biq_sR1] = (inputSampleR * biquad[biq_a1]) - (temp * biquad[biq_b1]) + biquad[biq_sR2];
|
||||
biquad[biq_sR2] = (inputSampleR * biquad[biq_a2]) - (temp * biquad[biq_b2]);
|
||||
inputSampleR = temp; // coefficient interpolating biquad filter
|
||||
|
||||
// encode/decode courtesy of torridgristle under the MIT license
|
||||
if (inputSampleL > 1.0)
|
||||
inputSampleL = 1.0;
|
||||
else if (inputSampleL > 0.0)
|
||||
inputSampleL = 1.0 - pow(1.0 - inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleL < -1.0)
|
||||
inputSampleL = -1.0;
|
||||
else if (inputSampleL < 0.0)
|
||||
inputSampleL = -1.0 + pow(1.0 + inputSampleL, (1.0 / powFactor));
|
||||
if (inputSampleR > 1.0)
|
||||
inputSampleR = 1.0;
|
||||
else if (inputSampleR > 0.0)
|
||||
inputSampleR = 1.0 - pow(1.0 - inputSampleR, (1.0 / powFactor));
|
||||
if (inputSampleR < -1.0)
|
||||
inputSampleR = -1.0;
|
||||
else if (inputSampleR < 0.0)
|
||||
inputSampleR = -1.0 + pow(1.0 + inputSampleR, (1.0 / powFactor));
|
||||
|
||||
inputSampleL *= outTrim;
|
||||
inputSampleR *= outTrim;
|
||||
|
||||
temp = (inputSampleL * fixB[fix_a0]) + fixB[fix_sL1];
|
||||
fixB[fix_sL1] = (inputSampleL * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sL2];
|
||||
fixB[fix_sL2] = (inputSampleL * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleL = temp; // fixed biquad filtering ultrasonics
|
||||
temp = (inputSampleR * fixB[fix_a0]) + fixB[fix_sR1];
|
||||
fixB[fix_sR1] = (inputSampleR * fixB[fix_a1]) - (temp * fixB[fix_b1]) + fixB[fix_sR2];
|
||||
fixB[fix_sR2] = (inputSampleR * fixB[fix_a2]) - (temp * fixB[fix_b2]);
|
||||
inputSampleR = temp; // fixed biquad filtering ultrasonics
|
||||
|
||||
if (wet < 1.0) {
|
||||
inputSampleL = (inputSampleL * wet) + (drySampleL * (1.0 - wet));
|
||||
inputSampleR = (inputSampleR * wet) + (drySampleR * (1.0 - wet));
|
||||
}
|
||||
|
||||
// begin 32 bit stereo floating point dither
|
||||
int expon;
|
||||
frexpf((float)inputSampleL, &expon);
|
||||
fpdL ^= fpdL << 13;
|
||||
fpdL ^= fpdL >> 17;
|
||||
fpdL ^= fpdL << 5;
|
||||
inputSampleL += ((double(fpdL) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
frexpf((float)inputSampleR, &expon);
|
||||
fpdR ^= fpdR << 13;
|
||||
fpdR ^= fpdR >> 17;
|
||||
fpdR ^= fpdR << 5;
|
||||
inputSampleR += ((double(fpdR) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
|
||||
// end 32 bit stereo floating point dither
|
||||
|
||||
*out1 = inputSampleL;
|
||||
*out2 = inputSampleR;
|
||||
|
||||
in1++;
|
||||
in2++;
|
||||
out1++;
|
||||
out2++;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
double samplerate;
|
||||
enum {
|
||||
biq_freq,
|
||||
biq_reso,
|
||||
biq_a0,
|
||||
biq_a1,
|
||||
biq_a2,
|
||||
biq_b1,
|
||||
biq_b2,
|
||||
biq_aA0,
|
||||
biq_aA1,
|
||||
biq_aA2,
|
||||
biq_bA1,
|
||||
biq_bA2,
|
||||
biq_aB0,
|
||||
biq_aB1,
|
||||
biq_aB2,
|
||||
biq_bB1,
|
||||
biq_bB2,
|
||||
biq_sL1,
|
||||
biq_sL2,
|
||||
biq_sR1,
|
||||
biq_sR2,
|
||||
biq_total
|
||||
}; // coefficient interpolating biquad filter, stereo
|
||||
std::array<double, biq_total> biquad;
|
||||
|
||||
double powFactorA;
|
||||
double powFactorB;
|
||||
double inTrimA;
|
||||
double inTrimB;
|
||||
double outTrimA;
|
||||
double outTrimB;
|
||||
|
||||
enum {
|
||||
fix_freq,
|
||||
fix_reso,
|
||||
fix_a0,
|
||||
fix_a1,
|
||||
fix_a2,
|
||||
fix_b1,
|
||||
fix_b2,
|
||||
fix_sL1,
|
||||
fix_sL2,
|
||||
fix_sR1,
|
||||
fix_sR2,
|
||||
fix_total
|
||||
}; // fixed frequency biquad filter for ultrasonics, stereo
|
||||
std::array<double, fix_total> fixA;
|
||||
std::array<double, fix_total> fixB;
|
||||
|
||||
uint32_t fpdL;
|
||||
uint32_t fpdR;
|
||||
// default stuff
|
||||
|
||||
float A;
|
||||
float B;
|
||||
float C;
|
||||
float D;
|
||||
float E;
|
||||
float F; // parameters. Always 0-1, and we scale/alter them elsewhere.
|
||||
};
|
||||
}
|
||||
112
filter/ysvf.h
Normal file
112
filter/ysvf.h
Normal file
@@ -0,0 +1,112 @@
|
||||
#pragma once
|
||||
#include "ylowpass.h"
|
||||
#include "yhighpass.h"
|
||||
#include "ybandpass.h"
|
||||
#include "ynotch.h"
|
||||
|
||||
namespace trnr::lib::filter {
|
||||
|
||||
enum filter_types {
|
||||
lowpass = 0,
|
||||
highpass,
|
||||
bandpass,
|
||||
notch
|
||||
};
|
||||
|
||||
class ysvf {
|
||||
public:
|
||||
ysvf(double _samplerate)
|
||||
: lowpass { _samplerate }
|
||||
, highpass { _samplerate }
|
||||
, bandpass { _samplerate }
|
||||
, notch { _samplerate }
|
||||
{}
|
||||
|
||||
void set_samplerate(double _samplerate) {
|
||||
lowpass.set_samplerate(_samplerate);
|
||||
highpass.set_samplerate(_samplerate);
|
||||
bandpass.set_samplerate(_samplerate);
|
||||
notch.set_samplerate(_samplerate);
|
||||
}
|
||||
|
||||
void set_filter_type(filter_types type) {
|
||||
filter_type = type;
|
||||
}
|
||||
|
||||
void set_drive(float value) {
|
||||
lowpass.set_drive(value);
|
||||
highpass.set_drive(value);
|
||||
bandpass.set_drive(value);
|
||||
notch.set_drive(value);
|
||||
}
|
||||
|
||||
void set_frequency(float value) {
|
||||
lowpass.set_frequency(value);
|
||||
highpass.set_frequency(value);
|
||||
bandpass.set_frequency(value);
|
||||
notch.set_frequency(value);
|
||||
}
|
||||
|
||||
void set_resonance(float value) {
|
||||
lowpass.set_resonance(value);
|
||||
highpass.set_resonance(value);
|
||||
bandpass.set_resonance(value);
|
||||
notch.set_resonance(value);
|
||||
}
|
||||
|
||||
void set_edge(float value) {
|
||||
lowpass.set_edge(value);
|
||||
highpass.set_edge(value);
|
||||
bandpass.set_edge(value);
|
||||
notch.set_edge(value);
|
||||
}
|
||||
|
||||
void set_output(float value) {
|
||||
lowpass.set_output(value);
|
||||
highpass.set_output(value);
|
||||
bandpass.set_output(value);
|
||||
notch.set_output(value);
|
||||
}
|
||||
|
||||
void set_mix(float value) {
|
||||
lowpass.set_mix(value);
|
||||
highpass.set_mix(value);
|
||||
bandpass.set_mix(value);
|
||||
notch.set_mix(value);
|
||||
}
|
||||
|
||||
void process_block(double** inputs, double** outputs, int block_size) {
|
||||
|
||||
switch (filter_type) {
|
||||
case filter_types::lowpass:
|
||||
lowpass.processblock(inputs, outputs, block_size);
|
||||
break;
|
||||
case filter_types::highpass:
|
||||
highpass.processblock(inputs, outputs, block_size);
|
||||
break;
|
||||
case filter_types::bandpass:
|
||||
bandpass.processblock(inputs, outputs, block_size);
|
||||
break;
|
||||
case filter_types::notch:
|
||||
notch.processblock(inputs, outputs, block_size);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
filter_types filter_type;
|
||||
ylowpass lowpass;
|
||||
yhighpass highpass;
|
||||
ybandpass bandpass;
|
||||
ynotch notch;
|
||||
|
||||
double clamp(double& value, double min, double max) {
|
||||
if (value < min) {
|
||||
value = min;
|
||||
} else if (value > max) {
|
||||
value = max;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
};
|
||||
}
|
||||
Reference in New Issue
Block a user