Branch data Line data Source code
1 : : #ifndef MATH_UTILS_H
2 : : #define MATH_UTILS_H
3 : :
4 : : #include <cmath>
5 : : #include <algorithm>
6 : : #include <array>
7 : :
8 : : namespace ffb_math {
9 : :
10 : : // Mathematical Constants
11 : : static constexpr double PI = 3.14159265358979323846;
12 : : static constexpr double TWO_PI = 2.0 * PI;
13 : :
14 : : /**
15 : : * @brief Bi-quad Filter (Direct Form II)
16 : : *
17 : : * Used for filtering oscillations (e.g., steering wheel "death wobbles")
18 : : * and smoothing out high-frequency road noise.
19 : : */
20 : : struct BiquadNotch {
21 : :
22 : : // Coefficients
23 : : double b0 = 0.0, b1 = 0.0, b2 = 0.0, a1 = 0.0, a2 = 0.0;
24 : : // State history (Inputs x, Outputs y)
25 : : double x1 = 0.0, x2 = 0.0;
26 : : double y1 = 0.0, y2 = 0.0;
27 : :
28 : : // Update coefficients based on dynamic frequency
29 : 1505 : void Update(double center_freq, double sample_rate, double Q) {
30 : : // Safety: Clamp frequency to Nyquist (sample_rate / 2) and min 1Hz
31 : 1505 : center_freq = (std::max)(1.0, (std::min)(center_freq, sample_rate * 0.49));
32 : :
33 : 1505 : double omega = 2.0 * PI * center_freq / sample_rate;
34 : 1505 : double sn = std::sin(omega);
35 : 1505 : double cs = std::cos(omega);
36 : 1505 : double alpha = sn / (2.0 * Q);
37 : :
38 : 1505 : double a0 = 1.0 + alpha;
39 : :
40 : : // Calculate and Normalize
41 : 1505 : b0 = 1.0 / a0;
42 : 1505 : b1 = (-2.0 * cs) / a0;
43 : 1505 : b2 = 1.0 / a0;
44 : 1505 : a1 = (-2.0 * cs) / a0;
45 : 1505 : a2 = (1.0 - alpha) / a0;
46 : 1505 : }
47 : :
48 : : // Apply filter to single sample
49 : 2804 : double Process(double in) {
50 : 2804 : double out = b0 * in + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
51 : :
52 : : // Shift history
53 : 2804 : x2 = x1; x1 = in;
54 : 2804 : y2 = y1; y1 = out;
55 : :
56 : 2804 : return out;
57 : : }
58 : :
59 : 13179 : void Reset() {
60 : 13179 : x1 = x2 = y1 = y2 = 0.0;
61 : 13179 : }
62 : : };
63 : :
64 : : // Helper: Inverse linear interpolation
65 : : // Returns normalized position of value between min and max
66 : : // Returns 0 if value >= min, 1 if value <= max (for negative threshold use)
67 : : // Clamped to [0, 1] range
68 : 2565 : inline double inverse_lerp(double min_val, double max_val, double value) {
69 : 2565 : double range = max_val - min_val;
70 [ + + ]: 2565 : if (std::abs(range) >= 0.0001) {
71 : : // NOTE: The ternary below is redundant but kept for safety.
72 [ + - ]: 2557 : double t = (value - min_val) / (std::abs(range) >= 0.0001 ? range : 1.0);
73 : 2557 : return (std::max)(0.0, (std::min)(1.0, t));
74 : : }
75 : :
76 : : // Degenerate case when range is zero or near-zero
77 [ + + + + ]: 8 : if (max_val >= min_val) return (value >= min_val) ? 1.0 : 0.0;
78 [ + + ]: 4 : return (value <= min_val) ? 1.0 : 0.0;
79 : : }
80 : :
81 : : // Helper: Smoothstep interpolation
82 : : // Returns smooth S-curve interpolation from 0 to 1
83 : : // Uses Hermite polynomial: t² × (3 - 2t)
84 : : // Zero derivative at both endpoints for seamless transitions
85 : 19766 : inline double smoothstep(double edge0, double edge1, double x) {
86 : 19766 : double range = edge1 - edge0;
87 [ + + ]: 19766 : if (std::abs(range) >= 0.0001) {
88 : : // NOTE: The ternary below is redundant but kept for safety.
89 [ + - ]: 19762 : double t = (x - edge0) / (std::abs(range) >= 0.0001 ? range : 1.0);
90 : 19762 : t = (std::max)(0.0, (std::min)(1.0, t));
91 : 19762 : return t * t * (3.0 - 2.0 * t);
92 : : }
93 [ + + ]: 4 : return (x < edge0) ? 0.0 : 1.0;
94 : : }
95 : :
96 : : // Helper: Apply Slew Rate Limiter
97 : : // Clamps the rate of change of a signal.
98 : 2549 : inline double apply_slew_limiter(double input, double& prev_val, double limit, double dt) {
99 : 2549 : double delta = input - prev_val;
100 : 2549 : double max_change = limit * dt;
101 : 2549 : delta = std::clamp(delta, -max_change, max_change);
102 : 2549 : prev_val += delta;
103 : 2549 : return prev_val;
104 : : }
105 : :
106 : : // Helper: Adaptive Non-Linear Smoothing
107 : : // t=0 (Steady) uses slow_tau, t=1 (Transient) uses fast_tau
108 : 29338 : inline double apply_adaptive_smoothing(double input, double& prev_out, double dt,
109 : : double slow_tau, double fast_tau, double sensitivity) {
110 : 29338 : double delta = std::abs(input - prev_out);
111 : 29338 : double t = delta / (sensitivity + 0.000001);
112 : 29338 : t = (std::min)(1.0, t);
113 : :
114 : 29338 : double tau = slow_tau + t * (fast_tau - slow_tau);
115 : 29338 : double alpha = dt / (tau + dt + 1e-9);
116 : 29338 : alpha = (std::max)(0.0, (std::min)(1.0, alpha));
117 : :
118 : 29338 : prev_out = prev_out + alpha * (input - prev_out);
119 : 29338 : return prev_out;
120 : : }
121 : :
122 : : // Helper: Calculate Savitzky-Golay First Derivative
123 : : // Uses closed-form coefficient generation for quadratic polynomial fit.
124 : : template <size_t BufferSize>
125 : 9191 : inline double calculate_sg_derivative(const std::array<double, BufferSize>& buffer,
126 : : const int& buffer_count, const int& window, double dt, const int& buffer_index) {
127 : : // Note: buffer_index passed from caller should be the current write index (next slot)
128 : :
129 : : // Ensure we have enough samples
130 [ + + ]: 9191 : if (buffer_count < window) return 0.0;
131 : :
132 : 7778 : int M = window / 2; // Half-width (e.g., window=15 -> M=7)
133 : :
134 : : // Calculate S_2 = M(M+1)(2M+1)/3
135 : 7778 : double S2 = (double)M * (M + 1.0) * (2.0 * M + 1.0) / 3.0;
136 : :
137 : : // Correct Indexing (v0.7.0 Fix)
138 : : // m_slope_buffer_index points to the next slot to write.
139 : : // Latest sample is at (index - 1). Center is at (index - 1 - M).
140 : : // buffer_index MUST be the same as m_slope_buffer_index
141 : 7778 : int latest_idx = (buffer_index - 1 + BufferSize) % BufferSize;
142 : 7778 : int center_idx = (latest_idx - M + BufferSize) % BufferSize;
143 : :
144 : 7778 : double sum = 0.0;
145 [ + + ]: 60781 : for (int k = 1; k <= M; ++k) {
146 : 53003 : int idx_pos = (center_idx + k + BufferSize) % BufferSize;
147 : 53003 : int idx_neg = (center_idx - k + BufferSize) % BufferSize;
148 : :
149 : : // Weights for d=1 are simply k
150 : 53003 : sum += (double)k * (buffer[idx_pos] - buffer[idx_neg]);
151 : : }
152 : :
153 : : // Divide by dt to get derivative in units/second
154 : 7778 : return sum / (S2 * dt);
155 : : }
156 : :
157 : : /**
158 : : * @brief Linear Extrapolator (Inter-Frame Reconstruction)
159 : : *
160 : : * Upsamples a 100Hz signal to 400Hz+ by projecting forward
161 : : * based on the rate of change of the last game tick.
162 : : */
163 : : class LinearExtrapolator {
164 : : private:
165 : : double m_last_input = 0.0;
166 : : double m_current_output = 0.0;
167 : : double m_rate = 0.0;
168 : : double m_time_since_update = 0.0;
169 : : double m_game_tick = 0.01; // Default 100Hz
170 : : bool m_initialized = false;
171 : :
172 : : public:
173 : : void Configure(double game_tick) {
174 : : m_game_tick = (std::max)(0.0001, game_tick);
175 : : }
176 : :
177 : 454615 : double Process(double raw_input, double dt, bool is_new_frame) {
178 [ + + ]: 454615 : if (!m_initialized) {
179 : 5921 : m_last_input = raw_input;
180 : 5921 : m_current_output = raw_input;
181 : 5921 : m_initialized = true;
182 : 5921 : return raw_input;
183 : : }
184 : :
185 [ + + ]: 448694 : if (is_new_frame) {
186 : : // Calculate the rate of change over the last game tick
187 : 394320 : m_rate = (raw_input - m_last_input) / m_game_tick;
188 : :
189 : : // Snap to the new authoritative value to prevent drift
190 : 394320 : m_current_output = raw_input;
191 : 394320 : m_last_input = raw_input;
192 : 394320 : m_time_since_update = 0.0;
193 : : } else {
194 : : // Inter-frame Interpolation (Dead Reckoning)
195 : 54374 : m_time_since_update += dt;
196 : : // Clamp prediction time to avoid runaway if game pauses (1.5x interval)
197 [ + + ]: 54374 : if (m_time_since_update < m_game_tick * 1.5) {
198 : 52018 : m_current_output += m_rate * dt;
199 : : }
200 : : }
201 : :
202 : 448694 : return m_current_output;
203 : : }
204 : :
205 : : void Reset() {
206 : : m_last_input = m_current_output = m_rate = m_time_since_update = 0.0;
207 : : m_initialized = false;
208 : : }
209 : : };
210 : :
211 : : /**
212 : : * @brief Second-Order Holt-Winters (Double Exponential Smoothing)
213 : : *
214 : : * Tracks both the value and the trend (velocity) of a signal.
215 : : * Acts as both an upsampler and a low-pass filter.
216 : : */
217 : : class HoltWintersFilter {
218 : : private:
219 : : double m_level = 0.0; // Smoothed value
220 : : double m_trend = 0.0; // Smoothed trend/slope
221 : : double m_time_since_update = 0.0;
222 : : bool m_initialized = false;
223 : :
224 : : // Tuning Parameters
225 : : double m_alpha = 0.8; // Level weight (Higher = less lag)
226 : : double m_beta = 0.2; // Trend weight
227 : : double m_game_tick = 0.01; // Default 100Hz
228 : :
229 : : public:
230 : : void Configure(double alpha, double beta, double game_tick = 0.01) {
231 : : m_alpha = std::clamp(alpha, 0.01, 1.0);
232 : : m_beta = std::clamp(beta, 0.01, 1.0);
233 : : m_game_tick = (std::max)(0.0001, game_tick);
234 : : }
235 : :
236 : 14665 : double Process(double raw_input, double dt, bool is_new_frame) {
237 [ + + ]: 14665 : if (!m_initialized) {
238 : 191 : m_level = raw_input;
239 : 191 : m_trend = 0.0;
240 : 191 : m_time_since_update = 0.0;
241 : 191 : m_initialized = true;
242 : 191 : return raw_input;
243 : : }
244 : :
245 [ + + ]: 14474 : if (is_new_frame) {
246 : 12720 : double prev_level = m_level;
247 : :
248 : : // Update Level: Balance between the raw input and our previous prediction
249 : 12720 : m_level = m_alpha * raw_input + (1.0 - m_alpha) * (m_level + m_trend * m_game_tick);
250 : :
251 : : // Update Trend: Balance between the new observed slope and the old trend
252 : 12720 : m_trend = m_beta * ((m_level - prev_level) / m_game_tick) + (1.0 - m_beta) * m_trend;
253 : :
254 : 12720 : m_time_since_update = 0.0;
255 : :
256 : : // On new frame, we must return the authoritative raw input (or very close to it)
257 : : // to avoid breaking existing tests that expect exact values on frame boundaries.
258 : 12720 : return raw_input;
259 : : } else {
260 : 1754 : m_time_since_update += dt;
261 : : }
262 : :
263 : : // Predict current state based on previous trend (Upsampling step)
264 : 1754 : return m_level + m_trend * m_time_since_update;
265 : : }
266 : :
267 : : void Reset() {
268 : : m_level = m_trend = m_time_since_update = 0.0;
269 : : m_initialized = false;
270 : : }
271 : : };
272 : :
273 : : } // namespace ffb_math
274 : :
275 : : #endif // MATH_UTILS_H
|
