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 #ifdef HAVE_SSE_INTRINSICS
  24 #include <emmintrin.h>
  25 #endif
  26
  27 #include <cmath>
  28 #include <cstdlib>
  29 #include <array>
  30 #include <complex>
  31 #include <algorithm>
  32
  33 #include "al/auxeffectslot.h"
  34 #include "alcmain.h"
  35 #include "alcomplex.h"
  36 #include "alcontext.h"
  37 #include "alnumeric.h"
  38 #include "alu.h"
  39
  40
  41 namespace {
  42
  43 using complex_d = std::complex<double>;
  44
  45 #define STFT_SIZE      1024
  46 #define STFT_HALF_SIZE (STFT_SIZE>>1)
  47 #define OVERSAMP       (1<<2)
  48
  49 #define STFT_STEP    (STFT_SIZE / OVERSAMP)
  50 #define FIFO_LATENCY (STFT_STEP * (OVERSAMP-1))
  51
  52 /* Define a Hann window, used to filter the STFT input and output. */
  53 /* Making this constexpr seems to require C++14. */
  54 std::array<ALdouble,STFT_SIZE> InitHannWindow()
  55 {
  56     std::array<ALdouble,STFT_SIZE> ret;
  57     /* Create lookup table of the Hann window for the desired size, i.e. HIL_SIZE */
  58     for(size_t i{0};i < STFT_SIZE>>1;i++)
  59     {
  60         constexpr double scale{al::MathDefs<double>::Pi() / double{STFT_SIZE-1}};
  61         const double val{std::sin(static_cast<double>(i) * scale)};
  62         ret[i] = ret[STFT_SIZE-1-i] = val * val;
  63     }
  64     return ret;
  65 }
  66 alignas(16) const std::array<ALdouble,STFT_SIZE> HannWindow = InitHannWindow();
  67
  68
  69 struct ALphasor {
  70     ALdouble Amplitude;
  71     ALdouble Phase;
  72 };
  73
  74 struct ALfrequencyDomain {
  75     ALdouble Amplitude;
  76     ALdouble Frequency;
  77 };
  78
  79
  80 /* Converts complex to ALphasor */
  81 inline ALphasor rect2polar(const complex_d &number)
  82 {
  83     ALphasor polar;
  84     polar.Amplitude = std::abs(number);
  85     polar.Phase     = std::arg(number);
  86     return polar;
  87 }
  88
  89 /* Converts ALphasor to complex */
  90 inline complex_d polar2rect(const ALphasor &number)
  91 { return std::polar<double>(number.Amplitude, number.Phase); }
  92
  93
  94 struct PshifterState final : public EffectState {
  95     /* Effect parameters */
  96     size_t  mCount;
  97     ALuint  mPitchShiftI;
  98     ALfloat mPitchShift;
  99     ALfloat mFreqPerBin;
 100
 101     /* Effects buffers */
 102     ALfloat mInFIFO[STFT_SIZE];
 103     ALfloat mOutFIFO[STFT_STEP];
 104     ALdouble mLastPhase[STFT_HALF_SIZE+1];
 105     ALdouble mSumPhase[STFT_HALF_SIZE+1];
 106     ALdouble mOutputAccum[STFT_SIZE];
 107
 108     complex_d mFFTbuffer[STFT_SIZE];
 109
 110     ALfrequencyDomain mAnalysis_buffer[STFT_HALF_SIZE+1];
 111     ALfrequencyDomain mSyntesis_buffer[STFT_HALF_SIZE+1];
 112
 113     alignas(16) ALfloat mBufferOut[BUFFERSIZE];
 114
 115     /* Effect gains for each output channel */
 116     ALfloat mCurrentGains[MAX_OUTPUT_CHANNELS];
 117     ALfloat mTargetGains[MAX_OUTPUT_CHANNELS];
 118
 119
 120     ALboolean deviceUpdate(const ALCdevice *device) override;
 121     void update(const ALCcontext *context, const ALeffectslot *slot, const EffectProps *props, const EffectTarget target) override;
 122     void process(const size_t samplesToDo, const al::span<const FloatBufferLine> samplesIn, const al::span<FloatBufferLine> samplesOut) override;
 123
 124     DEF_NEWDEL(PshifterState)
 125 };
 126
 127 ALboolean PshifterState::deviceUpdate(const ALCdevice *device)
 128 {
 129     /* (Re-)initializing parameters and clear the buffers. */
 130     mCount       = FIFO_LATENCY;
 131     mPitchShiftI = FRACTIONONE;
 132     mPitchShift  = 1.0f;
 133     mFreqPerBin  = static_cast<float>(device->Frequency) / float{STFT_SIZE};
 134
 135     std::fill(std::begin(mInFIFO),          std::end(mInFIFO),          0.0f);
 136     std::fill(std::begin(mOutFIFO),         std::end(mOutFIFO),         0.0f);
 137     std::fill(std::begin(mLastPhase),       std::end(mLastPhase),       0.0);
 138     std::fill(std::begin(mSumPhase),        std::end(mSumPhase),        0.0);
 139     std::fill(std::begin(mOutputAccum),     std::end(mOutputAccum),     0.0);
 140     std::fill(std::begin(mFFTbuffer),       std::end(mFFTbuffer),       complex_d{});
 141     std::fill(std::begin(mAnalysis_buffer), std::end(mAnalysis_buffer), ALfrequencyDomain{});
 142     std::fill(std::begin(mSyntesis_buffer), std::end(mSyntesis_buffer), ALfrequencyDomain{});
 143
 144     std::fill(std::begin(mCurrentGains), std::end(mCurrentGains), 0.0f);
 145     std::fill(std::begin(mTargetGains),  std::end(mTargetGains),  0.0f);
 146
 147     return AL_TRUE;
 148 }
 149
 150 void PshifterState::update(const ALCcontext*, const ALeffectslot *slot, const EffectProps *props, const EffectTarget target)
 151 {
 152     const float pitch{std::pow(2.0f,
 153         static_cast<ALfloat>(props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune) / 1200.0f
 154     )};
 155     mPitchShiftI = fastf2u(pitch*FRACTIONONE);
 156     mPitchShift  = static_cast<float>(mPitchShiftI) * (1.0f/FRACTIONONE);
 157
 158     ALfloat coeffs[MAX_AMBI_CHANNELS];
 159     CalcDirectionCoeffs({0.0f, 0.0f, -1.0f}, 0.0f, coeffs);
 160
 161     mOutTarget = target.Main->Buffer;
 162     ComputePanGains(target.Main, coeffs, slot->Params.Gain, mTargetGains);
 163 }
 164
 165 void PshifterState::process(const size_t samplesToDo, const al::span<const FloatBufferLine> samplesIn, const al::span<FloatBufferLine> samplesOut)
 166 {
 167     /* Pitch shifter engine based on the work of Stephan Bernsee.
 168      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 169      */
 170
 171     static constexpr ALdouble expected{al::MathDefs<double>::Tau() / OVERSAMP};
 172     const ALdouble freq_per_bin{mFreqPerBin};
 173     ALfloat *RESTRICT bufferOut{mBufferOut};
 174     size_t count{mCount};
 175
 176     for(size_t i{0u};i < samplesToDo;)
 177     {
 178         do {
 179             /* Fill FIFO buffer with samples data */
 180             mInFIFO[count] = samplesIn[0][i];
 181             bufferOut[i] = mOutFIFO[count - FIFO_LATENCY];
 182
 183             count++;
 184         } while(++i < samplesToDo && count < STFT_SIZE);
 185
 186         /* Check whether FIFO buffer is filled */
 187         if(count < STFT_SIZE) break;
 188         count = FIFO_LATENCY;
 189
 190         /* Real signal windowing and store in FFTbuffer */
 191         for(ALuint k{0u};k < STFT_SIZE;k++)
 192         {
 193             mFFTbuffer[k].real(mInFIFO[k] * HannWindow[k]);
 194             mFFTbuffer[k].imag(0.0);
 195         }
 196
 197         /* ANALYSIS */
 198         /* Apply FFT to FFTbuffer data */
 199         complex_fft(mFFTbuffer, -1.0);
 200
 201         /* Analyze the obtained data. Since the real FFT is symmetric, only
 202          * STFT_HALF_SIZE+1 samples are needed.
 203          */
 204         for(ALuint k{0u};k < STFT_HALF_SIZE+1;k++)
 205         {
 206             /* Compute amplitude and phase */
 207             ALphasor component{rect2polar(mFFTbuffer[k])};
 208
 209             /* Compute phase difference and subtract expected phase difference */
 210             double tmp{(component.Phase - mLastPhase[k]) - k*expected};
 211
 212             /* Map delta phase into +/- Pi interval */
 213             int qpd{double2int(tmp / al::MathDefs<double>::Pi())};
 214             tmp -= al::MathDefs<double>::Pi() * (qpd + (qpd%2));
 215
 216             /* Get deviation from bin frequency from the +/- Pi interval */
 217             tmp /= expected;
 218
 219             /* Compute the k-th partials' true frequency, twice the amplitude
 220              * for maintain the gain (because half of bins are used) and store
 221              * amplitude and true frequency in analysis buffer.
 222              */
 223             mAnalysis_buffer[k].Amplitude = 2.0 * component.Amplitude;
 224             mAnalysis_buffer[k].Frequency = (k + tmp) * freq_per_bin;
 225
 226             /* Store actual phase[k] for the calculations in the next frame*/
 227             mLastPhase[k] = component.Phase;
 228         }
 229
 230         /* PROCESSING */
 231         /* pitch shifting */
 232         for(ALuint k{0u};k < STFT_HALF_SIZE+1;k++)
 233         {
 234             mSyntesis_buffer[k].Amplitude = 0.0;
 235             mSyntesis_buffer[k].Frequency = 0.0;
 236         }
 237
 238         for(size_t k{0u};k < STFT_HALF_SIZE+1;k++)
 239         {
 240             size_t j{(k*mPitchShiftI) >> FRACTIONBITS};
 241             if(j >= STFT_HALF_SIZE+1) break;
 242
 243             mSyntesis_buffer[j].Amplitude += mAnalysis_buffer[k].Amplitude;
 244             mSyntesis_buffer[j].Frequency  = mAnalysis_buffer[k].Frequency * mPitchShift;
 245         }
 246
 247         /* SYNTHESIS */
 248         /* Synthesis the processing data */
 249         for(ALuint k{0u};k < STFT_HALF_SIZE+1;k++)
 250         {
 251             ALphasor component;
 252             ALdouble tmp;
 253
 254             /* Compute bin deviation from scaled freq */
 255             tmp = mSyntesis_buffer[k].Frequency/freq_per_bin - k;
 256
 257             /* Calculate actual delta phase and accumulate it to get bin phase */
 258             mSumPhase[k] += (k + tmp) * expected;
 259
 260             component.Amplitude = mSyntesis_buffer[k].Amplitude;
 261             component.Phase     = mSumPhase[k];
 262
 263             /* Compute phasor component to cartesian complex number and storage it into FFTbuffer*/
 264             mFFTbuffer[k] = polar2rect(component);
 265         }
 266         /* zero negative frequencies for recontruct a real signal */
 267         for(ALuint k{STFT_HALF_SIZE+1};k < STFT_SIZE;k++)
 268             mFFTbuffer[k] = complex_d{};
 269
 270         /* Apply iFFT to buffer data */
 271         complex_fft(mFFTbuffer, 1.0);
 272
 273         /* Windowing and add to output */
 274         for(ALuint k{0u};k < STFT_SIZE;k++)
 275             mOutputAccum[k] += HannWindow[k] * mFFTbuffer[k].real() /
 276                                (0.5 * STFT_HALF_SIZE * OVERSAMP);
 277
 278         /* Shift accumulator, input & output FIFO */
 279         size_t j, k;
 280         for(k = 0;k < STFT_STEP;k++) mOutFIFO[k] = static_cast<ALfloat>(mOutputAccum[k]);
 281         for(j = 0;k < STFT_SIZE;k++,j++) mOutputAccum[j] = mOutputAccum[k];
 282         for(;j < STFT_SIZE;j++) mOutputAccum[j] = 0.0;
 283         for(k = 0;k < FIFO_LATENCY;k++)
 284             mInFIFO[k] = mInFIFO[k+STFT_STEP];
 285     }
 286     mCount = count;
 287
 288     /* Now, mix the processed sound data to the output. */
 289     MixSamples({bufferOut, samplesToDo}, samplesOut, mCurrentGains, mTargetGains,
 290         maxz(samplesToDo, 512), 0);
 291 }
 292
 293
 294 void Pshifter_setParamf(EffectProps*, ALCcontext *context, ALenum param, ALfloat)
 295 { context->setError(AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param); }
 296 void Pshifter_setParamfv(EffectProps*, ALCcontext *context, ALenum param, const ALfloat*)
 297 { context->setError(AL_INVALID_ENUM, "Invalid pitch shifter float-vector property 0x%04x", param); }
 298
 299 void Pshifter_setParami(EffectProps *props, ALCcontext *context, ALenum param, ALint val)
 300 {
 301     switch(param)
 302     {
 303         case AL_PITCH_SHIFTER_COARSE_TUNE:
 304             if(!(val >= AL_PITCH_SHIFTER_MIN_COARSE_TUNE && val <= AL_PITCH_SHIFTER_MAX_COARSE_TUNE))
 305                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter coarse tune out of range");
 306             props->Pshifter.CoarseTune = val;
 307             break;
 308
 309         case AL_PITCH_SHIFTER_FINE_TUNE:
 310             if(!(val >= AL_PITCH_SHIFTER_MIN_FINE_TUNE && val <= AL_PITCH_SHIFTER_MAX_FINE_TUNE))
 311                 SETERR_RETURN(context, AL_INVALID_VALUE,,"Pitch shifter fine tune out of range");
 312             props->Pshifter.FineTune = val;
 313             break;
 314
 315         default:
 316             context->setError(AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x",
 317                 param);
 318     }
 319 }
 320 void Pshifter_setParamiv(EffectProps *props, ALCcontext *context, ALenum param, const ALint *vals)
 321 { Pshifter_setParami(props, context, param, vals[0]); }
 322
 323 void Pshifter_getParami(const EffectProps *props, ALCcontext *context, ALenum param, ALint *val)
 324 {
 325     switch(param)
 326     {
 327         case AL_PITCH_SHIFTER_COARSE_TUNE:
 328             *val = props->Pshifter.CoarseTune;
 329             break;
 330         case AL_PITCH_SHIFTER_FINE_TUNE:
 331             *val = props->Pshifter.FineTune;
 332             break;
 333
 334         default:
 335             context->setError(AL_INVALID_ENUM, "Invalid pitch shifter integer property 0x%04x",
 336                 param);
 337     }
 338 }
 339 void Pshifter_getParamiv(const EffectProps *props, ALCcontext *context, ALenum param, ALint *vals)
 340 { Pshifter_getParami(props, context, param, vals); }
 341
 342 void Pshifter_getParamf(const EffectProps*, ALCcontext *context, ALenum param, ALfloat*)
 343 { context->setError(AL_INVALID_ENUM, "Invalid pitch shifter float property 0x%04x", param); }
 344 void Pshifter_getParamfv(const EffectProps*, ALCcontext *context, ALenum param, ALfloat*)
 345 { context->setError(AL_INVALID_ENUM, "Invalid pitch shifter float vector-property 0x%04x", param); }
 346
 347 DEFINE_ALEFFECT_VTABLE(Pshifter);
 348
 349
 350 struct PshifterStateFactory final : public EffectStateFactory {
 351     EffectState *create() override;
 352     EffectProps getDefaultProps() const noexcept override;
 353     const EffectVtable *getEffectVtable() const noexcept override { return &Pshifter_vtable; }
 354 };
 355
 356 EffectState *PshifterStateFactory::create()
 357 { return new PshifterState{}; }
 358
 359 EffectProps PshifterStateFactory::getDefaultProps() const noexcept
 360 {
 361     EffectProps props{};
 362     props.Pshifter.CoarseTune = AL_PITCH_SHIFTER_DEFAULT_COARSE_TUNE;
 363     props.Pshifter.FineTune   = AL_PITCH_SHIFTER_DEFAULT_FINE_TUNE;
 364     return props;
 365 }
 366
 367 } // namespace
 368
 369 EffectStateFactory *PshifterStateFactory_getFactory()
 370 {
 371     static PshifterStateFactory PshifterFactory{};
 372     return &PshifterFactory;
 373 }