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 : 1509 : void Update(double center_freq, double sample_rate, double Q) {
30 : : // Safety: Clamp frequency to Nyquist (sample_rate / 2) and min 1Hz
31 : 1509 : center_freq = (std::max)(1.0, (std::min)(center_freq, sample_rate * 0.49));
32 : :
33 : 1509 : double omega = 2.0 * PI * center_freq / sample_rate;
34 : 1509 : double sn = std::sin(omega);
35 : 1509 : double cs = std::cos(omega);
36 : 1509 : double alpha = sn / (2.0 * Q);
37 : :
38 : 1509 : double a0 = 1.0 + alpha;
39 : :
40 : : // Calculate and Normalize
41 : 1509 : b0 = 1.0 / a0;
42 : 1509 : b1 = (-2.0 * cs) / a0;
43 : 1509 : b2 = 1.0 / a0;
44 : 1509 : a1 = (-2.0 * cs) / a0;
45 : 1509 : a2 = (1.0 - alpha) / a0;
46 : 1509 : }
47 : :
48 : : // Apply filter to single sample
49 : 2808 : double Process(double in) {
50 : 2808 : double out = b0 * in + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
51 : :
52 : : // Shift history
53 : 2808 : x2 = x1; x1 = in;
54 : 2808 : y2 = y1; y1 = out;
55 : :
56 : 2808 : return out;
57 : : }
58 : :
59 : 14360 : void Reset() {
60 : 14360 : x1 = x2 = y1 = y2 = 0.0;
61 : 14360 : }
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 : 15385 : inline double inverse_lerp(double min_val, double max_val, double value) {
69 : 15385 : double range = max_val - min_val;
70 [ + + ]: 15385 : if (std::abs(range) >= 0.0001) {
71 : : // NOTE: The ternary below is redundant but kept for safety.
72 [ + - ]: 15377 : double t = (value - min_val) / (std::abs(range) >= 0.0001 ? range : 1.0);
73 : 15377 : 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 : 31102 : inline double smoothstep(double edge0, double edge1, double x) {
86 : 31102 : double range = edge1 - edge0;
87 [ + + ]: 31102 : if (std::abs(range) >= 0.0001) {
88 : : // NOTE: The ternary below is redundant but kept for safety.
89 [ + - ]: 31098 : double t = (x - edge0) / (std::abs(range) >= 0.0001 ? range : 1.0);
90 : 31098 : t = (std::max)(0.0, (std::min)(1.0, t));
91 : 31098 : 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 : 15206 : inline double apply_slew_limiter(double input, double& prev_val, double limit, double dt) {
99 : 15206 : double delta = input - prev_val;
100 : 15206 : double max_change = limit * dt;
101 : 15206 : delta = std::clamp(delta, -max_change, max_change);
102 : 15206 : prev_val += delta;
103 : 15206 : 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 : 31989 : inline double apply_adaptive_smoothing(double input, double& prev_out, double dt,
109 : : double slow_tau, double fast_tau, double sensitivity) {
110 : 31989 : double delta = std::abs(input - prev_out);
111 : 31989 : double t = delta / (sensitivity + 0.000001);
112 : 31989 : t = (std::min)(1.0, t);
113 : :
114 : 31989 : double tau = slow_tau + t * (fast_tau - slow_tau);
115 : 31989 : double alpha = dt / (tau + dt + 1e-9);
116 : 31989 : alpha = (std::max)(0.0, (std::min)(1.0, alpha));
117 : :
118 : 31989 : prev_out = prev_out + alpha * (input - prev_out);
119 : 31989 : return prev_out;
120 : : }
121 : :
122 : : /**
123 : : * @brief Cubic S-Curve transformation for Load (Lateral or Longitudinal)
124 : : * f(x) = 1.5x - 0.5x^3
125 : : */
126 : 11 : inline double apply_load_transform_cubic(double x) {
127 : 11 : return 1.5 * x - 0.5 * (x * x * x);
128 : : }
129 : :
130 : : /**
131 : : * @brief Quadratic (Signed) transformation for Load (Lateral or Longitudinal)
132 : : * f(x) = 2x - x|x|
133 : : */
134 : 5 : inline double apply_load_transform_quadratic(double x) {
135 : 5 : return 2.0 * x - x * std::abs(x);
136 : : }
137 : :
138 : : /**
139 : : * @brief Locked-Center Hermite Spline transformation for Load (Lateral or Longitudinal)
140 : : * f(x) = x * (1 + |x| - x^2)
141 : : */
142 : 5 : inline double apply_load_transform_hermite(double x) {
143 : 5 : double abs_x = std::abs(x);
144 : 5 : return x * (1.0 + abs_x - (abs_x * abs_x));
145 : : }
146 : :
147 : : // Helper: Calculate Savitzky-Golay First Derivative
148 : : // Uses closed-form coefficient generation for quadratic polynomial fit.
149 : : template <size_t BufferSize>
150 : 59819 : inline double calculate_sg_derivative(const std::array<double, BufferSize>& buffer,
151 : : const int& buffer_count, const int& window, double dt, const int& buffer_index) {
152 : : // Note: buffer_index passed from caller should be the current write index (next slot)
153 : :
154 : : // Ensure we have enough samples
155 [ + + ]: 59819 : if (buffer_count < window) return 0.0;
156 : :
157 : 52978 : int M = window / 2; // Half-width (e.g., window=15 -> M=7)
158 : :
159 : : // Calculate S_2 = M(M+1)(2M+1)/3
160 : 52978 : double S2 = (double)M * (M + 1.0) * (2.0 * M + 1.0) / 3.0;
161 : :
162 : : // Correct Indexing (v0.7.0 Fix)
163 : : // m_slope_buffer_index points to the next slot to write.
164 : : // Latest sample is at (index - 1). Center is at (index - 1 - M).
165 : : // buffer_index MUST be the same as m_slope_buffer_index
166 : 52978 : int latest_idx = (buffer_index - 1 + BufferSize) % BufferSize;
167 : 52978 : int center_idx = (latest_idx - M + BufferSize) % BufferSize;
168 : :
169 : 52978 : double sum = 0.0;
170 [ + + ]: 422237 : for (int k = 1; k <= M; ++k) {
171 : 369259 : int idx_pos = (center_idx + k + BufferSize) % BufferSize;
172 : 369259 : int idx_neg = (center_idx - k + BufferSize) % BufferSize;
173 : :
174 : : // Weights for d=1 are simply k
175 : 369259 : sum += (double)k * (buffer[idx_pos] - buffer[idx_neg]);
176 : : }
177 : :
178 : : // Divide by dt to get derivative in units/second
179 : 52978 : return sum / (S2 * dt);
180 : : }
181 : :
182 : : /**
183 : : * @brief Linear Extrapolator (Inter-Frame Reconstruction)
184 : : *
185 : : * Upsamples a 100Hz signal to 400Hz+ by projecting forward
186 : : * based on the rate of change of the last game tick.
187 : : */
188 : : class LinearExtrapolator {
189 : : private:
190 : : double m_last_input = 0.0;
191 : : double m_current_output = 0.0;
192 : : double m_rate = 0.0;
193 : : double m_time_since_update = 0.0;
194 : : double m_game_tick = 0.01; // Default 100Hz
195 : : bool m_initialized = false;
196 : :
197 : : public:
198 : : void Configure(double game_tick) {
199 : : m_game_tick = (std::max)(0.0001, game_tick);
200 : : }
201 : :
202 : 506752 : double Process(double raw_input, double dt, bool is_new_frame) {
203 [ + + ]: 506752 : if (!m_initialized) {
204 : 9120 : m_last_input = raw_input;
205 : 9120 : m_current_output = raw_input;
206 : 9120 : m_initialized = true;
207 : 9120 : return raw_input;
208 : : }
209 : :
210 [ + + ]: 497632 : if (is_new_frame) {
211 : : // Calculate the rate of change over the last game tick
212 : 497120 : m_rate = (raw_input - m_last_input) / m_game_tick;
213 : :
214 : : // Snap to the new authoritative value to prevent drift
215 : 497120 : m_current_output = raw_input;
216 : 497120 : m_last_input = raw_input;
217 : 497120 : m_time_since_update = 0.0;
218 : : } else {
219 : : // Inter-frame Interpolation (Dead Reckoning)
220 : 512 : m_time_since_update += dt;
221 : : // Clamp prediction time to avoid runaway if game pauses (1.5x interval)
222 [ + - ]: 512 : if (m_time_since_update < m_game_tick * 1.5) {
223 : 512 : m_current_output += m_rate * dt;
224 : : }
225 : : }
226 : :
227 : 497632 : return m_current_output;
228 : : }
229 : :
230 : 800 : void Reset() {
231 : 800 : m_last_input = m_current_output = m_rate = m_time_since_update = 0.0;
232 : 800 : m_initialized = false;
233 : 800 : }
234 : : };
235 : :
236 : : /**
237 : : * @brief Second-Order Holt-Winters (Double Exponential Smoothing)
238 : : *
239 : : * Tracks both the value and the trend (velocity) of a signal.
240 : : * Acts as both an upsampler and a low-pass filter.
241 : : */
242 : : class HoltWintersFilter {
243 : : private:
244 : : double m_level = 0.0; // Smoothed value
245 : : double m_trend = 0.0; // Smoothed trend/slope
246 : : double m_time_since_update = 0.0;
247 : : bool m_initialized = false;
248 : :
249 : : // Tuning Parameters
250 : : double m_alpha = 0.8; // Level weight (Higher = less lag)
251 : : double m_beta = 0.2; // Trend weight
252 : : double m_game_tick = 0.01; // Default 100Hz
253 : :
254 : : public:
255 : 1 : void Configure(double alpha, double beta, double game_tick = 0.01) {
256 : 1 : m_alpha = std::clamp(alpha, 0.01, 1.0);
257 : 1 : m_beta = std::clamp(beta, 0.01, 1.0);
258 : 1 : m_game_tick = (std::max)(0.0001, game_tick);
259 : 1 : }
260 : :
261 : 15838 : double Process(double raw_input, double dt, bool is_new_frame) {
262 [ + + ]: 15838 : if (!m_initialized) {
263 : 286 : m_level = raw_input;
264 : 286 : m_trend = 0.0;
265 : 286 : m_time_since_update = 0.0;
266 : 286 : m_initialized = true;
267 : 286 : return raw_input;
268 : : }
269 : :
270 [ + + ]: 15552 : if (is_new_frame) {
271 : 15536 : double prev_level = m_level;
272 : :
273 : : // Update Level: Balance between the raw input and our previous prediction
274 : 15536 : m_level = m_alpha * raw_input + (1.0 - m_alpha) * (m_level + m_trend * m_game_tick);
275 : :
276 : : // Update Trend: Balance between the new observed slope and the old trend
277 : 15536 : m_trend = m_beta * ((m_level - prev_level) / m_game_tick) + (1.0 - m_beta) * m_trend;
278 : :
279 : 15536 : m_time_since_update = 0.0;
280 : :
281 : : // FIX: Return the smoothed level to maintain a continuous signal
282 : 15536 : return m_level;
283 : : } else {
284 : 16 : m_time_since_update += dt;
285 : : }
286 : :
287 : : // Predict current state based on previous trend (Upsampling step)
288 : 16 : return m_level + m_trend * m_time_since_update;
289 : : }
290 : :
291 : 25 : void Reset() {
292 : 25 : m_level = m_trend = m_time_since_update = 0.0;
293 : 25 : m_initialized = false;
294 : 25 : }
295 : : };
296 : :
297 : : } // namespace ffb_math
298 : :
299 : : #endif // MATH_UTILS_H
|
