even more stuff

core/src/dsp/math/add.h

@@ -0,0 +1,42 @@
+#pragma once
+#include "../operator.h"
+
+namespace dsp::math {
+    template <class T>
+    class Add : public Operator<T, T, T> {
+        using base_type = Operator<T, T, T>;
+    public:
+        Add() {}
+
+        Add(stream<T>* a, stream<T>* b) { base_type::init(a, b); }
+
+        static inline int process(int count, const T* a, const T*b, T* out) {
+            if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
+                volk_32f_x2_add_32f((float*)out, (float*)a, (float*)b, count * 2);
+            }
+            else {
+                volk_32f_x2_add_32f(out, a, b, count);
+            }
+            return count;
+        }
+
+        int run() {
+            int a_count = base_type::_a->read();
+            if (a_count < 0) { return -1; }
+            int b_count = base_type::_b->read();
+            if (b_count < 0) { return -1; }
+            if (a_count != b_count) {
+                base_type::_a->flush();
+                base_type::_b->flush();
+                return 0;
+            }
+
+            process(a_count, base_type::_a->readBuf, base_type::_b->readBuf, base_type::out.writeBuf);
+
+            base_type::_a->flush();
+            base_type::_b->flush();
+            if (!base_type::out.swap(a_count)) { return -1; }
+            return a_count;
+        }
+    };
+}

core/src/dsp/math/conjugate.h

@@ -0,0 +1,28 @@
+#pragma once
+#include "../processor.h"
+
+namespace dsp::math {
+    class Conjugate : public Processor<complex_t, complex_t> {
+        using base_type = Processor<complex_t, complex_t>;
+    public:
+        Conjugate() {}
+
+        Conjugate(stream<complex_t>* in) { base_type::init(in); }
+
+        inline static int process(int count, const complex_t* in, complex_t* out) {
+            volk_32fc_conjugate_32fc((lv_32fc_t*)out, (lv_32fc_t*)in, count);
+            return count;
+        }
+
+        virtual int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
+            process(count, base_type::_in->readBuf, base_type::out.writeBuf);
+
+            base_type::_in->flush();
+            if (!base_type::out.swap(count)) { return -1; }
+            return count;
+        }
+    };
+}

core/src/dsp/math/constants.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#define DB_M_PI     3.14159265358979323846
+#define FL_M_PI     3.1415926535f
+
+#define DB_M_SQRT2  1.4142135623730951
+#define FL_M_SQRT2  1.4142135623f

core/src/dsp/math/delay.h

@@ -0,0 +1,76 @@
+#pragma once
+#include "../processor.h"
+
+namespace dsp::math {
+    template<class T>
+    class Delay : public Processor<T, T> {
+        using base_type = Processor<T, T>;
+    public:
+        Delay() {}
+
+        Delay(stream<T>* in, int delay) { init(in, delay); }
+
+        ~Delay() {
+            if (!base_type::_block_init) { return; }
+            base_type::stop();
+            buffer::free(buffer);
+        }
+
+        void init(stream<T>* in, int delay) {
+            _delay = delay;
+
+            buffer = buffer::alloc<float>(STREAM_BUFFER_SIZE + 64000);
+            bufStart = &buffer[_delay];
+            buffer::clear(buffer, _delay);
+
+            base_type::init(in);
+        }
+
+        void setDelay(int delay) {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            _delay = delay;
+            bufStart = &buffer[_delay];
+            reset();
+            base_type::tempStart();
+        }
+
+        void reset() {
+            assert(base_type::_block_init);
+            std::lock_guard<std::recursive_mutex> lck(base_type::ctrlMtx);
+            base_type::tempStop();
+            buffer::clear(buffer, _delay);
+            base_type::tempStart();
+        }
+
+        inline int process(int count, const T* in, T* out) {
+            // Copy data into delay buffer
+            memcpy(bufStart, in, count * sizeof(T));
+
+            // Copy data out of the delay buffer
+            memcpy(out, buffer, count * sizeof(T));
+
+            // Move end of the delay buffer to the front
+            memmove(buffer, &buffer[count], _delay * sizeof(T));
+
+            return count;
+        }
+
+        virtual int run() {
+            int count = base_type::_in->read();
+            if (count < 0) { return -1; }
+
+            process(count, base_type::_in->readBuf, base_type::out.writeBuf);
+
+            base_type::_in->flush();
+            if (!base_type::out.swap(count)) { return -1; }
+            return count;
+        }
+
+    private:
+        int _delay;
+        T* buffer;
+        T* bufStart;
+    };
+}

core/src/dsp/math/fast_atan2.h

@@ -0,0 +1,26 @@
+#pragma once
+#include <math.h>
+#include "constants.h"
+
+#define FAST_ATAN2_COEF1 FL_M_PI / 4.0f
+#define FAST_ATAN2_COEF2 3.0f * FAST_ATAN2_COEF1
+
+namespace dsp::math {
+    inline float fastAtan2(float x, float y) {
+        float abs_y = fabsf(y);
+        float r, angle;
+        if (x == 0.0f && y == 0.0f) { return 0.0f; }
+        if (x >= 0.0f) {
+            r = (x - abs_y) / (x + abs_y);
+            angle = FAST_ATAN2_COEF1 - FAST_ATAN2_COEF1 * r;
+        }
+        else {
+            r = (x + abs_y) / (abs_y - x);
+            angle = FAST_ATAN2_COEF2 - FAST_ATAN2_COEF1 * r;
+        }
+        if (y < 0.0f) {
+            return -angle;
+        }
+        return angle;
+    }
+}

core/src/dsp/math/freq_to_omega.h

@@ -0,0 +1,9 @@
+#pragma once
+#include <math.h>
+#include "constants.h"
+
+namespace dsp::math {
+    inline double freqToOmega(double freq, double samplerate) {
+        return 2.0 * DB_M_PI * (freq / samplerate);
+    }
+}

core/src/dsp/math/multiply.h

@@ -0,0 +1,42 @@
+#pragma once
+#include "../operator.h"
+
+namespace dsp::math {
+    template <class T>
+    class Multiply : public Operator<T, T, T> {
+        using base_type = Operator<T, T, T>;
+    public:
+        Multiply() {}
+
+        Multiply(stream<T>* a, stream<T>* b) { base_type::init(a, b); }
+
+        inline static int process(int count, const T* a, const T* b, T* out) {
+            if constexpr (std::is_same_v<T, complex_t>) {
+                volk_32fc_x2_multiply_32fc((lv_32fc_t*)out, (lv_32fc_t*)a, (lv_32fc_t*)b, count);
+            }
+            else {
+                volk_32f_x2_multiply_32f(out, a, b, count);
+            }
+            return count;
+        }
+
+        int run() {
+            int a_count = base_type::_a->read();
+            if (a_count < 0) { return -1; }
+            int b_count = base_type::_b->read();
+            if (b_count < 0) { return -1; }
+            if (a_count != b_count) {
+                base_type::_a->flush();
+                base_type::_b->flush();
+                return 0;
+            }
+
+            process(a_count, base_type::_a->readBuf, base_type::_b->readBuf, base_type::out.writeBuf);
+
+            base_type::_a->flush();
+            base_type::_b->flush();
+            if (!base_type::out.swap(a_count)) { return -1; }
+            return a_count;
+        }
+    };
+}

core/src/dsp/math/norm_phase_diff.h

@@ -0,0 +1,11 @@
+#pragma once
+#include "constants.h"
+
+namespace dsp::math {
+    template<class T>
+    T normPhaseDiff(T diff) {
+        if (diff > FL_M_PI) { diff -= 2.0f * FL_M_PI; }
+        else if (diff <= -FL_M_PI) { diff += 2.0f * FL_M_PI; }
+        return diff;
+    }
+}

core/src/dsp/math/phasor.h

@@ -0,0 +1,10 @@
+#pragma once
+#include <math.h>
+#include "../types.h"
+
+namespace dsp::math {
+    inline complex_t phasor(float x) {
+        complex_t cplx = { cosf(x), sinf(x) };
+        return cplx;
+    }
+}

core/src/dsp/math/sinc.h

@@ -0,0 +1,8 @@
+#pragma once
+#include <math.h>
+
+namespace dsp::math {
+    inline double sinc(double x) {
+        return (x == 0.0) ? 1.0 : (sin(x) / x);
+    }
+}

core/src/dsp/math/subtract.h

@@ -0,0 +1,42 @@
+#pragma once
+#include "../operator.h"
+
+namespace dsp::math {
+    template <class T>
+    class Subtract : public Operator<T, T, T> {
+        using base_type = Operator<T, T, T>;
+    public:
+        Subtract() {}
+
+        Subtract(stream<T>* a, stream<T>* b) { init(a, b); }
+
+        inline static int process(int count, const T* a, const T* b, T* out) {
+            if constexpr (std::is_same_v<T, complex_t> || std::is_same_v<T, stereo_t>) {
+                volk_32f_x2_subtract_32f((float*)out, (float*)a, (float*)b, count * 2);
+            }
+            else {
+                volk_32f_x2_subtract_32f(out, a, b, count);
+            }
+            return count;
+        }
+
+        int run() {
+            int a_count = base_type::_a->read();
+            if (a_count < 0) { return -1; }
+            int b_count = base_type::_b->read();
+            if (b_count < 0) { return -1; }
+            if (a_count != b_count) {
+                base_type::_a->flush();
+                base_type::_b->flush();
+                return 0;
+            }
+
+            process(a_count, base_type::_a->readBuf, base_type::_b->readBuf, base_type::out.writeBuf);
+
+            base_type::_a->flush();
+            base_type::_b->flush();
+            if (!base_type::out.swap(a_count)) { return -1; }
+            return a_count;
+        }
+    };
+}