Added DaisySP

daisysp/PhysicalModeling/resonator.cpp

@@ -0,0 +1,158 @@
+#include "resonator.h"
+#include <math.h>
+
+using namespace daisysp;
+
+void Resonator::Init(float position, int resolution, float sample_rate)
+{
+    sample_rate_ = sample_rate;
+
+    SetFreq(440.f);
+    SetStructure(.5f);
+    SetBrightness(.5f);
+    SetDamping(.5f);
+
+    resolution_ = fmin(resolution, kMaxNumModes);
+
+    for(int i = 0; i < resolution; ++i)
+    {
+        mode_amplitude_[i] = cos(position * TWOPI_F) * 0.25f;
+    }
+
+    for(int i = 0; i < kMaxNumModes / kModeBatchSize; ++i)
+    {
+        mode_filters_[i].Init();
+    }
+}
+
+inline float NthHarmonicCompensation(int n, float stiffness)
+{
+    float stretch_factor = 1.0f;
+    for(int i = 0; i < n - 1; ++i)
+    {
+        stretch_factor += stiffness;
+        if(stiffness < 0.0f)
+        {
+            stiffness *= 0.93f;
+        }
+        else
+        {
+            stiffness *= 0.98f;
+        }
+    }
+    return 1.0f / stretch_factor;
+}
+
+float Resonator::Process(const float in)
+{
+    //convert Hz to cycles / sample
+    float out = 0.f;
+
+    float stiffness  = CalcStiff(structure_);
+    float f0         = frequency_ * NthHarmonicCompensation(3, stiffness);
+    float brightness = brightness_;
+
+    float harmonic       = f0;
+    float stretch_factor = 1.0f;
+
+    float input  = damping_ * 79.7f;
+    float q_sqrt = powf(2.f, input * ratiofrac_);
+
+    float q = 500.0f * q_sqrt * q_sqrt;
+    brightness *= 1.0f - structure_ * 0.3f;
+    brightness *= 1.0f - damping_ * 0.3f;
+    float q_loss = brightness * (2.0f - brightness) * 0.85f + 0.15f;
+
+    float mode_q[kModeBatchSize];
+    float mode_f[kModeBatchSize];
+    float mode_a[kModeBatchSize];
+    int   batch_counter = 0;
+
+    ResonatorSvf<kModeBatchSize>* batch_processor = &mode_filters_[0];
+
+    for(int i = 0; i < resolution_; ++i)
+    {
+        float mode_frequency = harmonic * stretch_factor;
+        if(mode_frequency >= 0.499f)
+        {
+            mode_frequency = 0.499f;
+        }
+        const float mode_attenuation = 1.0f - mode_frequency * 2.0f;
+
+        mode_f[batch_counter] = mode_frequency;
+        mode_q[batch_counter] = 1.0f + mode_frequency * q;
+        mode_a[batch_counter] = mode_amplitude_[i] * mode_attenuation;
+        ++batch_counter;
+
+        if(batch_counter == kModeBatchSize)
+        {
+            batch_counter = 0;
+            batch_processor
+                ->Process<ResonatorSvf<kModeBatchSize>::BAND_PASS, true>(
+                    mode_f, mode_q, mode_a, in, &out);
+            ++batch_processor;
+        }
+
+        stretch_factor += stiffness;
+        if(stiffness < 0.0f)
+        {
+            // Make sure that the partials do not fold back into negative frequencies.
+            stiffness *= 0.93f;
+        }
+        else
+        {
+            // This helps adding a few extra partials in the highest frequencies.
+            stiffness *= 0.98f;
+        }
+        harmonic += f0;
+        q *= q_loss;
+    }
+
+    return out;
+}
+
+void Resonator::SetFreq(float freq)
+{
+    frequency_ = freq / sample_rate_;
+}
+
+void Resonator::SetStructure(float structure)
+{
+    structure_ = fmax(fmin(structure, 1.f), 0.f);
+}
+
+void Resonator::SetBrightness(float brightness)
+{
+    brightness_ = fmax(fmin(brightness, 1.f), 0.f);
+}
+
+void Resonator::SetDamping(float damping)
+{
+    damping_ = fmax(fmin(damping, 1.f), 0.f);
+}
+
+float Resonator::CalcStiff(float sig)
+{
+    if(sig < .25f)
+    {
+        sig = .25 - sig;
+        sig = -sig * .25;
+    }
+    else if(sig < .3f)
+    {
+        sig = 0.f;
+    }
+    else if(sig < .9f)
+    {
+        sig -= .3f;
+        sig *= stiff_frac_2;
+    }
+    else
+    {
+        sig -= .9f;
+        sig *= 10; // div by .1
+        sig *= sig;
+        sig = 1.5 - cos(sig * PI_F) * .5f;
+    }
+    return sig;
+}