Line data    Source code

       1             : /**
       2             :  * \file IIRFilter.h
       3             :  */
       4             : 
       5             : #ifndef ATK_EQ_IIRFILTER_H
       6             : #define ATK_EQ_IIRFILTER_H
       7             : 
       8             : #include <ATK/config.h>
       9             : #include <ATK/Core/TypeTraits.h>
      10             : #include <ATK/EQ/config.h>
      11             : 
      12             : #include <gsl/gsl>
      13             : 
      14             : #include <algorithm>
      15             : #include <cassert>
      16             : #include <vector>
      17             : 
      18             : namespace ATK
      19             : {
      20             :   /// IIR filter template class (Direct Form I)
      21             :   template<class Coefficients >
      22             :   class IIRFilter final : public Coefficients
      23             :   {
      24             :   protected:
      25             :     /// Simplify parent calls
      26             :     using Parent = Coefficients;
      27             :     using typename Parent::DataType;
      28             :     using typename Parent::AlignedScalarVector;
      29             :     using Parent::converted_inputs;
      30             :     using Parent::outputs;
      31             :     using Parent::coefficients_in;
      32             :     using Parent::coefficients_out;
      33             :     using Parent::input_sampling_rate;
      34             :     using Parent::output_sampling_rate;
      35             :     using Parent::nb_input_ports;
      36             :     using Parent::nb_output_ports;
      37             :     
      38             :     using Parent::in_order;
      39             :     using Parent::out_order;
      40             :     using Parent::input_delay;
      41             :     using Parent::output_delay;
      42             :     using Parent::setup;
      43             :     
      44             :   public:
      45             :     /*!
      46             :      * @brief Constructor
      47             :      * @param nb_channels is the number of input and output channels
      48             :      */
      49         333 :     explicit IIRFilter(gsl::index nb_channels = 1)
      50         333 :       :Parent(nb_channels)
      51             :     {
      52         333 :     }
      53             : 
      54             :     /// Move constructor
      55          31 :     IIRFilter(IIRFilter&& other)
      56          31 :     :Parent(std::move(other))
      57             :     {
      58          31 :     }
      59             : 
      60         827 :     void setup() final
      61             :     {
      62         827 :       Parent::setup();
      63         826 :       input_delay = in_order;
      64         826 :       output_delay = out_order;
      65             : 
      66         393 :       if (out_order > 0)
      67             :       {
      68         826 :         coefficients_out_2.resize(out_order, 0);
      69        1810 :         for (unsigned int i = 1; i < out_order; ++i)
      70             :         {
      71         984 :           coefficients_out_2[i] = coefficients_out[out_order - 1] * coefficients_out[i] + coefficients_out[i - 1];
      72             :         }
      73         826 :         coefficients_out_2[0] = coefficients_out[out_order - 1] * coefficients_out[0];
      74             :       }
      75         393 :       if (out_order > 1)
      76             :       {
      77         538 :         coefficients_out_3.resize(out_order, 0);
      78        1614 :         for (unsigned int i = 0; i < 2; ++i)
      79             :         {
      80        1076 :           coefficients_out_3[i] = coefficients_out[out_order - 2]  * coefficients_out[i] + coefficients_out[out_order - 1] * coefficients_out_2[i];
      81             :         }
      82         984 :         for (unsigned int i = 2; i < out_order; ++i)
      83             :         {
      84         446 :           coefficients_out_3[i] = coefficients_out[out_order - 2]  * coefficients_out[i] + coefficients_out[out_order - 1] * coefficients_out_2[i] + coefficients_out[i - 2];
      85             :         }
      86         105 :         if (out_order > 2)
      87             :         {
      88         222 :           coefficients_out_4.resize(out_order, 0);
      89         888 :           for (unsigned int i = 0; i < 3; ++i)
      90             :           {
      91         666 :             coefficients_out_4[i] = coefficients_out[out_order - 3]  * coefficients_out[i] + coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i];
      92             :           }
      93         446 :           for (unsigned int i = 3; i < out_order; ++i)
      94             :           {
      95         224 :             coefficients_out_4[i] = coefficients_out[out_order - 3]  * coefficients_out[i] + coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i] + coefficients_out[i - 3];
      96             :           }
      97             :         }
      98             :         else // out_order = 2
      99             :         {
     100         316 :           coefficients_out_4.resize(out_order, 0);
     101         948 :           for (unsigned int i = 0; i < 2; ++i)
     102             :           {
     103         632 :             coefficients_out_4[i] = coefficients_out[out_order - 2] * coefficients_out_2[i] + coefficients_out[out_order - 1] * coefficients_out_3[i];
     104             :           }
     105             :         }
     106             :       }
     107         826 :     }
     108             :     
     109             :     template<typename T>
     110         391 :     void handle_recursive_iir(const T* ATK_RESTRICT coefficients_out_ptr, const T* ATK_RESTRICT coefficients_out_2_ptr, const T* ATK_RESTRICT coefficients_out_3_ptr, const T* ATK_RESTRICT coefficients_out_4_ptr, DataType* ATK_RESTRICT output, gsl::index size) const
     111             :     {
     112         391 :       gsl::index i = 0;
     113         145 :       if (out_order > 2)
     114             :       {
     115     3806919 :         for (i = 0; i < std::min(size - 3, size); i += 4)
     116             :         {
     117     3806708 :           DataType tempout = output[i];
     118     3806708 :           DataType tempout2 = output[i] * coefficients_out_ptr[out_order - 1] + output[i + 1];
     119     3806708 :           DataType tempout3 = output[i] * coefficients_out_ptr[out_order - 2] + tempout2 * coefficients_out_ptr[out_order - 1] + output[i + 2];
     120     3806708 :           DataType tempout4 = output[i] * coefficients_out_ptr[out_order - 3] + tempout2 * coefficients_out_ptr[out_order - 2] + tempout3 * coefficients_out_ptr[out_order - 1] + output[i + 3];
     121             :           
     122    18678684 :           ATK_VECTORIZE_REMAINDER for (unsigned int j = 0; j < out_order; ++j)
     123             :           {
     124    14871986 :             tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     125    14871986 :             tempout2 += coefficients_out_2_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     126    14871986 :             tempout3 += coefficients_out_3_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     127    14871986 :             tempout4 += coefficients_out_4_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     128             :           }
     129     3806708 :           output[i] = tempout;
     130     3806708 :           output[i + 1] = tempout2;
     131     3806708 :           output[i + 2] = tempout3;
     132     3806708 :           output[i + 3] = tempout4;
     133             :         }
     134             :       }
     135           0 :       else if(out_order == 2)
     136             :       {
     137     7283890 :         for (i = 0; i < std::min(size - 3, size); i += 4)
     138             :         {
     139     7283712 :           DataType tempout = output[i];
     140     7283712 :           DataType tempout2 = output[i] * coefficients_out_ptr[out_order - 1] + output[i + 1];
     141     7283712 :           DataType tempout3 = output[i] * coefficients_out_ptr[out_order - 2] + tempout2 * coefficients_out_ptr[out_order - 1] + output[i + 2];
     142     7283712 :           DataType tempout4 = tempout2 * coefficients_out_ptr[out_order - 2] + tempout3 * coefficients_out_ptr[out_order - 1] + output[i + 3];
     143             :           
     144    21851136 :           ATK_VECTORIZE_REMAINDER for (unsigned int j = 0; j < out_order; ++j)
     145             :           {
     146    14567424 :             tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     147    14567424 :             tempout2 += coefficients_out_2_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     148    14567424 :             tempout3 += coefficients_out_3_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     149    14567424 :             tempout4 += coefficients_out_4_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     150             :           }
     151     7283712 :           output[i] = tempout;
     152     7283712 :           output[i + 1] = tempout2;
     153     7283712 :           output[i + 2] = tempout3;
     154     7283712 :           output[i + 3] = tempout4;
     155             :         }
     156             :       }
     157         395 :       for (; i < size; ++i)
     158             :       {
     159           4 :         DataType tempout = output[i];
     160          16 :         for (unsigned int j = 0; j < out_order; ++j)
     161             :         {
     162          12 :           tempout += coefficients_out_ptr[j] * output[static_cast<int64_t>(i) - out_order + j];
     163             :         }
     164           4 :         output[i] = tempout;
     165             :       }
     166         391 :     }
     167             :     
     168         391 :     void process_impl(gsl::index size) const final
     169             :     {
     170         391 :       assert(input_sampling_rate == output_sampling_rate);
     171         391 :       assert(nb_input_ports == nb_output_ports);
     172         391 :       assert(coefficients_in.data());
     173         391 :       assert(out_order == 0 || coefficients_out.data() != nullptr);
     174             : 
     175         391 :       const auto* ATK_RESTRICT coefficients_in_ptr = coefficients_in.data();
     176         391 :       const auto* ATK_RESTRICT coefficients_out_ptr = coefficients_out.data();
     177         391 :       const auto* ATK_RESTRICT coefficients_out_2_ptr = coefficients_out_2.data();
     178         391 :       const auto* ATK_RESTRICT coefficients_out_3_ptr = coefficients_out_3.data();
     179         391 :       const auto* ATK_RESTRICT coefficients_out_4_ptr = coefficients_out_4.data();
     180             : 
     181         782 :       for(gsl::index channel = 0; channel < nb_input_ports; ++channel)
     182             :       {
     183         391 :         const DataType* ATK_RESTRICT input = converted_inputs[channel] - static_cast<int64_t>(in_order);
     184         391 :         DataType* ATK_RESTRICT output = outputs[channel];
     185             : 
     186    44362143 :         for(gsl::index i = 0; i < size; ++i)
     187             :         {
     188    44361584 :           output[i] = 0;
     189             :         }
     190             : 
     191        2013 :         for (gsl::index j = 0; j < in_order + 1; ++j)
     192             :         {
     193   162120982 :           for (gsl::index i = 0; i < size; ++i)
     194             :           {
     195   162119300 :             output[i] += coefficients_in_ptr[j] * input[i + j];
     196             :           }
     197             :         }
     198             : 
     199         391 :         handle_recursive_iir(coefficients_out_ptr, coefficients_out_2_ptr, coefficients_out_3_ptr, coefficients_out_4_ptr, output, size);
     200             :       }
     201         391 :     }
     202             :     
     203             :     /// Returns the vector of internal coefficients for the MA section 
     204             :     const AlignedScalarVector& get_coefficients_in() const
     205             :     {
     206             :       return coefficients_in;
     207             :     }
     208             :     
     209             :     /// Returns the vector of internal coefficients for the AR section, without degree 0 implicitely set to -1
     210             :     const AlignedScalarVector& get_coefficients_out() const
     211             :     {
     212             :       return coefficients_out;
     213             :     }
     214             : 
     215             :   protected:
     216             :     AlignedScalarVector coefficients_out_2;
     217             :     AlignedScalarVector coefficients_out_3;
     218             :     AlignedScalarVector coefficients_out_4;
     219             :   };
     220             : 
     221             :   /// IIR filter template class. Transposed Direct Form II implementation
     222             :   template<class Coefficients >
     223             :   class IIRTDF2Filter final : public Coefficients
     224             :   {
     225             :   public:
     226             :     /// Simplify parent calls
     227             :     using Parent = Coefficients;
     228             :     using typename Parent::DataType;
     229             :     using typename Parent::AlignedScalarVector;
     230             :     using Parent::converted_inputs;
     231             :     using Parent::outputs;
     232             :     using Parent::coefficients_in;
     233             :     using Parent::coefficients_out;
     234             :     using Parent::input_sampling_rate;
     235             :     using Parent::output_sampling_rate;
     236             :     using Parent::nb_input_ports;
     237             :     using Parent::nb_output_ports;
     238             :     
     239             :     using Parent::in_order;
     240             :     using Parent::out_order;
     241             :     using Parent::input_delay;
     242             :     using Parent::output_delay;
     243             :     using Parent::setup;
     244             :   protected:
     245             :     mutable typename Parent::AlignedVector state;
     246             :   public:
     247           1 :     explicit IIRTDF2Filter(gsl::index nb_channels = 1)
     248           1 :       :Parent(nb_channels)
     249             :     {
     250           1 :     }
     251             :     
     252             :     /// Move constructor
     253             :     IIRTDF2Filter(IIRTDF2Filter&& other)
     254             :       :Parent(std::move(other))
     255             :     {
     256             :     }
     257             :     
     258           4 :     void setup() final
     259             :     {
     260           4 :       Parent::setup();
     261           4 :       input_delay = in_order;
     262           4 :       output_delay = out_order;
     263           4 :       state.assign(nb_input_ports * (std::max(input_delay, output_delay) + 1), TypeTraits<DataType>::Zero());
     264           4 :     }
     265             :     
     266           2 :     void process_impl(gsl::index size) const final
     267             :     {
     268           2 :       assert(input_sampling_rate == output_sampling_rate);
     269             :       
     270           4 :       for(gsl::index channel = 0; channel < nb_input_ports; ++channel)
     271             :       {
     272           2 :         const DataType* ATK_RESTRICT input = converted_inputs[channel];
     273           2 :         DataType* ATK_RESTRICT output = outputs[channel];
     274           2 :         DataType* ATK_RESTRICT current_state = &state[channel * (std::max(input_delay, output_delay) + 1)];
     275             :         
     276      131074 :         for(gsl::index i = 0; i < size; ++i)
     277             :         {
     278      131072 :           output[i] = coefficients_in[in_order] * input[i] + current_state[0];
     279      131072 :           auto min_order = std::min(input_delay, output_delay);
     280             :           
     281      524288 :           for(gsl::index j = 0; j < min_order; ++j)
     282             :           {
     283      393216 :             current_state[j] = current_state[j + 1] + input[i] * coefficients_in[in_order - static_cast<int64_t>(j) - 1] + output[i] * coefficients_out[out_order - static_cast<int64_t>(j) - 1];
     284             :           }
     285      131072 :           for(gsl::index j = min_order; j < input_delay; ++j)
     286             :           {
     287           0 :             current_state[j] = current_state[j + 1] + input[i] * coefficients_in[in_order - static_cast<int64_t>(j) - 1];
     288             :           }
     289      131072 :           for(gsl::index j = min_order; j < output_delay; ++j)
     290             :           {
     291           0 :             current_state[j] = current_state[j + 1] + output[i] * coefficients_out[out_order - static_cast<int64_t>(j) - 1];
     292             :           }
     293             :         }
     294             :       }
     295           2 :     }
     296             : 
     297             :     /// Returns the vector of internal coefficients for the MA section
     298             :     const AlignedScalarVector& get_coefficients_in() const
     299             :     {
     300             :       return coefficients_in;
     301             :     }
     302             :     
     303             :     /// Returns the vector of internal coefficients for the AR section, without degree 0 implicitely set to -1
     304             :     const AlignedScalarVector& get_coefficients_out() const
     305             :     {
     306             :       return coefficients_out;
     307             :     }
     308             :   };
     309             : 
     310             : }
     311             : 
     312             : #endif