kfr

commit 528a4a161cd6fc1c4cf9d397787f9514959c2738
parent 26967fc77ecd9cf7335d72b0155ba77ddb70c82a
Author: [email protected] <[email protected]>
Date:   Tue,  2 Aug 2016 22:31:13 +0300

Goertzel algorithm has been fixed

Diffstat:
M
include/kfr/dsp/goertzel.hpp
 | 
147
+++++++++++++++++++++++++++++++++++++------------------------------------------

1 file changed, 69 insertions(+), 78 deletions(-)
diff --git a/include/kfr/dsp/goertzel.hpp b/include/kfr/dsp/goertzel.hpp
@@ -32,95 +32,86 @@ namespace kfr
 namespace internal
 {
 
-template <cpu_t c = cpu_t::native, cpu_t cc = c>
-struct in_goertzel : in_sin_cos<cc>
+template <typename T>
+struct expression_goertzel : output_expression
 {
-private:
-    using in_sin_cos<cc>::sin;
-    using in_sin_cos<cc>::cos;
-
-public:
-    template <typename T>
-    struct expression_goertzel : output_expression
+    expression_goertzel(complex<T>& result, T omega)
+        : result(result), omega(omega), coeff(2 * cos(omega)), q0(), q1(), q2()
     {
-        expression_goertzel(complex<T>& result, identity<T> omega)
-            : result(result), omega(omega), coeff(2 * cos(omega)), q0(), q1(), q2()
-        {
-        }
-        ~expression_goertzel()
-        {
-            result.real(q1 - q2 * cos(omega));
-            result.imag(q2 * sin(omega));
-        }
-        template <typename U, size_t N>
-        KFR_INLINE void operator()(coutput_t, size_t index, const vec<U, N>& x)
+    }
+    ~expression_goertzel()
+    {
+        result.real(q1 - q2 * cos(omega));
+        result.imag(q2 * sin(omega));
+    }
+    template <typename U, size_t N>
+    KFR_INLINE void operator()(coutput_t, size_t index, const vec<U, N>& x)
+    {
+        vec<T, N> in = cast<T>(x);
+        KFR_LOOP_UNROLL
+        for (size_t i = 0; i < N; i++)
         {
-            vec<T, N> in = cast<T>(x);
-            KFR_LOOP_UNROLL
-            for (size_t i = 0; i < N; i++)
-            {
-                q0 = coeff * q1 - q2 + in[i];
-                q2 = q1;
-                q1 = q0;
-            }
+            q0 = coeff * q1 - q2 + in[i];
+            q2 = q1;
+            q1 = q0;
         }
-        complex<T>& result;
-        const T omega;
-        const T coeff;
-        T q0;
-        T q1;
-        T q2;
-    };
+    }
+    complex<T>& result;
+    const T omega;
+    const T coeff;
+    T q0;
+    T q1;
+    T q2;
+};
 
-    template <typename T, size_t width>
-    struct expression_parallel_goertzel : output_expression
+template <typename T, size_t width>
+struct expression_parallel_goertzel : output_expression
+{
+    expression_parallel_goertzel(complex<T> result[], vec<T, width> omega)
+        : result(result), omega(omega), coeff(cos(omega)), q0(), q1(), q2()
     {
-        expression_parallel_goertzel(complex<T> result[], vec<T, width> omega)
-            : result(result), omega(omega), coeff(cos(omega)), q0(), q1(), q2()
-        {
-        }
-        ~expression_parallel_goertzel()
-        {
-            const vec<T, width> re = q1 - q2 * cos(omega);
-            const vec<T, width> im = q2 * sin(omega);
-            for (size_t i = 0; i < width; i++)
-            {
-                result[i].real(re[i]);
-                result[i].imag(im[i]);
-            }
-        }
-        template <typename U, size_t N>
-        KFR_INLINE void operator()(coutput_t, size_t index, const vec<U, N>& x)
+    }
+    ~expression_parallel_goertzel()
+    {
+        const vec<T, width> re = q1 - q2 * cos(omega);
+        const vec<T, width> im = q2 * sin(omega);
+        for (size_t i = 0; i < width; i++)
         {
-            const vec<T, N> in = cast<T>(x);
-            KFR_LOOP_UNROLL
-            for (size_t i = 0; i < N; i++)
-            {
-                q0 = coeff * q1 - q2 + in[i];
-                q2 = q1;
-                q1 = q0;
-            }
+            result[i].real(re[i]);
+            result[i].imag(im[i]);
         }
-        complex<T> result[];
-        const vec<T, width> omega;
-        const vec<T, width> coeff;
-        vec<T, width> q0;
-        vec<T, width> q1;
-        vec<T, width> q2;
-    };
-
-    template <typename T>
-    KFR_SINTRIN expression_goertzel<T> goertzel(complex<T>& result, identity<T> omega)
-    {
-        return expression_goertzel<T>(result, omega);
     }
-
-    template <typename T, size_t width>
-    KFR_SINTRIN expression_parallel_goertzel<T, width> goertzel(complex<T> (&result)[width],
-                                                                const T (&omega)[width])
+    template <typename U, size_t N>
+    KFR_INLINE void operator()(coutput_t, size_t index, const vec<U, N>& x)
     {
-        return expression_parallel_goertzel<T, width>(result, read<width>(omega));
+        const vec<T, N> in = cast<T>(x);
+        KFR_LOOP_UNROLL
+        for (size_t i = 0; i < N; i++)
+        {
+            q0 = coeff * q1 - q2 + in[i];
+            q2 = q1;
+            q1 = q0;
+        }
     }
+    complex<T> result[];
+    const vec<T, width> omega;
+    const vec<T, width> coeff;
+    vec<T, width> q0;
+    vec<T, width> q1;
+    vec<T, width> q2;
+};
 };
+
+template <typename T>
+KFR_SINTRIN internal::expression_goertzel<T> goertzel(complex<T>& result, identity<T> omega)
+{
+    return internal::expression_goertzel<T>(result, omega);
+}
+
+template <typename T, size_t width>
+KFR_SINTRIN internal::expression_parallel_goertzel<T, width> goertzel(complex<T> (&result)[width],
+                                                                      const T (&omega)[width])
+{
+    return internal::expression_parallel_goertzel<T, width>(result, read<width>(omega));
 }
 }