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