filters.hpp

Go to the documentation of this file.

#pragma once
#include <ossia/detail/config.hpp>
 
#include <ossia/detail/math.hpp>
#include <ossia/detail/small_vector.hpp>
 
#include <cmath>
 
#include <algorithm>
#include <array>
#include <cstdint>
#include <type_traits>
 
namespace ossia
{
 
template <typename T>
[[nodiscard]] OSSIA_INLINE T lowpass_alpha(T cutoff, T dt) noexcept
{
  const T r = T(ossia::two_pi) * cutoff * dt;
  return r / (r + T(1));
}
 
template <typename T>
[[nodiscard]] OSSIA_INLINE T lag_alpha(T tau, T dt) noexcept
{
  return dt / (dt + tau);
}
 
template <typename T = float>
struct one_pole_filter
{
  using value_type = T;
 
  T y{};
 
  bool primed{};
 
  OSSIA_INLINE void reset() noexcept
  {
    y = T{};
    primed = false;
  }
 
  OSSIA_INLINE void reset(T x) noexcept
  {
    y = x;
    primed = true;
  }
 
  OSSIA_INLINE void assign_parameters(const one_pole_filter&) noexcept { }
 
  [[nodiscard]] OSSIA_INLINE T operator()(T x, T alpha) noexcept
  {
    if(!primed) [[unlikely]]
    {
      primed = true;
      return y = x;
    }
 
    // One FMA, versus two multiplies for the alpha * x + (1 - alpha) * y form.
    return y += alpha * (x - y);
  }
};
 
template <typename T = float>
struct one_euro_filter
{
  using value_type = T;
 
  T min_cutoff{1};
 
  T beta{0};
 
  T d_cutoff{1};
 
  one_pole_filter<T> x_filter{};
 
  one_pole_filter<T> dx_filter{};
 
  T x_prev{};
 
  OSSIA_INLINE void reset() noexcept
  {
    x_filter.reset();
    dx_filter.reset();
    x_prev = T{};
  }
 
  OSSIA_INLINE void assign_parameters(const one_euro_filter& p) noexcept
  {
    min_cutoff = p.min_cutoff;
    beta = p.beta;
    d_cutoff = p.d_cutoff;
  }
 
  [[nodiscard]] OSSIA_INLINE T operator()(T x, T dt) noexcept
  {
    if(dt <= T(0)) [[unlikely]]
      return x_filter.primed ? x_filter.y : x;
 
    // Shared between both alphas: alpha = (k * cutoff) / (k * cutoff + 1).
    const T k = T(ossia::two_pi) * dt;
 
    // First sample: no derivative to speak of.
    const T dx = x_filter.primed ? (x - x_prev) / dt : T(0);
    x_prev = x;
 
    const T kd = k * d_cutoff;
    const T edx = dx_filter(dx, kd / (kd + T(1)));
 
    const T cutoff = min_cutoff + beta * std::abs(edx);
    const T kc = k * cutoff;
    return x_filter(x, kc / (kc + T(1)));
  }
};
 
template <typename T = float, std::size_t N = 8>
struct moving_average_filter
{
  static_assert(N >= 1, "a moving average needs at least one sample");
 
  using value_type = T;
 
  using accumulator_type = std::conditional_t<std::is_same_v<T, float>, double, T>;
 
  std::array<T, N> history{};
 
  accumulator_type sum{};
 
  std::uint32_t head{};
 
  std::uint32_t count{};
 
  OSSIA_INLINE void reset() noexcept
  {
    sum = accumulator_type{};
    head = 0;
    count = 0;
  }
 
  OSSIA_INLINE void assign_parameters(const moving_average_filter&) noexcept { }
 
  [[nodiscard]] OSSIA_INLINE T operator()(T x) noexcept
  {
    if(count == N)
      sum -= history[head];
    else
      ++count;
 
    sum += x;
    history[head] = x;
    head = (head + 1 == N) ? 0 : head + 1;
 
    return T(sum / accumulator_type(count));
  }
};
 
template <typename T = float, std::size_t N = 5>
struct median_filter
{
  static_assert(N >= 1, "a median needs at least one sample");
 
  using value_type = T;
 
  std::array<T, N> history{};
 
  std::array<T, N> sorted{};
 
  std::uint32_t head{};
 
  std::uint32_t count{};
 
  OSSIA_INLINE void reset() noexcept
  {
    head = 0;
    count = 0;
  }
 
  OSSIA_INLINE void assign_parameters(const median_filter&) noexcept { }
 
  [[nodiscard]] OSSIA_INLINE T operator()(T x) noexcept
  {
    const auto begin = sorted.begin();
 
    if(count == N)
    {
      // Drop the sample falling out of the window, then insert the new one.
      const auto last = begin + N;
      const auto old = std::lower_bound(begin, last, history[head]);
      std::move(old + 1, last, old);
 
      const auto pos = std::lower_bound(begin, last - 1, x);
      std::move_backward(pos, last - 1, last);
      *pos = x;
    }
    else
    {
      const auto last = begin + count;
      const auto pos = std::lower_bound(begin, last, x);
      std::move_backward(pos, last, last + 1);
      *pos = x;
      ++count;
    }
 
    history[head] = x;
    head = (head + 1 == N) ? 0 : head + 1;
 
    const std::uint32_t n = count;
    if(n & 1u)
      return sorted[n / 2];
    return (sorted[n / 2 - 1] + sorted[n / 2]) * T(0.5);
  }
};
 
template <typename Filter, std::size_t N = 4>
struct multi_filter
{
  using filter_type = Filter;
  using value_type = typename Filter::value_type;
 
  Filter prototype{};
 
  ossia::small_vector<Filter, N> filters{};
 
  OSSIA_INLINE void ensure(std::size_t n)
  {
    if(filters.size() != n) [[unlikely]]
      filters.assign(n, prototype);
  }
 
  OSSIA_INLINE void configure() noexcept
  {
    for(auto& f : filters)
      f.assign_parameters(prototype);
  }
 
  void reset() noexcept
  {
    for(auto& f : filters)
      f.reset();
  }
 
  void clear() noexcept { filters.clear(); }
 
  template <typename... Args>
  OSSIA_INLINE void operator()(value_type* v, std::size_t n, Args... args) noexcept
  {
    ensure(n);
    for(std::size_t i = 0; i < n; i++)
      v[i] = filters[i](v[i], args...);
  }
};
 
template <typename T>
[[nodiscard]] OSSIA_INLINE T unwrap_angle(T a, T reference) noexcept
{
  // remainder() lands in [-pi, pi] for a divisor of two_pi.
  return reference + T(ossia::remainder(a - reference, T(ossia::two_pi)));
}
 
}