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 #include "alcomplex.h"
  33
  34
  35 #define STFT_SIZE      1024
  36 #define STFT_HALF_SIZE (STFT_SIZE>>1)
  37 #define OVERSAMP       (1<<2)
  38
  39 #define STFT_STEP    (STFT_SIZE / OVERSAMP)
  40 #define FIFO_LATENCY (STFT_STEP * (OVERSAMP-1))
  41
  42
  43 typedef struct ALphasor {
  44     ALdouble Amplitude;
  45     ALdouble Phase;
  46 } ALphasor;
  47
  48 typedef struct ALFrequencyDomain {
  49     ALdouble Amplitude;
  50     ALdouble Frequency;
  51 } ALfrequencyDomain;
  52
  53
  54 typedef struct ALpshifterState {
  55     DERIVE_FROM_TYPE(ALeffectState);
  56
  57     /* Effect parameters */
  58     ALsizei count;
  59     ALsizei PitchShiftI;
  60     ALfloat PitchShift;
  61     ALfloat FreqPerBin;
  62
  63     /*Effects buffers*/
  64     ALfloat InFIFO[STFT_SIZE];
  65     ALfloat OutFIFO[STFT_STEP];
  66     ALdouble LastPhase[STFT_HALF_SIZE+1];
  67     ALdouble SumPhase[STFT_HALF_SIZE+1];
  68     ALdouble 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 ALdouble 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-1-i] = val * val;
 103     }
 104 }
 105 static alonce_flag HannInitOnce = AL_ONCE_FLAG_INIT;
 106
 107
 108 static inline ALint double2int(ALdouble d)
 109 {
 110 #if ((defined(__GNUC__) || defined(__clang__)) && (defined(__i386__) || defined(__x86_64__)) && \
 111      !defined(__SSE2_MATH__)) || (defined(_MSC_VER) && defined(_M_IX86_FP) && _M_IX86_FP < 2)
 112     ALint sign, shift;
 113     ALint64 mant;
 114     union {
 115         ALdouble d;
 116         ALint64 i64;
 117     } conv;
 118
 119     conv.d = d;
 120     sign = (conv.i64>>63) | 1;
 121     shift = ((conv.i64>>52)&0x7ff) - (1023+52);
 122
 123     /* Over/underflow */
 124     if(UNLIKELY(shift >= 63 || shift < -52))
 125         return 0;
 126
 127     mant = (conv.i64&I64(0xfffffffffffff)) | I64(0x10000000000000);
 128     if(LIKELY(shift < 0))
 129         return (ALint)(mant >> -shift) * sign;
 130     return (ALint)(mant << shift) * sign;
 131
 132 #else
 133
 134     return (ALint)d;
 135 #endif
 136 }
 137
 138
 139 /* Converts ALcomplex to ALphasor */
 140 static inline ALphasor rect2polar(ALcomplex number)
 141 {
 142     ALphasor polar;
 143
 144     polar.Amplitude = sqrt(number.Real*number.Real + number.Imag*number.Imag);
 145     polar.Phase     = atan2(number.Imag, number.Real);
 146
 147     return polar;
 148 }
 149
 150 /* Converts ALphasor to ALcomplex */
 151 static inline ALcomplex polar2rect(ALphasor number)
 152 {
 153     ALcomplex cartesian;
 154
 155     cartesian.Real = number.Amplitude * cos(number.Phase);
 156     cartesian.Imag = number.Amplitude * sin(number.Phase);
 157
 158     return cartesian;
 159 }
 160
 161
 162 static void ALpshifterState_Construct(ALpshifterState *state)
 163 {
 164     ALeffectState_Construct(STATIC_CAST(ALeffectState, state));
 165     SET_VTABLE2(ALpshifterState, ALeffectState, state);
 166
 167     alcall_once(&HannInitOnce, InitHannWindow);
 168 }
 169
 170 static ALvoid ALpshifterState_Destruct(ALpshifterState *state)
 171 {
 172     ALeffectState_Destruct(STATIC_CAST(ALeffectState,state));
 173 }
 174
 175 static ALboolean ALpshifterState_deviceUpdate(ALpshifterState *state, ALCdevice *device)
 176 {
 177     /* (Re-)initializing parameters and clear the buffers. */
 178     state->count       = FIFO_LATENCY;
 179     state->PitchShiftI = FRACTIONONE;
 180     state->PitchShift  = 1.0f;
 181     state->FreqPerBin  = device->Frequency / (ALfloat)STFT_SIZE;
 182
 183     memset(state->InFIFO,          0, sizeof(state->InFIFO));
 184     memset(state->OutFIFO,         0, sizeof(state->OutFIFO));
 185     memset(state->FFTbuffer,       0, sizeof(state->FFTbuffer));
 186     memset(state->LastPhase,       0, sizeof(state->LastPhase));
 187     memset(state->SumPhase,        0, sizeof(state->SumPhase));
 188     memset(state->OutputAccum,     0, sizeof(state->OutputAccum));
 189     memset(state->Analysis_buffer, 0, sizeof(state->Analysis_buffer));
 190     memset(state->Syntesis_buffer, 0, sizeof(state->Syntesis_buffer));
 191
 192     memset(state->CurrentGains, 0, sizeof(state->CurrentGains));
 193     memset(state->TargetGains,  0, sizeof(state->TargetGains));
 194
 195     return AL_TRUE;
 196 }
 197
 198 static ALvoid ALpshifterState_update(ALpshifterState *state, const ALCcontext *context, const ALeffectslot *slot, const ALeffectProps *props)
 199 {
 200     const ALCdevice *device = context->Device;
 201     ALfloat coeffs[MAX_AMBI_COEFFS];
 202     float pitch;
 203
 204     pitch = powf(2.0f,
 205         (ALfloat)(props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune) / 1200.0f
 206     );
 207     state->PitchShiftI = fastf2i(pitch*FRACTIONONE);
 208     state->PitchShift  = state->PitchShiftI * (1.0f/FRACTIONONE);
 209
 210     CalcAngleCoeffs(0.0f, 0.0f, 0.0f, coeffs);
 211     ComputePanGains(&device->Dry, coeffs, slot->Params.Gain, state->TargetGains);
 212 }
 213
 214 static ALvoid ALpshifterState_process(ALpshifterState *state, ALsizei SamplesToDo, const ALfloat (*restrict SamplesIn)[BUFFERSIZE], ALfloat (*restrict SamplesOut)[BUFFERSIZE], ALsizei NumChannels)
 215 {
 216     /* Pitch shifter engine based on the work of Stephan Bernsee.
 217      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 218      */
 219
 220     static const ALdouble expected = M_PI*2.0 / OVERSAMP;
 221     const ALdouble freq_per_bin = state->FreqPerBin;
 222     ALfloat *restrict bufferOut = state->BufferOut;
 223     ALsizei count = state->count;
 224     ALsizei i, j, k;
 225
 226     for(i = 0;i < SamplesToDo;)
 227     {
 228         do {
 229             /* Fill FIFO buffer with samples data */
 230             state->InFIFO[count] = SamplesIn[0][i];
 231             bufferOut[i] = state->OutFIFO[count - FIFO_LATENCY];
 232
 233             count++;
 234         } while(++i < SamplesToDo && count < STFT_SIZE);
 235
 236         /* Check whether FIFO buffer is filled */
 237         if(count < STFT_SIZE) break;
 238         count = FIFO_LATENCY;
 239
 240         /* Real signal windowing and store in FFTbuffer */
 241         for(k = 0;k < STFT_SIZE;k++)
 242         {
 243             state->FFTbuffer[k].Real = state->InFIFO[k] * HannWindow[k];
 244             state->FFTbuffer[k].Imag = 0.0;
 245         }
 246
 247         /* ANALYSIS */
 248         /* Apply FFT to FFTbuffer data */
 249         complex_fft(state->FFTbuffer, STFT_SIZE, -1.0);
 250
 251         /* Analyze the obtained data. Since the real FFT is symmetric, only
 252          * STFT_HALF_SIZE+1 samples are needed.
 253          */
 254         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 255         {
 256             ALphasor component;
 257             ALdouble tmp;
 258             ALint qpd;
 259
 260             /* Compute amplitude and phase */
 261             component = rect2polar(state->FFTbuffer[k]);
 262
 263             /* Compute phase difference and subtract expected phase difference */
 264             tmp = (component.Phase - state->LastPhase[k]) - k*expected;
 265
 266             /* Map delta phase into +/- Pi interval */
 267             qpd = double2int(tmp / M_PI);
 268             tmp -= M_PI * (qpd + (qpd%2));
 269
 270             /* Get deviation from bin frequency from the +/- Pi interval */
 271             tmp /= expected;
 272
 273             /* Compute the k-th partials' true frequency, twice the amplitude
 274              * for maintain the gain (because half of bins are used) and store
 275              * amplitude and true frequency in analysis buffer.
 276              */
 277             state->Analysis_buffer[k].Amplitude = 2.0 * component.Amplitude;
 278             state->Analysis_buffer[k].Frequency = (k + tmp) * freq_per_bin;
 279
 280             /* Store actual phase[k] for the calculations in the next frame*/
 281             state->LastPhase[k] = component.Phase;
 282         }
 283
 284         /* PROCESSING */
 285         /* pitch shifting */
 286         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 287         {
 288             state->Syntesis_buffer[k].Amplitude = 0.0;
 289             state->Syntesis_buffer[k].Frequency = 0.0;
 290         }
 291
 292         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 293         {
 294             j = (k*state->PitchShiftI) >> FRACTIONBITS;
 295             if(j >= STFT_HALF_SIZE+1) break;
 296
 297             state->Syntesis_buffer[j].Amplitude += state->Analysis_buffer[k].Amplitude;
 298             state->Syntesis_buffer[j].Frequency  = state->Analysis_buffer[k].Frequency *
 299                                                    state->PitchShift;
 300         }
 301
 302         /* SYNTHESIS */
 303         /* Synthesis the processing data */
 304         for(k = 0;k < STFT_HALF_SIZE+1;k++)
 305         {
 306             ALphasor component;
 307             ALdouble tmp;
 308
 309             /* Compute bin deviation from scaled freq */
 310             tmp = state->Syntesis_buffer[k].Frequency/freq_per_bin - k;
 311
 312             /* Calculate actual delta phase and accumulate it to get bin phase */
 313             state->SumPhase[k] += (k + tmp) * expected;
 314
 315             component.Amplitude = state->Syntesis_buffer[k].Amplitude;
 316             component.Phase     = state->SumPhase[k];
 317
 318             /* Compute phasor component to cartesian complex number and storage it into FFTbuffer*/
 319             state->FFTbuffer[k] = polar2rect(component);
 320         }
 321         /* zero negative frequencies for recontruct a real signal */
 322         for(k = STFT_HALF_SIZE+1;k < STFT_SIZE;k++)
 323         {
 324             state->FFTbuffer[k].Real = 0.0;
 325             state->FFTbuffer[k].Imag = 0.0;
 326         }
 327
 328         /* Apply iFFT to buffer data */
 329         complex_fft(state->FFTbuffer, STFT_SIZE, 1.0);
 330
 331         /* Windowing and add to output */
 332         for(k = 0;k < STFT_SIZE;k++)
 333             state->OutputAccum[k] += HannWindow[k] * state->FFTbuffer[k].Real /
 334                                      (0.5 * STFT_HALF_SIZE * OVERSAMP);
 335
 336         /* Shift accumulator, input & output FIFO */
 337         for(k = 0;k < STFT_STEP;k++) state->OutFIFO[k] = (ALfloat)state->OutputAccum[k];
 338         for(j = 0;k < STFT_SIZE;k++,j++) state->OutputAccum[j] = state->OutputAccum[k];
 339         for(;j < STFT_SIZE;j++) state->OutputAccum[j] = 0.0;
 340         for(k = 0;k < FIFO_LATENCY;k++)
 341             state->InFIFO[k] = state->InFIFO[k+STFT_STEP];
 342     }
 343     state->count = count;
 344
 345     /* Now, mix the processed sound data to the output. */
 346     MixSamples(bufferOut, NumChannels, SamplesOut, state->CurrentGains, state->TargetGains,
 347                maxi(SamplesToDo, 512), 0, SamplesToDo);
 348 }
 349
 350 typedef struct PshifterStateFactory {
 351     DERIVE_FROM_TYPE(EffectStateFactory);
 352 } PshifterStateFactory;
 353
 354 static ALeffectState *PshifterStateFactory_create(PshifterStateFactory *UNUSED(factory))
 355 {
 356     ALpshifterState *state;
 357
 358     NEW_OBJ0(state, ALpshifterState)();
 359     if(!state) return NULL;
 360
 361     return STATIC_CAST(ALeffectState, state);
 362 }
 363
 364 DEFINE_EFFECTSTATEFACTORY_VTABLE(PshifterStateFactory);
 365
 366 EffectStateFactory *PshifterStateFactory_getFactory(void)
 367 {
 368     static PshifterStateFactory PshifterFactory = { { GET_VTABLE2(PshifterStateFactory, EffectStateFactory) } };
 369
 370     return STATIC_CAST(EffectStateFactory, &PshifterFactory);
 371 }
 372
 373
 374 void ALpshifter_setParamf(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat UNUSED(val))
 375 {
 376     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param );
 377 }
 378
 379 void ALpshifter_setParamfv(ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, const ALfloat *UNUSED(vals))
 380 {
 381     alSetError( context, AL_INVALID_ENUM, "Invalid pitch shifter float-vector property 0x%04x", param );
 382 }
 383
 384 void ALpshifter_setParami(ALeffect *effect, ALCcontext *context, ALenum param, ALint val)
 385 {
 386     ALeffectProps *props = &effect->Props;
 387     switch(param)
 388     {
 389         case AL_PITCH_SHIFTER_COARSE_TUNE:
 390             if(!(val >= AL_PITCH_SHIFTER_MIN_COARSE_TUNE && val <= AL_PITCH_SHIFTER_MAX_COARSE_TUNE))
 391                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter coarse tune out of range");
 392             props->Pshifter.CoarseTune = val;
 393             break;
 394
 395         case AL_PITCH_SHIFTER_FINE_TUNE:
 396             if(!(val >= AL_PITCH_SHIFTER_MIN_FINE_TUNE && val <= AL_PITCH_SHIFTER_MAX_FINE_TUNE))
 397                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter fine tune out of range");
 398             props->Pshifter.FineTune = val;
 399             break;
 400
 401         default:
 402             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 403     }
 404 }
 405 void ALpshifter_setParamiv(ALeffect *effect, ALCcontext *context, ALenum param, const ALint *vals)
 406 {
 407     ALpshifter_setParami(effect, context, param, vals[0]);
 408 }
 409
 410 void ALpshifter_getParami(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *val)
 411 {
 412     const ALeffectProps *props = &effect->Props;
 413     switch(param)
 414     {
 415         case AL_PITCH_SHIFTER_COARSE_TUNE:
 416             *val = (ALint)props->Pshifter.CoarseTune;
 417             break;
 418         case AL_PITCH_SHIFTER_FINE_TUNE:
 419             *val = (ALint)props->Pshifter.FineTune;
 420             break;
 421
 422         default:
 423             alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x", param);
 424     }
 425 }
 426 void ALpshifter_getParamiv(const ALeffect *effect, ALCcontext *context, ALenum param, ALint *vals)
 427 {
 428     ALpshifter_getParami(effect, context, param, vals);
 429 }
 430
 431 void ALpshifter_getParamf(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(val))
 432 {
 433     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param);
 434 }
 435
 436 void ALpshifter_getParamfv(const ALeffect *UNUSED(effect), ALCcontext *context, ALenum param, ALfloat *UNUSED(vals))
 437 {
 438     alSetError(context, AL_INVALID_ENUM, "Invalid pitch shifter float vector-property 0x%04x", param);
 439 }
 440
 441 DEFINE_ALEFFECT_VTABLE(ALpshifter);