kfr

Fast, modern C++ DSP framework, FFT, Sample Rate Conversion, FIR/IIR/Biquad Filters (SSE, AVX, AVX-512, ARM NEON)
Log | Files | Refs | README

goertzel.hpp (4320B)

      1 /** @addtogroup dsp_extra
      2  *  @{
      3  */
      4 /*
      5   Copyright (C) 2016-2023 Dan Cazarin (https://www.kfrlib.com)
      6   This file is part of KFR
      7 
      8   KFR is free software: you can redistribute it and/or modify
      9   it under the terms of the GNU General Public License as published by
     10   the Free Software Foundation, either version 2 of the License, or
     11   (at your option) any later version.
     12 
     13   KFR is distributed in the hope that it will be useful,
     14   but WITHOUT ANY WARRANTY; without even the implied warranty of
     15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     16   GNU General Public License for more details.
     17 
     18   You should have received a copy of the GNU General Public License
     19   along with KFR.
     20 
     21   If GPL is not suitable for your project, you must purchase a commercial license to use KFR.
     22   Buying a commercial license is mandatory as soon as you develop commercial activities without
     23   disclosing the source code of your own applications.
     24   See https://www.kfrlib.com for details.
     25  */
     26 #pragma once
     27 
     28 #include "../base/basic_expressions.hpp"
     29 #include "../math/sin_cos.hpp"
     30 #include "../simd/complex.hpp"
     31 #include "../simd/vec.hpp"
     32 
     33 namespace kfr
     34 {
     35 inline namespace CMT_ARCH_NAME
     36 {
     37 
     38 template <typename T>
     39 struct expression_goertzel : expression_traits_defaults
     40 {
     41     constexpr static size_t dims = 1;
     42 
     43     using value_type = T;
     44 
     45     constexpr static shape<1> get_shape(const expression_goertzel&) { return shape<1>(infinite_size); }
     46     constexpr static shape<1> get_shape() { return shape<1>(infinite_size); }
     47 
     48     expression_goertzel(complex<T>& result, T omega)
     49         : result(result), omega(omega), coeff(2 * cos(omega)), q0(0), q1(0), q2(0)
     50     {
     51     }
     52     ~expression_goertzel()
     53     {
     54         result.real(q1 - q2 * cos(omega));
     55         result.imag(q2 * sin(omega));
     56     }
     57 
     58     template <size_t N, index_t VecAxis>
     59     friend KFR_INTRINSIC void set_elements(expression_goertzel& self, shape<1>, axis_params<VecAxis, N>,
     60                                            const identity<vec<T, N>>& x)
     61     {
     62         vec<T, N> in = x;
     63         CMT_LOOP_UNROLL
     64         for (size_t i = 0; i < N; i++)
     65         {
     66             self.q0 = self.coeff * self.q1 - self.q2 + in[i];
     67             self.q2 = self.q1;
     68             self.q1 = self.q0;
     69         }
     70     }
     71     complex<T>& result;
     72     const T omega;
     73     const T coeff;
     74     T q0;
     75     T q1;
     76     T q2;
     77 };
     78 
     79 template <typename T, size_t width>
     80 struct expression_parallel_goertzel : expression_traits_defaults
     81 {
     82     constexpr static size_t dims = 1;
     83 
     84     using value_type = T;
     85 
     86     constexpr static shape<1> get_shape(const expression_parallel_goertzel&)
     87     {
     88         return shape<1>(infinite_size);
     89     }
     90     constexpr static shape<1> get_shape() { return shape<1>(infinite_size); }
     91 
     92     expression_parallel_goertzel(complex<T> result[], vec<T, width> omega)
     93         : result(result), omega(omega), coeff(2 * cos(omega)), q0(T(0)), q1(T(0)), q2(T(0))
     94     {
     95     }
     96     ~expression_parallel_goertzel()
     97     {
     98         const vec<T, width> re = q1 - q2 * cos(omega);
     99         const vec<T, width> im = q2 * sin(omega);
    100         for (size_t i = 0; i < width; i++)
    101         {
    102             result[i].real(re[i]);
    103             result[i].imag(im[i]);
    104         }
    105     }
    106     template <size_t N, index_t VecAxis>
    107     friend KFR_INTRINSIC void set_elements(expression_parallel_goertzel& self, shape<1>,
    108                                            axis_params<VecAxis, N>, const identity<vec<T, N>>& x)
    109     {
    110         const vec<T, N> in = x;
    111         CMT_LOOP_UNROLL
    112         for (size_t i = 0; i < N; i++)
    113         {
    114             self.q0 = self.coeff * self.q1 - self.q2 + in[i];
    115             self.q2 = self.q1;
    116             self.q1 = self.q0;
    117         }
    118     }
    119     complex<T>* result;
    120     const vec<T, width> omega;
    121     const vec<T, width> coeff;
    122     vec<T, width> q0;
    123     vec<T, width> q1;
    124     vec<T, width> q2;
    125 };
    126 
    127 template <typename T>
    128 KFR_INTRINSIC expression_goertzel<T> goertzel(complex<T>& result, identity<T> omega)
    129 {
    130     return expression_goertzel<T>(result, omega);
    131 }
    132 
    133 template <typename T, size_t width>
    134 KFR_INTRINSIC expression_parallel_goertzel<T, width> goertzel(complex<T> (&result)[width],
    135                                                               const T (&omega)[width])
    136 {
    137     return expression_parallel_goertzel<T, width>(result, read<width>(omega));
    138 }
    139 } // namespace CMT_ARCH_NAME
    140 } // namespace kfr