123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723 |
- /*******************************************************************************
-
- "A Collection of Useful C++ Classes for Digital Signal Processing"
- By Vinnie Falco
-
- Official project location:
- https://github.com/vinniefalco/DSPFilters
-
- See Documentation.cpp for contact information, notes, and bibliography.
-
- --------------------------------------------------------------------------------
-
- License: MIT License (http://www.opensource.org/licenses/mit-license.php)
- Copyright (c) 2009 by Vinnie Falco
-
- Permission is hereby granted, free of charge, to any person obtaining a copy
- of this software and associated documentation files (the "Software"), to deal
- in the Software without restriction, including without limitation the rights
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- copies of the Software, and to permit persons to whom the Software is
- furnished to do so, subject to the following conditions:
-
- The above copyright notice and this permission notice shall be included in
- all copies or substantial portions of the Software.
-
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- THE SOFTWARE.
-
- *******************************************************************************/
-
- #ifndef DSPFILTERS_UTILITIES_H
- #define DSPFILTERS_UTILITIES_H
-
- #include "Common.h"
-
- namespace Dsp
- {
-
- /*
- * Utilities
- *
- * These routines are handy for manipulating buffers of samples.
- *
- */
-
- //------------------------------------------------------------------------------
-
- // Add src samples to dest, without clip or overflow checking.
- template <class Td,
- class Ts>
- void add(int samples,
- Td* dest,
- Ts const* src,
- int destSkip = 0,
- int srcSkip = 0)
- {
- if (srcSkip != 0 || destSkip != 0)
- {
- ++srcSkip;
- ++destSkip;
- while (--samples >= 0)
- {
- *dest = Td(*src);
- dest += destSkip;
- src += srcSkip;
- }
- }
- else { while (--samples >= 0) *dest++ += Td(*src++); }
- }
-
- // Multichannel add
- template <typename Td,
- typename Ts>
- void add(int channels,
- int samples,
- Td* const* dest,
- Ts const* const* src) { for (int i = channels; --i >= 0;) add(samples, dest[i], src[i]); }
-
- //--------------------------------------------------------------------------
-
- // Copy samples from src to dest, which may not overlap. Performs an implicit
- // type conversion if Ts and Td are different (for example, float to double).
- template <typename Td,
- typename Ts>
- void copy(int samples,
- Td* dest,
- Ts const* src,
- int destSkip = 0,
- int srcSkip = 0)
- {
- if (srcSkip != 0)
- {
- if (destSkip != 0)
- {
- ++srcSkip;
- ++destSkip;
- while (--samples >= 0)
- {
- *dest++ = *src++;
- dest += destSkip;
- src += srcSkip;
- }
- }
- else
- {
- ++srcSkip;
- while (--samples >= 0)
- {
- *dest++ = *src++;
- src += srcSkip;
- }
- }
- }
- else if (destSkip != 0)
- {
- ++destSkip;
- while (--samples >= 0)
- {
- *dest = *src++;
- dest += destSkip;
- }
- }
- else { while (--samples >= 0) *dest++ = *src++; }
- }
-
- // Wrapper that uses memcpy if there is no skip and the types are the same
- template <typename Ty>
- void copy(int samples,
- Ty* dest,
- Ty const* src,
- int destSkip = 0,
- int srcSkip = 0)
- {
- if (destSkip != 0 || srcSkip != 0) copy<Ty, Ty>(samples, dest, src, destSkip, srcSkip);
- else ::memcpy(dest, src, samples * sizeof(src[0]));
- }
-
- // Copy a set of channels from src to dest, with implicit type conversion.
- template <typename Td,
- typename Ts>
- void copy(int channels,
- int samples,
- Td* const* dest,
- Ts const* const* src,
- int destSkip = 0,
- int srcSkip = 0) { for (int i = channels; --i >= 0;) copy(samples, dest[i], src[i], destSkip, srcSkip); }
-
- //--------------------------------------------------------------------------
-
- // Deinterleave channels. Performs implicit type conversion.
- template <typename Td, typename Ts>
- void deinterleave(int channels,
- int samples,
- Td* const* dest,
- Ts const* src)
- {
- assert(channels > 1);
-
- switch (channels)
- {
- case 2:
- {
- Td* l = dest[0];
- Td* r = dest[1];
- int n = (samples + 7) / 8;
- switch (samples % 8)
- {
- case 0: do
- {
- *l++ = *src++;
- *r++ = *src++;
- case 7: *l++ = *src++;
- *r++ = *src++;
- case 6: *l++ = *src++;
- *r++ = *src++;
- case 5: *l++ = *src++;
- *r++ = *src++;
- case 4: *l++ = *src++;
- *r++ = *src++;
- case 3: *l++ = *src++;
- *r++ = *src++;
- case 2: *l++ = *src++;
- *r++ = *src++;
- case 1: *l++ = *src++;
- *r++ = *src++;
- } while (--n > 0);
- default: break;
- }
- }
- break;
-
- default:
- for (int i = channels; --i >= 0;) copy(samples, dest[i], src + i, 0, channels - 1);
- break;
- }
- }
-
- // Convenience for a stereo pair of channels
- template <typename Td,
- typename Ts>
- void deinterleave(int samples,
- Td* left,
- Td* right,
- Ts const* src)
- {
- Td* dest[2];
- dest[0] = left;
- dest[1] = right;
- deinterleave(2, samples, dest, src);
- }
-
- //--------------------------------------------------------------------------
-
- // Fade dest
- template <typename Td,
- typename Ty>
- void fade(int samples,
- Td* dest,
- Ty start = 0,
- Ty end = 1)
- {
- Ty t = start;
- Ty dt = (end - start) / samples;
-
- while (--samples >= 0)
- {
- *dest++ *= t;
- t += dt;
- }
- }
-
- // Fade dest cannels
- template <typename Td,
- typename Ty>
- void fade(int channels,
- int samples,
- Td* const* dest,
- Ty start = 0,
- Ty end = 1) { for (int i = channels; --i >= 0;) fade(samples, dest[i], start, end); }
-
- // Fade src into dest
- template <typename Td,
- typename Ts,
- typename Ty>
- void fade(int samples,
- Td* dest,
- Ts const* src,
- Ty start = 0,
- Ty end = 1)
- {
- Ty t = start;
- Ty dt = (end - start) / samples;
-
- while (--samples >= 0)
- {
- *dest = Td(*dest + t * (*src++ - *dest));
- ++dest;
- t += dt;
- }
- }
-
- // Fade src channels into dest channels
- template <typename Td,
- typename Ts,
- typename Ty>
- void fade(int channels,
- int samples,
- Td* const* dest,
- Ts const* const* src,
- Ty start = 0,
- Ty end = 1) { for (int i = channels; --i >= 0;) fade(samples, dest[i], src[i], start, end); }
-
- //--------------------------------------------------------------------------
-
- // Interleave separate channels from source pointers to destination
- // (Destination requires channels*frames samples of storage). Performs
- // implicit type conversion.
- template <typename Td,
- typename Ts>
- void interleave(int channels,
- size_t samples,
- Td* dest,
- Ts const* const* src)
- {
- assert(channels>1);
-
- if (samples == 0) return;
-
- switch (channels)
- {
- case 2:
- {
- const Ts* l = src[0];
- const Ts* r = src[1];
-
- // note that Duff's Device only works when samples>0
- int n = (samples + 7) / 8;
- switch (samples % 8)
- {
- case 0: do
- {
- *dest++ = *l++;
- *dest++ = *r++;
- case 7: *dest++ = *l++;
- *dest++ = *r++;
- case 6: *dest++ = *l++;
- *dest++ = *r++;
- case 5: *dest++ = *l++;
- *dest++ = *r++;
- case 4: *dest++ = *l++;
- *dest++ = *r++;
- case 3: *dest++ = *l++;
- *dest++ = *r++;
- case 2: *dest++ = *l++;
- *dest++ = *r++;
- case 1: *dest++ = *l++;
- *dest++ = *r++;
- } while (--n > 0);
- default: break;
- }
- }
- break;
-
- default: { for (int i = channels; --i >= 0;) copy(samples, dest + i, src[i], channels - 1, 0); }
- break;
- }
- }
-
- //--------------------------------------------------------------------------
-
- // Convenience for a stereo channel pair
- template <typename Td,
- typename Ts>
- void interleave(int samples,
- Td* dest,
- Ts const* left,
- Ts const* right)
- {
- const Ts* src[2];
- src[0] = left;
- src[1] = right;
- interleave(2, samples, dest, src);
- }
-
- //--------------------------------------------------------------------------
-
- // Multiply samples by a constant, without clip or overflow checking.
- template <typename Td,
- typename Ty>
- void multiply(int samples,
- Td* dest,
- Ty factor,
- int destSkip = 0)
- {
- if (destSkip != 0)
- {
- ++destSkip;
- while (--samples >= 0)
- {
- *dest = Td(*dest * factor);
- dest += destSkip;
- }
- }
- else
- {
- while (--samples >= 0)
- {
- *dest = Td(*dest * factor);
- ++dest;
- }
- }
- }
-
- // Multiply a set of channels by a constant.
- template <typename Td,
- typename Ty>
- void multiply(int channels,
- int samples,
- Td* const* dest,
- Ty factor,
- int destSkip = 0) { for (int i = channels; --i >= 0;) multiply(samples, dest[i], factor, destSkip); }
-
- //--------------------------------------------------------------------------
-
- // Copy samples from src to dest in reversed order. Performs implicit
- // type conversion. src and dest may not overlap.
- template <typename Td,
- typename Ts>
- void reverse(int samples,
- Td* dest,
- Ts const* src,
- int destSkip = 0,
- int srcSkip = 0)
- {
- src += (srcSkip + 1) * samples;
-
- if (srcSkip != 0 || destSkip == 0)
- {
- ++srcSkip;
- ++destSkip;
- while (--samples >= 0)
- {
- src -= srcSkip;
- *dest = *src;
- dest += destSkip;
- }
- }
- else { while (--samples >= 0) *dest++ = *--src; }
- }
-
- template <typename Td, typename Ts>
- void reverse(int channels, size_t frames, Td* const* dest, const Ts* const* src) { for (int i = channels; --i >= 0;) reverse(frames, dest[i], src[i]); }
-
- //--------------------------------------------------------------------------
-
- template <typename Tn>
- void to_mono(int samples, Tn* dest, Tn const* left, Tn const* right)
- {
- #if 1
- while (samples-- > 0) *dest++ = (*left++ + *right++) * Tn(0.70710678118654752440084436210485);
- #else
- while (samples-- > 0)
- *dest++ = (*left++ + *right++) * Tn(0.5);
- #endif
- }
-
- //--------------------------------------------------------------------------
-
- template <typename T>
- void validate(int numChannels, int nSamples, T const* const* src)
- {
- for (int i = 0; i < numChannels; ++i)
- {
- T const* p = src[i];
- for (int j = nSamples; j > 0; --j)
- {
- T v = *p++;
- assert(v < 2 && v > -2);
- }
- }
- }
-
- //--------------------------------------------------------------------------
-
- #if 0
- /*
- * this stuff all depends on is_pod which is not always available
- *
- */
- namespace detail {
-
- template <typename Ty,
- bool isPod>
- struct zero
- {
- static void process (int samples,
- Ty* dest,
- int destSkip)
- {
- if (destSkip != 0)
- {
- ++destSkip;
- while (--samples >= 0)
- {
- *dest = Ty();
- dest += destSkip;
- }
- }
- else
- {
- std::fill (dest, dest + samples, Ty());
- }
- }
- };
-
- template <typename Ty>
- struct zero<Ty, true>
- {
- static void process (int samples,
- Ty* dest,
- int destSkip)
- {
- if (destSkip != 0)
- zero<Ty,false>::process (samples, dest, destSkip);
- else
- ::memset (dest, 0, samples * sizeof(dest[0]));
- }
- };
-
- }
-
- // Fill a channel with zeros. This works even if Ty is not a basic type.
- template <typename Ty>
- void zero (int samples,
- Ty* dest,
- int destSkip = 0)
- {
- detail::zero<Ty, tr1::is_pod<Ty>::value>::process (samples, dest, destSkip );
- }
-
- #else
- // Fill a channel with zeros. This works even if Ty is not a basic type.
- template <typename Ty>
- void zero(int samples,
- Ty* dest,
- int destSkip = 0)
- {
- if (destSkip != 0)
- {
- ++destSkip;
- while (--samples >= 0)
- {
- *dest = Ty();
- dest += destSkip;
- }
- }
- else { std::fill(dest, dest + samples, Ty()); }
- }
-
- #endif
-
- // Fill a set of channels with zero.
- template <typename Ty>
- void zero(int channels,
- int samples,
- Ty* const* dest,
- int destSkip = 0) { for (int i = channels; --i >= 0;) zero(samples, dest[i], destSkip); }
-
- //------------------------------------------------------------------------------
-
- // Implementation of Brent's Method provided by
- // John D. Cook (http://www.johndcook.com/)
- // The return value of Minimize is the minimum of the function f.
- // The location where f takes its minimum is returned in the variable minLoc.
- // Notation and implementation based on Chapter 5 of Richard Brent's book
- // "Algorithms for Minimization Without Derivatives".
- //
- // Reference:
- // http://www.codeproject.com/KB/recipes/one_variable_optimize.aspx?msg=2779038
-
- template <class TFunction>
- double BrentMinimize(TFunction& f, // [in] objective function to minimize
- double leftEnd, // [in] smaller value of bracketing interval
- double rightEnd, // [in] larger value of bracketing interval
- double epsilon, // [in] stopping tolerance
- double& minLoc) // [out] location of minimum
- {
- double e, q, r, u, w, fw, fx;
- static const double c = 0.5 * (3.0 - std::sqrt(5.0));
- static const double SQRT_DBL_EPSILON = std::sqrt(DBL_EPSILON);
-
- double& a = leftEnd;
- double& b = rightEnd;
- double& x = minLoc;
-
- double v = w = x = a + c * (b - a);
- double d = e = 0.0;
- double fv = fw = fx = f(x);
- int counter = 0;
- loop:
- counter++;
- double m = 0.5 * (a + b);
- double tol = SQRT_DBL_EPSILON * fabs(x) + epsilon;
- double t2 = 2.0 * tol;
- // Check stopping criteria
- if (fabs(x - m) > t2 - 0.5 * (b - a))
- {
- double p = q = r = 0.0;
- if (fabs(e) > tol)
- {
- // fit parabola
- r = (x - w) * (fx - fv);
- q = (x - v) * (fx - fw);
- p = (x - v) * q - (x - w) * r;
- q = 2.0 * (q - r);
- (q > 0.0) ? p = -p : q = -q;
- r = e;
- e = d;
- }
- if (fabs(p) < fabs(0.5 * q * r) && p < q * (a - x) && p < q * (b - x))
- {
- // A parabolic interpolation step
- d = p / q;
- u = x + d;
- // f must not be evaluated too close to a or b
- if (u - a < t2 || b - u < t2) d = (x < m) ? tol : -tol;
- }
- else
- {
- // A golden section step
- e = (x < m) ? b : a;
- e -= x;
- d = c * e;
- }
- // f must not be evaluated too close to x
- if (fabs(d) >= tol) u = x + d;
- else if (d > 0.0) u = x + tol;
- else u = x - tol;
- double fu = f(u);
- // Update a, b, v, w, and x
- if (fu <= fx)
- {
- (u < x) ? b = x : a = x;
- v = w;
- fv = fw;
- w = x;
- fw = fx;
- x = u;
- fx = fu;
- }
- else
- {
- (u < x) ? a = u : b = u;
- if (fu <= fw || w == x)
- {
- v = w;
- fv = fw;
- w = u;
- fw = fu;
- }
- else if (fu <= fv || v == x || v == w)
- {
- v = u;
- fv = fu;
- }
- }
- goto loop; // Yes, the dreaded goto statement. But the code
- // here is faithful to Brent's orginal pseudocode.
- }
- return fx;
- }
-
- //------------------------------------------------------------------------------
-
- // Tracks the peaks in the signal stream using the attack and release parameters
- template <int Channels = 2, typename Value=float>
- class EnvelopeFollower
- {
- public:
- EnvelopeFollower() { for (int i = 0; i < Channels; ++i) m_env[i] = 0; }
-
- Value operator[](int channel) const { return m_env[channel]; }
-
- void Setup(int sampleRate, double attackMs, double releaseMs)
- {
- m_a = pow(0.01, 1.0 / (attackMs * sampleRate * 0.001));
- m_r = pow(0.01, 1.0 / (releaseMs * sampleRate * 0.001));
- }
-
- void Process(size_t samples, const Value** src)
- {
- for (int i = 0; i < Channels; ++i)
- {
- const Value* cur = src[i];
-
- double e = m_env[i];
- for (int n = samples; n; n--)
- {
- double v = std::abs(*cur++);
- if (v > e) e = m_a * (e - v) + v;
- else e = m_r * (e - v) + v;
- }
- m_env[i] = e;
- }
- }
-
- double m_env[Channels];
-
- protected:
- double m_a = 0;
- double m_r = 0;
- };
-
- //------------------------------------------------------------------------------
-
- // Helpful for discovering discontinuities in buffers
- template <int Channels = 2, typename Value=float>
- class SlopeDetector
- {
- public:
- SlopeDetector() : m_firstTime(true) { for (int i = 0; i < Channels; ++i) m_slope[i] = 0; }
-
- Value getSlope(int channel) const { return m_slope[channel]; }
-
- void process(size_t nSamples, const Value** input)
- {
- for (int i = 0; i < Channels; ++i)
- {
- const Value* src = input[i];
- int n = nSamples;
-
- if (m_firstTime)
- {
- m_prev[i] = *src++;
- --n;
- }
-
- while (n > 0)
- {
- n--;
- Value cur = *src++;
- Value diff = std::abs(cur - m_prev[i]);
- m_slope[i] = std::max(diff, m_slope[i]);
- m_prev[i] = cur;
- }
- }
-
- m_firstTime = false;
- }
-
- private:
- bool m_firstTime = false;
- Value m_slope [Channels];
- Value m_prev [Channels];
- };
- } // namespace Dsp
-
- #endif
|