Alc/effects/pshifter.c

   1 /**
   2  * OpenAL cross platform audio library
   3  * Copyright (C) 2018 by Raul Herraiz.
   4  * This library is free software; you can redistribute it and/or
   5  *  modify it under the terms of the GNU Library General Public
   6  *  License as published by the Free Software Foundation; either
   7  *  version 2 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  *  Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  *  License along with this library; if not, write to the
  16  *  Free Software Foundation, Inc.,
  17  *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  18  * Or go to http://www.gnu.org/copyleft/lgpl.html
  19  */
  20
  21 #include "config.h"
  22
  23 #include <math.h>
  24 #include <stdlib.h>
  25
  26 #include "alMain.h"
  27 #include "alAuxEffectSlot.h"
  28 #include "alError.h"
  29 #include "alu.h"
  30 #include "filters/defs.h"
  31
  32
  33 #define STFT_SIZE      1024
  34 #define STFT_HALF_SIZE (STFT_SIZE>>1)
  35 #define OVERSAMP       (1<<2)
  36
  37 #define STFT_STEP    (STFT_SIZE / OVERSAMP)
  38 #define FIFO_LATENCY (STFT_STEP * (OVERSAMP-1))
  39
  40 typedef struct ALcomplex {
  41     ALfloat Real;
  42     ALfloat Imag;
  43 } ALcomplex;
  44
  45 typedef struct ALphasor {
  46     ALfloat Amplitude;
  47     ALfloat Phase;
  48 } ALphasor;
  49
  50 typedef struct ALFrequencyDomain {
  51     ALfloat Amplitude;
  52     ALfloat Frequency;
  53 } ALfrequencyDomain;
  54
  55 typedef struct ALpshifterState {
  56     DERIVE_FROM_TYPE(ALeffectState);
  57
  58     /* Effect parameters */
  59     ALsizei count;
  60     ALfloat PitchShift;
  61     ALfloat FreqPerBin;
  62
  63     /*Effects buffers*/
  64     ALfloat InFIFO[STFT_SIZE];
  65     ALfloat OutFIFO[STFT_STEP];
  66     ALfloat LastPhase[STFT_HALF_SIZE+1];
  67     ALfloat SumPhase[STFT_HALF_SIZE+1];
  68     ALfloat OutputAccum[STFT_SIZE];
  69
  70     ALcomplex FFTbuffer[STFT_SIZE];
  71
  72     ALfrequencyDomain Analysis_buffer[STFT_HALF_SIZE+1];
  73     ALfrequencyDomain Syntesis_buffer[STFT_HALF_SIZE+1];
  74
  75     alignas(16) ALfloat BufferOut[BUFFERSIZE];
  76
  77     /* Effect gains for each output channel */
  78     ALfloat CurrentGains[MAX_OUTPUT_CHANNELS];
  79     ALfloat TargetGains[MAX_OUTPUT_CHANNELS];
  80 } ALpshifterState;
  81
  82 static ALvoid ALpshifterState_Destruct(ALpshifterState *state);
  83 static ALboolean ALpshifterState_deviceUpdate(ALpshifterState *state, ALCdevice *device);
  84 static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props);
  85 static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels);
  86 DECLARE_DEFAULT_ALLOCATORS(ALpshifterState)
  87
  88 DEFINE_ALEFFECTSTATE_VTABLE(ALpshifterState);
  89
  90
  91 /* Define a Hann window, used to filter the STFT input and output. */
  92 alignas(16) static ALfloat HannWindow[STFT_SIZE];
  93
  94 static void InitHannWindow(void)
  95 {
  96     ALsizei i;
  97
  98     /* Create lookup table of the Hann window for the desired size, i.e. STFT_SIZE */
  99     for(i = 0;i < STFT_SIZE>>1;i++)
 100     {
 101         ALdouble val = sin(M_PI * (ALdouble)i / (ALdouble)(STFT_SIZE-1));
 102         HannWindow[i] = HannWindow[STFT_SIZE-(i+1)] = (ALfloat)(val * val);
 103     }
 104 }
 105 static alonce_flag HannInitOnce = AL_ONCE_FLAG_INIT;
 106
 107
 108 /* Converts ALcomplex to ALphasor */
 109 static inline ALphasor rect2polar(ALcomplex number)
 110 {
 111     ALphasor polar;
 112
 113     polar.Amplitude = sqrtf(number.Real*number.Real + number.Imag*number.Imag);
 114     polar.Phase     = atan2f(number.Imag , number.Real);
 115
 116     return polar;
 117 }
 118
 119 /* Converts ALphasor to ALcomplex */
 120 static inline ALcomplex polar2rect(ALphasor  number)
 121 {
 122     ALcomplex cartesian;
 123
 124     cartesian.Real = number.Amplitude * cosf(number.Phase);
 125     cartesian.Imag = number.Amplitude * sinf(number.Phase);
 126
 127     return cartesian;
 128 }
 129
 130 /* Addition of two complex numbers (ALcomplex format) */
 131 static inline ALcomplex complex_add(ALcomplex a, ALcomplex b)
 132 {
 133     ALcomplex result;
 134
 135     result.Real = a.Real + b.Real;
 136     result.Imag = a.Imag + b.Imag;
 137
 138     return result;
 139 }
 140
 141 /* Subtraction of two complex numbers (ALcomplex format) */
 142 static inline ALcomplex complex_sub(ALcomplex a, ALcomplex b)
 143 {
 144     ALcomplex result;
 145
 146     result.Real = a.Real - b.Real;
 147     result.Imag = a.Imag - b.Imag;
 148
 149     return result;
 150 }
 151
 152 /* Multiplication of two complex numbers (ALcomplex format) */
 153 static inline ALcomplex complex_mult(ALcomplex a, ALcomplex b)
 154 {
 155     ALcomplex result;
 156
 157     result.Real = a.Real*b.Real - a.Imag*b.Imag;
 158     result.Imag = a.Imag*b.Real + a.Real*b.Imag;
 159
 160     return result;
 161 }
 162
 163 /* Iterative implementation of 2-radix FFT (In-place algorithm). Sign = -1 is
 164  * FFT and 1 is iFFT (inverse). Fills FFTBuffer[0...FFTSize-1] with the
 165  * Discrete Fourier Transform (DFT) of the time domain data stored in
 166  * FFTBuffer[0...FFTSize-1]. FFTBuffer is an array of complex numbers
 167  * (ALcomplex), FFTSize MUST BE power of two.
 168  */
 169 static inline ALvoid FFT(ALcomplex *FFTBuffer, ALsizei FFTSize, ALfloat Sign)
 170 {
 171     ALsizei i, j, k, mask, step, step2;
 172     ALcomplex temp, u, w;
 173     ALfloat arg;
 174
 175     /* Bit-reversal permutation applied to a sequence of FFTSize items */
 176     for(i = 1;i < FFTSize-1;i++)
 177     {
 178         for(mask = 0x1, j = 0;mask < FFTSize;mask <<= 1)
 179         {
 180             if((i&mask) != 0)
 181                 j++;
 182             j <<= 1;
 183         }
 184         j >>= 1;
 185
 186         if(i < j)
 187         {
 188             temp         = FFTBuffer[i];
 189             FFTBuffer[i] = FFTBuffer[j];
 190             FFTBuffer[j] = temp;
 191         }
 192     }
 193
 194     /* Iterative form of Danielson–Lanczos lemma */
 195     for(i = 1, step = 2;i < FFTSize;i<<=1, step<<=1)
 196     {
 197         step2 = step >> 1;
 198         arg   = F_PI / step2;
 199
 200         w.Real = cosf(arg);
 201         w.Imag = sinf(arg) * Sign;
 202
 203         u.Real = 1.0f;
 204         u.Imag = 0.0f;
 205
 206         for(j = 0;j < step2;j++)
 207         {
 208             for(k = j;k < FFTSize;k+=step)
 209             {
 210                 temp               = complex_mult(FFTBuffer[k+step2], u);
 211                 FFTBuffer[k+step2] = complex_sub(FFTBuffer[k], temp);
 212                 FFTBuffer[k]       = complex_add(FFTBuffer[k], temp);
 213             }
 214
 215             u = complex_mult(u, w);
 216         }
 217     }
 218 }
 219
 220
 221 static void ALpshifterState_Construct(ALpshifterState *state)
 222 {
 223     ALeffectState_Construct(STATIC_CAST(ALeffectState, state));
 224     SET_VTABLE2(ALpshifterState, ALeffectState, state);
 225
 226     alcall_once(&HannInitOnce, InitHannWindow);
 227 }
 228
 229 static ALvoid ALpshifterState_Destruct(ALpshifterState *state)
 230 {
 231     ALeffectState_Destruct(STATIC_CAST(ALeffectState,state));
 232 }
 233
 234 static ALboolean ALpshifterState_deviceUpdate(ALpshifterState *state, ALCdevice *device)
 235 {
 236     /* (Re-)initializing parameters and clear the buffers. */
 237     state->count      = FIFO_LATENCY;
 238     state->PitchShift = 1.0f;
 239     state->FreqPerBin = device->Frequency / (ALfloat)STFT_SIZE;
 240
 241     memset(state->InFIFO,          0, sizeof(state->InFIFO));
 242     memset(state->OutFIFO,         0, sizeof(state->OutFIFO));
 243     memset(state->FFTbuffer,       0, sizeof(state->FFTbuffer));
 244     memset(state->LastPhase,       0, sizeof(state->LastPhase));
 245     memset(state->SumPhase,        0, sizeof(state->SumPhase));
 246     memset(state->OutputAccum,     0, sizeof(state->OutputAccum));
 247     memset(state->Analysis_buffer, 0, sizeof(state->Analysis_buffer));
 248     memset(state->Syntesis_buffer, 0, sizeof(state->Syntesis_buffer));
 249
 250     memset(state->CurrentGains, 0, sizeof(state->CurrentGains));
 251     memset(state->TargetGains,  0, sizeof(state->TargetGains));
 252
 253     return AL_TRUE;
 254 }
 255
 256 static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props)
 257 {
 258     const ALCdevice *device = context->Device;
 259     ALfloat coeffs[MAX_AMBI_COEFFS];
 260
 261     state->PitchShift = powf(2.0f,
 262         (ALfloat)(props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune) / 1200.0f
 263     );
 264
 265     CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs);
 266     ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains);
 267 }
 268
 269 static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels)
 270 {
 271     /* Pitch shifter engine based on the work of Stephan Bernsee.
 272      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 273      */
 274
 275     static const ALfloat expected = F_TAU / (ALfloat)OVERSAMP;
 276     const ALfloat freq_per_bin = state->FreqPerBin;
 277     ALfloat *restrict bufferOut = state->BufferOut;
 278     ALsizei count = state->count;
 279     ALsizei i, j, k;
 280
 281     for(i = 0;i < SamplesToDo;)
 282     {
 283         do {
 284             /* Fill FIFO buffer with samples data */
 285             state->InFIFO[count] = SamplesIn[0][i];
 286             bufferOut[i] = state->OutFIFO[count - FIFO_LATENCY];
 287
 288             count++;
 289         } while(++i < SamplesToDo && count < STFT_SIZE);
 290
 291         /* Check whether FIFO buffer is filled */
 292         if(count < STFT_SIZE) break;
 293         count = FIFO_LATENCY;
 294
 295         /* Real signal windowing and store in FFTbuffer */
 296         for(k = 0;k < STFT_SIZE;k++)
 297         {
 298             state->FFTbuffer[k].Real = state->InFIFO[k] * HannWindow[k];
 299             state->FFTbuffer[k].Imag = 0.0f;
 300         }
 301
 302         /* ANALYSIS */
 303         /* Apply FFT to FFTbuffer data */
 304         FFT(state->FFTbuffer, STFT_SIZE, -1.0f);
 305
 306         /* Analyze the obtained data. Since the real FFT is symmetric, only
 307          * STFT_HALF_SIZE+1 samples are needed.
 308          */
 309         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 310         {
 311             ALphasor component;
 312             ALfloat tmp;
 313             ALint qpd;
 314
 315             /* Compute amplitude and phase */
 316             component = rect2polar(state->FFTbuffer[k]);
 317
 318             /* Compute phase difference and subtract expected phase difference */
 319             tmp = (component.Phase - state->LastPhase[k]) - (ALfloat)k*expected;
 320
 321             /* Map delta phase into +/- Pi interval */
 322             qpd = fastf2i(tmp / F_PI);
 323             tmp -= F_PI * (ALfloat)(qpd + (qpd%2));
 324
 325             /* Get deviation from bin frequency from the +/- Pi interval */
 326             tmp /= expected;
 327
 328             /* Compute the k-th partials' true frequency, twice the amplitude
 329              * for maintain the gain (because half of bins are used) and store
 330              * amplitude and true frequency in analysis buffer.
 331              */
 332             state->Analysis_buffer[k].Amplitude = 2.0f * component.Amplitude;
 333             state->Analysis_buffer[k].Frequency = ((ALfloat)k + tmp) * freq_per_bin;
 334
 335             /* Store actual phase[k] for the calculations in the next frame*/
 336             state->LastPhase[k] = component.Phase;
 337         }
 338
 339         /* PROCESSING */
 340         /* pitch shifting */
 341         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 342         {
 343             state->Syntesis_buffer[k].Amplitude = 0.0f;
 344             state->Syntesis_buffer[k].Frequency = 0.0f;
 345         }
 346
 347         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 348         {
 349             j = fastf2i((ALfloat)k * state->PitchShift);
 350             if(j >= STFT_HALF_SIZE+1) break;
 351
 352             state->Syntesis_buffer[j].Amplitude += state->Analysis_buffer[k].Amplitude;
 353             state->Syntesis_buffer[j].Frequency  = state->Analysis_buffer[k].Frequency *
 354                                                    state->PitchShift;
 355         }
 356
 357         /* SYNTHESIS */
 358         /* Synthesis the processing data */
 359         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 360         {
 361             ALphasor component;
 362             ALfloat tmp;
 363
 364             /* Compute bin deviation from scaled freq */
 365             tmp = state->Syntesis_buffer[k].Frequency/freq_per_bin - (ALfloat)k;
 366
 367             /* Calculate actual delta phase and accumulate it to get bin phase */
 368             state->SumPhase[k] += ((ALfloat)k + tmp) * expected;
 369
 370             component.Amplitude = state->Syntesis_buffer[k].Amplitude;
 371             component.Phase     = state->SumPhase[k];
 372
 373             /* Compute phasor component to cartesian complex number and storage it into FFTbuffer*/
 374             state->FFTbuffer[k] = polar2rect(component);
 375         }
 376         /* zero negative frequencies for recontruct a real signal */
 377         for(k = STFT_HALF_SIZE+1;k < STFT_SIZE;k++)
 378         {
 379             state->FFTbuffer[k].Real = 0.0f;
 380             state->FFTbuffer[k].Imag = 0.0f;
 381         }
 382
 383         /* Apply iFFT to buffer data */
 384         FFT(state->FFTbuffer, STFT_SIZE, 1.0f);
 385
 386         /* Windowing and add to output */
 387         for(k = 0;k < STFT_SIZE;k++)
 388             state->OutputAccum[k] += HannWindow[k] * state->FFTbuffer[k].Real /
 389                                      (0.5f * STFT_HALF_SIZE * OVERSAMP);
 390
 391         /* Shift accumulator, input & output FIFO */
 392         for(k = 0;k < STFT_STEP;k++) state->OutFIFO[k] = state->OutputAccum[k];
 393         for(j = 0;k < STFT_SIZE;k++,j++) state->OutputAccum[j] = state->OutputAccum[k];
 394         for(;j < STFT_SIZE;j++) state->OutputAccum[j] = 0.0f;
 395         for(k = 0;k < FIFO_LATENCY;k++)
 396             state->InFIFO[k] = state->InFIFO[k+STFT_STEP];
 397     }
 398     state->count = count;
 399
 400     /* Now, mix the processed sound data to the output. */
 401     MixSamples(bufferOut, NumChannels, SamplesOut, state->CurrentGains, state->TargetGains,
 402                maxi(SamplesToDo, 512), 0, SamplesToDo);
 403 }
 404
 405 typedef struct PshifterStateFactory {
 406     DERIVE_FROM_TYPE(EffectStateFactory);
 407 } PshifterStateFactory;
 408
 409 static ALeffectState *PshifterStateFactory_create(PshifterStateFactory *UNUSED(factory))
 410 {
 411     ALpshifterState *state;
 412
 413     NEW_OBJ0(state, ALpshifterState)();
 414     if(!state) return NULL;
 415
 416     return STATIC_CAST(ALeffectState, state);
 417 }
 418
 419 DEFINE_EFFECTSTATEFACTORY_VTABLE(PshifterStateFactory);
 420
 421 EffectStateFactory *PshifterStateFactory_getFactory(void)
 422 {
 423     static PshifterStateFactory PshifterFactory = { { GET_VTABLE2(PshifterStateFactory, EffectStateFactory) } };
 424
 425     return STATIC_CAST(EffectStateFactory, &PshifterFactory);
 426 }
 427
 428
 429 void ALpshifter_setParamf(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat UNUSED(val))
 430 {
 431     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param );
 432 }
 433
 434 void ALpshifter_setParamfv(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, const ALfloat *UNUSED(vals))
 435 {
 436     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float-vector property 0x%04x", param );
 437 }
 438
 439 void ALpshifter_setParami(ALeffect *effect, ALCcontext *context, ALenum param, ALint val)
 440 {
 441     ALeffectProps *props = &effect->Props;
 442     switch(param)
 443     {
 444         case AL_PITCH_SHIFTER_COARSE_TUNE:
 445             if(!(val >= AL_PITCH_SHIFTER_MIN_COARSE_TUNE && val <= AL_PITCH_SHIFTER_MAX_COARSE_TUNE))
 446                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter coarse tune out of range");
 447             props->Pshifter.CoarseTune = val;
 448             break;
 449
 450         case AL_PITCH_SHIFTER_FINE_TUNE:
 451             if(!(val >= AL_PITCH_SHIFTER_MIN_FINE_TUNE && val <= AL_PITCH_SHIFTER_MAX_FINE_TUNE))
 452                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter fine tune out of range");
 453             props->Pshifter.FineTune = val;
 454             break;
 455
 456         default:
 457             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 458     }
 459 }
 460 void ALpshifter_setParamiv(ALeffect *effect, ALCcontext *context, ALenum param, const ALint *vals)
 461 {
 462     ALpshifter_setParami(effect, context, param, vals[0]);
 463 }
 464
 465 void ALpshifter_getParami(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *val)
 466 {
 467     const ALeffectProps *props = &effect->Props;
 468     switch(param)
 469     {
 470         case AL_PITCH_SHIFTER_COARSE_TUNE:
 471             *val = (ALint)props->Pshifter.CoarseTune;
 472             break;
 473         case AL_PITCH_SHIFTER_FINE_TUNE:
 474             *val = (ALint)props->Pshifter.FineTune;
 475             break;
 476
 477         default:
 478             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 479     }
 480 }
 481 void ALpshifter_getParamiv(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *vals)
 482 {
 483     ALpshifter_getParami(effect, context, param, vals);
 484 }
 485
 486 void ALpshifter_getParamf(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(val))
 487 {
 488     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param);
 489 }
 490
 491 void ALpshifter_getParamfv(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(vals))
 492 {
 493     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float vector-property 0x%04x", param);
 494 }
 495
 496 DEFINE_ALEFFECT_VTABLE(ALpshifter);