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 Frequency;
  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     ALfloat window[STFT_SIZE];
  70
  71     ALcomplex FFTbuffer[STFT_SIZE];
  72
  73     ALfrequencyDomain Analysis_buffer[STFT_HALF_SIZE+1];
  74     ALfrequencyDomain Syntesis_buffer[STFT_HALF_SIZE+1];
  75
  76     alignas(16) ALfloat BufferOut[BUFFERSIZE];
  77
  78     /* Effect gains for each output channel */
  79     ALfloat CurrentGains[MAX_OUTPUT_CHANNELS];
  80     ALfloat TargetGains[MAX_OUTPUT_CHANNELS];
  81 } ALpshifterState;
  82
  83 static ALvoid ALpshifterState_Destruct(ALpshifterState *state);
  84 static ALboolean ALpshifterState_deviceUpdate(ALpshifterState *state, ALCdevice *device);
  85 static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props);
  86 static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels);
  87 DECLARE_DEFAULT_ALLOCATORS(ALpshifterState)
  88
  89 DEFINE_ALEFFECTSTATE_VTABLE(ALpshifterState);
  90
  91
  92 /* Converts ALcomplex to ALphasor*/
  93 static inline ALphasor rect2polar( ALcomplex number )
  94 {
  95     ALphasor polar;
  96
  97     polar.Amplitude =  sqrtf ( number.Real*number.Real + number.Imag*number.Imag );
  98     polar.Phase     =  atan2f( number.Imag , number.Real );
  99
 100     return polar;
 101 }
 102
 103 /* Converts ALphasor to ALcomplex*/
 104 static inline ALcomplex polar2rect( ALphasor  number )
 105 {
 106     ALcomplex cartesian;
 107
 108     cartesian.Real   = number.Amplitude * cosf( number.Phase );
 109     cartesian.Imag   = number.Amplitude * sinf( number.Phase );
 110
 111     return cartesian;
 112 }
 113
 114 /* Addition of two complex numbers (ALcomplex format)*/
 115 static inline ALcomplex complex_add( ALcomplex a, ALcomplex b )
 116 {
 117     ALcomplex result;
 118
 119     result.Real = ( a.Real + b.Real );
 120     result.Imag = ( a.Imag + b.Imag );
 121
 122     return result;
 123 }
 124
 125 /* Subtraction of two complex numbers (ALcomplex format)*/
 126 static inline ALcomplex complex_sub( ALcomplex a, ALcomplex b )
 127 {
 128     ALcomplex result;
 129
 130     result.Real = ( a.Real - b.Real );
 131     result.Imag = ( a.Imag - b.Imag );
 132
 133     return result;
 134 }
 135
 136 /* Multiplication of two complex numbers (ALcomplex format)*/
 137 static inline ALcomplex complex_mult( ALcomplex a, ALcomplex b )
 138 {
 139     ALcomplex result;
 140
 141     result.Real = ( a.Real * b.Real - a.Imag * b.Imag );
 142     result.Imag = ( a.Imag * b.Real + a.Real * b.Imag );
 143
 144     return result;
 145 }
 146
 147 /* Iterative implementation of 2-radix FFT (In-place algorithm). Sign = -1 is FFT and 1 is
 148    iFFT (inverse). Fills FFTBuffer[0...FFTSize-1] with the Discrete Fourier Transform (DFT)
 149    of the time domain data stored in FFTBuffer[0...FFTSize-1]. FFTBuffer is an array of
 150    complex numbers (ALcomplex), FFTSize MUST BE power of two.*/
 151 static inline ALvoid FFT(ALcomplex *FFTBuffer, ALsizei FFTSize, ALfloat Sign)
 152 {
 153     ALfloat arg;
 154     ALsizei i, j, k, mask, step, step2;
 155     ALcomplex temp, u, w;
 156
 157     /*bit-reversal permutation applied to a sequence of FFTSize items*/
 158     for (i = 1; i < FFTSize-1; i++ )
 159     {
 160          for ( mask = 0x1, j = 0; mask < FFTSize; mask <<= 1 )
 161          {
 162               if ( ( i & mask ) != 0 ) j++;
 163
 164               j <<= 1;
 165          }
 166
 167          j >>= 1;
 168
 169          if ( i < j )
 170          {
 171               temp         = FFTBuffer[i];
 172               FFTBuffer[i] = FFTBuffer[j];
 173               FFTBuffer[j] = temp;
 174          }
 175     }
 176
 177     /* Iterative form of Danielson–Lanczos lemma */
 178     for ( i = 1, step = 2; i < FFTSize; i<<=1, step <<= 1 )
 179     {
 180          step2  = step >> 1;
 181          arg    = F_PI / step2;
 182
 183          w.Real = cosf( arg );
 184          w.Imag = sinf( arg ) * Sign;
 185
 186          u.Real = 1.0f;
 187          u.Imag = 0.0f;
 188
 189          for ( j = 0; j < step2; j++ )
 190          {
 191              for ( k = j; k < FFTSize; k += step )
 192              {
 193                   temp               = complex_mult( FFTBuffer[k+step2], u );
 194                   FFTBuffer[k+step2] = complex_sub( FFTBuffer[k], temp );
 195                   FFTBuffer[k]       = complex_add( FFTBuffer[k], temp );
 196              }
 197
 198              u = complex_mult(u,w);
 199          }
 200     }
 201 }
 202
 203
 204 static void ALpshifterState_Construct(ALpshifterState *state)
 205 {
 206     ALsizei i;
 207
 208     ALeffectState_Construct(STATIC_CAST(ALeffectState, state));
 209     SET_VTABLE2(ALpshifterState, ALeffectState, state);
 210
 211     /* Create lockup table of the Hann window for the desired size, i.e. STFT_size */
 212     for ( i = 0; i < STFT_SIZE>>1 ; i++ )
 213     {
 214          state->window[i] = state->window[STFT_SIZE-(i+1)]
 215                           = 0.5f * ( 1 - cosf(F_TAU*(ALfloat)i/(ALfloat)(STFT_SIZE-1)));
 216     }
 217 }
 218
 219 static ALvoid ALpshifterState_Destruct(ALpshifterState *state)
 220 {
 221     ALeffectState_Destruct(STATIC_CAST(ALeffectState,state));
 222 }
 223
 224 static ALboolean ALpshifterState_deviceUpdate(ALpshifterState *state, ALCdevice *device)
 225 {
 226     /* (Re-)initializing parameters and clear the buffers. */
 227     state->count      = FIFO_LATENCY;
 228     state->PitchShift = 1.0f;
 229     state->Frequency  = (ALfloat)device->Frequency;
 230
 231     memset(state->InFIFO,          0, sizeof(state->InFIFO));
 232     memset(state->OutFIFO,         0, sizeof(state->OutFIFO));
 233     memset(state->FFTbuffer,       0, sizeof(state->FFTbuffer));
 234     memset(state->LastPhase,       0, sizeof(state->LastPhase));
 235     memset(state->SumPhase,        0, sizeof(state->SumPhase));
 236     memset(state->OutputAccum,     0, sizeof(state->OutputAccum));
 237     memset(state->Analysis_buffer, 0, sizeof(state->Analysis_buffer));
 238     memset(state->Syntesis_buffer, 0, sizeof(state->Syntesis_buffer));
 239
 240     memset(state->CurrentGains, 0, sizeof(state->CurrentGains));
 241     memset(state->TargetGains,  0, sizeof(state->TargetGains));
 242
 243     return AL_TRUE;
 244 }
 245
 246 static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props)
 247 {
 248     const ALCdevice *device = context->Device;
 249     ALfloat coeffs[MAX_AMBI_COEFFS];
 250
 251     state->PitchShift = powf(2.0f,
 252         (ALfloat)(props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune) / 1200.0f
 253     );
 254
 255     CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs);
 256     ComputeDryPanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains);
 257 }
 258
 259 static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels)
 260 {
 261     /* Pitch shifter engine based on the work of Stephan Bernsee.
 262      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 263      */
 264
 265     static const ALfloat expected = F_TAU / (ALfloat)OVERSAMP;
 266     const ALfloat freq_bin = state->Frequency / (ALfloat)STFT_SIZE;
 267     ALfloat *restrict bufferOut = state->BufferOut;
 268     ALsizei i, j, k;
 269
 270     for(i = 0;i < SamplesToDo;)
 271     {
 272         do {
 273             /* Fill FIFO buffer with samples data */
 274             state->InFIFO[state->count] = SamplesIn[0][i];
 275             bufferOut[i] = state->OutFIFO[state->count - FIFO_LATENCY];
 276
 277             state->count++;
 278         } while(++i < SamplesToDo && state->count < STFT_SIZE);
 279
 280         /* Check whether FIFO buffer is filled */
 281         if(state->count < STFT_SIZE) break;
 282
 283         state->count = FIFO_LATENCY;
 284
 285         /* Real signal windowing and store in FFTbuffer */
 286         for(k = 0;k < STFT_SIZE;k++)
 287         {
 288             state->FFTbuffer[k].Real = state->InFIFO[k] * state->window[k];
 289             state->FFTbuffer[k].Imag = 0.0f;
 290         }
 291
 292         /* ANALYSIS */
 293         /* Apply FFT to FFTbuffer data */
 294         FFT(state->FFTbuffer, STFT_SIZE, -1.0f);
 295
 296         /* Analyze the obtained data. Since the real FFT is symmetric, only
 297          * STFT_HALF_SIZE+1 samples are needed.
 298          */
 299         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 300         {
 301             ALphasor component;
 302             ALfloat tmp;
 303
 304             /* Compute amplitude and phase */
 305             component = rect2polar(state->FFTbuffer[k]);
 306
 307             /* Compute phase difference and subtract expected phase difference */
 308             tmp = (component.Phase - state->LastPhase[k]) - (ALfloat)k*expected;
 309
 310             /* Map delta phase into +/- Pi interval */
 311             tmp -= F_PI * (ALfloat)(fastf2i(tmp/F_PI) + (fastf2i(tmp/F_PI)&1));
 312
 313             /* Get deviation from bin frequency from the +/- Pi interval */
 314             tmp /= expected;
 315
 316             /* Compute the k-th partials' true frequency, twice the amplitude
 317              * for maintain the gain (because half of bins are used) and store
 318              * amplitude and true frequency in analysis buffer.
 319              */
 320             state->Analysis_buffer[k].Amplitude = 2.0f * component.Amplitude;
 321             state->Analysis_buffer[k].Frequency = ((ALfloat)k + tmp) * freq_bin;
 322
 323             /* Store actual phase[k] for the calculations in the next frame*/
 324             state->LastPhase[k] = component.Phase;
 325         }
 326
 327         /* PROCESSING */
 328         /* pitch shifting */
 329         memset(state->Syntesis_buffer, 0, sizeof(state->Syntesis_buffer));
 330
 331         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 332         {
 333             j = fastf2i((ALfloat)k * state->PitchShift);
 334             if(j >= STFT_HALF_SIZE+1) break;
 335
 336             state->Syntesis_buffer[j].Amplitude += state->Analysis_buffer[k].Amplitude;
 337             state->Syntesis_buffer[j].Frequency  = state->Analysis_buffer[k].Frequency *
 338                                                    state->PitchShift;
 339         }
 340
 341         /* SYNTHESIS */
 342         /* Synthesis the processing data */
 343         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 344         {
 345             ALphasor component;
 346             ALfloat tmp;
 347
 348             /* Compute bin deviation from scaled freq */
 349             tmp = state->Syntesis_buffer[k].Frequency/freq_bin - (ALfloat)k;
 350
 351             /* Calculate actual delta phase and accumulate it to get bin phase */
 352             state->SumPhase[k] += ((ALfloat)k + tmp) * expected;
 353
 354             component.Amplitude = state->Syntesis_buffer[k].Amplitude;
 355             component.Phase     = state->SumPhase[k];
 356
 357             /* Compute phasor component to cartesian complex number and storage it into FFTbuffer*/
 358             state->FFTbuffer[k] = polar2rect(component);
 359         }
 360
 361         /* zero negative frequencies for recontruct a real signal */
 362         memset(&state->FFTbuffer[STFT_HALF_SIZE+1], 0, (STFT_HALF_SIZE-1)*sizeof(ALcomplex));
 363
 364         /* Apply iFFT to buffer data */
 365         FFT(state->FFTbuffer, STFT_SIZE, 1.0f);
 366
 367         /* Windowing and add to output */
 368         for(k = 0;k < STFT_SIZE;k++)
 369             state->OutputAccum[k] += 2.0f * state->window[k]*state->FFTbuffer[k].Real /
 370                                      (STFT_HALF_SIZE * OVERSAMP);
 371
 372         /* Shift accumulator, input & output FIFO */
 373         for(k = 0;k < STFT_STEP;k++) state->OutFIFO[k] = state->OutputAccum[k];
 374         for(j = 0;k < STFT_SIZE;k++,j++) state->OutputAccum[j] = state->OutputAccum[k];
 375         for(;j < STFT_SIZE;j++) state->OutputAccum[j] = 0.0f;
 376         for(k = 0;k < FIFO_LATENCY;k++)
 377             state->InFIFO[k] = state->InFIFO[k+STFT_STEP];
 378     }
 379
 380     /* Now, mix the processed sound data to the output. */
 381     MixSamples(bufferOut, NumChannels, SamplesOut, state->CurrentGains, state->TargetGains,
 382                maxi(SamplesToDo, 512), 0, SamplesToDo);
 383 }
 384
 385 typedef struct PshifterStateFactory {
 386     DERIVE_FROM_TYPE(EffectStateFactory);
 387 } PshifterStateFactory;
 388
 389 static ALeffectState *PshifterStateFactory_create(PshifterStateFactory *UNUSED(factory))
 390 {
 391     ALpshifterState *state;
 392
 393     NEW_OBJ0(state, ALpshifterState)();
 394     if(!state) return NULL;
 395
 396     return STATIC_CAST(ALeffectState, state);
 397 }
 398
 399 DEFINE_EFFECTSTATEFACTORY_VTABLE(PshifterStateFactory);
 400
 401 EffectStateFactory *PshifterStateFactory_getFactory(void)
 402 {
 403     static PshifterStateFactory PshifterFactory = { { GET_VTABLE2(PshifterStateFactory, EffectStateFactory) } };
 404
 405     return STATIC_CAST(EffectStateFactory, &PshifterFactory);
 406 }
 407
 408
 409 void ALpshifter_setParamf(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat UNUSED(val))
 410 {
 411     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param );
 412 }
 413
 414 void ALpshifter_setParamfv(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, const ALfloat *UNUSED(vals))
 415 {
 416     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float-vector property 0x%04x", param );
 417 }
 418
 419 void ALpshifter_setParami(ALeffect *effect, ALCcontext *context, ALenum param, ALint val)
 420 {
 421     ALeffectProps *props = &effect->Props;
 422     switch(param)
 423     {
 424         case AL_PITCH_SHIFTER_COARSE_TUNE:
 425             if(!(val >= AL_PITCH_SHIFTER_MIN_COARSE_TUNE && val <= AL_PITCH_SHIFTER_MAX_COARSE_TUNE))
 426                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter coarse tune out of range");
 427             props->Pshifter.CoarseTune = val;
 428             break;
 429
 430         case AL_PITCH_SHIFTER_FINE_TUNE:
 431             if(!(val >= AL_PITCH_SHIFTER_MIN_FINE_TUNE && val <= AL_PITCH_SHIFTER_MAX_FINE_TUNE))
 432                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter fine tune out of range");
 433             props->Pshifter.FineTune = val;
 434             break;
 435
 436         default:
 437             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 438     }
 439 }
 440 void ALpshifter_setParamiv(ALeffect *effect, ALCcontext *context, ALenum param, const ALint *vals)
 441 {
 442     ALpshifter_setParami(effect, context, param, vals[0]);
 443 }
 444
 445 void ALpshifter_getParami(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *val)
 446 {
 447     const ALeffectProps *props = &effect->Props;
 448     switch(param)
 449     {
 450         case AL_PITCH_SHIFTER_COARSE_TUNE:
 451             *val = (ALint)props->Pshifter.CoarseTune;
 452             break;
 453         case AL_PITCH_SHIFTER_FINE_TUNE:
 454             *val = (ALint)props->Pshifter.FineTune;
 455             break;
 456
 457         default:
 458             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 459     }
 460 }
 461 void ALpshifter_getParamiv(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *vals)
 462 {
 463     ALpshifter_getParami(effect, context, param, vals);
 464 }
 465
 466 void ALpshifter_getParamf(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(val))
 467 {
 468     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param);
 469 }
 470
 471 void ALpshifter_getParamfv(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(vals))
 472 {
 473     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float vector-property 0x%04x", param);
 474 }
 475
 476 DEFINE_ALEFFECT_VTABLE(ALpshifter);