matrixfurry.com
The open source OpenXR runtime
1// Copyright 2019, 2022, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Low-pass IIR filter for integers
6 * @author Rylie Pavlik <rylie.pavlik@collabora.com>
7 * @ingroup aux_math
8 */
9
10#pragma once
11
12#ifndef __cplusplus
13#error "This header is C++-only."
14#endif
15
16#include "util/u_time.h"
17
18#include "math/m_mathinclude.h"
19#include "math/m_rational.hpp"
20#include <cmath>
21#include <type_traits>
22#include <stdexcept>
23#include <cassert>
24
25namespace xrt::auxiliary::math {
26
27
28namespace detail {
29 /*!
30 * The shared implementation (between vector and scalar versions) of an integer
31 * IIR/exponential low-pass filter.
32 */
33 template <typename Value, typename Scalar> struct IntegerLowPassIIR
34 {
35 // For fixed point, you'd need more bits of data storage. See
36 // https://www.embeddedrelated.com/showarticle/779.php
37 static_assert(std::is_integral<Scalar>::value,
38 "Filter is designed only for integral values. Use the other one for floats.");
39
40 /*!
41 * Constructor
42 *
43 * @param alpha_ The alpha value used to blend between new input and existing state. Larger values mean
44 * more influence from new input. @p alpha_.isBetweenZeroAndOne() must be true.
45 *
46 * @param val The value to initialize the filter with. Does not
47 * affect the filter itself: only seen if you get the state
48 * before initializing the filter with the first sample.
49 */
50 explicit IntegerLowPassIIR(math::Rational<Scalar> alpha_, Value const &val)
51 : state(val), alpha(alpha_.withNonNegativeDenominator())
52 {
53 if (!alpha.isBetweenZeroAndOne()) {
54 throw std::invalid_argument("Alpha must be between zero and one.");
55 }
56 }
57
58 /*!
59 * Reset the filter to newly-created state.
60 */
61 void
62 reset(Value const &val) noexcept
63 {
64 state = val;
65 initialized = false;
66 }
67
68 /*!
69 * Filter a sample, with an optional weight.
70 *
71 * @param sample The value to filter
72 * @param weight An optional value between 0 and 1. The smaller
73 * this value, the less the current sample influences the filter
74 * state. For the first call, this is always assumed to be 1.
75 */
76 void
77 addSample(Value const &sample, math::Rational<Scalar> weight = math::Rational<Scalar>::simplestUnity())
78 {
79 if (!initialized) {
80 initialized = true;
81 state = sample;
82 return;
83 }
84 math::Rational<Scalar> weightedAlpha = alpha * weight;
85
86 math::Rational<Scalar> oneMinusWeightedAlpha = weightedAlpha.complement();
87
88 Value scaledStateNumerator = oneMinusWeightedAlpha.numerator * state;
89 Value scaledSampleNumerator = weightedAlpha.numerator * sample;
90
91 // can't use the re-arranged update from the float impl because we might be unsigned.
92 state = (scaledStateNumerator + scaledSampleNumerator) / weightedAlpha.denominator;
93 }
94
95 Value state;
96 math::Rational<Scalar> alpha;
97 bool initialized{false};
98 };
99} // namespace detail
100
101/*!
102 * A very simple integer low-pass filter, using a "one-pole infinite impulse response"
103 * design (one-pole IIR), also known as an exponential filter.
104 *
105 * Configurable in scalar type.
106 */
107template <typename Scalar> class IntegerLowPassIIRFilter
108{
109public:
110 /*!
111 * Constructor
112 *
113 * @note Taking alpha, not a cutoff frequency, here, because it's easier with the rational math.
114 *
115 * @param alpha The alpha value used to blend between new input and existing state. Larger values mean
116 * more influence from new input. @ref math::Rational::isBetweenZeroAndOne() must be true for @p alpha.
117 */
118 explicit IntegerLowPassIIRFilter(math::Rational<Scalar> alpha) noexcept : impl_(alpha, 0)
119 {
120 assert(alpha.isBetweenZeroAndOne());
121 }
122
123 /*!
124 * Reset the filter to newly-created state.
125 */
126 void
127 reset() noexcept
128 {
129 impl_.reset(0);
130 }
131
132 /*!
133 * Filter a sample, with an optional weight.
134 *
135 * @param sample The value to filter
136 * @param weight An optional value between 0 and 1. The smaller this
137 * value, the less the current sample influences the filter state. For
138 * the first call, this is always assumed to be 1 regardless of what you pass.
139 */
140 void
141 addSample(Scalar sample, math::Rational<Scalar> weight = math::Rational<Scalar>::simplestUnity())
142 {
143 impl_.addSample(sample, weight);
144 }
145
146 /*!
147 * Get the filtered value.
148 */
149 Scalar
150 getState() const noexcept
151 {
152 return impl_.state;
153 }
154
155
156 /*!
157 * Get whether we have initialized state.
158 */
159 bool
160 isInitialized() const noexcept
161 {
162 return impl_.initialized;
163 }
164
165private:
166 detail::IntegerLowPassIIR<Scalar, Scalar> impl_;
167};
168
169} // namespace xrt::auxiliary::math