al_Functions.hpp

#ifndef INCLUDE_AL_MATH_FUNCTIONS_HPP
#define INCLUDE_AL_MATH_FUNCTIONS_HPP
 
/*  Allocore --
  Multimedia / virtual environment application class library
 
  Copyright (C) 2009. AlloSphere Research Group, Media Arts & Technology, UCSB.
  Copyright (C) 2012. The Regents of the University of California.
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
 
    Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.
 
    Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
 
    Neither the name of the University of California nor the names of its
    contributors may be used to endorse or promote products derived from
    this software without specific prior written permission.
 
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.
 
 
  File description:
  This includes various commonly used mathematical functions that are not
  included in the standard C/C++ math libraries.
 
  File author(s):
  Lance Putnam, 2006, putnam.lance@gmail.com
*/
 
#include <algorithm>
#include <cmath>
 
#include "al/math/al_Constants.hpp"
 
namespace al {
 
 
bool aeq(float a, float b, int maxULP = 10);
bool aeq(double a, double b, int maxULP = 10);
 
template <class T>
inline T ampTodB(const T& amp) {
  return 20 * std::log(amp);
}
 
template <class T>
T atLeast(const T& v, const T& eps);
 
 
// Author: Jim Shima, http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm.
// |error| < 0.01 rad
template <class T>
T atan2Fast(const T& y, const T& x);
 
 
uint32_t bitsSet(uint32_t v);
 
template <class T>
T ceil(const T& val, const T& step);
template <class T>
T ceil(const T& val, const T& step, const T& recStep);
 
inline uint32_t ceilEven(uint32_t v) { return v += v & 1UL; }
 
 
uint32_t ceilPow2(uint32_t value);
 
template <class T>
T clip(const T& value, const T& hi = T(1), const T& lo = T(0));
 
 
template <class T>
T clip(const T& v, int& clipFlag, const T& hi, const T& lo);
 
template <class T>
T clipS(const T& value, const T& hi = T(1));
 
template <class T>
inline T dBToAmp(const T& db) {
  return ::pow(10, db / 20.);
}
 
template <class T>
bool even(const T& v);
 
template <class T>
T erf(const T& v);
 
uint32_t factorial(uint32_t n0to12);
 
double factorialSqrt(int v);
 
template <class T>
T floor(const T& val, const T& step);
template <class T>
T floor(const T& val, const T& step, const T& recStep);
 
 
uint32_t floorPow2(uint32_t value);
 
 
template <class T>
T fold(const T& value, const T& hi = T(1), const T& lo = T(0));
 
template <class T>
T foldOnce(const T& value, const T& hi = T(1), const T& lo = T(0));
 
template <class T>
T gaussian(const T& v);
 
template <class T>
T gcd(const T& x, const T& y);
 
template <typename T, typename... Ts>
T gcd(const T& x, const Ts&... rest) {
  return gcd(x, gcd(rest...));
}
 
template <class T>
T gudermannian(const T& x);
 
 
template <class T>
T laguerreL(int n, int k, T x);
 
template <class T>
T lcm(const T& x, const T& y);
 
template <class T>
T legendreP(int l, int m, T t);
template <class T>
T legendreP(int l, int m, T ct, T st);
 
template <class T>
bool lessAbs(const T& v, const T& eps = T(0.000001));
 
template <typename T>
T max(const T& v1, const T& v2, const T& v3);
 
template <class T>
T mean(const T& v1, const T& v2);
 
template <class T>
T min(const T& v1, const T& v2, const T& v3);
 
template <class T>
inline T nearestDiv(T of, T to) {
  return to / round(to / of);
}
 
template <class T>
T nextAfter(const T& x, const T& y);
 
template <class T>
T nextMultiple(T val, T multiple);
 
template <class T>
T numInt(const T& v);
 
template <class T>
bool odd(const T& v);
 
template <class T>
T poly(const T& x, const T& a0, const T& a1, const T& a2);
 
template <class T>
T poly(const T& x, const T& a0, const T& a1, const T& a2, const T& a3);
 
template <class T>
T pow2(const T& v);  
template <class T>
T pow2S(const T& v);  
template <class T>
T pow3(const T& v);  
template <class T>
T pow3Abs(const T& v);  
template <class T>
T pow4(const T& v);  
template <class T>
T pow5(const T& v);  
template <class T>
T pow6(const T& v);  
template <class T>
T pow8(const T& v);  
template <class T>
T pow16(const T& v);  
template <class T>
T pow64(const T& v);  
 
 
template <class T>
T powN(T base, unsigned power);
 
unsigned char prime(uint32_t n);
 
template <class T>
T round(const T& v, const T& step);
 
template <class T>
T round(const T& v, const T& step, const T& recStep);
 
template <class T>
T roundAway(const T& v);
 
template <class T>
T roundAway(const T& v, const T& step);
 
template <class T>
T sgn(const T& v, const T& norm = T(1));
 
template <class T>
T sinc(const T& radians, const T& eps = T(0.0001));
 
template <class T>
T slope(const T& x1, const T& y1, const T& x2, const T& y2);
 
template <class T>
void sort(T& value1, T& value2);
 
template <class T>
T sumOfSquares(T n);
 
 
uint32_t trailingZeroes(uint32_t v);
 
template <class T>
T trunc(const T& v, const T& step);
 
template <class T>
T trunc(const T& v, const T& step, const T& recStep);
 
template <class T>
bool within(const T& v, const T& lo, const T& hi);
 
template <class T>
bool within3(const T& v1, const T& v2, const T& v3, const T& lo, const T& hi);
 
template <class T>
bool withinIE(const T& v, const T& lo, const T& hi);
 
template <class T>
T wrap(const T& value, const T& hi = T(1), const T& lo = T(0));
 
 
template <class T>
T wrap(const T& value, long& numWraps, const T& hi = T(1), const T& lo = T(0));
 
template <class T>
T wrapAdd1(const T& v, const T& max) {
  ++v;
  return v == max ? 0 : v;
}
 
template <class T>
T wrapOnce(const T& value, const T& hi = T(1));
 
template <class T>
T wrapOnce(const T& value, const T& hi, const T& lo);
 
template <class T>
T wrapPhase(const T& radians);
 
template <class T>
T wrapPhaseOnce(const T& radians);
 
// Implementation
//------------------------------------------------------------------------------
 
#define TEM template <class T>
 
namespace {
template <class T>
const T roundEps();
template <>
inline const float roundEps<float>() {
  return 0.499999925f;
}
template <>
inline const double roundEps<double>() {
  return 0.499999985;
}
 
inline uint32_t deBruijn(uint32_t v) {
  static const uint32_t deBruijnBitPosition[32] = {
      0,  1,  28, 2,  29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4,  8,
      31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6,  11, 5,  10, 9};
  return deBruijnBitPosition[(uint32_t(v * 0x077CB531UL)) >> 27];
}
 
const uint32_t mFactorial12u[13] = {1,       1,        2,        6,     24,
                                    120,     720,      5040,     40320, 362880,
                                    3628800, 39916800, 479001600};
 
const uint8_t mPrimes54[54] = {
    /*    0    1    2    3    4    5    6    7    8    9   */
    2,   3,   5,   7,   11,  13,  17,  19,  23,  29,   // 0
    31,  37,  41,  43,  47,  53,  59,  61,  67,  71,   // 1
    73,  79,  83,  89,  97,  101, 103, 107, 109, 113,  // 2
    127, 131, 137, 139, 149, 151, 157, 163, 167, 173,  // 3
    179, 181, 191, 193, 197, 199, 211, 223, 227, 229,  // 4
    233, 239, 241, 251                                 // 5
};
}  // namespace
 
inline bool aeq(float a, float b, int maxULP) {
  // Make sure maxULP is non-negative and small enough that the
  // default NAN won't compare as equal to anything.
  // assert(maxULP > 0 && maxULP < 4 * 1024 * 1024);
  union {
    float f;
    int32_t i;
  } u;
  u.f = a;
  int32_t ai = u.i;
  u.f = b;
  int32_t bi = u.i;
  // Make ai and bi lexicographically ordered as a twos-complement int
  if (ai < 0) ai = 0x80000000 - ai;
  if (bi < 0) bi = 0x80000000 - bi;
  return std::abs((float)(ai - bi)) <= maxULP;
}
 
inline bool aeq(double a, double b, int maxULP) {
  // Make sure maxULP is non-negative and small enough that the
  // default NAN won't compare as equal to anything.
  // assert(maxULP > 0 && maxULP < 4 * 1024 * 1024);
  union {
    double f;
    int64_t i;
  } u;
  u.f = a;
  int64_t ai = u.i;
  u.f = b;
  int64_t bi = u.i;
  // Make ai and bi lexicographically ordered as a twos-complement int
  if (ai < 0) ai = 0x8000000000000000ULL - ai;
  if (bi < 0) bi = 0x8000000000000000ULL - bi;
  return std::abs((double)(ai - bi)) <= maxULP;
}
 
TEM inline T atLeast(const T& v, const T& e) {
  return (v >= T(0)) ? std::max(v, e) : std::min(v, -e);
}
 
TEM T atan2Fast(const T& y, const T& x) {
  T r, angle;
  T ay = std::abs(y) + T(1e-10);  // kludge to prevent 0/0 condition
 
  if (x < T(0)) {
    r = (x + ay) / (ay - x);
    angle = T(M_3PI_4);
  } else {
    r = (x - ay) / (x + ay);
    angle = T(M_PI_4);
  }
 
  angle += (T(0.1963) * r * r - T(0.9817)) * r;
  return y < T(0) ? -angle : angle;
}
 
inline uint32_t bitsSet(uint32_t v) {
  v = v - ((v >> 1) & 0x55555555);                 // reuse input as temporary
  v = (v & 0x33333333) + ((v >> 2) & 0x33333333);  // temp
  return ((v + ((v >> 4) & 0xF0F0F0F)) * 0x1010101) >> 24;  // count
}
 
TEM inline T ceil(const T& v, const T& s) { return std::ceil(v / s) * s; }
TEM inline T ceil(const T& v, const T& s, const T& r) {
  return std::ceil(v * r) * s;
}
 
inline uint32_t ceilPow2(uint32_t v) {
  --v;
  v |= v >> 1;
  v |= v >> 2;
  v |= v >> 4;
  v |= v >> 8;
  v |= v >> 16;
  return v + 1;
}
 
TEM inline T clip(const T& v, const T& hi, const T& lo) {
  if (v < lo)
    return lo;
  else if (v > hi)
    return hi;
  return v;
}
 
TEM inline T clip(const T& v, int& clipFlag, const T& hi, const T& lo) {
  clipFlag = 0;
  if (v < lo) {
    clipFlag = -1;
    return lo;
  } else if (v > hi) {
    clipFlag = 1;
    return hi;
  }
  return v;
}
 
TEM inline T clipS(const T& v, const T& hi) { return al::clip(v, hi, -hi); }
 
TEM inline bool even(const T& v) { return 0 == al::odd(v); }
 
TEM inline T erf(const T& x) {
  static T a = 0.147;
  const T x2 = x * x;
  const T ax2 = a * x2;
  return al::sgn(x) *
         std::sqrt(T(1) - std::exp(-x2 * (T(4. / M_PI) + ax2) / (T(1) + ax2)));
}
 
inline uint32_t factorial(uint32_t v) { return mFactorial12u[v]; }
 
inline double factorialSqrt(int v) {
  if (v <= 1) return 1;
  double r = 1;
  for (int i = 2; i <= v; ++i) r *= std::sqrt(i);
  return r;
}
 
TEM inline T floor(const T& v, const T& s) { return std::floor(v / s) * s; }
TEM inline T floor(const T& v, const T& s, const T& r) {
  return std::floor(v * r) * s;
}
 
inline uint32_t floorPow2(uint32_t v) {
  v |= v >> 1;
  v |= v >> 2;
  v |= v >> 4;
  v |= v >> 8;
  v |= v >> 16;
  return (v >> 1) + 1;
}
 
TEM inline T fold(const T& v, const T& hi, const T& lo) {
  long numWraps;
  T R = al::wrap(v, numWraps, hi, lo);
  if (numWraps & 1) R = hi + lo - R;
  return R;
}
 
TEM inline T foldOnce(const T& v, const T& hi, const T& lo) {
  if (v > hi) return hi + (hi - v);
  if (v < lo) return lo + (lo - v);
  return v;
}
 
TEM inline T gaussian(const T& v) { return std::exp(-v * v); }
 
TEM T gcd(const T& x, const T& y) {
  if (y == T(0)) return std::abs(x);
  return al::gcd(y, static_cast<T>(std::remainder(x, y)));
}
 
TEM T gudermannian(const T& x) { return T(2) * std::atan(exp(x)) - T(M_PI_2); }
 
TEM T laguerreL(int n, int k, T x) {
  //  T res = 1, bin = 1;
  //
  //  for(int i=n; i>=1; --i){
  //    bin = bin * (k+i) / (n + 1 - i);
  //    res = bin - x * res / i;
  //  }
  //  return res;
 
  if (n < 0) return T(0);
 
  T L1 = 0, R = 1;
  for (int i = 1; i <= n; ++i) {
    T L0 = L1;
    L1 = R;
    R = ((2 * i + k - 1 - x) * L1 - (i + k - 1) * L0) / i;
  }
  return R;
}
 
TEM inline T lcm(const T& x, const T& y) { return (x * y) / al::gcd(x, y); }
 
TEM T legendreP(int l, int m, T ct, T st) {
  switch (l) {
    case 0:
      return 1.;
 
    case 1:
      switch (m) {
        case 0:
          return ct;
        case 1:
          return -st;
        default:
          return 0.;
      }
 
    case 2:
      switch (m) {
        case 0:
          return -0.5 + 1.5 * ct * ct;
        case 1:
          return -3.0 * ct * st;
        case 2:
          return 3.0 * st * st;
        default:
          return 0.;
      }
 
    case 3:
      switch (m) {
        case 0:
          return ct * (-1.5 + 2.5 * ct * ct);
        case 1:
          return (1.5 - 7.5 * ct * ct) * st;
        case 2:
          return 15. * ct * st * st;
        case 3:
          return -15. * st * st * st;
        default:
          return 0.;
      }
 
    case 4:
      switch (m) {
        case 0:
          ct *= ct;
          return 0.375 + ct * (-3.75 + 4.375 * ct);
        case 1:
          return ct * (7.5 - 17.5 * ct * ct) * st;
        case 2:
          return (-7.5 + 52.5 * ct * ct) * st * st;
        case 3:
          return -105. * ct * st * st * st;
        case 4:
          st *= st;
          return 105. * st * st;
        default:
          return 0.;
      }
 
    default:;
  }
 
  //  if(l<0){ /*printf("l=%d. l must be non-negative.\n");*/ return 0; }
  //  if(m<-l || m>l){ /*printf("m=%d. m must be -l <= m <= l.\n");*/ return 0;
  //  }
 
  // First compute answer for |m|
 
  // compute P_l^m(x) by the recurrence relation
  //    (l-m)P_l^m(x) = x(2l-1)P_{l-1}^m(x) - (l+m-1)P_{l-2}^m(x)
  // with
  //    P_m^m(x) = (-1)^m (2m-1)!! (1-x)^{m/2},
  //    P_{m+1}^m(x) = x(2m+1) P_m^m(x).
 
  T P = 0;                      // the result
  int M = std::abs((double)m);  // M = |m|
  T y1 = 1.;                    // recursion state variable
 
  for (int i = 1; i <= M; ++i) y1 *= -((i << 1) - 1) * st;
 
  if (l == M)
    P = y1;
 
  else {
    T y = ((M << 1) + 1) * ct * y1;
    if (l == (M + 1))
      P = y;
 
    else {
      T c = (M << 1) - 1;
      for (int k = M + 2; k <= l; ++k) {
        T y2 = y1;
        y1 = y;
        T d = c / (k - M);
        y = (2. + d) * ct * y1 - (1. + d) * y2;
      }
      P = y;
    }
  }
 
  //  // In the case that m<0,
  //  // compute P_n^{-|m|}(x) by the formula
  //  //    P_l^{-|m|}(x) = (-1)^{|m|}((l-|m|)!/(l+|m|)!)^{1/2} P_l^{|m|}(x).
  //  // NOTE: when l and |m| are large, we risk numerical underflow...
  //  if(m<0){
  //    for(int i=l-M+1; i<=l+M; ++i) P *= 1. / i;
  //    if(al::odd(M)) P = -P;
  //  }
 
  return P;
}
 
TEM T legendreP(int l, int m, T t) {
  return al::legendreP(l, m, std::cos(t), std::sin(t));
}
 
TEM inline bool lessAbs(const T& v, const T& eps) { return std::abs(v) < eps; }
 
TEM inline T max(const T& v1, const T& v2, const T& v3) {
  return std::max(std::max(v1, v2), v3);
}
TEM inline T mean(const T& v1, const T& v2) { return (v1 + v2) * T(0.5); }
TEM inline T min(const T& v1, const T& v2, const T& v3) {
  return std::min(std::min(v1, v2), v3);
}
 
#ifdef __MSYS__
// MSYS2 does not support C++11 std::nextafter like it should
template <class T>
inline T nextAfter(const T& x, const T& y) {
  return x < y ? x + 1 : x - 1;
}
template <>
inline float nextAfter(const float& x, const float& y) {
  return nextafterf(x, y);
}
template <>
inline double nextAfter(const double& x, const double& y) {
  return nextafter(x, y);
}
#else
TEM inline T nextAfter(const T& x, const T& y) { return std::nextafter(x, y); }
#endif
 
TEM inline T nextMultiple(T v, T m) {
  uint32_t div = (uint32_t)(v / m);
  return T(div + 1) * m;
}
 
TEM inline T numInt(const T& v) { return std::floor(std::log10(v)) + 1; }
 
TEM inline bool odd(const T& v) { return v & T(1); }
 
TEM inline T poly(const T& v, const T& a0, const T& a1, const T& a2) {
  return a0 + v * (a1 + v * a2);
}
TEM inline T poly(const T& v, const T& a0, const T& a1, const T& a2, T a3) {
  return a0 + v * (a1 + v * (a2 + v * a3));
}
 
TEM inline T pow2(const T& v) { return v * v; }
TEM inline T pow2S(const T& v) { return v * std::abs(v); }
TEM inline T pow3(const T& v) { return v * v * v; }
TEM inline T pow3Abs(const T& v) { return std::abs(pow3(v)); }
TEM inline T pow4(const T& v) { return pow2(pow2(v)); }
TEM inline T pow5(const T& v) { return v * pow4(v); }
TEM inline T pow6(const T& v) { return pow3(pow2(v)); }
TEM inline T pow8(const T& v) { return pow4(pow2(v)); }
TEM inline T pow16(const T& v) { return pow4(pow4(v)); }
TEM inline T pow64(const T& v) { return pow8(pow8(v)); }
 
TEM inline T powN(T base, unsigned power) {
  switch (power) {
    case 0:
      return T(1);
    case 1:
      return base;
    case 2:
      return pow2(base);
    case 3:
      return pow3(base);
    case 4:
      return pow4(base);
    case 5:
      return pow5(base);
    case 6:
      return pow6(base);
    case 7:
      return pow6(base) * base;
    case 8:
      return pow8(base);
    case 9:
      return pow8(base) * base;
    default: {
      T r = pow8(base) * pow2(base);
      for (unsigned i = 10; i < power; ++i) r *= base;
      return r;
    }
  }
}
 
inline uint8_t prime(uint32_t n) { return mPrimes54[n]; }
 
TEM inline T round(const T& v, const T& s) { return std::round(v / s) * s; }
TEM inline T round(const T& v, const T& s, const T& r) {
  return std::round(v * r) * s;
}
TEM inline T roundAway(const T& v) {
  return v < T(0) ? al::floor(v) : al::ceil(v);
}
TEM inline T roundAway(const T& v, const T& s) {
  return v < T(0) ? al::floor(v, s) : al::ceil(v, s);
}
 
TEM inline T sgn(const T& v, const T& norm) {
  return v == T(0) ? T(0) : v < T(0) ? -norm : norm;
}
 
TEM inline T sinc(const T& r, const T& eps) {
  return (std::abs(r) > eps) ? std::sin(r) / r : std::cos(r);
}
 
TEM inline T slope(const T& x1, const T& y1, const T& x2, const T& y2) {
  return (y2 - y1) / (x2 - x1);
}
 
TEM inline void sort(T& v1, T& v2) {
  if (v1 > v2) {
    T t = v1;
    v1 = v2;
    v2 = t;
  }
}
 
TEM inline T sumOfSquares(T n) {
  static const T c1_6 = 1 / T(6);
  static const T c2_6 = c1_6 * T(2);
  return n * (n + 1) * (c2_6 * n + c1_6);
}
 
inline uint32_t trailingZeroes(uint32_t v) { return deBruijn(v & -v); }
 
TEM inline T trunc(const T& v, const T& s) { return std::trunc(v / s) * s; }
TEM inline T trunc(const T& v, const T& s, const T& r) {
  return std::trunc(v * r) * s;
}
 
TEM inline bool within(const T& v, const T& lo, const T& hi) {
  return !((v < lo) || (v > hi));
}
TEM inline bool withinIE(const T& v, const T& lo, const T& hi) {
  return (!(v < lo)) && (v < hi);
}
 
TEM inline bool within3(const T& v1, const T& v2, const T& v3, const T& lo,
                        const T& hi) {
  return al::within(v1, lo, hi) && al::within(v2, lo, hi) &&
         al::within(v3, lo, hi);
}
 
// TODO: fuse the following two functions
TEM inline T wrap(const T& v, const T& hi, const T& lo) {
  if (lo == hi) return lo;
 
  T R = v;
  T diff = hi - lo;
 
  if (R >= hi) {
    R -= diff;
    if (R >= hi) R -= diff * uint32_t((R - lo) / diff);
  } else if (R < lo) {
    R += diff;
 
    // If value is very slightly less than 'lo', then less significant
    // digits might get truncated by adding a larger number.
    if (R == diff) return al::nextAfter(R, lo);
 
    if (R < lo) R += diff * uint32_t(((lo - R) / diff) + 1);
    if (R == diff) return lo;
  }
  return R;
}
 
TEM inline T wrap(const T& v, long& numWraps, const T& hi, const T& lo) {
  if (lo == hi) {
    numWraps = 0xFFFFFFFF;
    return lo;
  }
 
  T R = v;
  T diff = hi - lo;
  numWraps = 0;
 
  if (R >= hi) {
    R -= diff;
    if (R >= hi) {
      numWraps = long((R - lo) / diff);
      R -= diff * numWraps;
    }
    ++numWraps;
  } else if (R < lo) {
    R += diff;
    if (R < lo) {
      numWraps = long((R - lo) / diff) - 1;
      R -= diff * numWraps;
    }
    --numWraps;
  }
  return R;
}
 
TEM inline T wrapOnce(const T& v, const T& hi) {
  if (v >= hi)
    return v - hi;
  else if (v < T(0))
    return v + hi;
  return v;
}
 
TEM inline T wrapOnce(const T& v, const T& hi, const T& lo) {
  if (v >= hi)
    return v - hi + lo;
  else if (v < lo)
    return v + hi - lo;
  return v;
}
 
TEM inline T wrapPhase(const T& r_) {
  // The result is    [r+pi - 2pi floor([r+pi] / 2pi)] - pi
  // which simplified is  r - 2pi floor([r+pi] / 2pi) .
  T r = r_;
  if (r >= T(M_PI)) {
    r -= T(M_2PI);
    if (r < T(M_PI)) return r;
    return r - T(long((r + M_PI) * M_1_2PI)) * M_2PI;
  } else if (r < T(-M_PI)) {
    r += T(M_2PI);
    if (r >= T(-M_PI)) return r;
    return r - T(long((r + M_PI) * M_1_2PI) - 1) * M_2PI;
  } else
    return r;
}
 
TEM inline T wrapPhaseOnce(const T& r) {
  if (r >= T(M_PI))
    return r - T(M_2PI);
  else if (r < T(-M_PI))
    return r + T(M_2PI);
  return r;
}
 
TEM inline T mapRange(T value, T inlow, T inhigh, T outlow, T outhigh) {
  float tmp = (value - inlow) / (inhigh - inlow);
  return tmp * (outhigh - outlow) + outlow;
}
 
TEM inline T lerp(T src, T dest, T amt) {
  return src * (T(1) - amt) + dest * amt;
}
 
#undef TEM
}  // namespace al
#endif