deps/libretro-common/audio/dsp_filters/vibrato.c

   1 /* Copyright  (C) 2010-2020 The RetroArch team
   2  *
   3  * ---------------------------------------------------------------------------------------
   4  * The following license statement only applies to this file (vibrato.c).
   5  * ---------------------------------------------------------------------------------------
   6  *
   7  * Permission is hereby granted, free of charge,
   8  * to any person obtaining a copy of this software and associated documentation files (the "Software"),
   9  * to deal in the Software without restriction, including without limitation the rights to
  10  * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
  11  * and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  16  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  19  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  21  */
  22
  23 #include <math.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include <retro_miscellaneous.h>
  28 #include <libretro_dspfilter.h>
  29 #include <string/stdstring.h>
  30
  31 #define sqr(a) ((a) * (a))
  32
  33 #define VIBRATO_BASE_DELAY_SEC 0.002f /* 2 ms */
  34 #define VIBRATO_FREQUENCY_DEFAULT_HZ 2.0f
  35 #define VIBRATO_FREQUENCY_MAX_HZ 14.0f
  36 #define VIBRATO_DEPTH_DEFAULT_PERCENT 50.0f
  37 #define VIBRATO_ADD_DELAY 3
  38
  39 static float hermite_interp(float x, float *y)
  40 {
  41    float c0 = y[1];
  42    float c1 = (1.0f / 2.0f) * (y[2] - y[0]);
  43    float c2 = (y[0] - (5.0f / 2.0f) * y[1]) + (2.0f * y[2] - (1.0f / 2.0f) * y[3]);
  44    float c3 = (1.0f / 2.0f) * (y[3] - y[0]) + (3.0f / 2.0f) * (y[1] - y[2]);
  45    return ((c3 * x + c2) * x + c1) * x + c0;
  46 }
  47
  48 struct vibrato_core
  49 {
  50    float* buffer;
  51    float freq;
  52    float samplerate;
  53    float depth;
  54    int phase;
  55    int writeindex;
  56    int size;
  57 };
  58
  59 struct vibrato
  60 {
  61    struct vibrato_core left, right;
  62 };
  63
  64 static void vibrato_free(void *data)
  65 {
  66    struct vibrato *vib = (struct vibrato*)data;
  67    free(vib->left.buffer);
  68    free(vib->right.buffer);
  69    free(data);
  70 }
  71
  72 static void vibratocore_init(struct vibrato_core *core,float depth,int samplerate,float freq)
  73 {
  74         core->size       = VIBRATO_BASE_DELAY_SEC * samplerate * 2;
  75         core->buffer     = malloc((core->size + VIBRATO_ADD_DELAY) * sizeof(float));
  76         memset(core->buffer, 0, (core->size   + VIBRATO_ADD_DELAY) * sizeof(float));
  77         core->samplerate = samplerate;
  78         core->freq       = freq;
  79         core->depth      = depth;
  80         core->phase      = 0;
  81         core->writeindex = 0;
  82 }
  83
  84 float vibratocore_core(struct vibrato_core *core,float in)
  85 {
  86    int ipart;
  87    float delay, readindex, fpart, value;
  88    float M                        = core->freq / core->samplerate;
  89    int maxphase                   = core->samplerate / core->freq;
  90    float lfo                      = sin(M * 2. * M_PI * core->phase++);
  91    int maxdelay                   = VIBRATO_BASE_DELAY_SEC * core->samplerate;
  92    core->phase                    = core->phase % maxphase;
  93    lfo                            = (lfo + 1) * 1.; // transform from [-1; 1] to [0; 1]
  94    delay                          =  lfo * core->depth * maxdelay;
  95    delay                         += VIBRATO_ADD_DELAY;
  96    readindex                      = core->writeindex - 1 - delay;
  97    while (readindex < 0)
  98       readindex                  += core->size;
  99    while (readindex >= core->size)
 100       readindex                  -= core->size;
 101    ipart                          = (int)readindex;    /* Integer part of the delay */
 102    fpart                          = readindex - ipart; /* fractional part of the delay */
 103    value                          = hermite_interp(fpart, &(core->buffer[ipart]));
 104    core->buffer[core->writeindex] = in;
 105    if (core->writeindex < VIBRATO_ADD_DELAY)
 106       core->buffer[core->size + core->writeindex] = in;
 107    core->writeindex++;
 108    if (core->writeindex == core->size)
 109       core->writeindex = 0;
 110    return value;
 111 }
 112
 113 static void vibrato_process(void *data,
 114       struct dspfilter_output *output,
 115       const struct dspfilter_input *input)
 116 {
 117    unsigned i;
 118    float *out;
 119    struct vibrato *vib = (struct vibrato*)data;
 120
 121    output->samples     = input->samples;
 122    output->frames      = input->frames;
 123    out                 = output->samples;
 124
 125    for (i = 0; i < input->frames; i++, out += 2)
 126    {
 127       float in[2] = { out[0], out[1] };
 128       out[0]      = vibratocore_core(&vib->left, in[0]);
 129       out[1]      = vibratocore_core(&vib->right, in[1]);
 130    }
 131 }
 132
 133 static void *vibrato_init(const struct dspfilter_info *info,
 134       const struct dspfilter_config *config, void *userdata)
 135 {
 136    float freq, depth;
 137    struct vibrato *vib = (struct vibrato*)calloc(1, sizeof(*vib));
 138    if (!vib)
 139       return NULL;
 140
 141    config->get_float(userdata, "freq", &freq,5.0f);
 142    config->get_float(userdata, "depth", &depth, 0.5f);
 143    vibratocore_init(&vib->left,depth,info->input_rate,freq);
 144    vibratocore_init(&vib->right,depth,info->input_rate,freq);
 145    return vib;
 146 }
 147
 148 static const struct dspfilter_implementation vibrato_plug = {
 149    vibrato_init,
 150    vibrato_process,
 151    vibrato_free,
 152
 153    DSPFILTER_API_VERSION,
 154    "Vibrato",
 155    "vibrato",
 156 };
 157
 158 #ifdef HAVE_FILTERS_BUILTIN
 159 #define dspfilter_get_implementation vibrato_dspfilter_get_implementation
 160 #endif
 161
 162 const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask)
 163 {
 164    return &vibrato_plug;
 165 }
 166
 167 #undef dspfilter_get_implementation