Alc/effects/pshifter.cpp

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