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