Line data    Source code

       1             : /**
       2             :  * \file UniversalFixedDelayLineFilter.hxx
       3             :  */
       4             : 
       5             : #include "UniversalFixedDelayLineFilter.h"
       6             : #include <ATK/Core/Utilities.h>
       7             : 
       8             : #include <cstring>
       9             : #include <stdexcept>
      10             : 
      11             : namespace ATK
      12             : {
      13             :   template<typename DataType>
      14             :   class UniversalFixedDelayLineFilter<DataType>::UFDLF_Impl
      15             :   {
      16             :   public:
      17             :     std::vector<DataType> delay_line;
      18             :     gsl::index index{0};
      19             : 
      20          13 :     explicit UFDLF_Impl(gsl::index max_delay)
      21          13 :       :delay_line(max_delay, TypeTraits<DataType>::Zero())
      22             :     {
      23          13 :     }
      24             :   };
      25             : 
      26             :   template<typename DataType_>
      27          13 :   UniversalFixedDelayLineFilter<DataType_>::UniversalFixedDelayLineFilter(gsl::index max_delay)
      28          13 :     :Parent(1, 2), impl(std::make_unique<UFDLF_Impl>(max_delay))
      29             :   {
      30          13 :   }
      31             :   
      32             :   template<typename DataType_>
      33          13 :   UniversalFixedDelayLineFilter<DataType_>::~UniversalFixedDelayLineFilter()
      34             :   {
      35          13 :   }
      36             :   
      37             :   template<typename DataType_>
      38           8 :   void UniversalFixedDelayLineFilter<DataType_>::set_delay(gsl::index delay)
      39             :   {
      40           8 :     if(delay == 0)
      41             :     {
      42           1 :       throw ATK::RuntimeError("Delay must be strictly positive");
      43             :     }
      44           7 :     if(delay >= impl->delay_line.size())
      45             :     {
      46           1 :       throw ATK::RuntimeError("Delay must be less than delay line size");
      47             :     }
      48             : 
      49           6 :     this->delay = delay;
      50           6 :   }
      51             : 
      52             :   template<typename DataType_>
      53           1 :   gsl::index UniversalFixedDelayLineFilter<DataType_>::get_delay() const
      54             :   {
      55           1 :     return delay;
      56             :   }
      57             : 
      58             :   template<typename DataType_>
      59           3 :   void UniversalFixedDelayLineFilter<DataType_>::set_blend(Scalar blend)
      60             :   {
      61           3 :     this->blend = blend;
      62           3 :   }
      63             : 
      64             :   template<typename DataType_>
      65           1 :   typename UniversalFixedDelayLineFilter<DataType_>::Scalar UniversalFixedDelayLineFilter<DataType_>::get_blend() const
      66             :   {
      67           1 :     return blend;
      68             :   }
      69             : 
      70             :   template<typename DataType_>
      71           5 :   void UniversalFixedDelayLineFilter<DataType_>::set_feedback(Scalar feedback)
      72             :   {
      73           5 :     if(std::abs(feedback) >= 1)
      74             :     {
      75           2 :       throw ATK::RuntimeError("Feedback must be between -1 and 1 to avoid divergence");
      76             :     }
      77           3 :     this->feedback = feedback;
      78           3 :   }
      79             : 
      80             :   template<typename DataType_>
      81           1 :   typename UniversalFixedDelayLineFilter<DataType_>::Scalar UniversalFixedDelayLineFilter<DataType_>::get_feedback() const
      82             :   {
      83           1 :     return feedback;
      84             :   }
      85             : 
      86             :   template<typename DataType_>
      87           3 :   void UniversalFixedDelayLineFilter<DataType_>::set_feedforward(Scalar feedforward)
      88             :   {
      89           3 :     this->feedforward = feedforward;
      90           3 :   }
      91             : 
      92             :   template<typename DataType_>
      93           1 :   typename UniversalFixedDelayLineFilter<DataType_>::Scalar UniversalFixedDelayLineFilter<DataType_>::get_feedforward() const
      94             :   {
      95           1 :     return feedforward;
      96             :   }
      97             : 
      98             :   template<typename DataType_>
      99           5 :   void UniversalFixedDelayLineFilter<DataType_>::full_setup()
     100             :   {
     101             :     // reset the delay line
     102           5 :     impl->delay_line.assign(impl->delay_line.size(), TypeTraits<DataType>::Zero());
     103           5 :     impl->index = 0;
     104           5 :   }
     105             : 
     106             :   template<typename DataType_>
     107        1063 :   void UniversalFixedDelayLineFilter<DataType_>::process_impl(gsl::index size) const
     108             :   {
     109        1063 :     const DataType* ATK_RESTRICT input = converted_inputs[0];
     110        1063 :     DataType* ATK_RESTRICT output = outputs[0];
     111        1063 :     DataType* ATK_RESTRICT processed_input = outputs[1];
     112             : 
     113        1063 :     DataType* ATK_RESTRICT delay_line = impl->delay_line.data();
     114        1063 :     auto delay_line_size = static_cast<gsl::index>(impl->delay_line.size());
     115             : 
     116        1063 :     auto max_index1 = size > delay - impl->index ? size - (delay - impl->index) : 0;
     117        1063 :     auto max_index2 = impl->index < delay ? max_index1 : std::min(size, delay);
     118        1063 :     auto size_before_index = std::min(impl->index, max_index2);
     119        1063 :     auto size_after_index = impl->index < delay ? std::min(size, delay - impl->index) : 0;
     120             : 
     121             :     // Update intermediate input
     122        3563 :     ATK_VECTORIZE for (gsl::index i = 0; i < size_after_index; ++i)
     123             :     {
     124        2500 :       processed_input[i] = input[i] + feedback * delay_line[delay_line_size + i - (delay - impl->index)];
     125             :     }
     126       50184 :     ATK_VECTORIZE for (gsl::index i = 0; i < size_before_index; ++i)
     127             :     {
     128       49121 :       processed_input[i + size_after_index] = input[i + size_after_index] + feedback * delay_line[i + size_after_index + impl->index - delay];
     129             :     }
     130      649375 :     ATK_VECTORIZE for(gsl::index i = delay; i < size; ++i)
     131             :     {
     132      648312 :       processed_input[i] = input[i] + feedback * processed_input[i - delay];
     133             :     }
     134             : 
     135             :     //update output
     136        3563 :     ATK_VECTORIZE for (gsl::index i = 0; i < size_after_index; ++i)
     137             :     {
     138        2500 :       output[i] = blend * processed_input[i] + feedforward * delay_line[delay_line_size + i - (delay - impl->index)];
     139             :     }
     140       50184 :     ATK_VECTORIZE for (gsl::index i = 0; i < size_before_index; ++i)
     141             :     {
     142       49121 :       output[i + size_after_index] = blend * processed_input[i + size_after_index] + feedforward * delay_line[i + size_after_index + impl->index - delay];
     143             :     }
     144      649375 :     ATK_VECTORIZE for (gsl::index i = delay; i < size; ++i)
     145             :     {
     146      648312 :       output[i] = blend * processed_input[i] + feedforward * processed_input[i - delay];
     147             :     }
     148             : 
     149             : 
     150        1063 :     if (size > delay_line_size)
     151             :     {
     152           7 :       impl->index = 0;
     153           7 :       memcpy(reinterpret_cast<void*>(delay_line), reinterpret_cast<const void*>(processed_input + size - delay_line_size), delay_line_size * sizeof(DataType_));
     154             :     }
     155             :     else
     156             :     {
     157        1056 :       auto new_index = std::min(impl->index + size, delay_line_size);
     158        1056 :       auto first_size = new_index - impl->index;
     159        1056 :       memcpy(reinterpret_cast<void*>(delay_line + impl->index), reinterpret_cast<const void*>(processed_input), first_size * sizeof(DataType_));
     160        1056 :       auto second_size = size - first_size;
     161             : 
     162        1056 :       if (impl->index + size > delay_line_size)
     163             :       {
     164         548 :         impl->index = second_size;
     165         548 :         memcpy(reinterpret_cast<void*>(delay_line), reinterpret_cast<const void*>(processed_input + first_size), second_size * sizeof(DataType_));
     166             :       }
     167             :       else
     168             :       {
     169         508 :         impl->index = new_index;
     170             :       }
     171             :     }
     172        1063 :   }
     173             : }