streamtools/digital-filter.cpp

   1 #include "digital-filter.hpp"
   2 #include <cmath>
   3 #include <iostream>
   4 #include <stdexcept>
   5
   6 template<> const short field_characteristics<short>::additive_identity = 0;
   7 template<> const short field_characteristics<short>::multiplicative_identity = 1;
   8 template<> const bool field_characteristics<short>::limited = true;
   9 template<> const short field_characteristics<short>::clip_min = -32768;
  10 template<> const short field_characteristics<short>::clip_max = 32767;
  11
  12 template<> const int64_t field_characteristics<int64_t>::additive_identity = 0;
  13 template<> const int64_t field_characteristics<int64_t>::multiplicative_identity = 1;
  14 template<> const bool field_characteristics<int64_t>::limited = true;
  15 //Not exactly correct, but...
  16 template<> const int64_t field_characteristics<int64_t>::clip_min = -9000000000000000000LL;
  17 template<> const int64_t field_characteristics<int64_t>::clip_max = 9000000000000000000LL;
  18
  19 template<> const double field_characteristics<double>::additive_identity = 0;
  20 template<> const double field_characteristics<double>::multiplicative_identity = 1;
  21 template<> const bool field_characteristics<double>::limited = false;
  22 template<> const double field_characteristics<double>::clip_min = 0;
  23 template<> const double field_characteristics<double>::clip_max = 0;
  24
  25 filter::~filter()
  26 {
  27 }
  28
  29
  30 silencer::silencer()
  31 {
  32 }
  33
  34 silencer::~silencer()
  35 {
  36 }
  37
  38 filter_number_t silencer::operator()(filter_number_t input)
  39 {
  40         return 0;
  41 }
  42
  43
  44 amplifier::amplifier(filter_number_t::base_type amount, int type)
  45 {
  46         switch(type) {
  47         case AMPLIFIER_GAIN_LINEAR:
  48                 linear_gain = amount;
  49                 break;
  50         case AMPLIFIER_GAIN_DB:
  51                 linear_gain = pow(10, amount / 10);
  52                 break;
  53         case AMPLIFIER_ATTENUATION_LINEAR:
  54                 linear_gain = 1 / amount;
  55                 break;
  56         case AMPLIFIER_ATTENUATION_DB:
  57                 linear_gain = 1 / pow(10, amount / 10);
  58                 break;
  59         default:
  60                 throw std::runtime_error("Bad amplifier type");
  61         }
  62 }
  63
  64 amplifier::~amplifier()
  65 {
  66 }
  67
  68 filter_number_t amplifier::operator()(filter_number_t input)
  69 {
  70         return linear_gain * input;
  71 }
  72
  73 digital_filter::~digital_filter()
  74 {
  75 }
  76
  77 digital_filter::digital_filter(const std::vector<filter_number_t>& num)
  78 {
  79         init_numerator(num);
  80         filter_denumerator.push_back(1);
  81         old_output.push_back(0);
  82 }
  83
  84 digital_filter::digital_filter(const std::vector<filter_number_t>& num, const std::vector<filter_number_t>& denum)
  85 {
  86         init_numerator(num);
  87         init_denumerator(denum);
  88 }
  89
  90 void digital_filter::init_numerator(const std::vector<filter_number_t>& num)
  91 {
  92         filter_numerator = num;
  93         for(size_t i = 0; i < num.size(); i++)
  94                 old_input.push_back(0);
  95 }
  96
  97 void digital_filter::init_denumerator(const std::vector<filter_number_t>& denum)
  98 {
  99         filter_denumerator = denum;
 100         for(size_t i = 0; i < denum.size(); i++)
 101                 old_output.push_back(0);
 102 }
 103
 104 inline size_t decmod(size_t num, size_t mod)
 105 {
 106         return ((num == 0) ? mod : num) - 1;
 107 }
 108
 109 filter_number_t digital_filter::operator()(filter_number_t input)
 110 {
 111         //Save input.
 112         uint64_t currsamples = sample_count;
 113         size_t input_index = currsamples % old_input.size();
 114         size_t output_index = currsamples % old_output.size();
 115         filter_number_t output;
 116         filter_number_t partial_old_input = 0;
 117         filter_number_t partial_old_output = 0;
 118         size_t i, j;
 119
 120         old_input[input_index] = input;
 121
 122         //Calculate partial old input.
 123         i = input_index;
 124         j = 0;
 125         do {
 126                 partial_old_input = partial_old_input + filter_numerator[j] * old_input[i];
 127                 i = decmod(i, old_input.size());
 128                 j++;
 129         } while(i != input_index);
 130
 131
 132         i = decmod(output_index, old_output.size());
 133         j = 1;
 134         while(i != output_index) {
 135                 partial_old_output = filter_denumerator[j] * old_output[i];
 136                 i = decmod(i, old_output.size());
 137                 j++;
 138         }
 139
 140         //a0*y0+a1*y1+...+an*yn = b0*x0+b1*x1+...+bm*xm
 141         //This can be simplfied to.
 142         //a0*y0+poo = poi = > a0*y0 = poi - poo => y0 = (poi - poo) / a0.
 143         output = (partial_old_input - partial_old_output) / filter_denumerator[0];
 144
 145         old_output[output_index] = output;
 146         sample_count++;
 147         return output;
 148 }
 149
 150 composite_filter::composite_filter()
 151 {
 152 }
 153
 154 composite_filter::~composite_filter()
 155 {
 156         for(std::list<filter*>::iterator i = filters.begin(); i != filters.end(); i++)
 157                 delete *i;
 158 }
 159
 160 void composite_filter::add(filter& filt)
 161 {
 162         filters.push_back(&filt);
 163 }
 164
 165 filter_number_t composite_filter::operator()(filter_number_t input)
 166 {
 167         for(std::list<filter*>::iterator i = filters.begin(); i != filters.end(); i++)
 168                 input = (**i)(input);
 169         return input;
 170 }
 171
 172 trivial_filter::trivial_filter()
 173 {
 174 }
 175
 176 trivial_filter::~trivial_filter()
 177 {
 178 }
 179
 180 filter_number_t trivial_filter::operator()(filter_number_t input)
 181 {
 182         return input;
 183 }
 184
 185 sample_number_t downconvert(filter_number_t input, uint64_t& clipped)
 186 {
 187         sample_number_t::base_type x, y;
 188
 189         if(sample_number_t::characteristics::limited) {
 190                 if(input.get_x() > sample_number_t::characteristics::clip_max) {
 191                         clipped++;
 192                         x = sample_number_t::characteristics::clip_max;
 193                 } else if(input.get_x() < sample_number_t::characteristics::clip_min) {
 194                         clipped++;
 195                         x = sample_number_t::characteristics::clip_min;
 196                 } else
 197                         x = (sample_number_t::base_type)input.get_x();
 198         } else
 199                 x = (sample_number_t::base_type)input.get_x();
 200
 201         if(sample_number_t::characteristics::limited) {
 202                 if(input.get_y() > sample_number_t::characteristics::clip_max) {
 203                         clipped++;
 204                         y = sample_number_t::characteristics::clip_max;
 205                 } else if(input.get_y() < sample_number_t::characteristics::clip_min) {
 206                         clipped++;
 207                         y = sample_number_t::characteristics::clip_min;
 208                 } else
 209                         y = (sample_number_t::base_type)input.get_y();
 210         } else
 211                 y = (sample_number_t::base_type)input.get_y();
 212         return sample_number_t(x, y);
 213 }
 214
 215 filter_number_t upconvert(sample_number_t sample)
 216 {
 217         return filter_number_t(sample.get_x(), sample.get_y());
 218 }
 219
 220 trivial_filter trivial;
 221
 222
 223 mixer::mixer()
 224 {
 225         accumulator = 0;
 226         output_filter = &trivial;
 227         clip_count = 0;
 228 }
 229
 230 mixer::~mixer()
 231 {
 232         for(std::map<uint32_t, filter*>::iterator i = input_filters.begin(); i != input_filters.end(); ++i)
 233                 if(i->second != &trivial)
 234                         delete i->second;
 235         if(output_filter != &trivial)
 236                 delete output_filter;
 237 }
 238
 239 filter& mixer::get_input_filter(uint32_t permchan)
 240 {
 241         if(!input_filters.count(permchan))
 242                 return trivial;
 243         return *input_filters[permchan];
 244 }
 245
 246
 247 filter_number_t mixer::get_input_volume(uint32_t permchan)
 248 {
 249         if(!input_volumes.count(permchan))
 250                 return filter_number_t::one();
 251         return input_volumes[permchan];
 252 }
 253
 254 void mixer::set_input_filter(uint32_t permchan, filter& f)
 255 {
 256         if(input_filters.count(permchan) && input_filters[permchan] != &trivial)
 257                 delete input_filters[permchan];
 258         input_filters[permchan] = &f;
 259 }
 260
 261 void mixer::set_output_filter(filter& f)
 262 {
 263         if(output_filter != &trivial)
 264                 delete output_filter;
 265         output_filter = &f;
 266 }
 267
 268 uint64_t mixer::get_clip_count()
 269 {
 270         return clip_count;
 271 }
 272
 273 void mixer::set_channel_volume(uint32_t permchan, filter_number_t volume)
 274 {
 275         input_volumes[permchan] = volume;
 276 }
 277
 278
 279 void mixer::send_sample(uint32_t permchan, sample_number_t sample)
 280 {
 281         filter_number_t _sample = upconvert(sample) * get_input_volume(permchan);
 282         accumulator = accumulator + get_input_filter(permchan)(_sample);
 283 }
 284
 285 sample_number_t mixer::recv_sample()
 286 {
 287         sample_number_t out = downconvert((*output_filter)(accumulator), clip_count);
 288         accumulator = filter_number_t::zero();
 289         return out;
 290 }