Line data    Source code

       1             : /**
       2             :  * \file helpers.h
       3             :  */
       4             : 
       5             : #ifndef ATK_EQ_HELPERS_H
       6             : #define ATK_EQ_HELPERS_H
       7             : 
       8             : #include <cmath>
       9             : #include <vector>
      10             : 
      11             : #include <boost/math/tools/polynomial.hpp>
      12             : 
      13             : /// Namespace to build filters based on their zpk description
      14             : namespace EQUtilities
      15             : {
      16             :   // Factor paameters
      17             :   template<typename DataType>
      18             :   struct ZPK
      19             :   {
      20             :     std::vector<std::complex<DataType> > z;
      21             :     std::vector<std::complex<DataType> > p;
      22             :     DataType k;
      23             :   };
      24             :   
      25             :   /// Transform the Wn=1 low pass analog filter in a Wn=Wn low pass filter
      26             :   template<typename DataType>
      27         233 :   void zpk_lp2lp(DataType Wn, ZPK<DataType>& zpk)
      28             :   {
      29         233 :     auto z_size = static_cast<gsl::index>(zpk.z.size());
      30         233 :     auto p_size = static_cast<gsl::index>(zpk.p.size());
      31         233 :     auto relative_degree = p_size - z_size;
      32             : 
      33         253 :     for(gsl::index i = 0; i < z_size; ++i)
      34             :     {
      35          20 :       zpk.z[i] *= Wn;
      36             :     }
      37         644 :     for(gsl::index i = 0; i < p_size; ++i)
      38             :     {
      39         411 :       zpk.p[i] *= Wn;
      40             :     }
      41             :     
      42         233 :     zpk.k *= static_cast<DataType>(std::pow(Wn, relative_degree));
      43         233 :   }
      44             : 
      45             :   /// Transform the Wn=1 low pass analog filter in a Wn=Wn, bw=bw band pass filter
      46             :   template<typename DataType>
      47          82 :   void zpk_lp2bp(DataType Wn, DataType bw, ZPK<DataType>& zpk)
      48             :   {
      49          82 :     auto z_size = static_cast<gsl::index>(zpk.z.size());
      50          82 :     auto p_size = static_cast<gsl::index>(zpk.p.size());
      51          82 :     auto relative_degree = p_size - z_size;
      52             : 
      53          92 :     for(gsl::index i = 0; i < z_size; ++i)
      54             :     {
      55          10 :       zpk.z[i] *= bw/2;
      56             :     }
      57         139 :     for(gsl::index i = 0; i < p_size; ++i)
      58             :     {
      59          57 :       zpk.p[i] *= bw/2;
      60             :     }
      61             :     
      62         164 :     std::vector<std::complex<DataType> > zbp;
      63          82 :     std::vector<std::complex<DataType> > pbp;
      64             : 
      65          92 :     for(gsl::index i = 0; i < z_size; ++i)
      66             :     {
      67          10 :       zbp.push_back(zpk.z[i] + std::sqrt(zpk.z[i]*zpk.z[i] - Wn*Wn));
      68          10 :       zbp.push_back(zpk.z[i] - std::sqrt(zpk.z[i]*zpk.z[i] - Wn*Wn));
      69             :     }
      70         139 :     for(gsl::index i = 0; i < p_size; ++i)
      71             :     {
      72          57 :       pbp.push_back(zpk.p[i] + std::sqrt(zpk.p[i]*zpk.p[i] - Wn*Wn));
      73          57 :       pbp.push_back(zpk.p[i] - std::sqrt(zpk.p[i]*zpk.p[i] - Wn*Wn));
      74             :     }
      75             : 
      76          82 :     zbp.resize(zbp.size() + relative_degree, 0);
      77          82 :     zpk.z.swap(zbp);
      78          82 :     zpk.p.swap(pbp);
      79             :     
      80          82 :     zpk.k *= static_cast<DataType>(std::pow(bw, relative_degree));
      81          82 :   }
      82             :   
      83             :   /// Transform the Wn=1 low pass analog filter in a Wn=Wn, bw=bw band stop filter
      84             :   template<typename DataType>
      85          82 :   void zpk_lp2bs(DataType Wn, DataType bw, ZPK<DataType>& zpk)
      86             :   {
      87          82 :     auto z_size = static_cast<gsl::index>(zpk.z.size());
      88          82 :     auto p_size = static_cast<gsl::index>(zpk.p.size());
      89          82 :     auto relative_degree = p_size - z_size;
      90             : 
      91          82 :     std::complex<DataType> f = 1;
      92          92 :     for(gsl::index i = 0; i < z_size; ++i)
      93             :     {
      94          10 :       f *= -zpk.z[i];
      95             :     }
      96         139 :     for(gsl::index i = 0; i < p_size; ++i)
      97             :     {
      98          57 :       f /= -zpk.p[i];
      99             :     }
     100          82 :     zpk.k *= f.real();
     101             : 
     102          92 :     for(gsl::index i = 0; i < z_size; ++i)
     103             :     {
     104          10 :       zpk.z[i] = bw / 2 / zpk.z[i];
     105             :     }
     106         139 :     for(gsl::index i = 0; i < p_size; ++i)
     107             :     {
     108          57 :       zpk.p[i] = bw / 2 / zpk.p[i];
     109             :     }
     110             :     
     111         164 :     std::vector<std::complex<DataType> > zbs;
     112          82 :     zbs.reserve(2 * z_size);
     113         164 :     std::vector<std::complex<DataType> > pbs;
     114          82 :     pbs.reserve(2 * p_size);
     115             :     
     116          92 :     for(gsl::index i = 0; i < z_size; ++i)
     117             :     {
     118          10 :       zbs.push_back(zpk.z[i] + std::sqrt(zpk.z[i]*zpk.z[i] - Wn*Wn));
     119          10 :       zbs.push_back(zpk.z[i] - std::sqrt(zpk.z[i]*zpk.z[i] - Wn*Wn));
     120             :     }
     121         139 :     for(gsl::index i = 0; i < p_size; ++i)
     122             :     {
     123          57 :       pbs.push_back(zpk.p[i] + std::sqrt(zpk.p[i]*zpk.p[i] - Wn*Wn));
     124          57 :       pbs.push_back(zpk.p[i] - std::sqrt(zpk.p[i]*zpk.p[i] - Wn*Wn));
     125             :     }
     126             :     
     127          82 :     zbs.resize(zbs.size() + relative_degree, std::complex<DataType>(0, Wn));
     128          82 :     zbs.resize(zbs.size() + relative_degree, std::complex<DataType>(0, -Wn));
     129          82 :     zpk.z.swap(zbs);
     130          82 :     zpk.p.swap(pbs);
     131          82 :   }
     132             : 
     133             :   /// Apply a bilinear transform on z, p, k
     134             :   template<typename DataType>
     135         417 :   void zpk_bilinear(std::size_t fs, ZPK<DataType>& zpk)
     136             :   {
     137         417 :     auto z_size = static_cast<gsl::index>(zpk.z.size());
     138         417 :     auto p_size = static_cast<gsl::index>(zpk.p.size());
     139             : 
     140         417 :     DataType fs2 = 2 * static_cast<DataType>(fs);
     141             :   
     142         417 :     std::complex<DataType> f = 1;
     143         638 :     for(gsl::index i = 0; i < z_size; ++i)
     144             :     {
     145         221 :       f *= fs2 - zpk.z[i];
     146             :     }
     147        1096 :     for(gsl::index i = 0; i < p_size; ++i)
     148             :     {
     149         679 :       f /= fs2 - zpk.p[i];
     150             :     }
     151         417 :     zpk.k *= f.real();
     152             :     
     153         638 :     for(gsl::index i = 0; i < z_size; ++i)
     154             :     {
     155         221 :       zpk.z[i] = (fs2 + zpk.z[i]) / (fs2 - zpk.z[i]);
     156             :     }
     157        1096 :     for(gsl::index i = 0; i < p_size; ++i)
     158             :     {
     159         679 :       zpk.p[i] = (fs2 + zpk.p[i]) / (fs2 - zpk.p[i]);
     160             :     }
     161             :     
     162         417 :     zpk.z.resize(zpk.p.size(), -1);
     163         417 :   }
     164             : 
     165             :   /// Transforms the z, p, k coefficients in b, a form
     166             :   template<typename DataType>
     167         417 :   void zpk2ba(const ZPK<DataType>& zpk, boost::math::tools::polynomial<DataType>& b, boost::math::tools::polynomial<DataType>& a)
     168             :   {
     169         417 :     auto z_size = static_cast<gsl::index>(zpk.z.size());
     170         417 :     auto p_size = static_cast<gsl::index>(zpk.p.size());
     171             : 
     172         417 :     b = boost::math::tools::polynomial<DataType>({ zpk.k });
     173             : 
     174        1096 :     for(gsl::index i = 0; i < z_size; ++i)
     175             :     {
     176         679 :       if(zpk.z[i].imag() == 0)
     177             :       {
     178        1050 :         boost::math::tools::polynomial<DataType> poly1({ -zpk.z[i].real(), 1 });
     179         525 :         if (b.size() <= 1)
     180             :         {
     181         232 :           b = poly1 * zpk.k;
     182             :         }
     183             :         else
     184             :         {
     185         293 :           b *= poly1;
     186             :         }
     187             :       }
     188         154 :       else if(zpk.z[i].imag() < 0)
     189             :       {
     190         128 :         boost::math::tools::polynomial<DataType> poly2({ zpk.z[i].real() * zpk.z[i].real() + zpk.z[i].imag() * zpk.z[i].imag(), -2 * zpk.z[i].real(), 1 });
     191          64 :         if (b.size() <= 1)
     192             :         {
     193          28 :           b = poly2 * zpk.k;
     194             :         }
     195             :         else
     196             :         {
     197          36 :           b *= poly2;
     198             :         }
     199             :       }
     200             :     }
     201             :     
     202         417 :     a = boost::math::tools::polynomial<DataType>({1});
     203        1096 :     for(gsl::index i = 0; i < p_size; ++i)
     204             :     {
     205         679 :       if(zpk.p[i].imag() == 0)
     206             :       {
     207         416 :         boost::math::tools::polynomial<DataType> poly1({ -zpk.p[i].real(), 1 });
     208         208 :         a *= poly1;
     209             :       }
     210         471 :       else if (zpk.p[i].imag() < 0)
     211             :       {
     212         292 :         boost::math::tools::polynomial<DataType> poly2({ zpk.p[i].real() * zpk.p[i].real() + zpk.p[i].imag() * zpk.p[i].imag(), -2 * zpk.p[i].real(), 1 });
     213         146 :         a *= poly2;
     214             :       }
     215             :     }
     216         417 :   }
     217             :   
     218             :   template<typename DataType, typename Container>
     219         397 :   void to_bilinear(const ZPK<DataType>& zpk, Container& coefficients_in, Container& coefficients_out, size_t order)
     220             :   {
     221         794 :     boost::math::tools::polynomial<DataType> b;
     222         794 :     boost::math::tools::polynomial<DataType> a;
     223             : 
     224         397 :     zpk2ba(zpk, b, a);
     225             :     
     226         397 :     auto in_size = std::min(order + 1, b.size());
     227        1319 :     for (size_t i = 0; i < in_size; ++i)
     228             :     {
     229         922 :       coefficients_in[i] = b[i];
     230             :     }
     231         397 :     auto out_size = std::min(order, a.size() - 1);
     232         857 :     for (size_t i = 0; i < out_size; ++i)
     233             :     {
     234         460 :       coefficients_out[i] = -a[i];
     235             :     }
     236         397 :   }
     237             : 
     238             :   template<typename DataType, typename Container>
     239         233 :   void populate_lp_coeffs(DataType Wn, int fs, size_t order, ZPK<DataType>& zpk, Container& coefficients_in, Container& coefficients_out)
     240             :   {
     241         233 :     DataType warped = 2 * fs * std::tan(boost::math::constants::pi<DataType>() *  Wn / fs);
     242         233 :     zpk_lp2lp(warped, zpk);
     243         233 :     zpk_bilinear(fs, zpk);
     244             :     
     245         233 :     to_bilinear(zpk, coefficients_in, coefficients_out, order);
     246         233 :   }
     247             :   
     248             :   template<typename DataType, typename Container>
     249          82 :   void populate_bp_coeffs(DataType wc1, DataType wc2, int fs, size_t order, ZPK<DataType>& zpk, Container& coefficients_in, Container& coefficients_out)
     250             :   {
     251          82 :     wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
     252          82 :     wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);
     253             :     
     254          82 :     zpk_lp2bp(std::sqrt(wc1 * wc2), wc2 - wc1, zpk);
     255          82 :     zpk_bilinear(fs, zpk);
     256             :     
     257          82 :     to_bilinear(zpk, coefficients_in, coefficients_out, order);
     258          82 :   }
     259             :   
     260             :   template<typename DataType, typename Container>
     261          82 :   void populate_bs_coeffs(DataType wc1, DataType wc2, int fs, size_t order, ZPK<DataType>& zpk, Container& coefficients_in, Container& coefficients_out)
     262             :   {
     263          82 :     wc1 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc1 / fs);
     264          82 :     wc2 = 2 * fs * std::tan(boost::math::constants::pi<DataType>() * wc2 / fs);
     265             :     
     266          82 :     zpk_lp2bs(std::sqrt(wc1 * wc2), wc2 - wc1, zpk);
     267          82 :     zpk_bilinear(fs, zpk);
     268             :     
     269          82 :     to_bilinear(zpk, coefficients_in, coefficients_out, order);
     270          82 :   }
     271             : }
     272             : 
     273             : #endif