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 <algorithm>
  24 #include <array>
  25 #include <cmath>
  26 #include <complex>
  27 #include <cstdlib>
  28 #include <iterator>
  29
  30 #include "alc/effects/base.h"
  31 #include "alcomplex.h"
  32 #include "almalloc.h"
  33 #include "alnumbers.h"
  34 #include "alnumeric.h"
  35 #include "alspan.h"
  36 #include "core/bufferline.h"
  37 #include "core/devformat.h"
  38 #include "core/device.h"
  39 #include "core/effectslot.h"
  40 #include "core/mixer.h"
  41 #include "core/mixer/defs.h"
  42 #include "intrusive_ptr.h"
  43
  44 struct ContextBase;
  45
  46
  47 namespace {
  48
  49 using uint = unsigned int;
  50 using complex_d = std::complex<double>;
  51
  52 #define STFT_SIZE      1024
  53 #define STFT_HALF_SIZE (STFT_SIZE>>1)
  54 #define OVERSAMP       (1<<2)
  55
  56 #define STFT_STEP    (STFT_SIZE / OVERSAMP)
  57 #define FIFO_LATENCY (STFT_STEP * (OVERSAMP-1))
  58
  59 /* Define a Hann window, used to filter the STFT input and output. */
  60 std::array<double,STFT_SIZE> InitHannWindow()
  61 {
  62     std::array<double,STFT_SIZE> ret;
  63     /* Create lookup table of the Hann window for the desired size, i.e. STFT_SIZE */
  64     for(size_t i{0};i < STFT_SIZE>>1;i++)
  65     {
  66         constexpr double scale{al::numbers::pi / double{STFT_SIZE}};
  67         const double val{std::sin(static_cast<double>(i+1) * scale)};
  68         ret[i] = ret[STFT_SIZE-1-i] = val * val;
  69     }
  70     return ret;
  71 }
  72 alignas(16) const std::array<double,STFT_SIZE> HannWindow = InitHannWindow();
  73
  74
  75 struct FrequencyBin {
  76     double Amplitude;
  77     double FreqBin;
  78 };
  79
  80
  81 struct PshifterState final : public EffectState {
  82     /* Effect parameters */
  83     size_t mCount;
  84     size_t mPos;
  85     uint mPitchShiftI;
  86     double mPitchShift;
  87
  88     /* Effects buffers */
  89     std::array<double,STFT_SIZE> mFIFO;
  90     std::array<double,STFT_HALF_SIZE+1> mLastPhase;
  91     std::array<double,STFT_HALF_SIZE+1> mSumPhase;
  92     std::array<double,STFT_SIZE> mOutputAccum;
  93
  94     std::array<complex_d,STFT_SIZE> mFftBuffer;
  95
  96     std::array<FrequencyBin,STFT_HALF_SIZE+1> mAnalysisBuffer;
  97     std::array<FrequencyBin,STFT_HALF_SIZE+1> mSynthesisBuffer;
  98
  99     alignas(16) FloatBufferLine mBufferOut;
 100
 101     /* Effect gains for each output channel */
 102     float mCurrentGains[MAX_OUTPUT_CHANNELS];
 103     float mTargetGains[MAX_OUTPUT_CHANNELS];
 104
 105
 106     void deviceUpdate(const DeviceBase *device, const Buffer &buffer) override;
 107     void update(const ContextBase *context, const EffectSlot *slot, const EffectProps *props,
 108         const EffectTarget target) override;
 109     void process(const size_t samplesToDo, const al::span<const FloatBufferLine> samplesIn,
 110         const al::span<FloatBufferLine> samplesOut) override;
 111
 112     DEF_NEWDEL(PshifterState)
 113 };
 114
 115 void PshifterState::deviceUpdate(const DeviceBase*, const Buffer&)
 116 {
 117     /* (Re-)initializing parameters and clear the buffers. */
 118     mCount       = 0;
 119     mPos         = FIFO_LATENCY;
 120     mPitchShiftI = MixerFracOne;
 121     mPitchShift  = 1.0;
 122
 123     std::fill(mFIFO.begin(),            mFIFO.end(),            0.0);
 124     std::fill(mLastPhase.begin(),       mLastPhase.end(),       0.0);
 125     std::fill(mSumPhase.begin(),        mSumPhase.end(),        0.0);
 126     std::fill(mOutputAccum.begin(),     mOutputAccum.end(),     0.0);
 127     std::fill(mFftBuffer.begin(),       mFftBuffer.end(),       complex_d{});
 128     std::fill(mAnalysisBuffer.begin(),  mAnalysisBuffer.end(),  FrequencyBin{});
 129     std::fill(mSynthesisBuffer.begin(), mSynthesisBuffer.end(), FrequencyBin{});
 130
 131     std::fill(std::begin(mCurrentGains), std::end(mCurrentGains), 0.0f);
 132     std::fill(std::begin(mTargetGains),  std::end(mTargetGains),  0.0f);
 133 }
 134
 135 void PshifterState::update(const ContextBase*, const EffectSlot *slot,
 136     const EffectProps *props, const EffectTarget target)
 137 {
 138     const int tune{props->Pshifter.CoarseTune*100 + props->Pshifter.FineTune};
 139     const float pitch{std::pow(2.0f, static_cast<float>(tune) / 1200.0f)};
 140     mPitchShiftI = fastf2u(pitch*MixerFracOne);
 141     mPitchShift  = mPitchShiftI * double{1.0/MixerFracOne};
 142
 143     const auto coeffs = CalcDirectionCoeffs({0.0f, 0.0f, -1.0f}, 0.0f);
 144
 145     mOutTarget = target.Main->Buffer;
 146     ComputePanGains(target.Main, coeffs.data(), slot->Gain, mTargetGains);
 147 }
 148
 149 void PshifterState::process(const size_t samplesToDo, const al::span<const FloatBufferLine> samplesIn, const al::span<FloatBufferLine> samplesOut)
 150 {
 151     /* Pitch shifter engine based on the work of Stephan Bernsee.
 152      * http://blogs.zynaptiq.com/bernsee/pitch-shifting-using-the-ft/
 153      */
 154
 155     /* Cycle offset per update expected of each frequency bin (bin 0 is none,
 156      * bin 1 is x1, bin 2 is x2, etc).
 157      */
 158     constexpr double expected_cycles{al::numbers::pi*2.0 / OVERSAMP};
 159
 160     for(size_t base{0u};base < samplesToDo;)
 161     {
 162         const size_t todo{minz(STFT_STEP-mCount, samplesToDo-base)};
 163
 164         /* Retrieve the output samples from the FIFO and fill in the new input
 165          * samples.
 166          */
 167         auto fifo_iter = mFIFO.begin()+mPos + mCount;
 168         std::transform(fifo_iter, fifo_iter+todo, mBufferOut.begin()+base,
 169             [](double d) noexcept -> float { return static_cast<float>(d); });
 170
 171         std::copy_n(samplesIn[0].begin()+base, todo, fifo_iter);
 172         mCount += todo;
 173         base += todo;
 174
 175         /* Check whether FIFO buffer is filled with new samples. */
 176         if(mCount < STFT_STEP) break;
 177         mCount = 0;
 178         mPos = (mPos+STFT_STEP) & (mFIFO.size()-1);
 179
 180         /* Time-domain signal windowing, store in FftBuffer, and apply a
 181          * forward FFT to get the frequency-domain signal.
 182          */
 183         for(size_t src{mPos}, k{0u};src < STFT_SIZE;++src,++k)
 184             mFftBuffer[k] = mFIFO[src] * HannWindow[k];
 185         for(size_t src{0u}, k{STFT_SIZE-mPos};src < mPos;++src,++k)
 186             mFftBuffer[k] = mFIFO[src] * HannWindow[k];
 187         forward_fft(mFftBuffer);
 188
 189         /* Analyze the obtained data. Since the real FFT is symmetric, only
 190          * STFT_HALF_SIZE+1 samples are needed.
 191          */
 192         for(size_t k{0u};k < STFT_HALF_SIZE+1;k++)
 193         {
 194             const double amplitude{std::abs(mFftBuffer[k])};
 195             const double phase{std::arg(mFftBuffer[k])};
 196
 197             /* Compute phase difference and subtract expected phase difference */
 198             double tmp{(phase - mLastPhase[k]) - static_cast<double>(k)*expected_cycles};
 199
 200             /* Map delta phase into +/- Pi interval */
 201             int qpd{double2int(tmp / al::numbers::pi)};
 202             tmp -= al::numbers::pi * (qpd + (qpd%2));
 203
 204             /* Get deviation from bin frequency from the +/- Pi interval */
 205             tmp /= expected_cycles;
 206
 207             /* Compute the k-th partials' true frequency and store the
 208              * amplitude and frequency bin in the analysis buffer.
 209              */
 210             mAnalysisBuffer[k].Amplitude = amplitude;
 211             mAnalysisBuffer[k].FreqBin = static_cast<double>(k) + tmp;
 212
 213             /* Store the actual phase[k] for the next frame. */
 214             mLastPhase[k] = phase;
 215         }
 216
 217         /* Shift the frequency bins according to the pitch adjustment,
 218          * accumulating the amplitudes of overlapping frequency bins.
 219          */
 220         std::fill(mSynthesisBuffer.begin(), mSynthesisBuffer.end(), FrequencyBin{});
 221         const size_t bin_count{minz(STFT_HALF_SIZE+1,
 222             (((STFT_HALF_SIZE+1)<<MixerFracBits) - (MixerFracOne>>1) - 1)/mPitchShiftI + 1)};
 223         for(size_t k{0u};k < bin_count;k++)
 224         {
 225             const size_t j{(k*mPitchShiftI + (MixerFracOne>>1)) >> MixerFracBits};
 226             mSynthesisBuffer[j].Amplitude += mAnalysisBuffer[k].Amplitude;
 227             mSynthesisBuffer[j].FreqBin    = mAnalysisBuffer[k].FreqBin * mPitchShift;
 228         }
 229
 230         /* Reconstruct the frequency-domain signal from the adjusted frequency
 231          * bins.
 232          */
 233         for(size_t k{0u};k < STFT_HALF_SIZE+1;k++)
 234         {
 235             /* Calculate actual delta phase and accumulate it to get bin phase */
 236             mSumPhase[k] += mSynthesisBuffer[k].FreqBin * expected_cycles;
 237
 238             mFftBuffer[k] = std::polar(mSynthesisBuffer[k].Amplitude, mSumPhase[k]);
 239         }
 240         for(size_t k{STFT_HALF_SIZE+1};k < STFT_SIZE;++k)
 241             mFftBuffer[k] = std::conj(mFftBuffer[STFT_SIZE-k]);
 242
 243         /* Apply an inverse FFT to get the time-domain siganl, and accumulate
 244          * for the output with windowing.
 245          */
 246         inverse_fft(mFftBuffer);
 247         for(size_t dst{mPos}, k{0u};dst < STFT_SIZE;++dst,++k)
 248             mOutputAccum[dst] += HannWindow[k]*mFftBuffer[k].real() * (4.0/OVERSAMP/STFT_SIZE);
 249         for(size_t dst{0u}, k{STFT_SIZE-mPos};dst < mPos;++dst,++k)
 250             mOutputAccum[dst] += HannWindow[k]*mFftBuffer[k].real() * (4.0/OVERSAMP/STFT_SIZE);
 251
 252         /* Copy out the accumulated result, then clear for the next iteration. */
 253         std::copy_n(mOutputAccum.begin() + mPos, STFT_STEP, mFIFO.begin() + mPos);
 254         std::fill_n(mOutputAccum.begin() + mPos, STFT_STEP, 0.0);
 255     }
 256
 257     /* Now, mix the processed sound data to the output. */
 258     MixSamples({mBufferOut.data(), samplesToDo}, samplesOut, mCurrentGains, mTargetGains,
 259         maxz(samplesToDo, 512), 0);
 260 }
 261
 262
 263 struct PshifterStateFactory final : public EffectStateFactory {
 264     al::intrusive_ptr<EffectState> create() override
 265     { return al::intrusive_ptr<EffectState>{new PshifterState{}}; }
 266 };
 267
 268 } // namespace
 269
 270 EffectStateFactory *PshifterStateFactory_getFactory()
 271 {
 272     static PshifterStateFactory PshifterFactory{};
 273     return &PshifterFactory;
 274 }