filter_sv.c

   1 /* -*- Mode: C ; c-basic-offset: 2 -*- */
   2 /*****************************************************************************
   3  *
   4  *   Copyright (C) 2007,2008,2009 Nedko Arnaudov <nedko@arnaudov.name>
   5  *   Copyright (C) 2002-2005 Nasca Octavian Paul
   6  *
   7  *   This program is free software; you can redistribute it and/or modify
   8  *   it under the terms of the GNU General Public License as published by
   9  *   the Free Software Foundation; version 2 of the License
  10  *
  11  *   This program is distributed in the hope that it will be useful,
  12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *   GNU General Public License for more details.
  15  *
  16  *   You should have received a copy of the GNU General Public License
  17  *   along with this program; if not, write to the Free Software
  18  *   Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
  19  *
  20  *****************************************************************************/
  21
  22 #include <stddef.h>
  23 #include <stdbool.h>
  24 #include <stdlib.h>
  25 #include <assert.h>
  26 #include <math.h>
  27
  28 #include "common.h"
  29 #include "globals.h"
  30 #include "filter_common.h"
  31 #include "filter_sv.h"
  32 #include "util.h"
  33
  34 //#define LOG_LEVEL LOG_LEVEL_DEBUG
  35 #include "log.h"
  36
  37 /* log2(1000) = 9.95748 */
  38 #define ZYN_FILTER_GET_REAL_FREQUENCY(freq_pitch) (pow(2.0, freqpitch + 9.96578428))
  39
  40 struct zyn_filter_sv
  41 {
  42   float sample_rate;
  43   int type;                     /* The type of the filter, one of ZYN_FILTER_SV_TYPE_XXX */
  44   float frequency;              /* -5..5  */
  45   float q_factor;               /* 0..1 */
  46   float frequency_tracking;     /* -1..1 */
  47   int additional_stages;        /* how many times the filter is applied (0->1, 1->2, etc.) */
  48   float gain;                   /* gain, in dB */
  49 };
  50
  51 struct zyn_filter_sv_stage
  52 {
  53   float low;
  54   float high;
  55   float band;
  56   float notch;
  57 };
  58
  59 struct zyn_filter_sv_parameters
  60 {
  61   float f;
  62   float q;
  63   float q_sqrt;
  64 };
  65
  66 struct zyn_filter_sv_processor
  67 {
  68   struct zyn_filter_sv * filter;
  69
  70   float sample_rate;
  71
  72   struct zyn_filter_sv_stage stages[MAX_FILTER_STAGES + 1];
  73
  74   struct zyn_filter_sv_parameters parameters;
  75
  76   zyn_sample_type interpolation_buffer[SOUND_BUFFER_SIZE];
  77
  78   int additional_stages;        /* how many times the filter is applied (0->1, 1->2, etc.) */
  79   float frequency;              /* Frequency, before conversion to Hz */
  80   float frequency_real;         /* Frequency, in Hz */
  81   float q_factor;
  82   float q_factor_computed;
  83   int type;
  84
  85   bool above_nyquist;                 /* this is true if the frequency is above the nyquist */
  86   bool old_above_nyquist;
  87   bool first_time;
  88
  89   float note_base_frequency;
  90   float velocity_adjust;
  91 };
  92
  93 bool
  94 zyn_filter_sv_create(
  95   float sample_rate,
  96   float frequency,
  97   float q_factor,
  98   zyn_filter_sv_handle * handle_ptr)
  99 {
 100   struct zyn_filter_sv * filter_ptr;
 101
 102   filter_ptr = malloc(sizeof(struct zyn_filter_sv));
 103   if (filter_ptr == NULL)
 104   {
 105     return false;
 106   }
 107
 108   filter_ptr->type = ZYN_FILTER_SV_TYPE_LOWPASS;
 109   filter_ptr->sample_rate = sample_rate;
 110   filter_ptr->frequency = frequency;
 111   filter_ptr->q_factor = q_factor;
 112   filter_ptr->frequency_tracking = 0.0;
 113   filter_ptr->additional_stages = 0;
 114   filter_ptr->gain = 0;
 115
 116   *handle_ptr = (zyn_filter_sv_handle)filter_ptr;
 117   return true;
 118 }
 119
 120 #define filter_ptr ((struct zyn_filter_sv *)filter_handle)
 121
 122 int
 123 zyn_filter_sv_get_type(
 124   zyn_filter_sv_handle filter_handle)
 125 {
 126   LOG_DEBUG("SV filter type is %d", filter_ptr->type);
 127   return filter_ptr->type;
 128 }
 129
 130 void
 131 zyn_filter_sv_set_type(
 132   zyn_filter_sv_handle filter_handle,
 133   int type)
 134 {
 135   assert(type >= 0 && type < ZYN_FILTER_SV_TYPES_COUNT);
 136   filter_ptr->type = type;
 137   LOG_DEBUG("SV filter type set to %d", filter_ptr->type);
 138 }
 139
 140 float
 141 zyn_filter_sv_get_frequency(
 142   zyn_filter_sv_handle filter_handle)
 143 {
 144   return filter_ptr->frequency;
 145 }
 146
 147 void
 148 zyn_filter_sv_set_frequency(
 149   zyn_filter_sv_handle filter_handle,
 150   float frequency)
 151 {
 152   filter_ptr->frequency = frequency;
 153 }
 154
 155 float
 156 zyn_filter_sv_get_q_factor(
 157   zyn_filter_sv_handle filter_handle)
 158 {
 159   return filter_ptr->q_factor;
 160 }
 161
 162 void
 163 zyn_filter_sv_set_q_factor(
 164   zyn_filter_sv_handle filter_handle,
 165   float q_factor)
 166 {
 167   filter_ptr->q_factor = q_factor;
 168 }
 169
 170 float
 171 zyn_filter_sv_get_frequency_tracking(
 172   zyn_filter_sv_handle filter_handle)
 173 {
 174   return filter_ptr->frequency_tracking;
 175 }
 176
 177 void
 178 zyn_filter_sv_set_frequency_tracking(
 179   zyn_filter_sv_handle filter_handle,
 180   float frequency_tracking)
 181 {
 182   filter_ptr->frequency_tracking = frequency_tracking;
 183 }
 184
 185 float
 186 zyn_filter_sv_get_gain(
 187   zyn_filter_sv_handle filter_handle)
 188 {
 189   return filter_ptr->gain;
 190 }
 191
 192 void
 193 zyn_filter_sv_set_gain(
 194   zyn_filter_sv_handle filter_handle,
 195   float gain)
 196 {
 197   filter_ptr->gain = gain;
 198 }
 199
 200 int
 201 zyn_filter_sv_get_stages(
 202   zyn_filter_sv_handle filter_handle)
 203 {
 204   return filter_ptr->additional_stages + 1;
 205 }
 206
 207 void
 208 zyn_filter_sv_set_stages(
 209   zyn_filter_sv_handle filter_handle,
 210   int stages)
 211 {
 212   assert(stages > 0);
 213   assert(stages <= MAX_FILTER_STAGES);
 214   filter_ptr->additional_stages  = stages - 1;
 215 }
 216
 217 void
 218 zyn_filter_sv_destroy(
 219   zyn_filter_sv_handle filter_handle)
 220 {
 221   free(filter_ptr);
 222 }
 223
 224 void
 225 zyn_filter_sv_processor_cleanup(
 226   struct zyn_filter_sv_processor * processor_ptr)
 227 {
 228   int i;
 229
 230   for (i = 0 ; i < MAX_FILTER_STAGES + 1 ; i++)
 231   {
 232     processor_ptr->stages[i].low = 0.0;
 233     processor_ptr->stages[i].high = 0.0;
 234     processor_ptr->stages[i].band = 0.0;
 235     processor_ptr->stages[i].notch = 0.0;
 236   }
 237
 238   processor_ptr->old_above_nyquist = false;
 239   processor_ptr->above_nyquist = false;
 240 }
 241
 242 void
 243 zyn_filter_sv_processor_compute_coefs(
 244   float sample_rate,
 245   float frequency,
 246   float q_factor,
 247   int additional_stages,
 248   struct zyn_filter_sv_parameters * parameters_ptr)
 249 {
 250   parameters_ptr->f = frequency / sample_rate * 4.0;
 251
 252   if (parameters_ptr->f > 0.99999)
 253   {
 254     parameters_ptr->f = 0.99999;
 255   }
 256
 257   parameters_ptr->q = 1.0 - atan(sqrt(q_factor)) * 2.0 / PI;
 258   parameters_ptr->q = pow(parameters_ptr->q, 1.0 / (additional_stages + 1));
 259   parameters_ptr->q_sqrt = sqrt(parameters_ptr->q);
 260 }
 261
 262 bool
 263 zyn_filter_sv_processor_create(
 264   zyn_filter_sv_handle filter_handle,
 265   zyn_filter_processor_handle * processor_handle_ptr)
 266 {
 267   struct zyn_filter_sv_processor * processor_ptr;
 268
 269   processor_ptr = malloc(sizeof(struct zyn_filter_sv_processor));
 270   if (processor_ptr == NULL)
 271   {
 272     return false;
 273   }
 274
 275   processor_ptr->filter = filter_ptr;
 276   processor_ptr->sample_rate = filter_ptr->sample_rate;
 277
 278   *processor_handle_ptr = (zyn_filter_processor_handle)processor_ptr;
 279   return true;
 280 }
 281
 282 #define processor_ptr ((struct zyn_filter_sv_processor *)processor_handle)
 283
 284 void
 285 zyn_filter_sv_processor_destroy(
 286   zyn_filter_processor_handle processor_handle)
 287 {
 288   free(processor_ptr);
 289 }
 290
 291 void
 292 zyn_filter_sv_process_single(
 293   int filter_type,
 294   zyn_sample_type * samples,
 295   struct zyn_filter_sv_stage * stage_ptr,
 296   struct zyn_filter_sv_parameters * parameters_ptr)
 297 {
 298   int i;
 299   float * out_ptr;
 300
 301   switch (filter_type)
 302   {
 303   case ZYN_FILTER_SV_TYPE_LOWPASS:
 304     out_ptr = &stage_ptr->low;
 305     break;
 306   case ZYN_FILTER_SV_TYPE_HIGHPASS:
 307     out_ptr = &stage_ptr->high;
 308     break;
 309   case ZYN_FILTER_SV_TYPE_BANDPASS:
 310     out_ptr = &stage_ptr->band;
 311     break;
 312   case ZYN_FILTER_SV_TYPE_NOTCH:
 313     out_ptr = &stage_ptr->notch;
 314     break;
 315   default:
 316     assert(0);
 317     return;
 318   }
 319
 320   for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 321   {
 322     stage_ptr->low = stage_ptr->low + parameters_ptr->f * stage_ptr->band;
 323     stage_ptr->high = parameters_ptr->q_sqrt * samples[i] - stage_ptr->low - parameters_ptr->q * stage_ptr->band;
 324     stage_ptr->band = parameters_ptr->f * stage_ptr->high + stage_ptr->band;
 325     stage_ptr->notch = stage_ptr->high + stage_ptr->low;
 326
 327     samples[i] = *out_ptr;
 328   }
 329 }
 330
 331 void
 332 zyn_filter_sv_processor_init(
 333   zyn_filter_processor_handle processor_handle,
 334   float note_base_frequency,
 335   float velocity_adjust)
 336 {
 337   processor_ptr->note_base_frequency = note_base_frequency;
 338   processor_ptr->velocity_adjust = velocity_adjust;
 339
 340   processor_ptr->first_time = true;
 341 }
 342
 343 void
 344 zyn_filter_sv_process(
 345   zyn_filter_processor_handle processor_handle,
 346   float frequency_adjust,
 347   zyn_sample_type * samples)
 348 {
 349   int i;
 350   float x;
 351   float rap;
 352   bool nyquist_threshold;
 353   bool needs_interpolation;
 354   struct zyn_filter_sv_parameters interpolation_parameters;
 355   float frequency;
 356   float frequency_real;
 357   bool needs_coefs_recalculation;
 358   float gain;
 359   bool frequency_changed;
 360
 361   LOG_DEBUG("---- SV process on %p, frequency_adjust is %f", processor_handle, frequency_adjust);
 362
 363   needs_interpolation = false;
 364   needs_coefs_recalculation = false;
 365
 366   /* center frequency */
 367   frequency = processor_ptr->filter->frequency;
 368
 369   /* frequency tracking */
 370   frequency += log(processor_ptr->note_base_frequency / 440.0) * processor_ptr->filter->frequency_tracking / LOG_2;
 371
 372   /* velocity adjust */
 373   frequency += processor_ptr->velocity_adjust;
 374
 375   /* lfo/envelope adjust */
 376   frequency += frequency_adjust;
 377
 378   frequency_changed = frequency != processor_ptr->frequency;
 379
 380   if (frequency_changed)
 381   {
 382     LOG_DEBUG("Frequency really changed (%f != %f)", frequency, processor_ptr->frequency);
 383   }
 384
 385   frequency_real = 0.0;         /* fix warning */
 386
 387   if (processor_ptr->first_time || frequency_changed)
 388   {
 389     /* convert to real frequency (Hz) */
 390     frequency_real = pow(2.0, frequency + 9.96578428); // log2(1000) = 9.95748
 391
 392     if (frequency_real < 0.1)
 393     {
 394       frequency_real = 0.1;
 395     }
 396   }
 397
 398   /* check if we need interpolation */
 399   if (!processor_ptr->first_time && frequency_changed)
 400   {
 401     if (frequency_real > processor_ptr->frequency_real)
 402     {
 403       rap = frequency_real / processor_ptr->frequency_real;
 404     }
 405     else
 406     {
 407       rap = processor_ptr->frequency_real / frequency_real;
 408     }
 409
 410     processor_ptr->old_above_nyquist = processor_ptr->above_nyquist;
 411     processor_ptr->above_nyquist = frequency_real > (processor_ptr->sample_rate / 2 - 500.0);
 412
 413     nyquist_threshold = ZYN_BOOL_XOR(processor_ptr->above_nyquist, processor_ptr->old_above_nyquist);
 414
 415     // if the frequency is changed fast, it needs interpolation (now, filter and coeficients backup)
 416     if (rap > 3.0 || nyquist_threshold)
 417     {
 418       LOG_DEBUG("needs interpolation");
 419       needs_interpolation = true;
 420       interpolation_parameters = processor_ptr->parameters;
 421     }
 422   }
 423
 424   /* now that we've checked for interpolation, update with current frequency values */
 425
 426   if (processor_ptr->first_time || frequency_changed)
 427   {
 428     LOG_DEBUG("Updating changed frequency");
 429     LOG_DEBUG("Frequency is %f", frequency);
 430     LOG_DEBUG("Frequency real is %f", frequency_real);
 431     processor_ptr->frequency = frequency;
 432     processor_ptr->frequency_real = frequency_real;
 433     LOG_DEBUG("Frequency is %f", processor_ptr->frequency);
 434     LOG_DEBUG("Frequency real is %f", processor_ptr->frequency_real);
 435
 436     needs_coefs_recalculation = true;
 437   }
 438
 439   if (processor_ptr->first_time || processor_ptr->q_factor != processor_ptr->filter->q_factor)
 440   {
 441     LOG_DEBUG("Q factor changed");
 442     processor_ptr->q_factor_computed = exp(pow(processor_ptr->filter->q_factor, 2) * log(1000.0)) - 0.9;
 443     processor_ptr->q_factor = processor_ptr->filter->q_factor;
 444     needs_coefs_recalculation = true;
 445   }
 446
 447   if (processor_ptr->first_time || processor_ptr->additional_stages != processor_ptr->filter->additional_stages)
 448   {
 449     LOG_DEBUG("Additional stages count changed");
 450     zyn_filter_sv_processor_cleanup(processor_ptr);
 451     processor_ptr->additional_stages = processor_ptr->filter->additional_stages;
 452     needs_coefs_recalculation = true;
 453   }
 454
 455   if (processor_ptr->first_time || processor_ptr->type != processor_ptr->filter->type)
 456   {
 457     LOG_DEBUG("Type changed");
 458     processor_ptr->type = processor_ptr->filter->type;
 459     needs_coefs_recalculation = true;
 460   }
 461
 462   if (needs_coefs_recalculation)
 463   {
 464     LOG_DEBUG("recalculating coefficients");
 465     zyn_filter_sv_processor_compute_coefs(
 466       processor_ptr->sample_rate,
 467       frequency_real,
 468       processor_ptr->q_factor_computed,
 469       processor_ptr->additional_stages,
 470       &processor_ptr->parameters);
 471   }
 472
 473   if (needs_interpolation)
 474   {
 475     copy_buffer(processor_ptr->interpolation_buffer, samples, SOUND_BUFFER_SIZE);
 476
 477     for (i = 0 ; i < processor_ptr->additional_stages + 1 ; i++)
 478     {
 479       zyn_filter_sv_process_single(
 480         processor_ptr->filter->type,
 481         processor_ptr->interpolation_buffer,
 482         processor_ptr->stages + i,
 483         &interpolation_parameters);
 484     }
 485   }
 486
 487   for (i = 0 ; i < processor_ptr->additional_stages + 1 ; i++)
 488   {
 489       zyn_filter_sv_process_single(
 490         processor_ptr->filter->type,
 491         samples,
 492         processor_ptr->stages + i,
 493         &processor_ptr->parameters);
 494   }
 495
 496   if (needs_interpolation)
 497   {
 498     for (i = 0 ; i < SOUND_BUFFER_SIZE ; i++)
 499     {
 500       x = i / (float)SOUND_BUFFER_SIZE;
 501       samples[i] = processor_ptr->interpolation_buffer[i] * (1.0 - x) + samples[i] * x;
 502     }
 503   }
 504
 505   gain = dB2rap(processor_ptr->filter->gain);
 506   if (gain > 1.0)
 507   {
 508     gain = sqrt(gain);
 509   }
 510
 511   LOG_DEBUG("Applying gain %f (%f dB)", gain, processor_ptr->filter->gain);
 512
 513   multiply_buffer(samples, gain, SOUND_BUFFER_SIZE);
 514
 515   if (processor_ptr->first_time)
 516   {
 517     processor_ptr->first_time = false;
 518   }
 519 }